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 <boost/assign/std/vector.hpp>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
#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_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, Create)
	TEST (Pose3, Print)
Vector6	testDerivAdjoint (const Vector6 &xi, const Vector6 &v)
Vector6	testDerivAdjointTranspose (const Vector6 &xi, const Vector6 &v)
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

Pose3 Agrawal06iros

(

const Vector &

xi

)

Agrawal06iros version of exponential map

Definition at line 167 of file testPose3.cpp.

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

Definition at line 666 of file testPose3.cpp.

Point3 l1	(	1	,
		0	,
		0
	)

Point3 l2	(	1	,
		1	,
		0
	)

Point3 l3	(	2	,
		2	,
		0
	)

Point3 l4	(	1	,
		4	,
		-	4
	)

int main

(

void

)

Definition at line 1069 of file testPose3.cpp.

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

static

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

static

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

Definition at line 631 of file testPose3.cpp.

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

Definition at line 596 of file testPose3.cpp.

TEST	(	Pose3	,
		equals
	)

Definition at line 44 of file testPose3.cpp.

TEST	(	Pose3	,
		constructors
	)

Definition at line 53 of file testPose3.cpp.

TEST	(	Pose3	,
		retract_first_order
	)

Definition at line 62 of file testPose3.cpp.

TEST	(	Pose3	,
		retract_expmap
	)

Definition at line 73 of file testPose3.cpp.

TEST	(	Pose3	,
		expmap_a_full
	)

Definition at line 82 of file testPose3.cpp.

TEST	(	Pose3	,
		expmap_a_full2
	)

Definition at line 93 of file testPose3.cpp.

TEST	(	Pose3	,
		expmap_b
	)

Definition at line 104 of file testPose3.cpp.

TEST	(	Pose3	,
		expmap_c_full
	)

Definition at line 124 of file testPose3.cpp.

TEST	(	Pose3	,
		Adjoint_full
	)

Definition at line 132 of file testPose3.cpp.

TEST	(	Pose3	,
		Adjoint_hat
	)

Definition at line 149 of file testPose3.cpp.

TEST	(	Pose3	,
		expmaps_galore_full
	)

Definition at line 181 of file testPose3.cpp.

TEST	(	Pose3	,
		translation
	)

Definition at line 211 of file testPose3.cpp.

TEST	(	Pose3	,
		rotation
	)

Definition at line 222 of file testPose3.cpp.

TEST	(	Pose3	,
		Adjoint_compose_full
	)

Definition at line 232 of file testPose3.cpp.

TEST	(	Pose3	,
		compose
	)

Definition at line 247 of file testPose3.cpp.

TEST	(	Pose3	,
		compose2
	)

Definition at line 266 of file testPose3.cpp.

TEST	(	Pose3	,
		inverse
	)

Definition at line 285 of file testPose3.cpp.

TEST	(	Pose3	,
		inverseDerivatives2
	)

Definition at line 298 of file testPose3.cpp.

TEST	(	Pose3	,
		compose_inverse
	)

Definition at line 312 of file testPose3.cpp.

TEST	(	Pose3	,
		Dtransform_from1_a
	)

Definition at line 323 of file testPose3.cpp.

TEST	(	Pose3	,
		Dtransform_from1_b
	)

Definition at line 330 of file testPose3.cpp.

TEST	(	Pose3	,
		Dtransform_from1_c
	)

Definition at line 338 of file testPose3.cpp.

TEST	(	Pose3	,
		Dtransform_from1_d
	)

Definition at line 347 of file testPose3.cpp.

TEST	(	Pose3	,
		Dtransform_from2
	)

Definition at line 360 of file testPose3.cpp.

TEST	(	Pose3	,
		Dtransform_to1
	)

Definition at line 371 of file testPose3.cpp.

TEST	(	Pose3	,
		Dtransform_to2
	)

Definition at line 379 of file testPose3.cpp.

TEST	(	Pose3	,
		transform_to_with_derivatives
	)

Definition at line 387 of file testPose3.cpp.

TEST	(	Pose3	,
		transform_from_with_derivatives
	)

Definition at line 397 of file testPose3.cpp.

TEST	(	Pose3	,
		transform_to_translate
	)

Definition at line 407 of file testPose3.cpp.

TEST	(	Pose3	,
		transform_to_rotate
	)

Definition at line 415 of file testPose3.cpp.

TEST	(	Pose3	,
		transformPoseFrom
	)

Definition at line 428 of file testPose3.cpp.

TEST	(	Pose3	,
		transformTo
	)

Definition at line 446 of file testPose3.cpp.

TEST	(	Pose3	,
		transformPoseTo
	)

Definition at line 458 of file testPose3.cpp.

TEST	(	Pose3	,
		transformPoseTo_with_derivatives
	)

Definition at line 464 of file testPose3.cpp.

TEST	(	Pose3	,
		transformPoseTo_with_derivatives2
	)

Definition at line 476 of file testPose3.cpp.

TEST	(	Pose3	,
		transformFrom
	)

Definition at line 488 of file testPose3.cpp.

TEST	(	Pose3	,
		transform_roundtrip
	)

Definition at line 495 of file testPose3.cpp.

TEST	(	Pose3	,
		Retract_LocalCoordinates
	)

Definition at line 502 of file testPose3.cpp.

TEST	(	Pose3	,
		retract_localCoordinates
	)

Definition at line 511 of file testPose3.cpp.

TEST	(	Pose3	,
		expmap_logmap
	)

Definition at line 519 of file testPose3.cpp.

TEST	(	Pose3	,
		retract_localCoordinates2
	)

Definition at line 527 of file testPose3.cpp.

TEST	(	Pose3	,
		manifold_expmap
	)

Definition at line 540 of file testPose3.cpp.

TEST	(	Pose3	,
		subgroups
	)

Definition at line 555 of file testPose3.cpp.

TEST	(	Pose3	,
		between
	)

Definition at line 571 of file testPose3.cpp.

TEST	(	Pose3	,
		range
	)

Definition at line 599 of file testPose3.cpp.

TEST	(	Pose3	,
		range_pose
	)

Definition at line 634 of file testPose3.cpp.

TEST	(	Pose3	,
		Bearing
	)

Definition at line 669 of file testPose3.cpp.

TEST	(	Pose3	,
		Bearing2
	)

Definition at line 680 of file testPose3.cpp.

TEST	(	Pose3	,
		PoseToPoseBearing
	)

Definition at line 691 of file testPose3.cpp.

TEST	(	Pose3	,
		unicycle
	)

Definition at line 713 of file testPose3.cpp.

TEST	(	Pose3	,
		adjointMap
	)

Definition at line 723 of file testPose3.cpp.

TEST	(	Pose3	,
		Align1
	)

Definition at line 733 of file testPose3.cpp.

TEST	(	Pose3	,
		Align2
	)

Definition at line 747 of file testPose3.cpp.

TEST	(	Pose3	,
		ExpmapDerivative1
	)

Definition at line 767 of file testPose3.cpp.

TEST	(	Pose3	,
		ExpmapDerivative2
	)

Definition at line 777 of file testPose3.cpp.

TEST	(	Pose3	,
		ExpmapDerivativeQr
	)

Definition at line 811 of file testPose3.cpp.

TEST	(	Pose3	,
		LogmapDerivative
	)

Definition at line 823 of file testPose3.cpp.

TEST	(	Pose3	,
		adjoint
	)

Definition at line 838 of file testPose3.cpp.

TEST	(	Pose3	,
		adjointTranspose
	)

Definition at line 856 of file testPose3.cpp.

TEST	(	Pose3	,
		stream
	)

Definition at line 872 of file testPose3.cpp.

TEST	(	Pose3	,
		Invariants
	)

Definition at line 881 of file testPose3.cpp.

TEST	(	Pose3	,
		LieGroupDerivatives
	)

Definition at line 896 of file testPose3.cpp.

TEST	(	Pose3	,
		ChartDerivatives
	)

Definition at line 906 of file testPose3.cpp.

TEST	(	Pose3	,
		TransformCovariance6MapTo2d
	)

Definition at line 919 of file testPose3.cpp.

TEST	(	Pose3	,
		TransformCovariance6
	)

Definition at line 962 of file testPose3.cpp.

TEST	(	Pose3	,
		interpolate
	)

Definition at line 1016 of file testPose3.cpp.

TEST	(	Pose3	,
		Create
	)

Definition at line 1049 of file testPose3.cpp.

TEST	(	Pose3	,
		Print
	)

Definition at line 1059 of file testPose3.cpp.

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

Definition at line 834 of file testPose3.cpp.

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

Definition at line 852 of file testPose3.cpp.

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

Definition at line 368 of file testPose3.cpp.

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

Definition at line 320 of file testPose3.cpp.

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

Definition at line 424 of file testPose3.cpp.

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

Definition at line 453 of file testPose3.cpp.

Variable Documentation

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

static

Definition at line 36 of file testPose3.cpp.

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

static

Definition at line 41 of file testPose3.cpp.

Pose3 x1

Definition at line 588 of file testPose3.cpp.

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

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

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))