Add covariances to ObjectPose

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;

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h

@@ -8,13 +8,17 @@
 #include <Eigen/Core>
 
 #include <SimoxUtility/shapes/OrientedBox.h>
+
 #include <VirtualRobot/VirtualRobot.h>
-#include "ArmarXCore/core/PackagePath.h"
+
+#include <ArmarXCore/core/PackagePath.h>
 
 #include <IceUtil/Time.h>
 
 #include <RobotAPI/interface/objectpose/object_pose_types.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
 #include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/PoseCovariance.h>
 
 
 namespace armarx::objpose
@@ -33,6 +37,8 @@ namespace armarx::objpose
      */
     struct ObjectPose
     {
+    public:
+
         ObjectPose();
         ObjectPose(const data::ObjectPose& ice);
 
@@ -47,6 +53,8 @@ namespace armarx::objpose
         void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot = true);
 
 
+    public:
+
         /// Name of the providing component.
         std::string providerName;
         /// Known or unknown object.
@@ -58,9 +66,14 @@ namespace armarx::objpose
         armarx::ObjectID objectID;
 
         Eigen::Matrix4f objectPoseRobot;
+        std::optional<PoseCovariance> objectPoseRobotCovariance;
+
         Eigen::Matrix4f objectPoseGlobal;
+        std::optional<PoseCovariance> objectPoseGlobalCovariance;
+
         Eigen::Matrix4f objectPoseOriginal;
         std::string objectPoseOriginalFrame;
+        std::optional<PoseCovariance> objectPoseOriginalCovariance;
 
         /// The object's joint values if it is articulated.
         std::map<std::string, float> objectJointValues;
@@ -102,8 +115,6 @@ namespace armarx::objpose
         std::optional<PackagePath> articulatedSimoxXmlPath;
 
     };
-    using ObjectPoseSeq = std::vector<ObjectPose>;
-    using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
 
 
     void fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment);