2D_3D_Geometry

Contents

1. 2D/3D points, poses and homogeneous coordinates
2. Spatial geometry classes in the MRPT
3. Derivation of the 3D rotation matrix
4. Conversions between classes
5. Common operators
6. Quaternions
7. An example: A mobile robot sensing with a camera

1. 2D/3D points, poses and homogeneous coordinates

Using Homogeneous coordinates is one way of composing 2D or 3D translations and rotations easily. This page describes a set a set of classes in the MRPT C++ library aimed for 2D/3D geometry computations, which internally rely on these matrices.

2. Spatial geometry classes in the MRPT

The base class for all 2D/3D point and pose classes is poses::CPoseOrPoint (see class hierarchy graph).

This class and those directly derived from it (CPose and CPoint) are abstract so you cannot directly create instances of them. Instead, the classes a user should always employ are CPoint2D, CPoint3D, CPose2D, CPose3D and CPose3DQuat, described next.

Note that these classes only represent one value, whereas another set of classes (see this page) describes probability density functions over geometry relations.

2.1. CPoint2D: (x,y)

This class represents a 2D point (z coordinates defaults to 0):

Homogeneous transfomation matrix	Spatial representation
$\mathbf{R} = \left( \begin{array}{cccc} 1 & 0 & 0 & x \\ 0 & 1 & 0 & y \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right)$

An example of a construction of such an object with some initial values:

// Constructor: CPoint2D (double x=0, double y=0)
CPoint2D    p( 3.0, 2.0 );

1 2	// Constructor: CPoint2D (double x=0, double y=0) CPoint2D p( 3.0, 2.0 );

2.2. CPoint3D: (x,y,z)

This class represents a 3D point:

Homogeneous transfomation matrix	Spatial representation
$\mathbf{R}=\left( \begin{array}{cccc} 1 & 0 & 0 & x \\ 0 & 1 & 0 & y \\ 0 & 0 & 1 & z \\ 0 & 0 & 0 & 1 \end{array} \right)$

An example of a construction of such an object with some initial values:

// Constructor: CPoint3D (double x=0, double y=0, double z=0)
CPoint3D    p( 3.0, 2.0, 1.0 );

1 2	// Constructor: CPoint3D (double x=0, double y=0, double z=0) CPoint3D p( 3.0, 2.0, 1.0 );

2.3. CPose2D: (x,y,ϕ)

This class represents a 2D location plus a heading angle (also called ”yaw” or ”orientation”). This is the typical representation of ground robots working on flat scenarios. The z coordinate defaults to 0.

Homogeneous transfomation matrix	Spatial representation
$\mathbf{R}=\left( \begin{array}{cccc} \cos\phi & -\sin\phi & 0 & x \\ \sin\phi & \cos\phi & 0 & y \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right)$

An example of a construction of such an object with some initial values:

// Constructor:  CPose2D (double x=0,double y=0, double phi=0)
CPose2D     p( 1.0, 0.0, mrpt::DEG2RAD( 45.0 ) );

1 2	// Constructor: CPose2D (double x=0,double y=0, double phi=0) CPose2D p( 1.0, 0.0, mrpt::DEG2RAD( 45.0 ) );

2.4. CPose3D: (x,y,z,yaw,pitch,roll)

This class represents a full 6D pose: a 3D position plus three attitude angles: yaw, pitch, and roll, defined as can be seen in the figure (note positive pitch angles lie below the XY plane):

Homogeneous transfomation matrix	Spatial representation
$\mathbf{R}=\left( \begin{array}{cccc} cycp & cyspsr-sycr & cyspcr+sysr & x \\ sycp & syspsr+cycr & syspcr-cysr & y \\ -sp & cpsr & cpcr & z \\ 0 & 0 & 0 & 1 \end{array} \right)$

The abbreviations employed in the matrix are:

$\begin{array}{ll} cy = \cos yaw & sy = \sin yaw \\ cp = \cos pitch & sp = \sin pitch \\ cr = \cos roll & sr = \sin roll \end{array}$

An example of a construction of such an object with some initial values:

// Constructor:  CPose3D (double x=0, double y=0, double z=0, double yaw=0, float double =0, float double =0)
CPose3D     p( 1.0, 2.0, 3.0, mrpt::DEG2RAD( 45.0 ), mrpt::DEG2RAD( -90.0 ), mrpt::DEG2RAD( 180.0 ) );

1 2	// Constructor: CPose3D (double x=0, double y=0, double z=0, double yaw=0, float double =0, float double =0) CPose3D p( 1.0, 2.0, 3.0, mrpt::DEG2RAD( 45.0 ), mrpt::DEG2RAD( -90.0 ), mrpt::DEG2RAD( 180.0 ) );

2.5. CPose3DQuat: 3D translation + quaternion (x,y,z, qr,qx,qy,qz)

This class represents a full 6D pose: a 3D position (x,y,z) plus a quaternion for the 3D rotation (qr,qx,qy,qz):

$\begin{pmatrix} a^2+b^2-c^2-d^2&2bc-2ad &2bd+2ac \\ 2bc+2ad &a^2-b^2+c^2-d^2&2cd-2ab \\ 2bd-2ac &2cd+2ab &a^2-b^2-c^2+d^2\\ \end{pmatrix}$

An example of a construction of such an object:

// Constructor:
CPose3DQuat     p1 = CPose3D( 1.0, 2.0, 3.0, mrpt::DEG2RAD( 45.0 ), mrpt::DEG2RAD( -90.0 ), mrpt::DEG2RAD( 180.0 ) );
CPose3DQuat     p2(x,y,z, CQuaternionDouble(1,0,0,0)  );

// Constructor:

CPose3DQuat p1 = CPose3D( 1.0, 2.0, 3.0, mrpt::DEG2RAD( 45.0 ), mrpt::DEG2RAD( -90.0 ), mrpt::DEG2RAD( 180.0 ) );

CPose3DQuat p2(x,y,z, CQuaternionDouble(1,0,0,0) );

3. Derivation of the 3D rotation matrix

The 3×3 transformation matrix (also called Direction Cosine Matrix, DCM) for an arbitrary combination of yaw, pitch and roll rotations can be derived from the
combination of the individual rotations:

$R_z R_y R_x = \left( \begin{array}{ccc} \cos yaw \cos pitch & \cos yaw \sin pitch \sin roll - \sin yaw \cos roll & \cos yaw \sin pitch \cos roll + \sin yaw \sin roll \\ \sin yaw \cos pitch & \sin yaw \sin pitch \sin roll + \cos yaw \cos roll & \sin yaw \sin pitch \cos roll - \cos yaw \sin roll \\ -\sin pitch & \cos pitch \sin roll & \cos pitch \cos roll \end{array} \right)$

with:

$R_x = \left( \begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos roll & -\sin roll \\ 0 & \sin roll & \cos roll \end{array} \right)$ $R_y = \left( \begin{array}{ccc} \cos pitch & 0 & \sin pitch \\ 0 & 1 & 0 \\ -\sin pitch & 0 & \cos pitch \end{array} \right)$ $R_z = \left( \begin{array}{ccc} \cos yaw & -\sin yaw & 0 \\ \sin yaw & \cos yaw & 0 \\ 0 & 0 & 1 \end{array} \right)$

Here we have used the fact that rotations over successively transformed axes are achieved by right-side matrix multiplications,
i.e. the order of the rotations is Rz, then Ry, then Rx. For more information about the order of matrix multiplications, see Euler angles -> DCM on Wikipedia.

More conventions are discussed in http://mathworld.wolfram.com/EulerAngles.html

4. Conversions between classes

Objects of any pose or point class can be assigned to any other, and the conversion will preserve all the information as possible. Most of the conversions can be made implicitly, as in:

CPose2D    p1;
CPose3D    p2 = p1;  // Ok: p2.x

1 2	CPose2D p1; CPose3D p2 = p1; // Ok: p2.x

But for conversions from classes with more information than the target class, they must be written explicitly by the user, as in the example below. This rule (introduced in MRPT 0.6.5) tries to avoid unintentional losses of information.

CPose2D    pose;
CPoint3D   pnt;
CPose3D    pose6D;

pose6D = pnt;     // Ok.
pose6D = pose;    // Ok.
pose   = pose6D;  // **ERROR** Doesn't compile
pose   = CPose2D(pose6D);  // Ok. pitch,roll and z are lost.

CPose2D pose;

CPoint3D pnt;

CPose3D pose6D;

pose6D = pnt; // Ok.

pose6D = pose; // Ok.

pose = pose6D; // **ERROR** Doesn't compile

pose = CPose2D(pose6D); // Ok. pitch,roll and z are lost.

5. Common operators

5.1. Pose composition / inverse composition

Pose composition is the process of changing the coordinate system reference of some given point or pose B to that described by another point or pose A, obtaining a new pose or point C. Using MRPT C++ library classes this process can be implemented as simply as:

C = A + B;

1	C = A + B;

The operation will naturally take into account all the required rotations (through homogeneous matrix multiplication). The inverse process, obtaining the relative coordinates B of a point with global coordinates C as seen from another reference A, is accomplished by the – operator:

B = C - A;

1	B = C - A;

Points, poses, 2D, and 3D can be all freely mixed in these operations. The following tables summarize the expected output from all the input combinations:

Does “a+b” return a Pose or a Point?

a \ b	Pose	Point
Pose	Pose	Point
Point	Pose	Point

Does “a-b” return a Pose or a Point?

a \ b	Pose	Point
Pose	Pose	Pose
Point	Point	Point

Does “a+b” (and “a-b”) return a 2D or a 3D object?

a \ b	2D	3D
2D	2D	3D
3D	3D	3D

5.2. Distances

There are a few distance-related methods applicable to any point or pose:

Distance from this object to any other point or pose (for a 2D object it will take into account Z coordinates if compared to a 3D object):

float 	distanceTo (const CPoseOrPoint &amp;b) const;

1	float distanceTo (const CPoseOrPoint &b) const;

Square of the distance from this object to any other point or pose:

float 	sqrDistanceTo (const CPoseOrPoint &amp;b) const;

1	float sqrDistanceTo (const CPoseOrPoint &b) const;

Distance from this object to any user supplied 2D or 3D coordinates:

float   distance2DTo (float ax, float ay) const;
float   distance3DTo (float ax, float ay, float az) const;

1 2	float distance2DTo (float ax, float ay) const; float distance3DTo (float ax, float ay, float az) const;

And their square distance equivalents:

float   distance2DToSquare (float ax, float ay) const;
float   distance3DToSquare (float ax, float ay, float az) const;

1 2	float distance2DToSquare (float ax, float ay) const; float distance3DToSquare (float ax, float ay, float az) const;

Check the doxygen reference documentation of poses::CPoseOrPoint for more details.

5.3. Norm

The method:

double	norm () const;

1	double norm () const;

Returns the Euclidean norm of the point:

$||\mathbf{x}|| = \sqrt{x^2+y^2+z^2}$

5.4. Output to console

All the point/pose classes define a convenient operator

6. Quaternions

An alternative representation of 3D rotations are quaternions. MRPT supports them through the class:

template mrpt::math::CQuaternion

There exist methods to convert from CPose3D‘s and quaternions, but note that this only holds for pure rotations (without translations).
For complete 6D poses with quaternions, see the class CPose3DQuat.

CPose3D  p(0,0,0, DEG2RAD(20),DEG2RAD(45),DEG2RAD(-80));
CQuaternionDouble q;
p.getAsQuaternion(q);   // Transform a 3D-pose into a quaternion.

CPose3D p(0,0,0, DEG2RAD(20),DEG2RAD(45),DEG2RAD(-80));

CQuaternionDouble q;

p.getAsQuaternion(q); // Transform a 3D-pose into a quaternion.

7. An example: A mobile robot sensing with a camera

This example considers a mobile robot with a known 2D pose (x,y,heading) R. It has a camera onboard, with a relative 6D pose C relative to the robot reference. We will compute the 3D position (x,y,z) of a global landmark as seen by the camera, where L and L’ will stand for the absolute and camera-relative coordinates of this landmark. The scenario is depicted in the figure:

Then, the landmark as observed by the camera is at:

$\mathbf{L}' = \mathbf{L} \ominus ( \mathbf{R} \oplus \mathbf{C} )$

Next, we’ll check the correctness of the calculations by projecting this observation from the camera, which must coincide with the original absolute coordinates:

$\mathbf{R} \oplus \mathbf{C} \oplus \mathbf{L'} \doteq \mathbf{L}$

As an example of the pose constructors, consider the camera pose relative to the robot:

// Camera pose relative to the robot: 6D (x,y,z,yaw,pitch,roll).
CPose3D  C(0.5,1.0,1.5 ,DEG2RAD(-90.0),DEG2RAD(0),DEG2RAD(-90.0)  );

1 2	// Camera pose relative to the robot: 6D (x,y,z,yaw,pitch,roll). CPose3D C(0.5,1.0,1.5 ,DEG2RAD(-90.0),DEG2RAD(0),DEG2RAD(-90.0) );

The first three arguments are the x,y,z coordinates of the camera. The rest are yaw=-90º, pitch=0º, and roll=-90º. This generates the common configuration of the +Z axis of the camera pointing forward (refer to the figure).

The corresponding source code for the whole example is listed next (also found in MRPT/samples/geometry3D/test.cpp):

[spoiler style=”2″ title=”See C++ source”]

#include &lt;mrpt/poses/CPoint3D.h&gt;
#include &lt;mrpt/poses/CPose2D.h&gt;
#include &lt;mrpt/poses/CPose3D.h&gt; 
#include &lt;iostream&gt;
  
using namespace mrpt::poses;
using namespace std;

// ------------------------------------------------------
//                  TestGeometry3D
// ------------------------------------------------------
void TestGeometry3D()
{
	// The landmark (global) position: 3D (x,y,z)
	CPoint3D L(0,4,2);

	// Robot pose: 2D (x,y,phi)
	CPose2D  R(2,1, DEG2RAD(45.0f) );

	// Camera pose relative to the robot: 6D (x,y,z,yaw,pitch,roll).
	CPose3D  C(0.5f,0.5f,1.5f ,DEG2RAD(-90.0f),DEG2RAD(0),DEG2RAD(-90.0f)  );

	// TEST 1. Relative position L' of the landmark wrt the camera
	// --------------------------------------------------------------
	cout &lt;&lt; "L: " &lt;&lt; L &lt;&lt; endl;
	cout &lt;&lt; "R: " &lt;&lt; R &lt;&lt; endl;
	cout &lt;&lt; "C: " &lt;&lt; C &lt;&lt; endl;
	cout &lt;&lt; "R+C:" &lt;&lt; (R+C) &lt;&lt; endl;
	//cout &lt;&lt; (R+C).getHomogeneousMatrix();

	CPoint3D L2;
	CTicTac tictac;
	tictac.Tic();
	size_t i,N = 10000;
	for (i=0;i&lt;N;i++)
		L2 = L - (R+C);
	cout &lt;&lt; "Computation in: " &lt;&lt; 1e6 * tictac.Tac()/((double)N) &lt;&lt; " us" &lt;&lt; endl;

	cout &lt;&lt; "L': " &lt;&lt; L2 &lt;&lt; endl;

	// TEST 2. Reconstruct the landmark position:
	// --------------------------------------------------------------
	CPoint3D L3 = R + C + L2;
	cout &lt;&lt; "R(+)C(+)L' = " &lt;&lt; L3 &lt;&lt; endl;
	cout &lt;&lt; "Should be equal to L = " &lt;&lt; L &lt;&lt; endl;

	// TEST 3. Distance from the camera to the landmark
	// --------------------------------------------------------------
	cout &lt;&lt; "|(R(+)C)-L|= " &lt;&lt; (R+C).distanceTo(L) &lt;&lt; endl;
	cout &lt;&lt; "|L-(R(+)C)|= " &lt;&lt; (R+C).distanceTo(L) &lt;&lt; endl;

}

// ------------------------------------------------------
//                        MAIN
// ------------------------------------------------------
int main()
{
	try
	{
		TestGeometry3D();
		return 0;
	} catch (exception &amp;e)
	{
		cerr &lt;&lt; "EXCEPCTION: " &lt;&lt; e.what() &lt;&lt; endl;
		return -1;
	}
	catch (...)
	{
		cerr &lt;&lt; "Untyped excepcion!!";
		return -1;
	}
}

#include <mrpt/poses/CPoint3D.h>

#include <mrpt/poses/CPose2D.h>

#include <mrpt/poses/CPose3D.h>

#include <iostream>

using namespace mrpt::poses;

using namespace std;

// ------------------------------------------------------

// TestGeometry3D

// ------------------------------------------------------

void TestGeometry3D()

{

// The landmark (global) position: 3D (x,y,z)

CPoint3D L(0,4,2);

// Robot pose: 2D (x,y,phi)

CPose2D R(2,1, DEG2RAD(45.0f) );

// Camera pose relative to the robot: 6D (x,y,z,yaw,pitch,roll).

CPose3D C(0.5f,0.5f,1.5f ,DEG2RAD(-90.0f),DEG2RAD(0),DEG2RAD(-90.0f) );

// TEST 1. Relative position L' of the landmark wrt the camera

// --------------------------------------------------------------

cout << "L: " << L << endl;

cout << "R: " << R << endl;

cout << "C: " << C << endl;

cout << "R+C:" << (R+C) << endl;

//cout << (R+C).getHomogeneousMatrix();

CPoint3D L2;

CTicTac tictac;

tictac.Tic();

size_t i,N = 10000;

for (i=0;i<N;i++)

L2 = L - (R+C);

cout << "Computation in: " << 1e6 * tictac.Tac()/((double)N) << " us" << endl;

cout << "L': " << L2 << endl;

// TEST 2. Reconstruct the landmark position:

// --------------------------------------------------------------

CPoint3D L3 = R + C + L2;

cout << "R(+)C(+)L' = " << L3 << endl;

cout << "Should be equal to L = " << L << endl;

// TEST 3. Distance from the camera to the landmark

// --------------------------------------------------------------

cout << "|(R(+)C)-L|= " << (R+C).distanceTo(L) << endl;

cout << "|L-(R(+)C)|= " << (R+C).distanceTo(L) << endl;

}

// ------------------------------------------------------

// MAIN

// ------------------------------------------------------

int main()

{

try

{

TestGeometry3D();

return 0;

} catch (exception &e)

{

cerr << "EXCEPCTION: " << e.what() << endl;

return -1;

}

catch (...)

{

cerr << "Untyped excepcion!!";

return -1;

}

[/spoiler]

And this is the output generated by the program:

 L: (0,4,2)
 R: (2.000,1.000,45.00deg)
 C: (x,y,z,yaw,pitch,roll)=(0.5,0.5,1.5,-90deg,0deg,-90deg)
 R+C:(x,y,z,yaw,pitch,roll)=(2,1.70711,1.5,-45deg,-0deg,-90deg)
 L': (-3.03553,-0.5,0.207106)
 R(+)C(+)L' = (-1.23207e-07,4,2)
 Should be equal to L = (0,4,2)
 |(R(+)C)-L|= 3.0834
 |L-(R(+)C)|= 3.0834

L: (0,4,2)

R: (2.000,1.000,45.00deg)

C: (x,y,z,yaw,pitch,roll)=(0.5,0.5,1.5,-90deg,0deg,-90deg)

R+C:(x,y,z,yaw,pitch,roll)=(2,1.70711,1.5,-45deg,-0deg,-90deg)

L': (-3.03553,-0.5,0.207106)

R(+)C(+)L' = (-1.23207e-07,4,2)

Should be equal to L = (0,4,2)

|(R(+)C)-L|= 3.0834

|L-(R(+)C)|= 3.0834