Copy gaussians to robot/global if the original frame matches

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp

@@ -109,10 +109,32 @@ namespace armarx::objpose
         framed.changeFrame(robot, robot->getRootNode()->getName());
         objectPoseRobot = framed.toEigen();
         objectPoseRobotGaussian = std::nullopt;  // ToDo: Transform covariance matrices
+        if (objectPoseOriginalGaussian)
+        {
+            if (objectPoseOriginalFrame == robot->getRootNode()->getName())
+            {
+                objectPoseGlobalGaussian = objectPoseOriginalGaussian;
+            }
+            else
+            {
+                // ToDo: Transform covariance matrices
+            }
+        }
 
         framed.changeToGlobal(robot);
         objectPoseGlobal = framed.toEigen();
-        objectPoseGlobalGaussian = std::nullopt;  // ToDo: Transform covariance matrices
+        objectPoseGlobalGaussian = std::nullopt;
+        if (objectPoseOriginalGaussian)
+        {
+            if (objectPoseOriginalFrame == armarx::GlobalFrame)
+            {
+                objectPoseGlobalGaussian = objectPoseOriginalGaussian;
+            }
+            else
+            {
+                // ToDo: Transform covariance matrices
+            }
+        }
 
         robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
         robotPose = robot->getGlobalPose();