testPose3Upright.cpp
Go to the documentation of this file.
1 
8 #include <CppUnitLite/TestHarness.h>
9 
10 #include <gtsam/base/TestableAssertions.h>
11 #include <gtsam/base/numericalDerivative.h>
12 
13 #include <gtsam_unstable/geometry/Pose3Upright.h>
14 
15 using namespace gtsam;
16 
17 static const double tol = 1e-5;
18 
19 /* ************************************************************************* */
20 TEST( testPose3Upright, basics ) {
21  Pose3Upright origin;
22  EXPECT_DOUBLES_EQUAL(0.0, origin.x(), tol);
23  EXPECT_DOUBLES_EQUAL(0.0, origin.y(), tol);
24  EXPECT_DOUBLES_EQUAL(0.0, origin.z(), tol);
25  EXPECT_DOUBLES_EQUAL(0.0, origin.theta(), tol);
26 
27  Pose3Upright actual1(Rot2::fromAngle(0.1), Point3(1.0, 2.0, 3.0));
28  EXPECT_DOUBLES_EQUAL(1.0, actual1.x(), tol);
29  EXPECT_DOUBLES_EQUAL(2.0, actual1.y(), tol);
30  EXPECT_DOUBLES_EQUAL(3.0, actual1.z(), tol);
31  EXPECT_DOUBLES_EQUAL(0.1, actual1.theta(), tol);
32 
33  Pose3Upright actual2(1.0, 2.0, 3.0, 0.1);
34  EXPECT_DOUBLES_EQUAL(1.0, actual2.x(), tol);
35  EXPECT_DOUBLES_EQUAL(2.0, actual2.y(), tol);
36  EXPECT_DOUBLES_EQUAL(3.0, actual2.z(), tol);
37  EXPECT_DOUBLES_EQUAL(0.1, actual2.theta(), tol);
38 }
39 
40 /* ************************************************************************* */
41 TEST( testPose3Upright, equals ) {
42  Pose3Upright origin, actual1(1.0, 2.0, 3.0, 0.1),
43  actual2(1.0, 2.0, 3.0, 0.1), actual3(4.0,-7.0, 3.0, 0.3);
44  EXPECT(assert_equal(origin, origin, tol));
45  EXPECT(assert_equal(actual1, actual1, tol));
46  EXPECT(assert_equal(actual1, actual2, tol));
47  EXPECT(assert_equal(actual2, actual1, tol));
48 
49  EXPECT(assert_inequal(actual1, actual3, tol));
50  EXPECT(assert_inequal(actual3, actual1, tol));
51  EXPECT(assert_inequal(actual1, origin, tol));
52  EXPECT(assert_inequal(origin, actual1, tol));
53 }
54 
55 /* ************************************************************************* */
56 TEST( testPose3Upright, conversions ) {
57  Pose3Upright pose(1.0, 2.0, 3.0, 0.1);
58  EXPECT(assert_equal(Point3(1.0, 2.0, 3.0), pose.translation(), tol));
59  EXPECT(assert_equal(Point2(1.0, 2.0), pose.translation2(), tol));
60  EXPECT(assert_equal(Rot2::fromAngle(0.1), pose.rotation2(), tol));
61  EXPECT(assert_equal(Rot3::Yaw(0.1), pose.rotation(), tol));
62  EXPECT(assert_equal(Pose2(1.0, 2.0, 0.1), pose.pose2(), tol));
63  EXPECT(assert_equal(Pose3(Rot3::Yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol));
64 }
65 
66 /* ************************************************************************* */
67 TEST( testPose3Upright, manifold ) {
68  Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.0), x2(4.0, 2.0, 7.0, 0.0);
69  EXPECT_LONGS_EQUAL(4, origin.dim());
70 
71  EXPECT(assert_equal(origin, origin.retract(Z_4x1), tol));
72  EXPECT(assert_equal(x1, x1.retract(Z_4x1), tol));
73  EXPECT(assert_equal(x2, x2.retract(Z_4x1), tol));
74 
75  Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0).finished(), delta21 = -delta12;
76  EXPECT(assert_equal(x2, x1.retract(delta12), tol));
77  EXPECT(assert_equal(x1, x2.retract(delta21), tol));
78 
79  EXPECT(assert_equal(delta12, x1.localCoordinates(x2), tol));
80  EXPECT(assert_equal(delta21, x2.localCoordinates(x1), tol));
81 }
82 
83 /* ************************************************************************* */
84 TEST( testPose3Upright, lie ) {
85  Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.1);
86  EXPECT(assert_equal(Z_4x1, Pose3Upright::Logmap(origin), tol));
87  EXPECT(assert_equal(origin, Pose3Upright::Expmap(Z_4x1), tol));
88 
89  EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol));
90 }
91 
92 /* ************************************************************************* */
93 Pose3Upright between_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.between(x2); }
94 TEST( testPose3Upright, between ) {
95  Pose3Upright x1(1.0, 2.0, 3.0, 0.1), x2(4.0,-2.0, 7.0, 0.3);
96  Pose3Upright expected(x1.pose2().between(x2.pose2()), x2.z() - x1.z());
97  EXPECT(assert_equal(expected, x1.between(x2), tol));
98 
99  Matrix actualH1, actualH2, numericH1, numericH2;
100  x1.between(x2, actualH1, actualH2);
101  numericH1 = numericalDerivative21(between_proxy, x1, x2, 1e-5);
102  numericH2 = numericalDerivative22(between_proxy, x1, x2, 1e-5);
103  EXPECT(assert_equal(numericH1, actualH1, tol));
104  EXPECT(assert_equal(numericH2, actualH2, tol));
105 }
106 
107 /* ************************************************************************* */
108 Pose3Upright compose_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.compose(x2); }
109 TEST( testPose3Upright, compose ) {
110  Pose3Upright x1(1.0, 2.0, 3.0, 0.1), x2(4.0,-2.0, 7.0, 0.3);
111  Pose3Upright expected(x1.pose2().between(x2.pose2()), x2.z() - x1.z());
112  EXPECT(assert_equal(x2, x1.compose(expected), tol));
113 
114  Matrix actualH1, actualH2, numericH1, numericH2;
115  x1.compose(expected, actualH1, actualH2);
116  numericH1 = numericalDerivative21(compose_proxy, x1, expected, 1e-5);
117  numericH2 = numericalDerivative22(compose_proxy, x1, expected, 1e-5);
118  EXPECT(assert_equal(numericH1, actualH1, tol));
119  EXPECT(assert_equal(numericH2, actualH2, tol));
120 }
121 
122 /* ************************************************************************* */
123 Pose3Upright inverse_proxy(const Pose3Upright& x1) { return x1.inverse(); }
124 TEST( testPose3Upright, inverse ) {
125  Pose3Upright x1(1.0, 2.0, 3.0, 0.1);
126  Pose3Upright expected(x1.pose2().inverse(), - x1.z());
127  EXPECT(assert_equal(expected, x1.inverse(), tol));
128 
129  Matrix actualH1, numericH1;
130  x1.inverse(actualH1);
131  numericH1 = numericalDerivative11(inverse_proxy, x1, 1e-5);
132  EXPECT(assert_equal(numericH1, actualH1, tol));
133 }
134 
135 /* ************************************************************************* */
136 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
137 /* ************************************************************************* */
TestableAssertions.h
Provides additional testing facilities for common data structures.
gtsam::Pose3Upright::Expmap
static Pose3Upright Expmap(const Vector &xi)
Exponential map at identity - create a rotation from canonical coordinates.
Definition: Pose3Upright.cpp:140
gtsam::Pose3Upright::dim
size_t dim() const
Dimensionality of tangent space = 4 DOF.
Definition: Pose3Upright.h:84
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Pose3Upright::rotation2
Rot2 rotation2() const
Definition: Pose3Upright.cpp:61
origin
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
Definition: gnuplot_common_settings.hh:12
inverse_proxy
Pose3Upright inverse_proxy(const Pose3Upright &x1)
Definition: testPose3Upright.cpp:123
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
gtsam::Pose3Upright::between
Pose3Upright between(const Pose3Upright &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
Definition: Pose3Upright.cpp:109
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
gtsam::Pose3Upright::compose
Pose3Upright compose(const Pose3Upright &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
compose this transformation onto another (first *this and then p2)
Definition: Pose3Upright.cpp:93
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
gtsam::Pose3Upright::Logmap
static Vector Logmap(const Pose3Upright &p)
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose3Upright.cpp:147
numericalDerivative.h
Some functions to compute numerical derivatives.
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const boost::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:173
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
gtsam::Pose3Upright::translation2
Point2 translation2() const
Definition: Pose3Upright.cpp:56
main
int main()
Definition: testPose3Upright.cpp:136
gtsam::Pose3Upright::retract
Pose3Upright retract(const Vector &v) const
Definition: Pose3Upright.cpp:125
gtsam::Pose3Upright::x
double x() const
Definition: Pose3Upright.h:64
gtsam::Pose3Upright::pose
Pose3 pose() const
Definition: Pose3Upright.cpp:76
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:47
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:177
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(boost::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:199
pose
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
gtsam::Pose3Upright::localCoordinates
Vector localCoordinates(const Pose3Upright &p2) const
Local 3D coordinates of Pose3Upright manifold neighborhood around current pose.
Definition: Pose3Upright.cpp:132
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Pose3Upright::theta
double theta() const
Definition: Pose3Upright.h:67
gtsam::Pose3Upright::z
double z() const
Definition: Pose3Upright.h:66
gtsam::Pose3Upright::rotation
Rot3 rotation() const
Definition: Pose3Upright.cpp:66
gtsam
traits
Definition: chartTesting.h:28
gtsam::Pose3Upright::translation
Point3 translation() const
Definition: Pose3Upright.cpp:51
between_proxy
Pose3Upright between_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
Definition: testPose3Upright.cpp:93
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:130
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
x1
Pose3 x1
Definition: testPose3.cpp:588
gtsam::compose
Expression< T > compose(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:23
gtsam::Pose3Upright::y
double y() const
Definition: Pose3Upright.h:65
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:51
gtsam::TEST
TEST(LPInitSolver, InfiniteLoopSingleVar)
Definition: testLPSolver.cpp:64
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:62
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:58
gtsam::Pose3Upright::pose2
Pose2 pose2() const
Definition: Pose3Upright.cpp:71
gtsam::tol
const G double tol
Definition: Group.h:83
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
gtsam::Pose2::inverse
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition: Pose2.cpp:201
gtsam::numericalDerivative11
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
Definition: numericalDerivative.h:118
gtsam::Pose3Upright::inverse
Pose3Upright inverse(boost::optional< Matrix & > H1=boost::none) const
inverse transformation with derivatives
Definition: Pose3Upright.cpp:81
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
inverse
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:332
compose_proxy
Pose3Upright compose_proxy(const Pose3Upright &x1, const Pose3Upright &x2)
Definition: testPose3Upright.cpp:108
gtsam::Pose3::z
double z() const
get z
Definition: Pose3.h:279
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:135
Pose3Upright.h
Variation of a Pose3 in which the rotation is constained to purely yaw This state is essentially a Po...

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:35