Merge branch 'fusion' into dev

.clang-format

@@ -0,0 +1,55 @@
+---
+BasedOnStyle: Microsoft
+AlignArrayOfStructures: Left
+AllowShortBlocksOnASingleLine: Always
+AllowShortCaseLabelsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: All
+BraceWrapping:
+  AfterCaseLabel: true
+  AfterClass: true
+  AfterControlStatement: Always
+  AfterEnum: true
+  AfterFunction: true
+  AfterNamespace: false
+  AfterObjCDeclaration: true
+  AfterStruct: true
+  AfterUnion: true
+  AfterExternBlock: true
+  BeforeCatch: true
+  BeforeElse: true
+  BeforeLambdaBody: false
+  BeforeWhile: false
+  IndentBraces: false
+  SplitEmptyFunction: false
+  SplitEmptyRecord: false
+  SplitEmptyNamespace: false
+BreakArrays: false
+BreakBeforeTernaryOperators: false
+BreakInheritanceList: AfterComma
+BreakStringLiterals: false
+ColumnLimit: 0
+ConstructorInitializerIndentWidth: 2
+ContinuationIndentWidth: 2
+IncludeBlocks: Regroup
+IndentRequiresClause: false
+IndentWidth: 2
+KeepEmptyLinesAtTheStartOfBlocks: false
+PenaltyBreakTemplateDeclaration: 1000000
+PointerAlignment: Left
+SpaceAfterTemplateKeyword: false
+SpaceBeforeParens: Custom
+SpaceBeforeParensOptions:
+  AfterControlStatements: false
+  AfterForeachMacros: false
+  AfterIfMacros: false
+SpacesInContainerLiterals: false
+TabWidth: 2
+BracedInitializerIndentWidth: 2
+NamespaceIndentation: All
+IncludeCategories:
+  - Regex:           '^\<.*\>'
+    Priority:        1
+    CaseSensitive:   false
+  - Regex:           '^".*"'
+    Priority:        2
+    CaseSensitive:   false

include/overworld/Perception/Managers/EntitiesPerceptionManager.h

@@ -49,7 +49,7 @@ namespace owds {
     onto::OntologiesManipulator ontos_;
     onto::OntologyManipulator* onto_;
 
-    void fusionAggregated(const std::string& entity_id, const std::string& module_name, Percept<T> percept);
+    void fusionAggregated(const std::string& entity_id, const std::string& module_name, const Percept<T>& percept);
     void fusionRegister(const std::string& entity_id, const std::string& sensor_id, const std::string& module_name);
     virtual void getPercepts(const std::string& module_name, std::map<std::string, Percept<T>>& percepts);
     virtual void reasoningOnUpdate() {}
@@ -100,13 +100,11 @@ namespace owds {
   template<typename T>
   void EntitiesPerceptionManager<T>::fusionRegister(const std::string& entity_id, const std::string& sensor_id, const std::string& module_name)
   {
-    if(!sensor_id.empty()) // we avoid problems with static object
+    if(sensor_id.empty() == false) // we avoid problems with static object
     {
       auto it = entities_percetion_register_.find(entity_id);
       if(it == entities_percetion_register_.end())
-        entities_percetion_register_.emplace(entity_id, std::map<std::string, std::set<std::string>>{
-                                                          {sensor_id, {module_name}}
-        });
+        entities_percetion_register_.emplace(entity_id, std::map<std::string, std::set<std::string>>{{sensor_id, {module_name}}});
       else
       {
         auto inner_it = it->second.find(sensor_id);
@@ -119,14 +117,12 @@ namespace owds {
   }
 
   template<typename T>
-  void EntitiesPerceptionManager<T>::fusionAggregated(const std::string& entity_id, const std::string& module_name, Percept<T> percept)
+  void EntitiesPerceptionManager<T>::fusionAggregated(const std::string& entity_id, const std::string& module_name, const Percept<T>& percept)
   {
     auto it = aggregated_.find(entity_id);
     if(it == aggregated_.end())
     {
-      auto p = aggregated_.emplace(entity_id, std::map<std::string, Percept<T>>{
-                                       {module_name, percept}
-      }).first;
+      aggregated_.emplace(entity_id, std::map<std::string, Percept<T>>{{module_name, percept}});
     }
     else
     {
@@ -185,6 +181,9 @@ namespace owds {
   template<typename T>
   void EntitiesPerceptionManager<T>::removeEntityPose(T* entity)
   {
+    auto percept = fusioned_percepts_.find(entity->id());
+    if(percept != fusioned_percepts_.end())
+      percept->second->unsetPose();
     entity->unsetPose();
     updateToBullet(entity);
   }

src/Perception/DataFusion/DataFusionBase.cpp

@@ -57,7 +57,7 @@ namespace owds {
         for(auto& inner_it : it.second)
           percept->merge(&inner_it.second, false); // to update the shape but not the pose
         percept->setSeen();
-        // If the precept was already in hand we do not have to update the transform
+        // If the percept was already in hand we do not have to update the transform
         if(percept->isInHand() == false)
         {
           hand->putPerceptInHand(percept);
@@ -68,11 +68,11 @@ namespace owds {
             pose_in_hand = pose_in_map.transformIn(hand->pose());
             percept->updatePose(pose_in_hand);
           }
-          //else
-            //percept->updatePose(Pose());
+          // else
+          // percept->updatePose(Pose());
         }
-        //else
-          //percept->updatePose(percept->poseRaw()); //update pose in hand
+        // else
+        // percept->updatePose(percept->poseRaw()); //update pose in hand
       }
       else
       {
@@ -81,13 +81,16 @@ namespace owds {
           Hand* hand = percept->getHandIn();
           auto pose_tmp = percept->pose();
           hand->removePerceptFromHand(percept->id());
+          for(auto& false_id : percept->getFalseIds())
+            hand->removePerceptFromHand(false_id);
+
           if(located_in_map)
             percept->updatePose(pose_in_map);
           else
             percept->updatePose(pose_tmp);
-          
-          //percept->updatePose(pose_tmp);
-          //nb_frame_unseen = 0;
+
+          // percept->updatePose(pose_tmp);
+          // nb_frame_unseen = 0;
         }
 
         percept->setNbFrameUnseen(nb_frame_unseen);

src/Perception/Managers/ObjectsPerceptionManager.cpp

@@ -26,7 +26,7 @@ namespace owds {
     {
       if(object.second->isStatic() == false)
       {
-        if(object.second->isLocated())
+        if(object.second->isLocated() && (object.second->isInHand() == false))
         {
           if(simulated_objects_.find(object.second->id()) == simulated_objects_.end())
           {
@@ -100,8 +100,7 @@ namespace owds {
           entity_id = merged_id->second;
       }
 
-      if(percept.second.isLocated())
-        fusionAggregated(entity_id, module_name, percept.second);
+      fusionAggregated(entity_id, module_name, percept.second);
 
       if(percept.second.getSensorId().empty() == false)
         fusionRegister(percept.first, percept.second.getSensorId(), percept.second.getModuleName());
@@ -532,24 +531,26 @@ namespace owds {
       if(false_obj->second->isLocated() == false)
         continue;
 
-      double obj_volume = false_obj->second->getAabbVolume();
+      double obj_volume = false_obj->second->getBbVolume();
       double min_error = 10000;
       Percept<Object>* to_be_merged = nullptr;
 
-      for(auto percept : fusioned_percepts_)
+      for(auto& percept : fusioned_percepts_)
       {
         if(percept.second->isStatic())
           continue;
         else if(percept.second->isLocated() == false)
           continue;
+        else if(percept.second->getBbVolume() == 0) // not yet initialized
+          continue;
         else if(merged_ids_.find(percept.first) != merged_ids_.end()) // we cannot merge two false ids together
           continue;
 
         if(percept.first != false_obj->first)
         {
           if(percept.second->pose().distanceTo(false_obj->second->pose()) <= 0.1) // TODO tune
           {
-            double error = std::abs(obj_volume - percept.second->getAabbVolume());
+            double error = std::abs(obj_volume - percept.second->getBbVolume());
             if(error < min_error)
             {
               to_be_merged = percept.second;
@@ -576,7 +577,9 @@ namespace owds {
       false_ids_to_be_merged_.erase(false_id);
       aggregated_.erase(false_id);
 
-      delete fusioned_percepts_.at(false_id);
+      auto to_be_removed = fusioned_percepts_.at(false_id);
+
+      delete to_be_removed;
       fusioned_percepts_.erase(false_id);
 
       if(entities_.find(false_id) != entities_.end())
@@ -610,6 +613,16 @@ namespace owds {
                              (bb.max[1] - bb.min[1]) / 2. + bb.min[1],
                              (bb.max[2] - bb.min[2]) / 2. + bb.min[2]});
     object->computeCorners();
+
+    auto percept_it = fusioned_percepts_.find(object->id());
+    if(percept_it != fusioned_percepts_.end())
+    {
+      percept_it->second->setBoundingBox({bb.max[0] - bb.min[0], bb.max[1] - bb.min[1], bb.max[2] - bb.min[2]});
+      percept_it->second->setOriginOffset({(bb.max[0] - bb.min[0]) / 2. + bb.min[0],
+                                           (bb.max[1] - bb.min[1]) / 2. + bb.min[1],
+                                           (bb.max[2] - bb.min[2]) / 2. + bb.min[2]});
+      percept_it->second->computeCorners();
+    }
   }
 
 } // namespace owds