Resolve "Add covariance to object pose"

Closes #69 (closed)

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp

@@ -32,15 +32,20 @@ namespace armarx::objpose
         isStatic = ice.isStatic;
         armarx::fromIce(ice.objectID, objectID);
 
-        objectPoseRobot = ::armarx::fromIce(ice.objectPoseRobot);
-        objectPoseGlobal = ::armarx::fromIce(ice.objectPoseGlobal);
-        objectPoseOriginal = ::armarx::fromIce(ice.objectPoseOriginal);
+        armarx::fromIce(ice.objectPoseRobot, objectPoseRobot);
+        objpose::fromIce(ice.objectPoseRobotCovariance, objectPoseRobotCovariance);
+
+        armarx::fromIce(ice.objectPoseGlobal, objectPoseGlobal);
+        objpose::fromIce(ice.objectPoseGlobalCovariance, objectPoseGlobalCovariance);
+
+        armarx::fromIce(ice.objectPoseOriginal, objectPoseOriginal);
         objectPoseOriginalFrame = ice.objectPoseOriginalFrame;
+        objpose::fromIce(ice.objectPoseOriginalCovariance, objectPoseOriginalCovariance);
 
         objectJointValues = ice.objectJointValues;
 
         robotConfig = ice.robotConfig;
-        robotPose = ::armarx::fromIce(ice.robotPose);
+        armarx::fromIce(ice.robotPose, robotPose);
 
         objpose::fromIce(ice.attachment, this->attachment);
 
@@ -64,10 +69,16 @@ namespace armarx::objpose
         ice.isStatic = isStatic;
         armarx::toIce(ice.objectID, objectID);
 
-        ice.objectPoseRobot = new Pose(objectPoseRobot);
-        ice.objectPoseGlobal = new Pose(objectPoseGlobal);
-        ice.objectPoseOriginal = new Pose(objectPoseOriginal);
+        armarx::toIce(ice.objectPoseRobot, objectPoseRobot);
+        objpose::toIce(ice.objectPoseRobotCovariance, objectPoseRobotCovariance);
+
+        armarx::toIce(ice.objectPoseGlobal, objectPoseGlobal);
+        objpose::toIce(ice.objectPoseGlobalCovariance, objectPoseGlobalCovariance);
+
+        armarx::toIce(ice.objectPoseOriginal, objectPoseOriginal);
         ice.objectPoseOriginalFrame = objectPoseOriginalFrame;
+        objpose::toIce(ice.objectPoseOriginalCovariance, objectPoseOriginalCovariance);
+
         ice.objectJointValues = objectJointValues;
 
         ice.robotConfig = robotConfig;
@@ -88,15 +99,20 @@ namespace armarx::objpose
         isStatic = provided.isStatic;
         armarx::fromIce(provided.objectID, objectID);
 
-        objectPoseOriginal = ::armarx::fromIce(provided.objectPose);
+        armarx::fromIce(provided.objectPose, objectPoseOriginal);
+        objpose::fromIce(provided.objectPoseCovariance, objectPoseOriginalCovariance);
         objectPoseOriginalFrame = provided.objectPoseFrame;
+
         objectJointValues = provided.objectJointValues;
 
         armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
         framed.changeFrame(robot, robot->getRootNode()->getName());
         objectPoseRobot = framed.toEigen();
+        objectPoseRobotCovariance = std::nullopt;  // ToDo: Transform covariance matrices
+
         framed.changeToGlobal(robot);
         objectPoseGlobal = framed.toEigen();
+        objectPoseGlobalCovariance = std::nullopt;  // ToDo: Transform covariance matrices
 
         robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
         robotPose = robot->getGlobalPose();
@@ -174,6 +190,7 @@ namespace armarx::objpose
         robotPose = agent->getGlobalPose();
         robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
     }
+
 }
 
 
@@ -211,7 +228,6 @@ namespace armarx
         return info;
     }
 
-
     void objpose::toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment)
     {
         ice.agentName = attachment.agentName;