Transform the PoseManifoldGaussians if necessary

source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp

 #include "ObjectPose.h"
 
 #include <SimoxUtility/math/pose/invert.h>
+#include <SimoxUtility/math/pose/pose.h>
 
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/RobotConfig.h>
@@ -108,32 +109,21 @@ namespace armarx::objpose
         armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
         framed.changeFrame(robot, robot->getRootNode()->getName());
         objectPoseRobot = framed.toEigen();
-        objectPoseRobotGaussian = std::nullopt;  // ToDo: Transform covariance matrices
-        if (objectPoseOriginalGaussian)
+        objectPoseRobotGaussian = std::nullopt;
+
+        objectPoseRobotGaussian = objectPoseOriginalGaussian;
+        if (objectPoseOriginalGaussian.has_value() and objectPoseOriginalFrame != robot->getRootNode()->getName())
         {
-            if (objectPoseOriginalFrame == robot->getRootNode()->getName())
-            {
-                objectPoseGlobalGaussian = objectPoseOriginalGaussian;
-            }
-            else
-            {
-                // ToDo: Transform covariance matrices
-            }
+            objectPoseRobotGaussian = objectPoseOriginalGaussian->getTransformed(objectPoseOriginal, objectPoseRobot);
         }
 
         framed.changeToGlobal(robot);
         objectPoseGlobal = framed.toEigen();
-        objectPoseGlobalGaussian = std::nullopt;
-        if (objectPoseOriginalGaussian)
+
+        objectPoseGlobalGaussian = objectPoseOriginalGaussian;
+        if (objectPoseOriginalGaussian.has_value() and objectPoseOriginalFrame != armarx::GlobalFrame)
         {
-            if (objectPoseOriginalFrame == armarx::GlobalFrame)
-            {
-                objectPoseGlobalGaussian = objectPoseOriginalGaussian;
-            }
-            else
-            {
-                // ToDo: Transform covariance matrices
-            }
+            objectPoseGlobalGaussian = objectPoseOriginalGaussian->getTransformed(objectPoseOriginal, objectPoseGlobal);
         }
 
         robotConfig = robot->getConfig()->getRobotNodeJointValueMap();

source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp

 #include "PoseManifoldGaussian.h"
 
 #include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/pose/invert.h>
 
 
 namespace armarx::objpose
@@ -54,4 +55,28 @@ namespace armarx::objpose
         return {angle, axis};
     }
 
+
+    PoseManifoldGaussian
+    PoseManifoldGaussian::getTransformed(const Eigen::Matrix4f& transform) const
+    {
+        Eigen::Matrix<float, 6, 6> covRotation = Eigen::Matrix<float, 6, 6>::Zero();
+        covRotation.block<3, 3>(0, 0) = covRotation.block<3, 3>(3, 3) = simox::math::orientation(transform);
+
+        PoseManifoldGaussian transformed = *this;
+        transformed.mean = transform * transformed.mean;
+        // We just need to apply the orientation.
+        transformed.covariance = covRotation * transformed.covariance * covRotation.transpose();
+        return transformed;
+    }
+
+
+    PoseManifoldGaussian
+    PoseManifoldGaussian::getTransformed(const Eigen::Matrix4f& fromFrame, const Eigen::Matrix4f& toFrame) const
+    {
+        // T_to = T_(from->to) * T_from
+        // T_(from->to) = T_to * (T_from)^-1
+        Eigen::Matrix4f transform = toFrame * simox::math::inverted_pose(fromFrame);
+        return getTransformed(transform);
+    }
+
 }

source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h

@@ -84,6 +84,24 @@ namespace armarx::objpose
         Eigen::AngleAxisf
         getScaledRotationAxis(int index, bool global = false) const;
 
+
+        /**
+         * @brief Transform this gaussian by the given transform (e.g. to another frame).
+         * @param transform The transform to apply.
+         * @return The transformed gaussian.
+         */
+        PoseManifoldGaussian
+        getTransformed(const Eigen::Matrix4f& transform) const;
+
+        /**
+         * @brief Transform this gaussian from one base coordinate system to another one.
+         * @param fromFrame The original (current) base coordinate system' pose.
+         * @param toFrame The new base coordinate system' pose.
+         * @return The same gaussian in thenew coordinate system.
+         */
+        PoseManifoldGaussian
+        getTransformed(const Eigen::Matrix4f& fromFrame, const Eigen::Matrix4f& toFrame) const;
+
     };
 
 }