testPose3.cpp File Reference

Unit tests for Pose3 class. More...

#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#include <functional>
#include "testPoseAdjointMap.h"

Include dependency graph for testPose3.cpp:

Go to the source code of this file.

Namespaces
	screwPose3

Functions
Pose3	Agrawal06iros (const Vector &xi)
Unit3	bearing_proxy (const Pose3 &pose, const Point3 &point)
Rot3	screwPose3::expectedR (c, -s, 0, s, c, 0, 0, 0, 1)
Point3	screwPose3::expectedT (0.29552, 0.0446635, 1)
Point3	l1 (1, 0, 0)
Point3	l2 (1, 1, 0)
Point3	l3 (2, 2, 0)
Point3	l4 (1, 4,-4)
int	main ()
static const Point3	P (0.2, 0.7,-2)
static const Point3	P2 (3.5,-8.2, 4.2)
double	range_pose_proxy (const Pose3 &pose, const Pose3 &point)
double	range_proxy (const Pose3 &pose, const Point3 &point)
	TEST (Pose3, equals)
	TEST (Pose3, constructors)
	TEST (Pose3, retract_first_order)
	TEST (Pose3, retract_expmap)
	TEST (Pose3, expmap_a_full)
	TEST (Pose3, expmap_a_full2)
	TEST (Pose3, expmap_b)
	TEST (Pose3, expmap_c_full)
	TEST (Pose3, Adjoint_full)
	TEST (Pose3, Adjoint_jacobians)
	TEST (Pose3, AdjointTranspose)
	TEST (Pose3, Adjoint_hat)
	TEST (Pose3, expmaps_galore_full)
	TEST (Pose3, translation)
	TEST (Pose3, rotation)
	TEST (Pose3, Adjoint_compose_full)
	TEST (Pose3, compose)
	TEST (Pose3, compose2)
	TEST (Pose3, inverse)
	TEST (Pose3, inverseDerivatives2)
	TEST (Pose3, compose_inverse)
	TEST (Pose3, Dtransform_from1_a)
	TEST (Pose3, Dtransform_from1_b)
	TEST (Pose3, Dtransform_from1_c)
	TEST (Pose3, Dtransform_from1_d)
	TEST (Pose3, Dtransform_from2)
	TEST (Pose3, Dtransform_to1)
	TEST (Pose3, Dtransform_to2)
	TEST (Pose3, transform_to_with_derivatives)
	TEST (Pose3, transform_from_with_derivatives)
	TEST (Pose3, transform_to_translate)
	TEST (Pose3, transform_to_rotate)
	TEST (Pose3, transformPoseFrom)
	TEST (Pose3, transformTo)
	TEST (Pose3, transformPoseTo)
	TEST (Pose3, transformPoseTo_with_derivatives)
	TEST (Pose3, transformPoseTo_with_derivatives2)
	TEST (Pose3, transformFrom)
	TEST (Pose3, transform_roundtrip)
	TEST (Pose3, Retract_LocalCoordinates)
	TEST (Pose3, retract_localCoordinates)
	TEST (Pose3, expmap_logmap)
	TEST (Pose3, retract_localCoordinates2)
	TEST (Pose3, manifold_expmap)
	TEST (Pose3, subgroups)
	TEST (Pose3, between)
	TEST (Pose3, range)
	TEST (Pose3, range_pose)
	TEST (Pose3, Bearing)
	TEST (Pose3, Bearing2)
	TEST (Pose3, PoseToPoseBearing)
	TEST (Pose3, unicycle)
	TEST (Pose3, adjointMap)
	TEST (Pose3, Align1)
	TEST (Pose3, Align2)
	TEST (Pose3, ExpmapDerivative1)
	TEST (Pose3, ExpmapDerivative2)
	TEST (Pose3, ExpmapDerivativeQr)
	TEST (Pose3, LogmapDerivative)
	TEST (Pose3, adjoint)
	TEST (Pose3, adjointTranspose)
	TEST (Pose3, stream)
	TEST (Pose3, Invariants)
	TEST (Pose3, LieGroupDerivatives)
	TEST (Pose3, ChartDerivatives)
	TEST (Pose3, TransformCovariance6MapTo2d)
	TEST (Pose3, TransformCovariance6)
	TEST (Pose3, interpolate)
	TEST (Pose3, interpolateJacobians)
	TEST (Pose3, Create)
	TEST (Pose3, Print)
Vector6	testDerivAdjoint (const Vector6 &xi, const Vector6 &v)
Vector6	testDerivAdjointTranspose (const Vector6 &xi, const Vector6 &v)
Pose3	testing_interpolate (const Pose3 &t1, const Pose3 &t2, double gamma)
Point3	transform_to_ (const Pose3 &pose, const Point3 &point)
Point3	transformFrom_ (const Pose3 &pose, const Point3 &point)
Pose3	transformPoseFrom_ (const Pose3 &wTa, const Pose3 &aTb)
Pose3	transformPoseTo_ (const Pose3 &pose, const Pose3 &pose2)

Variables
double	screwPose3::a =0.3
double	screwPose3::c =cos(a)
Pose3	screwPose3::expected (expectedR, expectedT)
static const Rot3	R = Rot3::Rodrigues(0.3,0,0)
double	screwPose3::s =sin(a)
static const Pose3	T (R, P2)
static const Pose3	T2 (Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
static const Pose3	T3 (Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3))
static const double	tol =1e-5
double	screwPose3::w =0.3
Pose3	x1
Pose3	x2 (Rot3::Ypr(0.0, 0.0, 0.0), l2)
Pose3	x3 (Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
Vector	screwPose3::xi = (Vector(6) << 0.0, 0.0, w, w, 0.0, 1.0).finished()
Pose3	xl1 (Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0))
Pose3	xl2 (Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
Pose3	xl3 (Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0))
Pose3	xl4 (Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4))

Detailed Description

Unit tests for Pose3 class.

Definition in file testPose3.cpp.

Function Documentation

Agrawal06iros()

Pose3 Agrawal06iros

(

const Vector &

xi

)

Agrawal06iros version of exponential map

Definition at line 242 of file testPose3.cpp.

bearing_proxy()

Unit3 bearing_proxy	(	const Pose3 &	pose,
		const Point3 &	point
	)

Definition at line 741 of file testPose3.cpp.

l1()

Point3 l1	(	1	,
		0	,
		0
	)

l2()

Point3 l2	(	1	,
		1	,
		0
	)

l3()

Point3 l3	(	2	,
		2	,
		0
	)

l4()

Point3 l4	(	1	,
		4	,
		-	4
	)

main()

int main

(

void

)

Definition at line 1211 of file testPose3.cpp.

P()

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

static

P2()

static const Point3 P2 ( 3.  5, -8.  2, 4.  2  )

static

range_pose_proxy()

double range_pose_proxy	(	const Pose3 &	pose,
		const Pose3 &	point
	)

Definition at line 706 of file testPose3.cpp.

range_proxy()

double range_proxy	(	const Pose3 &	pose,
		const Point3 &	point
	)

Definition at line 671 of file testPose3.cpp.

TEST() [1/71]

TEST	(	Pose3	,
		equals
	)

Definition at line 44 of file testPose3.cpp.

TEST() [2/71]

TEST	(	Pose3	,
		constructors
	)

Definition at line 53 of file testPose3.cpp.

TEST() [3/71]

TEST	(	Pose3	,
		retract_first_order
	)

Definition at line 62 of file testPose3.cpp.

TEST() [4/71]

TEST	(	Pose3	,
		retract_expmap
	)

Definition at line 73 of file testPose3.cpp.

TEST() [5/71]

TEST	(	Pose3	,
		expmap_a_full
	)

Definition at line 82 of file testPose3.cpp.

TEST() [6/71]

TEST	(	Pose3	,
		expmap_a_full2
	)

Definition at line 93 of file testPose3.cpp.

TEST() [7/71]

TEST	(	Pose3	,
		expmap_b
	)

Definition at line 104 of file testPose3.cpp.

TEST() [8/71]

TEST	(	Pose3	,
		expmap_c_full
	)

Definition at line 124 of file testPose3.cpp.

TEST() [9/71]

TEST	(	Pose3	,
		Adjoint_full
	)

Definition at line 132 of file testPose3.cpp.

TEST() [10/71]

TEST	(	Pose3	,
		Adjoint_jacobians
	)

Definition at line 149 of file testPose3.cpp.

TEST() [11/71]

TEST	(	Pose3	,
		AdjointTranspose
	)

Definition at line 184 of file testPose3.cpp.

TEST() [12/71]

TEST	(	Pose3	,
		Adjoint_hat
	)

Definition at line 224 of file testPose3.cpp.

TEST() [13/71]

TEST	(	Pose3	,
		expmaps_galore_full
	)

Definition at line 256 of file testPose3.cpp.

TEST() [14/71]

TEST	(	Pose3	,
		translation
	)

Definition at line 286 of file testPose3.cpp.

TEST() [15/71]

TEST	(	Pose3	,
		rotation
	)

Definition at line 297 of file testPose3.cpp.

TEST() [16/71]

TEST	(	Pose3	,
		Adjoint_compose_full
	)

Definition at line 307 of file testPose3.cpp.

TEST() [17/71]

TEST	(	Pose3	,
		compose
	)

Definition at line 322 of file testPose3.cpp.

TEST() [18/71]

TEST	(	Pose3	,
		compose2
	)

Definition at line 341 of file testPose3.cpp.

TEST() [19/71]

TEST	(	Pose3	,
		inverse
	)

Definition at line 360 of file testPose3.cpp.

TEST() [20/71]

TEST	(	Pose3	,
		inverseDerivatives2
	)

Definition at line 373 of file testPose3.cpp.

TEST() [21/71]

TEST	(	Pose3	,
		compose_inverse
	)

Definition at line 387 of file testPose3.cpp.

TEST() [22/71]

TEST	(	Pose3	,
		Dtransform_from1_a
	)

Definition at line 398 of file testPose3.cpp.

TEST() [23/71]

TEST	(	Pose3	,
		Dtransform_from1_b
	)

Definition at line 405 of file testPose3.cpp.

TEST() [24/71]

TEST	(	Pose3	,
		Dtransform_from1_c
	)

Definition at line 413 of file testPose3.cpp.

TEST() [25/71]

TEST	(	Pose3	,
		Dtransform_from1_d
	)

Definition at line 422 of file testPose3.cpp.

TEST() [26/71]

TEST	(	Pose3	,
		Dtransform_from2
	)

Definition at line 435 of file testPose3.cpp.

TEST() [27/71]

TEST	(	Pose3	,
		Dtransform_to1
	)

Definition at line 446 of file testPose3.cpp.

TEST() [28/71]

TEST	(	Pose3	,
		Dtransform_to2
	)

Definition at line 454 of file testPose3.cpp.

TEST() [29/71]

TEST	(	Pose3	,
		transform_to_with_derivatives
	)

Definition at line 462 of file testPose3.cpp.

TEST() [30/71]

TEST	(	Pose3	,
		transform_from_with_derivatives
	)

Definition at line 472 of file testPose3.cpp.

TEST() [31/71]

TEST	(	Pose3	,
		transform_to_translate
	)

Definition at line 482 of file testPose3.cpp.

TEST() [32/71]

TEST	(	Pose3	,
		transform_to_rotate
	)

Definition at line 490 of file testPose3.cpp.

TEST() [33/71]

TEST	(	Pose3	,
		transformPoseFrom
	)

Definition at line 503 of file testPose3.cpp.

TEST() [34/71]

TEST	(	Pose3	,
		transformTo
	)

Definition at line 521 of file testPose3.cpp.

TEST() [35/71]

TEST	(	Pose3	,
		transformPoseTo
	)

Definition at line 533 of file testPose3.cpp.

TEST() [36/71]

TEST	(	Pose3	,
		transformPoseTo_with_derivatives
	)

Definition at line 539 of file testPose3.cpp.

TEST() [37/71]

TEST	(	Pose3	,
		transformPoseTo_with_derivatives2
	)

Definition at line 551 of file testPose3.cpp.

TEST() [38/71]

TEST	(	Pose3	,
		transformFrom
	)

Definition at line 563 of file testPose3.cpp.

TEST() [39/71]

TEST	(	Pose3	,
		transform_roundtrip
	)

Definition at line 570 of file testPose3.cpp.

TEST() [40/71]

TEST	(	Pose3	,
		Retract_LocalCoordinates
	)

Definition at line 577 of file testPose3.cpp.

TEST() [41/71]

TEST	(	Pose3	,
		retract_localCoordinates
	)

Definition at line 586 of file testPose3.cpp.

TEST() [42/71]

TEST	(	Pose3	,
		expmap_logmap
	)

Definition at line 594 of file testPose3.cpp.

TEST() [43/71]

TEST	(	Pose3	,
		retract_localCoordinates2
	)

Definition at line 602 of file testPose3.cpp.

TEST() [44/71]

TEST	(	Pose3	,
		manifold_expmap
	)

Definition at line 615 of file testPose3.cpp.

TEST() [45/71]

TEST	(	Pose3	,
		subgroups
	)

Definition at line 630 of file testPose3.cpp.

TEST() [46/71]

TEST	(	Pose3	,
		between
	)

Definition at line 646 of file testPose3.cpp.

TEST() [47/71]

TEST	(	Pose3	,
		range
	)

Definition at line 674 of file testPose3.cpp.

TEST() [48/71]

TEST	(	Pose3	,
		range_pose
	)

Definition at line 709 of file testPose3.cpp.

TEST() [49/71]

TEST	(	Pose3	,
		Bearing
	)

Definition at line 744 of file testPose3.cpp.

TEST() [50/71]

TEST	(	Pose3	,
		Bearing2
	)

Definition at line 755 of file testPose3.cpp.

TEST() [51/71]

TEST	(	Pose3	,
		PoseToPoseBearing
	)

Definition at line 766 of file testPose3.cpp.

TEST() [52/71]

TEST	(	Pose3	,
		unicycle
	)

Definition at line 788 of file testPose3.cpp.

TEST() [53/71]

TEST	(	Pose3	,
		adjointMap
	)

Definition at line 798 of file testPose3.cpp.

TEST() [54/71]

TEST	(	Pose3	,
		Align1
	)

Definition at line 808 of file testPose3.cpp.

TEST() [55/71]

TEST	(	Pose3	,
		Align2
	)

Definition at line 821 of file testPose3.cpp.

TEST() [56/71]

TEST	(	Pose3	,
		ExpmapDerivative1
	)

Definition at line 838 of file testPose3.cpp.

TEST() [57/71]

TEST	(	Pose3	,
		ExpmapDerivative2
	)

Definition at line 848 of file testPose3.cpp.

TEST() [58/71]

TEST	(	Pose3	,
		ExpmapDerivativeQr
	)

Definition at line 882 of file testPose3.cpp.

TEST() [59/71]

TEST	(	Pose3	,
		LogmapDerivative
	)

Definition at line 894 of file testPose3.cpp.

TEST() [60/71]

TEST	(	Pose3	,
		adjoint
	)

Definition at line 909 of file testPose3.cpp.

TEST() [61/71]

TEST	(	Pose3	,
		adjointTranspose
	)

Definition at line 931 of file testPose3.cpp.

TEST() [62/71]

TEST	(	Pose3	,
		stream
	)

Definition at line 950 of file testPose3.cpp.

TEST() [63/71]

TEST	(	Pose3	,
		Invariants
	)

Definition at line 959 of file testPose3.cpp.

TEST() [64/71]

TEST	(	Pose3	,
		LieGroupDerivatives
	)

Definition at line 974 of file testPose3.cpp.

TEST() [65/71]

TEST	(	Pose3	,
		ChartDerivatives
	)

Definition at line 984 of file testPose3.cpp.

TEST() [66/71]

TEST	(	Pose3	,
		TransformCovariance6MapTo2d
	)

Definition at line 997 of file testPose3.cpp.

TEST() [67/71]

TEST	(	Pose3	,
		TransformCovariance6
	)

Definition at line 1040 of file testPose3.cpp.

TEST() [68/71]

TEST	(	Pose3	,
		interpolate
	)

Definition at line 1094 of file testPose3.cpp.

TEST() [69/71]

TEST	(	Pose3	,
		interpolateJacobians
	)

Definition at line 1129 of file testPose3.cpp.

TEST() [70/71]

TEST	(	Pose3	,
		Create
	)

Definition at line 1189 of file testPose3.cpp.

TEST() [71/71]

TEST	(	Pose3	,
		Print
	)

Definition at line 1201 of file testPose3.cpp.

testDerivAdjoint()

Vector6 testDerivAdjoint	(	const Vector6 &	xi,
		const Vector6 &	v
	)

Definition at line 905 of file testPose3.cpp.

testDerivAdjointTranspose()

Vector6 testDerivAdjointTranspose	(	const Vector6 &	xi,
		const Vector6 &	v
	)

Definition at line 927 of file testPose3.cpp.

testing_interpolate()

Pose3 testing_interpolate	(	const Pose3 &	t1,
		const Pose3 &	t2,
		double	gamma
	)

Definition at line 1127 of file testPose3.cpp.

transform_to_()

Point3 transform_to_	(	const Pose3 &	pose,
		const Point3 &	point
	)

Definition at line 443 of file testPose3.cpp.

transformFrom_()

Point3 transformFrom_	(	const Pose3 &	pose,
		const Point3 &	point
	)

Definition at line 395 of file testPose3.cpp.

transformPoseFrom_()

Pose3 transformPoseFrom_	(	const Pose3 &	wTa,
		const Pose3 &	aTb
	)

Definition at line 499 of file testPose3.cpp.

transformPoseTo_()

Pose3 transformPoseTo_	(	const Pose3 &	pose,
		const Pose3 &	pose2
	)

Definition at line 528 of file testPose3.cpp.

Variable Documentation

R

const Rot3 R = Rot3::Rodrigues(0.3,0,0)

static

Definition at line 36 of file testPose3.cpp.

T

const Pose3 T(R, P2)

static

T2

const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)

static

T3

const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3))

static

tol

const double tol =1e-5

static

Definition at line 41 of file testPose3.cpp.

x1

Pose3 x1

Definition at line 663 of file testPose3.cpp.

x2

Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)

x3

Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)

xl1

Pose3 xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0))

xl2

Pose3 xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))

xl3

Pose3 xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0))

xl4

Pose3 xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4))