#include <sim3.h>

Public Member Functions
	Sim3 ()

	Sim3 (const Eigen::Quaterniond &r, const Eigen::Vector3d &t, double s)

	Sim3 (const Eigen::Matrix3d &R, const Eigen::Vector3d &t, double s)

	Sim3 (const Vector7d &update)

Eigen::Vector3d	map (const Eigen::Vector3d &xyz) const

Vector7d	log () const

Sim3	inverse () const

double	operator[] (int i) const

double &	operator[] (int i)

Sim3	operator* (const Sim3 &other) const

Sim3 &	operator*= (const Sim3 &other)

void	normalizeRotation ()

const Eigen::Vector3d &	translation () const

Eigen::Vector3d &	translation ()

const Eigen::Quaterniond &	rotation () const

Eigen::Quaterniond &	rotation ()

const double &	scale () const

double &	scale ()

Protected Attributes
Eigen::Quaterniond	r

Eigen::Vector3d	t

double	s

Detailed Description

Definition at line 38 of file sim3.h.

Constructor & Destructor Documentation

g2o::Sim3::Sim3 ( )

inline

Definition at line 48 of file sim3.h.

Referenced by inverse().

       {
         r.setIdentity();
         t.fill(0.);
         s=1.;
       }

g2o::Sim3::Sim3	(	const Eigen::Quaterniond &	r,
		const Eigen::Vector3d &	t,
		double	s
	)

inline

Definition at line 55 of file sim3.h.

References normalizeRotation().

         : r(r),t(t),s(s)
       {
                         normalizeRotation();
       }

g2o::Sim3::Sim3	(	const Eigen::Matrix3d &	R,
		const Eigen::Vector3d &	t,
		double	s
	)

inline

Definition at line 61 of file sim3.h.

References normalizeRotation().

         : r(Eigen::Quaterniond(R)),t(t),s(s)
       {
                         normalizeRotation();
       }

g2o::Sim3::Sim3 ( const Vector7d & update )

inline

Definition at line 68 of file sim3.h.

References g2o::skew().

       {
 
         Eigen::Vector3d omega;
         for (int i=0; i<3; i++)
           omega[i]=update[i];
 
         Eigen::Vector3d upsilon;
         for (int i=0; i<3; i++)
           upsilon[i]=update[i+3];
 
         double sigma = update[6];
         double theta = omega.norm();
         Eigen::Matrix3d Omega = skew(omega);
         s = std::exp(sigma);
         Eigen::Matrix3d Omega2 = Omega*Omega;
         Eigen::Matrix3d I;
         I.setIdentity();
         Eigen::Matrix3d R;
 
         double eps = 0.00001;
         double A,B,C;
         if (fabs(sigma)<eps)
         {
           C = 1;
           if (theta<eps)
           {
             A = 1./2.;
             B = 1./6.;
             R = (I + Omega + Omega*Omega);
           }
           else
           {
             double 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)
           {
             double sigma2= sigma*sigma;
             A = ((sigma-1)*s+1)/sigma2;
             B= ((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;
             double a=s*sin(theta);
             double b=s*cos(theta);
             double theta2= theta*theta;
             double sigma2= sigma*sigma;
             double c=theta2+sigma2;
             A = (a*sigma+ (1-b)*theta)/(theta*c);
             B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2);
 
           }
         }
         r = Eigen::Quaterniond(R);
 
 
 
         Eigen::Matrix3d W = A*Omega + B*Omega2 + C*I;
         t = W*upsilon;
       }

Member Function Documentation

Sim3 g2o::Sim3::inverse ( ) const

inline

Definition at line 220 of file sim3.h.

References s, and Sim3().

Referenced by g2o::EdgeSim3::computeError(), and g2o::EdgeSim3::read().

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

Vector7d g2o::Sim3::log ( ) const

inline

Definition at line 142 of file sim3.h.

References g2o::deltaR(), and g2o::skew().

Referenced by g2o::EdgeSim3::computeError(), g2o::VertexSim3Expmap::write(), and g2o::EdgeSim3::write().

       {
         Vector7d res;
         double sigma = std::log(s);
 
         Eigen::Vector3d omega;
         Eigen::Vector3d upsilon;
 
         Eigen::Matrix3d R = r.toRotationMatrix();
         double d =  0.5*(R(0,0)+R(1,1)+R(2,2)-1);
 
         Eigen::Matrix3d Omega;
 
         double eps = 0.00001;
         Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
 
         double A,B,C;
         if (fabs(sigma)<eps)
         {
           C = 1;
           if (d>1-eps)
           {
             omega=0.5*deltaR(R);
             Omega = skew(omega);
             A = 1./2.;
             B = 1./6.;
           }
           else
           {
             double theta = acos(d);
             double 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)
           {
             double sigma2 = sigma*sigma;
             omega=0.5*deltaR(R);
             Omega = skew(omega);
             A = ((sigma-1)*s+1)/(sigma2);
             B = ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma);
           }
           else
           {
             double theta = acos(d);
             omega = theta/(2*sqrt(1-d*d))*deltaR(R);
             Omega = skew(omega);
             double theta2 = theta*theta;
             double a=s*sin(theta);
             double b=s*cos(theta);
             double c=theta2 + sigma*sigma;
             A = (a*sigma+ (1-b)*theta)/(theta*c);
             B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2);
           }
         }
 
         Eigen::Matrix3d 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;
       }

Eigen::Vector3d g2o::Sim3::map ( const Eigen::Vector3d & xyz ) const

inline

Definition at line 138 of file sim3.h.

Referenced by g2o::EdgeSim3ProjectXYZ::computeError().

                                                          {
         return s*(r*xyz) + t;
       }

void g2o::Sim3::normalizeRotation ( )

inline

Definition at line 265 of file sim3.h.

Referenced by Sim3().

                             {
         if (r.w()<0){
           r.coeffs() *= -1;
         }
         r.normalize();
     }

Sim3 g2o::Sim3::operator* ( const Sim3 & other ) const

inline

Definition at line 252 of file sim3.h.

References r, s, and t.

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

Sim3& g2o::Sim3::operator*= ( const Sim3 & other )

inline

Definition at line 260 of file sim3.h.

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

double g2o::Sim3::operator[] ( int i ) const

inline

Definition at line 225 of file sim3.h.

References s.

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

double& g2o::Sim3::operator[] ( int i )

inline

Definition at line 238 of file sim3.h.

References s.

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