[DataFusionBase] fix fusion when a fused percept is created directly …

…in hand

src/Perception/DataFusion/DataFusionBase.cpp

@@ -14,14 +14,18 @@ namespace owds {
 
       auto fused_percept_it = fusioned_percepts.find(pair.first);
       if(fused_percept_it == fusioned_percepts.end())
+      {
         fused_percept_it = fusioned_percepts.emplace(pair.first, new Percept<Object>(pair.second.front())).first;
+        fused_percept_it->second->removeFromHand();
+      }
 
       Percept<Object>* percept = fused_percept_it->second;
       std::string percept_id = pair.first;
 
       Hand* hand = nullptr;
       Pose pose_in_hand;
       Pose pose_in_map;
+      bool located_in_map = false;
       int nb_frame_unseen = 1000;
 
       // We try to find if the percept should be in hand
@@ -41,6 +45,7 @@ namespace owds {
         {
           // We take the pose of the most recently perceived percept
           pose_in_map = obj.pose();
+          located_in_map = true;
           nb_frame_unseen = obj.getNbFrameUnseen();
         }
       }
@@ -74,7 +79,12 @@ namespace owds {
         if(percept->isInHand())
         {
           Hand* hand = percept->getHandIn();
+          auto pose_tmp = percept->pose();
           hand->removePerceptFromHand(percept->id());
+          if(located_in_map)
+            percept->updatePose(pose_in_map);
+          else
+            percept->updatePose(pose_tmp);
         }
 
         percept->setNbFrameUnseen(nb_frame_unseen);