se3mat.h

#pragma once
#include "DLLInfo.h"
#include <Eigen/Geometry>
 
namespace NDEVR {

    class SE3mat
    {
    public:
        SE3mat()
        {
            R = Eigen::Matrix3<g_type>::Identity();
            t.setZero();
        }

        SE3mat(const Eigen::Matrix3<g_type>& R_, const Eigen::Vector3<g_type>& t_)
            : R(R_)
            , t(t_)
        {}
 

        void Retract(const Eigen::Vector3<g_type> dr, const Eigen::Vector3<g_type>& dt)
        {
            t += R * dt;
            R = R * ExpSO3(dr);
        }
 

        inline Eigen::Vector3<g_type> operator* (const Eigen::Vector3<g_type>& v) const {
            return R * v + t;
        }

        inline SE3mat& operator*= (const SE3mat& T2) {
            t += R * T2.t;
            R *= T2.R;
            return *this;
        }

        inline SE3mat inverse() const {
            Eigen::Matrix3<g_type> Rt = R.transpose();
            return SE3mat(Rt, -Rt * t);
        }
 
    protected:
        Eigen::Matrix3<g_type> R;   
        Eigen::Vector3<g_type> t;   
 
    public:
        static Eigen::Matrix3<g_type> ExpSO3(const Eigen::Vector3<g_type> r)
        {
            Eigen::Matrix3<g_type> W;
            W << 0, -r[2], r[1],
                r[2], 0, -r[0],
                -r[1], r[0], 0;
 
            const g_type theta = r.norm();
 
            if (theta < g_type(1e-6))
                return Eigen::Matrix3<g_type>::Identity() + W + g_type(0.5) * W * W;
            else
                return Eigen::Matrix3<g_type>::Identity() + W * sin(theta) / theta + W * W * (1 - cos(theta)) / (theta * theta);
        }

        static Eigen::Vector3<g_type> LogSO3(const Eigen::Matrix3<g_type> R)
        {
            const g_type tr = R(0, 0) + R(1, 1) + R(2, 2);
            const g_type theta = acos((tr - static_cast<g_type>(1)) * static_cast<g_type>(0.5));
            Eigen::Vector3<g_type> w;
            w << R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1);
            if (theta < g_type(1e-6))
                return g_type(0.5) * w;
            else
            {
                const g_type sin_theta = sin(theta);
                if (std::abs(sin_theta) < g_type(1e-10))
                {
                    // theta near pi: use alternative formula
                    // Find the column of R+I with the largest diagonal for numerical stability
                    Eigen::Matrix3<g_type> RpI = R + Eigen::Matrix3<g_type>::Identity();
                    int max_col = 0;
                    if (RpI(1, 1) > RpI(0, 0)) max_col = 1;
                    if (RpI(2, 2) > RpI(max_col, max_col)) max_col = 2;
                    Eigen::Vector3<g_type> v = RpI.col(max_col).normalized();
                    return theta * v;
                }
                return theta * w / (g_type(2.0) * sin_theta);
            }
        }
 
    };
 
}