testRot3.cpp File Reference

Unit tests for Rot3 class - common between Matrix and Quaternion. More...

#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <CppUnitLite/TestHarness.h>

Include dependency graph for testRot3.cpp:

Go to the source code of this file.

Classes
class	AngularVelocity

Macros
#define	CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation)
#define	CHECK_OMEGA(X, Y, Z)
#define	CHECK_OMEGA_ZERO(X, Y, Z)

Functions
AngularVelocity	bracket (const AngularVelocity &X, const AngularVelocity &Y)
Matrix	Cayley (const Matrix &A)
int	main ()
static Point3	P (0.2, 0.7, -2.0)
double	pitch_proxy (Rot3 const &R)
double	roll_proxy (Rot3 const &R)
Vector3	rpy_proxy (Rot3 const &R)
Vector3	RQ_proxy (Matrix3 const &R)
Rot3	RzRyRx_proxy (double const &a, double const &b, double const &c)
Rot3	RzRyRx_proxy (Vector3 const &xyz)
Rot3	slow_but_correct_Rodrigues (const Vector &w)
	TEST (Rot3, AxisAngle)
	TEST (Rot3, axisAngle)
	TEST (Rot3, AxisAngle2)
	TEST (Rot3, BCH)
	TEST (Rot3, between)
	TEST (Rot3, Cayley)
	TEST (Rot3, chart)
	TEST (Rot3, ChartDerivatives)
	TEST (Rot3, ClosestTo)
	TEST (Rot3, compose)
	TEST (Rot3, Concept)
	TEST (Rot3, constructor)
	TEST (Rot3, constructor2)
	TEST (Rot3, constructor3)
	TEST (Rot3, ConvertQuaternion)
	TEST (Rot3, determinant)
	TEST (Rot3, equals)
	TEST (Rot3, expmap_logmap)
	TEST (Rot3, ExpmapChainRule)
	TEST (Rot3, expmapChainRule)
	TEST (Rot3, expmapStability)
	TEST (Rot3, Invariants)
	TEST (Rot3, inverse)
	TEST (Rot3, LieGroupDerivatives)
	TEST (Rot3, log)
	TEST (Rot3, logmapStability)
	TEST (Rot3, manifold_expmap)
	TEST (Rot3, pitch_derivative)
	TEST (Rot3, quaternion)
	TEST (Rot3, retract)
	TEST (Rot3, retract_localCoordinates)
	TEST (Rot3, retract_localCoordinates2)
	TEST (Rot3, Rodrigues)
	TEST (Rot3, Rodrigues2)
	TEST (Rot3, Rodrigues3)
	TEST (Rot3, Rodrigues4)
	TEST (Rot3, roll_derivative)
	TEST (Rot3, rotate_derivatives)
	TEST (Rot3, rpy_derivative)
	TEST (Rot3, RQ)
	TEST (Rot3, RQ_derivative)
	TEST (Rot3, RzRyRx_scalars_derivative)
	TEST (Rot3, RzRyRx_vector_derivative)
	TEST (Rot3, slerp)
	TEST (Rot3, stream)
	TEST (Rot3, transpose)
	TEST (Rot3, unrotate)
	TEST (Rot3, xyz)
	TEST (Rot3, xyz_derivative)
	TEST (Rot3, yaw_derivative)
	TEST (Rot3, yaw_pitch_roll)
	TEST (Rot3, Ypr_derivative)
	TEST (Rot3, ypr_derivative)
Vector3	xyz_proxy (Rot3 const &R)
double	yaw_proxy (Rot3 const &R)
Rot3	Ypr_proxy (double const &y, double const &p, double const &r)
Vector3	ypr_proxy (Rot3 const &R)

Variables
static double	epsilon = 0.001
static double	error = 1e-9
static Rot3	R = Rot3::Rodrigues(0.1, 0.4, 0.2)

Detailed Description

Unit tests for Rot3 class - common between Matrix and Quaternion.

Unit tests for Rot3Q class.

Unit tests for Rot3 class, Quaternion specific.

Author

Alireza Fathi

Frank Dellaert

Varun Agrawal

Alireza Fathi

Richard Roberts

Definition in file testRot3.cpp.

Macro Definition Documentation

CHECK_AXIS_ANGLE

#define CHECK_AXIS_ANGLE	(		expectedAxis,
			expectedAngle,
			rotation
	)

Value:

  std::tie(actualAxis, actualAngle) = rotation.axisAngle();     \
  EXPECT(assert_equal(expectedAxis, actualAxis, 1e-9));         \
  EXPECT_DOUBLES_EQUAL(expectedAngle, actualAngle, 1e-9);       \
  EXPECT(assert_equal(rotation, Rot3::AxisAngle(expectedAxis, expectedAngle)))

CHECK_OMEGA

#define CHECK_OMEGA	(		X,
			Y,
			Z
	)

Value:

  w = (Vector(3) << (X), (Y), (Z)).finished(); \
  R = Rot3::Rodrigues(w);                \
  EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));

CHECK_OMEGA_ZERO

#define CHECK_OMEGA_ZERO	(		X,
			Y,
			Z
	)

Value:

  w = (Vector(3) << (X), (Y), (Z)).finished(); \
  R = Rot3::Rodrigues(w);                \
  EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));

Function Documentation

bracket()

AngularVelocity bracket	(	const AngularVelocity &	X,
		const AngularVelocity &	Y
	)

Definition at line 337 of file testRot3.cpp.

Cayley()

Matrix Cayley

(

const Matrix &

A

)

Definition at line 620 of file testRot3.cpp.

main()

int main

(

)

Definition at line 1000 of file testRot3.cpp.

P()

static Point3 P ( 0.  2, 0.  7, -2.  0  )

static

pitch_proxy()

double pitch_proxy

(

Rot3 const &

R

)

Definition at line 914 of file testRot3.cpp.

roll_proxy()

double roll_proxy

(

Rot3 const &

R

)

Definition at line 901 of file testRot3.cpp.

rpy_proxy()

Vector3 rpy_proxy

(

Rot3 const &

R

)

Definition at line 888 of file testRot3.cpp.

RQ_proxy()

Vector3 RQ_proxy

(

Matrix3 const &

R

)

Definition at line 824 of file testRot3.cpp.

RzRyRx_proxy() [1/2]

Rot3 RzRyRx_proxy	(	double const &	a,
		double const &	b,
		double const &	c
	)

Definition at line 773 of file testRot3.cpp.

RzRyRx_proxy() [2/2]

Rot3 RzRyRx_proxy

(

Vector3 const &

xyz

)

Definition at line 792 of file testRot3.cpp.

slow_but_correct_Rodrigues()

Rot3 slow_but_correct_Rodrigues

(

const Vector &

w

)

Definition at line 97 of file testRot3.cpp.

TEST() [1/53]

TEST	(	Rot3	,
		AxisAngle
	)

Definition at line 106 of file testRot3.cpp.

TEST() [2/53]

TEST	(	Rot3	,
		axisAngle
	)

Non-trivial angle 165

Non-trivial angle 195

Definition at line 714 of file testRot3.cpp.

TEST() [3/53]

TEST	(	Rot3	,
		AxisAngle2
	)

Definition at line 124 of file testRot3.cpp.

TEST() [4/53]

TEST	(	Rot3	,
		BCH
	)

Definition at line 342 of file testRot3.cpp.

TEST() [5/53]

TEST	(	Rot3	,
		between
	)

Definition at line 420 of file testRot3.cpp.

TEST() [6/53]

TEST	(	Rot3	,
		Cayley
	)

Definition at line 626 of file testRot3.cpp.

TEST() [7/53]

TEST	(	Rot3	,
		chart
	)

Definition at line 47 of file testRot3.cpp.

TEST() [8/53]

TEST	(	Rot3	,
		ChartDerivatives
	)

Definition at line 687 of file testRot3.cpp.

TEST() [9/53]

TEST	(	Rot3	,
		ClosestTo
	)

Definition at line 698 of file testRot3.cpp.

TEST() [10/53]

TEST	(	Rot3	,
		compose
	)

Definition at line 384 of file testRot3.cpp.

TEST() [11/53]

TEST	(	Rot3	,
		Concept
	)

Definition at line 40 of file testRot3.cpp.

TEST() [12/53]

TEST	(	Rot3	,
		constructor
	)

Definition at line 54 of file testRot3.cpp.

TEST() [13/53]

TEST	(	Rot3	,
		constructor2
	)

Definition at line 63 of file testRot3.cpp.

TEST() [14/53]

TEST	(	Rot3	,
		constructor3
	)

Definition at line 72 of file testRot3.cpp.

TEST() [15/53]

TEST	(	Rot3	,
		ConvertQuaternion
	)

Definition at line 601 of file testRot3.cpp.

TEST() [16/53]

TEST	(	Rot3	,
		determinant
	)

Definition at line 940 of file testRot3.cpp.

TEST() [17/53]

TEST	(	Rot3	,
		equals
	)

Definition at line 88 of file testRot3.cpp.

TEST() [18/53]

TEST	(	Rot3	,
		expmap_logmap
	)

Definition at line 280 of file testRot3.cpp.

TEST() [19/53]

TEST	(	Rot3	,
		ExpmapChainRule
	)

Definition at line 960 of file testRot3.cpp.

TEST() [20/53]

TEST	(	Rot3	,
		expmapChainRule
	)

Definition at line 979 of file testRot3.cpp.

TEST() [21/53]

TEST	(	Rot3	,
		expmapStability
	)

Definition at line 534 of file testRot3.cpp.

TEST() [22/53]

TEST	(	Rot3	,
		Invariants
	)

Definition at line 663 of file testRot3.cpp.

TEST() [23/53]

TEST	(	Rot3	,
		inverse
	)

Definition at line 404 of file testRot3.cpp.

TEST() [24/53]

TEST	(	Rot3	,
		LieGroupDerivatives
	)

Definition at line 678 of file testRot3.cpp.

TEST() [25/53]

TEST	(	Rot3	,
		log
	)

Definition at line 195 of file testRot3.cpp.

TEST() [26/53]

TEST	(	Rot3	,
		logmapStability
	)

Definition at line 550 of file testRot3.cpp.

TEST() [27/53]

TEST	(	Rot3	,
		manifold_expmap
	)

Definition at line 298 of file testRot3.cpp.

TEST() [28/53]

TEST	(	Rot3	,
		pitch_derivative
	)

Definition at line 916 of file testRot3.cpp.

TEST() [29/53]

TEST	(	Rot3	,
		quaternion
	)

Definition at line 563 of file testRot3.cpp.

TEST() [30/53]

TEST	(	Rot3	,
		retract
	)

Definition at line 183 of file testRot3.cpp.

TEST() [31/53]

TEST	(	Rot3	,
		retract_localCoordinates
	)

Definition at line 273 of file testRot3.cpp.

TEST() [32/53]

TEST	(	Rot3	,
		retract_localCoordinates2
	)

Definition at line 288 of file testRot3.cpp.

TEST() [33/53]

TEST	(	Rot3	,
		Rodrigues
	)

Definition at line 137 of file testRot3.cpp.

TEST() [34/53]

TEST	(	Rot3	,
		Rodrigues2
	)

Definition at line 146 of file testRot3.cpp.

TEST() [35/53]

TEST	(	Rot3	,
		Rodrigues3
	)

Definition at line 160 of file testRot3.cpp.

TEST() [36/53]

TEST	(	Rot3	,
		Rodrigues4
	)

Definition at line 169 of file testRot3.cpp.

TEST() [37/53]

TEST	(	Rot3	,
		roll_derivative
	)

Definition at line 903 of file testRot3.cpp.

TEST() [38/53]

TEST	(	Rot3	,
		rotate_derivatives
	)

Definition at line 355 of file testRot3.cpp.

TEST() [39/53]

TEST	(	Rot3	,
		rpy_derivative
	)

Definition at line 890 of file testRot3.cpp.

TEST() [40/53]

TEST	(	Rot3	,
		RQ
	)

Definition at line 504 of file testRot3.cpp.

TEST() [41/53]

TEST	(	Rot3	,
		RQ_derivative
	)

Definition at line 829 of file testRot3.cpp.

TEST() [42/53]

TEST	(	Rot3	,
		RzRyRx_scalars_derivative
	)

Definition at line 777 of file testRot3.cpp.

TEST() [43/53]

TEST	(	Rot3	,
		RzRyRx_vector_derivative
	)

Definition at line 794 of file testRot3.cpp.

TEST() [44/53]

TEST	(	Rot3	,
		slerp
	)

Definition at line 644 of file testRot3.cpp.

TEST() [45/53]

TEST	(	Rot3	,
		stream
	)

Definition at line 634 of file testRot3.cpp.

TEST() [46/53]

TEST	(	Rot3	,
		transpose
	)

Definition at line 80 of file testRot3.cpp.

TEST() [47/53]

TEST	(	Rot3	,
		unrotate
	)

Definition at line 369 of file testRot3.cpp.

TEST() [48/53]

TEST	(	Rot3	,
		xyz
	)

Definition at line 452 of file testRot3.cpp.

TEST() [49/53]

TEST	(	Rot3	,
		xyz_derivative
	)

Definition at line 864 of file testRot3.cpp.

TEST() [50/53]

TEST	(	Rot3	,
		yaw_derivative
	)

Definition at line 929 of file testRot3.cpp.

TEST() [51/53]

TEST	(	Rot3	,
		yaw_pitch_roll
	)

Definition at line 483 of file testRot3.cpp.

TEST() [52/53]

TEST	(	Rot3	,
		Ypr_derivative
	)

Definition at line 809 of file testRot3.cpp.

TEST() [53/53]

TEST	(	Rot3	,
		ypr_derivative
	)

Definition at line 877 of file testRot3.cpp.

xyz_proxy()

Vector3 xyz_proxy

(

Rot3 const &

R

)

Definition at line 862 of file testRot3.cpp.

yaw_proxy()

double yaw_proxy

(

Rot3 const &

R

)

Definition at line 927 of file testRot3.cpp.

Ypr_proxy()

Rot3 Ypr_proxy	(	double const &	y,
		double const &	p,
		double const &	r
	)

Definition at line 805 of file testRot3.cpp.

ypr_proxy()

Vector3 ypr_proxy

(

Rot3 const &

R

)

Definition at line 875 of file testRot3.cpp.

Variable Documentation

epsilon

double epsilon = 0.001

static

Definition at line 37 of file testRot3.cpp.

error

double error = 1e-9

static

Definition at line 37 of file testRot3.cpp.

R

Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2)

static

Definition at line 35 of file testRot3.cpp.