3D rotation represented by the quaternion. More...

Public Member Functions
	SO3_ ()
	Default constructor, Idendity Matrix.

	SO3_ (const Precision &X, const Precision &Y, const Precision &Z, const Precision &W)
	Construct from Quaternion.

	SO3_ (const Point3_< Precision > &direction, Precision angle)
	Construct from a direction and angle in radius.

	SO3_ (const Precision *M)
	Construct from a rotation matrix data (ColMajor).

	SO3_ (const Matrix< Precision, 3, 3 > &M)
	Construct from a rotation matrix.

template<typename Scalar >
	SO3_ (const SO3_< Scalar > &r)
	Construct from another precision.

Point3_< Precision >	log () const

void	fromMatrix (const Precision *m)
	This is an unsafe operation. Please make sure that your pointer is both valid and has an appropriate size Wrong usage: Precision* p; getMatrix(p);. More...

Matrix< Precision, 3, 3 >	getMatrix () const

void	getMatrix (Precision *m) const
	return the matrix M

Precision	getRoll () const

Precision	getPitch () const

Precision	getYaw () const

SO3_	operator* (const SO3_ &rq) const

Point3_< Precision >	operator* (const Point3_< Precision > &p) const

bool	operator== (const SO3_ &rq) const

void	normalise ()

SO3_	inverse () const

void	getValue (Precision &X, Precision &Y, Precision &Z, Precision &W) const

	operator std::string () const

Precision	getX () const

Precision	getY () const

Precision	getZ () const

Precision	getW () const

void	setX (Precision X)

void	setY (Precision Y)

void	setZ (Precision Z)

void	setW (Precision W)

SO3_	mul (const SO3_ &rq) const

Point3_< Precision >	trans (const Point3_< Precision > &p) const

std::string	toString () const

Static Public Member Functions
template<typename Scalar >
static SO3_< Precision >	exp (const Point3_< Scalar > &r)
	Construct from lie algebra representation.

template<typename T >
static T	sine (T x)

template<typename T >
static T	cosine (T x)

template<typename Scalar >
static SO3_< Scalar >	expFast (const Point3_< Scalar > &l)

static SO3_	fromPitchYawRollAngle (const Precision &pitch, const Precision &yaw, const Precision &roll)

static SO3_	fromPitchYawRoll (const Point3d &pyr)

static SO3_	fromPitchYawRoll (const Precision &pitch, const Precision &yaw, const Precision &roll)
	Convert from Euler Angles, Please use "Radian" rather than degree to present angle.

static SO3_	FromAxis (const Point3_< Precision > &p, Precision angle)

Public Attributes
Precision	x

Precision	y

Precision	z

Precision	w

Detailed Description

template<class Precision = double>
class GSLAM::SO3_< Precision >

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:

TEST(Transform,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.inverse()*q);
    std::uniform_real_distribution<double> pt_gen(-1000,1000);
    Point3d xyz(pt_gen(e),pt_gen(e),pt_gen(e));
    Point3d p1=q.inverse()*q*xyz;
    EXPECT_NEAR((xyz-p1).norm(),0,1e-6);
}