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 <gtsam/slam/expressions.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
	test_cases

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, adjoint)
	TEST (Pose3, Adjoint_compose_full)
	TEST (Pose3, Adjoint_full)
	TEST (Pose3, Adjoint_hat)
	TEST (Pose3, Adjoint_jacobians)
	TEST (Pose3, adjointMap)
	TEST (Pose3, AdjointTranspose)
	TEST (Pose3, adjointTranspose)
	TEST (Pose3, Align1)
	TEST (Pose3, Align2)
	TEST (Pose3, Bearing)
	TEST (Pose3, Bearing2)
	TEST (Pose3, between)
	TEST (Pose3, ChartDerivatives)
	TEST (Pose3, compose)
	TEST (Pose3, compose2)
	TEST (Pose3, compose_inverse)
	TEST (Pose3, Concept)
	TEST (Pose3, constructors)
	TEST (Pose3, Create)
	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, equals)
	TEST (Pose3, expmap_a_full)
	TEST (Pose3, expmap_a_full2)
	TEST (Pose3, expmap_b)
	TEST (Pose3, expmap_c_full)
	TEST (Pose3, expmap_logmap)
	TEST (Pose3, ExpmapChainRule)
	TEST (Pose3, ExpmapDerivative)
	TEST (Pose3, ExpmapDerivatives)
	TEST (Pose3, expmaps_galore_full)
	TEST (Pose3, expressionWrappers)
	TEST (Pose3, HatAndVee)
	TEST (Pose3, interpolate)
	TEST (Pose3, interpolateJacobians)
	TEST (Pose3, interpolateRtJacobians)
	TEST (Pose3, Invariants)
	TEST (Pose3, inverse)
	TEST (Pose3, inverseDerivatives2)
	TEST (Pose3, LieGroupDerivatives)
	TEST (Pose3, Logmap)
	TEST (Pose3, LogmapDerivatives)
	TEST (Pose3, manifold_expmap)
	TEST (Pose3, PoseToPoseBearing)
	TEST (Pose3, Print)
	TEST (Pose3, range)
	TEST (Pose3, range_pose)
	TEST (Pose3, retract_expmap)
	TEST (Pose3, retract_first_order)
	TEST (Pose3, Retract_LocalCoordinates)
	TEST (Pose3, retract_localCoordinates)
	TEST (Pose3, retract_localCoordinates2)
	TEST (Pose3, rotation)
	TEST (Pose3, stream)
	TEST (Pose3, subgroups)
	TEST (Pose3, transform_from_with_derivatives)
	TEST (Pose3, transform_roundtrip)
	TEST (Pose3, transform_to_rotate)
	TEST (Pose3, transform_to_translate)
	TEST (Pose3, transform_to_with_derivatives)
	TEST (Pose3, TransformCovariance6)
	TEST (Pose3, TransformCovariance6MapTo2d)
	TEST (Pose3, transformFrom)
	TEST (Pose3, transformPoseFrom)
	TEST (Pose3, transformPoseTo)
	TEST (Pose3, transformPoseTo_with_derivatives)
	TEST (Pose3, transformPoseTo_with_derivatives2)
	TEST (Pose3, transformTo)
	TEST (Pose3, translation)
	TEST (Pose3, unicycle)
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)
Pose3	testing_interpolate_rt (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)
std::vector< Vector3 >	test_cases::large
auto	test_cases::omegas = [](bool nearZero) { return nearZero ? small : large; }
static const Rot3	R = Rot3::Rodrigues(0.3,0,0)
double	screwPose3::s =sin(a)
std::vector< Vector3 >	test_cases::small
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
std::vector< Vector3 >	test_cases::vs
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 271 of file testPose3.cpp.

bearing_proxy()

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

Definition at line 770 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

(

)

Definition at line 1405 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 735 of file testPose3.cpp.

range_proxy()

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

Definition at line 700 of file testPose3.cpp.

TEST() [1/76]

TEST	(	Pose3	,
		adjoint
	)

Definition at line 964 of file testPose3.cpp.

TEST() [2/76]

TEST	(	Pose3	,
		Adjoint_compose_full
	)

Definition at line 336 of file testPose3.cpp.

TEST() [3/76]

TEST	(	Pose3	,
		Adjoint_full
	)

Definition at line 140 of file testPose3.cpp.

TEST() [4/76]

TEST	(	Pose3	,
		Adjoint_hat
	)

Definition at line 254 of file testPose3.cpp.

TEST() [5/76]

TEST	(	Pose3	,
		Adjoint_jacobians
	)

Definition at line 157 of file testPose3.cpp.

TEST() [6/76]

TEST	(	Pose3	,
		adjointMap
	)

Definition at line 818 of file testPose3.cpp.

TEST() [7/76]

TEST	(	Pose3	,
		AdjointTranspose
	)

Definition at line 192 of file testPose3.cpp.

TEST() [8/76]

TEST	(	Pose3	,
		adjointTranspose
	)

Definition at line 986 of file testPose3.cpp.

TEST() [9/76]

TEST	(	Pose3	,
		Align1
	)

Definition at line 828 of file testPose3.cpp.

TEST() [10/76]

TEST	(	Pose3	,
		Align2
	)

Definition at line 841 of file testPose3.cpp.

TEST() [11/76]

TEST	(	Pose3	,
		Bearing
	)

Definition at line 773 of file testPose3.cpp.

TEST() [12/76]

TEST	(	Pose3	,
		Bearing2
	)

Definition at line 784 of file testPose3.cpp.

TEST() [13/76]

TEST	(	Pose3	,
		between
	)

Definition at line 675 of file testPose3.cpp.

TEST() [14/76]

TEST	(	Pose3	,
		ChartDerivatives
	)

Definition at line 1039 of file testPose3.cpp.

TEST() [15/76]

TEST	(	Pose3	,
		compose
	)

Definition at line 351 of file testPose3.cpp.

TEST() [16/76]

TEST	(	Pose3	,
		compose2
	)

Definition at line 370 of file testPose3.cpp.

TEST() [17/76]

TEST	(	Pose3	,
		compose_inverse
	)

Definition at line 416 of file testPose3.cpp.

TEST() [18/76]

TEST	(	Pose3	,
		Concept
	)

Definition at line 45 of file testPose3.cpp.

TEST() [19/76]

TEST	(	Pose3	,
		constructors
	)

Definition at line 61 of file testPose3.cpp.

TEST() [20/76]

TEST	(	Pose3	,
		Create
	)

Definition at line 1358 of file testPose3.cpp.

TEST() [21/76]

TEST	(	Pose3	,
		Dtransform_from1_a
	)

Definition at line 427 of file testPose3.cpp.

TEST() [22/76]

TEST	(	Pose3	,
		Dtransform_from1_b
	)

Definition at line 434 of file testPose3.cpp.

TEST() [23/76]

TEST	(	Pose3	,
		Dtransform_from1_c
	)

Definition at line 442 of file testPose3.cpp.

TEST() [24/76]

TEST	(	Pose3	,
		Dtransform_from1_d
	)

Definition at line 451 of file testPose3.cpp.

TEST() [25/76]

TEST	(	Pose3	,
		Dtransform_from2
	)

Definition at line 464 of file testPose3.cpp.

TEST() [26/76]

TEST	(	Pose3	,
		Dtransform_to1
	)

Definition at line 475 of file testPose3.cpp.

TEST() [27/76]

TEST	(	Pose3	,
		Dtransform_to2
	)

Definition at line 483 of file testPose3.cpp.

TEST() [28/76]

TEST	(	Pose3	,
		equals
	)

Definition at line 52 of file testPose3.cpp.

TEST() [29/76]

TEST	(	Pose3	,
		expmap_a_full
	)

Definition at line 90 of file testPose3.cpp.

TEST() [30/76]

TEST	(	Pose3	,
		expmap_a_full2
	)

Definition at line 101 of file testPose3.cpp.

TEST() [31/76]

TEST	(	Pose3	,
		expmap_b
	)

Definition at line 112 of file testPose3.cpp.

TEST() [32/76]

TEST	(	Pose3	,
		expmap_c_full
	)

Definition at line 132 of file testPose3.cpp.

TEST() [33/76]

TEST	(	Pose3	,
		expmap_logmap
	)

Definition at line 623 of file testPose3.cpp.

TEST() [34/76]

TEST	(	Pose3	,
		ExpmapChainRule
	)

Definition at line 1380 of file testPose3.cpp.

TEST() [35/76]

TEST	(	Pose3	,
		ExpmapDerivative
	)

Definition at line 858 of file testPose3.cpp.

TEST() [36/76]

TEST	(	Pose3	,
		ExpmapDerivatives
	)

Definition at line 905 of file testPose3.cpp.

TEST() [37/76]

TEST	(	Pose3	,
		expmaps_galore_full
	)

Definition at line 285 of file testPose3.cpp.

TEST() [38/76]

TEST	(	Pose3	,
		expressionWrappers
	)

Definition at line 1324 of file testPose3.cpp.

TEST() [39/76]

TEST	(	Pose3	,
		HatAndVee
	)

Definition at line 231 of file testPose3.cpp.

TEST() [40/76]

TEST	(	Pose3	,
		interpolate
	)

Definition at line 1149 of file testPose3.cpp.

TEST() [41/76]

TEST	(	Pose3	,
		interpolateJacobians
	)

Definition at line 1184 of file testPose3.cpp.

TEST() [42/76]

TEST	(	Pose3	,
		interpolateRtJacobians
	)

Definition at line 1257 of file testPose3.cpp.

TEST() [43/76]

TEST	(	Pose3	,
		Invariants
	)

Definition at line 1014 of file testPose3.cpp.

TEST() [44/76]

TEST	(	Pose3	,
		inverse
	)

Definition at line 389 of file testPose3.cpp.

TEST() [45/76]

TEST	(	Pose3	,
		inverseDerivatives2
	)

Definition at line 402 of file testPose3.cpp.

TEST() [46/76]

TEST	(	Pose3	,
		LieGroupDerivatives
	)

Definition at line 1029 of file testPose3.cpp.

TEST() [47/76]

TEST	(	Pose3	,
		Logmap
	)

Definition at line 923 of file testPose3.cpp.

TEST() [48/76]

TEST	(	Pose3	,
		LogmapDerivatives
	)

Definition at line 936 of file testPose3.cpp.

TEST() [49/76]

TEST	(	Pose3	,
		manifold_expmap
	)

Definition at line 644 of file testPose3.cpp.

TEST() [50/76]

TEST	(	Pose3	,
		PoseToPoseBearing
	)

Definition at line 795 of file testPose3.cpp.

TEST() [51/76]

TEST	(	Pose3	,
		Print
	)

Definition at line 1370 of file testPose3.cpp.

TEST() [52/76]

TEST	(	Pose3	,
		range
	)

Definition at line 703 of file testPose3.cpp.

TEST() [53/76]

TEST	(	Pose3	,
		range_pose
	)

Definition at line 738 of file testPose3.cpp.

TEST() [54/76]

TEST	(	Pose3	,
		retract_expmap
	)

Definition at line 81 of file testPose3.cpp.

TEST() [55/76]

TEST	(	Pose3	,
		retract_first_order
	)

Definition at line 70 of file testPose3.cpp.

TEST() [56/76]

TEST	(	Pose3	,
		Retract_LocalCoordinates
	)

Definition at line 606 of file testPose3.cpp.

TEST() [57/76]

TEST	(	Pose3	,
		retract_localCoordinates
	)

Definition at line 615 of file testPose3.cpp.

TEST() [58/76]

TEST	(	Pose3	,
		retract_localCoordinates2
	)

Definition at line 631 of file testPose3.cpp.

TEST() [59/76]

TEST	(	Pose3	,
		rotation
	)

Definition at line 326 of file testPose3.cpp.

TEST() [60/76]

TEST	(	Pose3	,
		stream
	)

Definition at line 1005 of file testPose3.cpp.

TEST() [61/76]

TEST	(	Pose3	,
		subgroups
	)

Definition at line 659 of file testPose3.cpp.

TEST() [62/76]

TEST	(	Pose3	,
		transform_from_with_derivatives
	)

Definition at line 501 of file testPose3.cpp.

TEST() [63/76]

TEST	(	Pose3	,
		transform_roundtrip
	)

Definition at line 599 of file testPose3.cpp.

TEST() [64/76]

TEST	(	Pose3	,
		transform_to_rotate
	)

Definition at line 519 of file testPose3.cpp.

TEST() [65/76]

TEST	(	Pose3	,
		transform_to_translate
	)

Definition at line 511 of file testPose3.cpp.

TEST() [66/76]

TEST	(	Pose3	,
		transform_to_with_derivatives
	)

Definition at line 491 of file testPose3.cpp.

TEST() [67/76]

TEST	(	Pose3	,
		TransformCovariance6
	)

Definition at line 1095 of file testPose3.cpp.

TEST() [68/76]

TEST	(	Pose3	,
		TransformCovariance6MapTo2d
	)

Definition at line 1052 of file testPose3.cpp.

TEST() [69/76]

TEST	(	Pose3	,
		transformFrom
	)

Definition at line 592 of file testPose3.cpp.

TEST() [70/76]

TEST	(	Pose3	,
		transformPoseFrom
	)

Definition at line 532 of file testPose3.cpp.

TEST() [71/76]

TEST	(	Pose3	,
		transformPoseTo
	)

Definition at line 562 of file testPose3.cpp.

TEST() [72/76]

TEST	(	Pose3	,
		transformPoseTo_with_derivatives
	)

Definition at line 568 of file testPose3.cpp.

TEST() [73/76]

TEST	(	Pose3	,
		transformPoseTo_with_derivatives2
	)

Definition at line 580 of file testPose3.cpp.

TEST() [74/76]

TEST	(	Pose3	,
		transformTo
	)

Definition at line 550 of file testPose3.cpp.

TEST() [75/76]

TEST	(	Pose3	,
		translation
	)

Definition at line 315 of file testPose3.cpp.

TEST() [76/76]

TEST	(	Pose3	,
		unicycle
	)

Definition at line 808 of file testPose3.cpp.

testDerivAdjoint()

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

Definition at line 960 of file testPose3.cpp.

testDerivAdjointTranspose()

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

Definition at line 982 of file testPose3.cpp.

testing_interpolate()

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

Definition at line 1182 of file testPose3.cpp.

testing_interpolate_rt()

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

Definition at line 1255 of file testPose3.cpp.

transform_to_()

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

Definition at line 472 of file testPose3.cpp.

transformFrom_()

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

Definition at line 424 of file testPose3.cpp.

transformPoseFrom_()

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

Definition at line 528 of file testPose3.cpp.

transformPoseTo_()

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

Definition at line 557 of file testPose3.cpp.

Variable Documentation

R

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

static

Definition at line 37 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 42 of file testPose3.cpp.

x1

Pose3 x1

Definition at line 692 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))