Namespaces | Functions | Variables
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 ( ,
,
 
)

◆ l2()

Point3 l2 ( ,
,
 
)

◆ l3()

Point3 l3 ( ,
,
 
)

◆ l4()

Point3 l4 ( ,
,
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))


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:56