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, BruteForceExpmap)
	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, HatAndVee)
	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 365 of file testRot3.cpp.

Cayley()

Matrix Cayley

(

const Matrix &

A

)

Definition at line 648 of file testRot3.cpp.

main()

int main

(

)

Definition at line 1028 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 942 of file testRot3.cpp.

roll_proxy()

double roll_proxy

(

Rot3 const &

R

)

Definition at line 929 of file testRot3.cpp.

rpy_proxy()

Vector3 rpy_proxy

(

Rot3 const &

R

)

Definition at line 916 of file testRot3.cpp.

RQ_proxy()

Vector3 RQ_proxy

(

Matrix3 const &

R

)

Definition at line 852 of file testRot3.cpp.

RzRyRx_proxy() [1/2]

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

Definition at line 801 of file testRot3.cpp.

RzRyRx_proxy() [2/2]

Rot3 RzRyRx_proxy

(

Vector3 const &

xyz

)

Definition at line 820 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/55]

TEST	(	Rot3	,
		AxisAngle
	)

Definition at line 106 of file testRot3.cpp.

TEST() [2/55]

TEST	(	Rot3	,
		axisAngle
	)

Non-trivial angle 165

Non-trivial angle 195

Definition at line 742 of file testRot3.cpp.

TEST() [3/55]

TEST	(	Rot3	,
		AxisAngle2
	)

Definition at line 124 of file testRot3.cpp.

TEST() [4/55]

TEST	(	Rot3	,
		BCH
	)

Definition at line 370 of file testRot3.cpp.

TEST() [5/55]

TEST	(	Rot3	,
		between
	)

Definition at line 448 of file testRot3.cpp.

TEST() [6/55]

TEST	(	Rot3	,
		BruteForceExpmap
	)

Definition at line 350 of file testRot3.cpp.

TEST() [7/55]

TEST	(	Rot3	,
		Cayley
	)

Definition at line 654 of file testRot3.cpp.

TEST() [8/55]

TEST	(	Rot3	,
		chart
	)

Definition at line 47 of file testRot3.cpp.

TEST() [9/55]

TEST	(	Rot3	,
		ChartDerivatives
	)

Definition at line 715 of file testRot3.cpp.

TEST() [10/55]

TEST	(	Rot3	,
		ClosestTo
	)

Definition at line 726 of file testRot3.cpp.

TEST() [11/55]

TEST	(	Rot3	,
		compose
	)

Definition at line 412 of file testRot3.cpp.

TEST() [12/55]

TEST	(	Rot3	,
		Concept
	)

Definition at line 40 of file testRot3.cpp.

TEST() [13/55]

TEST	(	Rot3	,
		constructor
	)

Definition at line 54 of file testRot3.cpp.

TEST() [14/55]

TEST	(	Rot3	,
		constructor2
	)

Definition at line 63 of file testRot3.cpp.

TEST() [15/55]

TEST	(	Rot3	,
		constructor3
	)

Definition at line 72 of file testRot3.cpp.

TEST() [16/55]

TEST	(	Rot3	,
		ConvertQuaternion
	)

Definition at line 629 of file testRot3.cpp.

TEST() [17/55]

TEST	(	Rot3	,
		determinant
	)

Definition at line 968 of file testRot3.cpp.

TEST() [18/55]

TEST	(	Rot3	,
		equals
	)

Definition at line 88 of file testRot3.cpp.

TEST() [19/55]

TEST	(	Rot3	,
		expmap_logmap
	)

Definition at line 280 of file testRot3.cpp.

TEST() [20/55]

TEST	(	Rot3	,
		ExpmapChainRule
	)

Definition at line 988 of file testRot3.cpp.

TEST() [21/55]

TEST	(	Rot3	,
		expmapChainRule
	)

Definition at line 1007 of file testRot3.cpp.

TEST() [22/55]

TEST	(	Rot3	,
		expmapStability
	)

Definition at line 562 of file testRot3.cpp.

TEST() [23/55]

TEST	(	Rot3	,
		HatAndVee
	)

Definition at line 328 of file testRot3.cpp.

TEST() [24/55]

TEST	(	Rot3	,
		Invariants
	)

Definition at line 691 of file testRot3.cpp.

TEST() [25/55]

TEST	(	Rot3	,
		inverse
	)

Definition at line 432 of file testRot3.cpp.

TEST() [26/55]

TEST	(	Rot3	,
		LieGroupDerivatives
	)

Definition at line 706 of file testRot3.cpp.

TEST() [27/55]

TEST	(	Rot3	,
		log
	)

Definition at line 195 of file testRot3.cpp.

TEST() [28/55]

TEST	(	Rot3	,
		logmapStability
	)

Definition at line 578 of file testRot3.cpp.

TEST() [29/55]

TEST	(	Rot3	,
		manifold_expmap
	)

Definition at line 298 of file testRot3.cpp.

TEST() [30/55]

TEST	(	Rot3	,
		pitch_derivative
	)

Definition at line 944 of file testRot3.cpp.

TEST() [31/55]

TEST	(	Rot3	,
		quaternion
	)

Definition at line 591 of file testRot3.cpp.

TEST() [32/55]

TEST	(	Rot3	,
		retract
	)

Definition at line 183 of file testRot3.cpp.

TEST() [33/55]

TEST	(	Rot3	,
		retract_localCoordinates
	)

Definition at line 273 of file testRot3.cpp.

TEST() [34/55]

TEST	(	Rot3	,
		retract_localCoordinates2
	)

Definition at line 288 of file testRot3.cpp.

TEST() [35/55]

TEST	(	Rot3	,
		Rodrigues
	)

Definition at line 137 of file testRot3.cpp.

TEST() [36/55]

TEST	(	Rot3	,
		Rodrigues2
	)

Definition at line 146 of file testRot3.cpp.

TEST() [37/55]

TEST	(	Rot3	,
		Rodrigues3
	)

Definition at line 160 of file testRot3.cpp.

TEST() [38/55]

TEST	(	Rot3	,
		Rodrigues4
	)

Definition at line 169 of file testRot3.cpp.

TEST() [39/55]

TEST	(	Rot3	,
		roll_derivative
	)

Definition at line 931 of file testRot3.cpp.

TEST() [40/55]

TEST	(	Rot3	,
		rotate_derivatives
	)

Definition at line 383 of file testRot3.cpp.

TEST() [41/55]

TEST	(	Rot3	,
		rpy_derivative
	)

Definition at line 918 of file testRot3.cpp.

TEST() [42/55]

TEST	(	Rot3	,
		RQ
	)

Definition at line 532 of file testRot3.cpp.

TEST() [43/55]

TEST	(	Rot3	,
		RQ_derivative
	)

Definition at line 857 of file testRot3.cpp.

TEST() [44/55]

TEST	(	Rot3	,
		RzRyRx_scalars_derivative
	)

Definition at line 805 of file testRot3.cpp.

TEST() [45/55]

TEST	(	Rot3	,
		RzRyRx_vector_derivative
	)

Definition at line 822 of file testRot3.cpp.

TEST() [46/55]

TEST	(	Rot3	,
		slerp
	)

Definition at line 672 of file testRot3.cpp.

TEST() [47/55]

TEST	(	Rot3	,
		stream
	)

Definition at line 662 of file testRot3.cpp.

TEST() [48/55]

TEST	(	Rot3	,
		transpose
	)

Definition at line 80 of file testRot3.cpp.

TEST() [49/55]

TEST	(	Rot3	,
		unrotate
	)

Definition at line 397 of file testRot3.cpp.

TEST() [50/55]

TEST	(	Rot3	,
		xyz
	)

Definition at line 480 of file testRot3.cpp.

TEST() [51/55]

TEST	(	Rot3	,
		xyz_derivative
	)

Definition at line 892 of file testRot3.cpp.

TEST() [52/55]

TEST	(	Rot3	,
		yaw_derivative
	)

Definition at line 957 of file testRot3.cpp.

TEST() [53/55]

TEST	(	Rot3	,
		yaw_pitch_roll
	)

Definition at line 511 of file testRot3.cpp.

TEST() [54/55]

TEST	(	Rot3	,
		Ypr_derivative
	)

Definition at line 837 of file testRot3.cpp.

TEST() [55/55]

TEST	(	Rot3	,
		ypr_derivative
	)

Definition at line 905 of file testRot3.cpp.

xyz_proxy()

Vector3 xyz_proxy

(

Rot3 const &

R

)

Definition at line 890 of file testRot3.cpp.

yaw_proxy()

double yaw_proxy

(

Rot3 const &

R

)

Definition at line 955 of file testRot3.cpp.

Ypr_proxy()

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

Definition at line 833 of file testRot3.cpp.

ypr_proxy()

Vector3 ypr_proxy

(

Rot3 const &

R

)

Definition at line 903 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.