PoseManifoldGaussian.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
namespace armarx::objpose
{
 
    /**
     * @brief A "gaussian" distribution in pose space (i.e. pose manifold).
     *
     * The mean is a specific pose, represented as 4x4 homogeneous matrix.
     *
     * 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.
     */
    struct PoseManifoldGaussian
    {
    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;
            Eigen::Matrix3f orientation;
            Eigen::Vector3f size;
        };
 
        /// Get the parameters of a 3D ellipsoid illustrating this gaussian.
        Ellipsoid 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;
 
 
        /**
         * @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;
    };
 
} // namespace armarx::objpose