Refactor code into functions

source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp

@@ -177,8 +177,8 @@ namespace armarx
                 if (i % 2 == 1)
                 {
                     Eigen::Matrix3f rot = Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ()).toRotationMatrix();
-                    pose.objectPoseGaussian->covariance.block<3, 3>(0, 0)
-                            = rot * pose.objectPoseGaussian->covariance.block<3, 3>(0, 0) * rot.transpose();
+                    pose.objectPoseGaussian->positionCovariance()
+                            = rot * pose.objectPoseGaussian->positionCovariance() * rot.transpose();
                 }
 
                 // Rotational (co)variance.

source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp

@@ -7,7 +7,7 @@ namespace armarx::objpose
 {
 
     PoseManifoldGaussian::Ellipsoid
-    PoseManifoldGaussian::getTranslationEllipsoid() const
+    PoseManifoldGaussian::getPositionEllipsoid() const
     {
         const float minSize = 0.005;
 
@@ -40,4 +40,18 @@ namespace armarx::objpose
         return result;
     }
 
+
+    Eigen::AngleAxisf
+    PoseManifoldGaussian::getScaledRotationAxis(int index, bool global) const
+    {
+        Eigen::Vector3f axis = covariance.block<3, 1>(3, 3 + index);
+        const float angle = axis.norm();
+        axis /= angle;
+        if (global)
+        {
+            axis = simox::math::orientation(mean) * axis;
+        }
+        return {angle, axis};
+    }
+
 }

source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h

 #pragma once
 
 #include <Eigen/Core>
+#include <Eigen/Geometry>
 
 
 namespace armarx::objpose
@@ -11,9 +12,13 @@ namespace armarx::objpose
      *
      * The mean is a specific pose, represented as 4x4 homogeneous matrix.
      *
-     * The covariance is a 6x6 covariance matrix. The first 3 dimensions
-     * represent the position (translation) part, the second 3 dimensions
-     * represent the orientation (rotation) part.
+     * The covariance is a 6 x 6 covariance matrix.
+     * The first 3 dimensions represent the position (translation) part,
+     * the second 3 dimensions represent the orientation (rotation) part.
+     * A column of the pure orientational part (lower right 3 x 3 matrix)
+     * is a scaled rotation axis, i.e. rotation axis whose norm is the
+     * rotation angle (in this case the "angle variance").
+     *
      * Note that the lower left and upper right 3x3 parts of the covariance
      * matrix can be non-zero.
      */
@@ -21,12 +26,39 @@ namespace armarx::objpose
     {
     public:
 
+        /// The mean (i.e. a pose).
         Eigen::Matrix4f mean = Eigen::Matrix4f::Identity();
+        /// The covariance matrix.
         Eigen::Matrix<float, 6, 6> covariance = Eigen::Matrix<float, 6, 6>::Identity();
 
 
     public:
 
+        /// Get the pure positional part of the covariance matrix.
+        Eigen::Block<Eigen::Matrix<float, 6, 6>, 3, 3>
+        positionCovariance()
+        {
+            return covariance.block<3, 3>(0, 0);
+        }
+        Eigen::Block<const Eigen::Matrix<float, 6, 6>, 3, 3>
+        positionCovariance() const
+        {
+            return covariance.block<3, 3>(0, 0);
+        }
+
+        /// Get the pure orientational part of the covariance matrix.
+        Eigen::Block<Eigen::Matrix<float, 6, 6>, 3, 3>
+        orientationCovariance()
+        {
+            return covariance.block<3, 3>(3, 3);
+        }
+        Eigen::Block<const Eigen::Matrix<float, 6, 6>, 3, 3>
+        orientationCovariance() const
+        {
+            return covariance.block<3, 3>(3, 3);
+        }
+
+
         struct Ellipsoid
         {
             Eigen::Vector3f center;
@@ -34,8 +66,23 @@ namespace armarx::objpose
             Eigen::Vector3f size;
         };
 
+        /// Get the parameters of a 3D ellipsoid illustrating this gaussian.
         Ellipsoid
-        getTranslationEllipsoid() const;
+        getPositionEllipsoid() const;
+
+
+        /**
+         * @brief Get a column of the pure orientational covariance matrix
+         * as axis-angle rotation.
+         *
+         * @param index
+         *  The column's index.
+         * @param global
+         *  If true, rotate the axis by the mean orientation.
+         * @return
+         */
+        Eigen::AngleAxisf
+        getScaledRotationAxis(int index, bool global = false) const;
 
     };
 

source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp

@@ -182,7 +182,7 @@ namespace armarx::armem::server::obj::instance
         if (objectPose.objectPoseGlobalGaussian.has_value() and (gaussiansPosition or gaussiansOrientation))
         {
             const objpose::PoseManifoldGaussian& gaussian = objectPose.objectPoseGlobalGaussian.value();
-            objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getTranslationEllipsoid();
+            objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
 
             if (gaussiansPosition)
             {
@@ -216,11 +216,8 @@ namespace armarx::armem::server::obj::instance
                 const float pi = static_cast<float>(M_PI);
                 for (int i = 0; i < 3; ++i)
                 {
-                    const Eigen::Vector3f scaledRotationAxis = gaussian.covariance.block<3, 1>(3, 3+i);
-                    const float angle = scaledRotationAxis.norm();
-                    const Eigen::Vector3f axis =
-                            simox::math::orientation(gaussian.mean)
-                            * (scaledRotationAxis / angle);
+                    const bool global = true;
+                    Eigen::AngleAxisf rot = gaussian.getScaledRotationAxis(i, global);
 
                     Eigen::Vector4i color = Eigen::Vector4i::Zero();
                     color(i) = 255;
@@ -228,12 +225,12 @@ namespace armarx::armem::server::obj::instance
 
                     layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
                               .position(gaussiansOrientationDisplaced
-                                        ? ellipsoid.center + gaussiansOrientationScale * axis
+                                        ? ellipsoid.center + gaussiansOrientationScale * rot.axis()
                                         : ellipsoid.center)
+                              .normal(rot.axis())
                               .radius(gaussiansOrientationScale)
-                              .normal(axis)
-                              .completion(simox::math::rescale(angle, 0.f, pi * 2, 0.f, 1.f))
-                              .width(simox::math::rescale(angle, 0.f, pi * 2, 2.f, 7.f))
+                              .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
+                              .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
                               .color(simox::Color(color))
                               );
                 }