PoseManifoldGaussian.cpp

Go to the documentation of this file.

#include "PoseManifoldGaussian.h"
 
#include <SimoxUtility/math/pose/invert.h>
#include <SimoxUtility/math/pose/pose.h>
 
namespace armarx::objpose
{
 
    PoseManifoldGaussian::Ellipsoid
    PoseManifoldGaussian::getPositionEllipsoid() const
    {
        const float minSize = 0.005;
 
        const Eigen::Matrix3f covarianceTranslation = this->covariance.block<3, 3>(0, 0);
 
        const Eigen::EigenSolver<Eigen::Matrix3f> eigen(covarianceTranslation);
        const Eigen::Vector3f eval = eigen.eigenvalues().real();
        const Eigen::Matrix3f evec = eigen.eigenvectors().real();
 
        Ellipsoid result;
        result.center = simox::math::position(this->mean);
 
        /* Eigen values are local x, y, and z of ellipsoid:
         * (       )
         * ( x y z ) = u
         * (       )
         *
         * Eigen vectors are sorted by the singular values:
         * ( e_x )
         * ( e_y ) = ev, (e_x >= e_y >= e_z)
         * ( e_z )
         */
        result.orientation = evec.determinant() > 0 ? evec : -evec;
 
        result.size = eval;
 
        // Handle very small values.
        result.size = result.size.cwiseMax(Eigen::Vector3f::Constant(minSize));
 
        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};
    }
 
    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);
    }
 
} // namespace armarx::objpose