sim3.h

#pragma once
#include "se3_ops.h"
#include <Eigen/Geometry>
 
namespace NDEVR
{
    using namespace Eigen;
    typedef  Eigen::Matrix <g_type, 7, 1> Vector7d; 
    typedef  Eigen::Matrix <g_type, 7, 7> Matrix7d; 

    struct Sim3
    {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    protected:
        Quaternion<g_type> r;   
        Vector3<g_type> t;      
        g_type s;               
 
 
    public:
        Sim3()
        {
            r.setIdentity();
            t.fill(0.);
            s = 1.;
        }

        Sim3(const Quaternion<g_type>& r, const Vector3<g_type>& t, g_type s)
            : r(r), t(t), s(s)
        {
        }

        Sim3(const Matrix3<g_type>& R, const Vector3<g_type>& t, g_type s)
            : r(Quaternion<g_type>(R)), t(t), s(s)
        {
        }

        Sim3(const Vector7d& update, bool fix_scale)
        {
 
            Vector3<g_type> omega;
            for (int i = 0; i < 3; i++)
                omega[i] = update[i];
 
            Vector3<g_type> upsilon;
            for (int i = 0; i < 3; i++)
                upsilon[i] = update[i + 3];
 
            g_type sigma = fix_scale ? cast<g_type>(0) : update[6];
            g_type theta = omega.norm();
            Matrix3<g_type> Omega = skew(omega);
            s = std::exp(sigma);
            Matrix3<g_type> Omega2 = Omega * Omega;
            Matrix3<g_type> I;
            I.setIdentity();
            Matrix3<g_type> R;
 
            g_type eps = g_type(0.00001);
            g_type A, B, C;
            if (fabs(sigma) < eps)
            {
                C = 1;
                if (theta < eps)
                {
                    A = g_type(1. / 2.);
                    B = g_type(1. / 6.);
                    R = (I + Omega + Omega * Omega);
                }
                else
                {
                    g_type theta2 = theta * theta;
                    A = (1 - cos(theta)) / (theta2);
                    B = (theta - sin(theta)) / (theta2 * theta);
                    R = I + sin(theta) / theta * Omega + (1 - cos(theta)) / (theta * theta) * Omega2;
                }
            }
            else
            {
                C = (s - 1) / sigma;
                if (theta < eps)
                {
                    g_type sigma2 = sigma * sigma;
                    A = ((sigma - 1) * s + 1) / sigma2;
                    B = ((g_type(0.5) * sigma2 - sigma + 1) * s) / (sigma2 * sigma);
                    R = (I + Omega + Omega2);
                }
                else
                {
                    R = I + sin(theta) / theta * Omega + (1 - cos(theta)) / (theta * theta) * Omega2;
 
 
 
                    g_type a = s * sin(theta);
                    g_type b = s * cos(theta);
                    g_type theta2 = theta * theta;
                    g_type sigma2 = sigma * sigma;
 
                    g_type c = theta2 + sigma2;
                    A = (a * sigma + (1 - b) * theta) / (theta * c);
                    B = (C - ((b - 1) * sigma + a * theta) / (c)) * g_type(1.) / (theta2);
 
                }
            }
            r = Quaternion<g_type>(R);
 
 
 
            Matrix3<g_type> W = A * Omega + B * Omega2 + C * I;
            t = W * upsilon;
        }

        Vector3<g_type> map(const Vector3<g_type>& xyz) const {
            return s * (r * xyz) + t;
        }

        Vector7d log() const
        {
            Vector7d res;
            g_type sigma = std::log(s);
            Vector3<g_type> omega;
            Vector3<g_type> upsilon;
 
 
            Matrix3<g_type> R = r.toRotationMatrix();
            g_type d = g_type(0.5) * (R(0, 0) + R(1, 1) + R(2, 2) - 1);
 
            Matrix3<g_type> Omega;
 
            g_type eps = g_type(0.00001);
            Matrix3<g_type> I = Matrix3<g_type>::Identity();
 
            g_type A, B, C;
            if (fabs(sigma) < eps)
            {
                C = 1;
                if (d > 1 - eps)
                {
                    omega = 0.5 * deltaR(R);
                    Omega = skew(omega);
                    A = g_type(1.0 / 2.0);
                    B = g_type(1.0 / 6.0);
                }
                else
                {
                    g_type theta = acos(d);
                    g_type theta2 = theta * theta;
                    omega = theta / (2 * sqrt(1 - d * d)) * deltaR(R);
                    Omega = skew(omega);
                    A = (1 - cos(theta)) / (theta2);
                    B = (theta - sin(theta)) / (theta2 * theta);
                }
            }
            else
            {
                C = (s - 1) / sigma;
                if (d > 1 - eps)
                {
 
                    g_type sigma2 = sigma * sigma;
                    omega = 0.5 * deltaR(R);
                    Omega = skew(omega);
                    A = ((sigma - 1) * s + 1) / (sigma2);
                    B = ((g_type(0.5) * sigma2 - sigma + 1) * s) / (sigma2 * sigma);
                }
                else
                {
                    g_type theta = acos(d);
                    omega = theta / (2 * sqrt(1 - d * d)) * deltaR(R);
                    Omega = skew(omega);
                    g_type theta2 = theta * theta;
                    g_type a = s * sin(theta);
                    g_type b = s * cos(theta);
                    g_type c = theta2 + sigma * sigma;
                    A = (a * sigma + (1 - b) * theta) / (theta * c);
                    B = (C - ((b - 1) * sigma + a * theta) / (c)) * g_type(1.) / (theta2);
                }
            }
 
            Matrix3<g_type> W = A * Omega + B * Omega * Omega + C * I;
 
            upsilon = W.lu().solve(t);
 
 
            for (int i = 0; i < 3; i++)
                res[i] = omega[i];
 
            for (int i = 0; i < 3; i++)
                res[i + 3] = upsilon[i];
 
            res[6] = sigma;
 
            return res;
 
        }
 

        Sim3 inverse() const
        {
            return Sim3(r.conjugate(), r.conjugate() * ((g_type(- 1.) / s) * t), g_type(1.) / s);
        }
 

        g_type operator[](int i) const
        {
            assert(i < 8);
            if (i < 4) {
 
                return r.coeffs()[i];
            }
            if (i < 7) {
                return t[i - 4];
            }
            return s;
        }

        g_type& operator[](int i)
        {
            assert(i < 8);
            if (i < 4) {
 
                return r.coeffs()[i];
            }
            if (i < 7)
            {
                return t[i - 4];
            }
            return s;
        }

        Sim3 operator *(const Sim3& other) const {
            Sim3 ret;
            ret.r = r * other.r;
            ret.t = s * (r * other.t) + t;
            ret.s = s * other.s;
            return ret;
        }

        Sim3& operator *=(const Sim3& other) {
            Sim3 ret = (*this) * other;
            *this = ret;
            return *this;
        }

        inline const Vector3<g_type>& translation() const { return t; }
        inline Vector3<g_type>& translation() { return t; }
        inline const Quaternion<g_type>& rotation() const { return r; }
        inline Quaternion<g_type>& rotation() { return r; }
        inline const g_type& scale() const { return s; }
        inline g_type& scale() { return s; }
 
    };
 
}