GSLAM implemented some high performance transformation classes in headers without other dependencies.

The below table is a comparison with other three popular c++ transform implementations. The table statistics time usages for looping 1M times of transform multiply, point transform, exponential and logarithm in milli-seconds on an i7-6700 CPU running 64bit Ubuntu.

Function	GSLAM	Sophus	TooN	Ceres
SO3::mult	14.9	34.3	17.8	159.1
SO3::trans	15.4	17.2	14.5	90.4
SO3::exp	80.7	98.4	106.8	-
SO3::log	55.7	72.5	63.8	-
SE3::mult	28.6	55.2	29.3	-
SE3::trans	19.3	19.8	12.1	-
SE3::exp	152.4	19. 8	99.2	-
SE3::log	152.7	194.0	205.8	-
SIM3::mult	33.2	58.5	34.5	-
SIM3::trans	16.9	17.2	13.7	-
SIM3::exp	180.2	286.8	229.0	-
SIM3::log	202.5	341.6	303.6	-

SO3: 3D rotation represented by the quaternion.

Introduction

For the rotational component, there are several choices for representation, including the matrix, Euler angle, unit quaternion and Lie algebra \(so(3)\). For a given transformation, we can use any of these for representation and can convert one to another.:

An unit orthogonal 3x3 matrix \( \mathbf{R} \).
The Euler angle representation uses 3 variables such as yaw, roll, pitch.
Quaternion \(q(x,y,z,w)\): the most efficient way to perform multiple.
Lie algebra \([a,b,c]\): the common representation to perform manifold optimization.

This implementation use Quaternion for computation. Class SO3 use 4 paraments to present a 3 dimesion rotation matrix, since 3D rotation matrices are members of the Special Orthogonal Lie group SO3.

Every rotation in the 3D euclidean space can be represented by a rotation with one direction. Consider a rotation with direction \((a,b,c)^T\) and angle theta in radians.

w – \(cos(theta/2)\)
x – \(a*sin(theta/2)\)
y – \(b*sin(theta/2)\)
z – \(c*sin(theta/2)\)

this ensures that \(x^2+y^2+z^2+w^2=1\). A quaternion \(q(x,y,z,w)\) is used to present a 3d rotation.

Constructors

Users can construct a SO3 from different data structure:

SO3 I;// default, idendity
SO3 R1(x,y,z,w);// from quaternion
SO3 R2(Poin3d(rx,ry,rz),angle)// from axis and angle
SO3 R3=SO3::exp(Point3d(rx,ry,rz));// from lie algebra
SO3 R4=SO3(Matrix3d(m));// from matrix
SO3 R4=SO3(m);// from pointer
SO3 R5=SO3::fromPitchYawRoll(pitch,yaw,roll); // from Euler

Usages

The following testing codes demonstrated the basic usages of SO3:

std::default_random_engine e;
double pitch=std::uniform_real_distribution<double>(-M_PI/2,M_PI/2)(e);
double yaw  =std::uniform_real_distribution<double>(-M_PI,M_PI)(e);
double roll =std::uniform_real_distribution<double>(-M_PI/2,M_PI/2)(e);
SO3 q=SO3::fromPitchYawRoll(pitch,yaw,roll);
EXPECT_NEAR(pitch,q.getPitch(),1e-5);
EXPECT_NEAR(yaw,q.getYaw(),1e-5);
EXPECT_NEAR(roll,q.getRoll(),1e-5);
// X: forward Y: right Z: down
SO3 qRoll =SO3::exp(Point3d(roll,0,0));
SO3 qPitch=SO3::exp(Point3d(0,pitch,0));
SO3 qYaw  =SO3::exp(Point3d(0,0,yaw));
SO3 q1=qYaw*qPitch*qRoll;
EXPECT_EQ(q,q1);
Matrix3d m=q.getMatrix();
q1=SO3(m);
EXPECT_EQ(q,q1);
Point3d abc=q.log();
q1=SO3::exp(abc);
EXPECT_EQ(q,q1);
EXPECT_EQ(SO3(),q.inv()*q);
std::uniform_real_distribution<double> pt_gen(-1000,1000);
Point3d xyz(pt_gen(e),pt_gen(e),pt_gen(e));
Point3d p1=q.inv()*q*xyz;
EXPECT_NEAR((xyz-p1).norm(),0,1e-6);

SE3: 3D Rigid Transformation.

A 3D rigid transformation can be presented with an special 3D Homography matrix \(\mathbf{T}\) constructed with translation \(t\) and rotation \(R\).

\( \mathbf{T} = \left[\begin{array}{cc}\mathbf{R}& \mathbf{t} \\ \mathbf{0}^T& 1 \end{array} \right]= \left[\begin{array}{cccc}rx_x & ry_x & rz_x & x \\ rx_y & ry_y & rz_y & y\\ rx_z & ry_z & rz_z & z\\ 0&0&0& 1\end{array} \right]. \)

Here \(\mathbf{R}=(\mathbf{rx},\mathbf{ry},\mathbf{rz})\) is the rotation , \(\mathbf{t}=(x,y,z)^T\) is the translation.

When matrix \(T\) means a camera to world transform:

\(\mathbf{rx}\) means the direction of X axis;
\(\mathbf{ry}\) means the direction of Y axis;
\(\mathbf{rz}\) means the direction of Z axis;
\(\mathbf{t}\) means the translation of this coordinate.

Here below is little test example with some usages demo:

TEST(Transform,SE3){
    std::default_random_engine e;
    double pitch=std::uniform_real_distribution<double>(-M_PI/2,M_PI/2)(e);
    double yaw  =std::uniform_real_distribution<double>(-M_PI,M_PI)(e);
    double roll =std::uniform_real_distribution<double>(-M_PI/2,M_PI/2)(e);
    std::uniform_real_distribution<double> pt_gen(-1000,1000);
    SE3 T(SO3::fromPitchYawRoll(pitch,yaw,roll),
          Point3d(pt_gen(e),pt_gen(e),pt_gen(e)));
    Point3d pt(pt_gen(e),pt_gen(e),pt_gen(e));
    Point3d error=T*pt-T.get_translation()-T.get_rotation()*pt;
    EXPECT_NEAR(error.norm(),0,1e-8);
    SE3 T1=T.inverse()*T;
    EXPECT_NEAR(T1.get_translation().norm(),0,1e-8);
    EXPECT_EQ(T1.get_rotation(),SO3());
    Vector6d se3=T.log();
    SE3 T2=T.inverse()*SE3::exp(se3);
    EXPECT_NEAR(T2.get_translation().norm(),0,1e-8);
    EXPECT_EQ(T2.get_rotation(),SO3());
}

Warning: It should be noticed that when a SE3 is representing the world-to-camera transform, the function get_translation() does not acqually return the translation.

SIM3: 3D similarity transformation

A SIM3 is the equivalent of a 4*4 Matrix \(S\):

\( \mathbf{S} = \left[\begin{array}{cc}s\mathbf{R}& \mathbf{t} \\ \mathbf{0}^T& 1 \end{array} \right]. \)

\(\mathbf{R}\): Rotation – 3dof
\(\mathbf{t}\): Translation – 3dof
s: scale –1dof

The following testing example demostrated some basic usages of SIM3.

TEST(Transform,SIM3){
    std::default_random_engine e;
    double pitch=std::uniform_real_distribution<double>(-M_PI/2,M_PI/2)(e);
    double yaw  =std::uniform_real_distribution<double>(-M_PI,M_PI)(e);
    double roll =std::uniform_real_distribution<double>(-M_PI/2,M_PI/2)(e);
    std::uniform_real_distribution<double> pt_gen(-1000,1000);
    SIM3 S(SO3::fromPitchYawRoll(pitch,yaw,roll),
          Point3d(pt_gen(e),pt_gen(e),pt_gen(e)),
          100);
    SIM3 S1=SIM3::exp(S.log());
    EXPECT_EQ(S.get_rotation(),S1.get_rotation());
    EXPECT_NEAR((S.get_translation()-S1.get_translation()).norm(),0,1e-6);
    EXPECT_NEAR(S.get_scale(),S1.get_scale(),1e-6);
    SIM3 SS=S*S1;
    Point3d pt(pt_gen(e),pt_gen(e),pt_gen(e));
    Point3d diff=S*S1*pt-SS*pt;
    EXPECT_NEAR(diff.norm(),0,1e-10);
    SIM3 ST=S*S1.get_se3();
    EXPECT_EQ(S.get_scale(),ST.get_scale());
}