1/**
2 * @file testPose3Upright.cpp
3 *
4 * @date Jan 24, 2012
5 * @author Alex Cunningham
6 */
7
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
15using namespace gtsam;
16
17static const double tol = 1e-5;
18
19/* ************************************************************************* */
20TEST( 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(theta: 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/* ************************************************************************* */
41TEST( 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/* ************************************************************************* */
56TEST( 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/* ************************************************************************* */
67TEST( 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/* ************************************************************************* */
84TEST( 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/* ************************************************************************* */
93Pose3Upright between_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.between(p2: x2); }
94TEST( 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(g: 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(p2: x2, H1: actualH1, H2: actualH2);
101 numericH1 = numericalDerivative21(h: between_proxy, x1, x2, delta: 1e-5);
102 numericH2 = numericalDerivative22(h: between_proxy, x1, x2, delta: 1e-5);
103 EXPECT(assert_equal(numericH1, actualH1, tol));
104 EXPECT(assert_equal(numericH2, actualH2, tol));
105}
106
107/* ************************************************************************* */
108Pose3Upright compose_proxy(const Pose3Upright& x1, const Pose3Upright& x2) { return x1.compose(p2: x2); }
109TEST( 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(g: 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(p2: expected, H1: actualH1, H2: actualH2);
116 numericH1 = numericalDerivative21(h: compose_proxy, x1, x2: expected, delta: 1e-5);
117 numericH2 = numericalDerivative22(h: compose_proxy, x1, x2: expected, delta: 1e-5);
118 EXPECT(assert_equal(numericH1, actualH1, tol));
119 EXPECT(assert_equal(numericH2, actualH2, tol));
120}
121
122/* ************************************************************************* */
123Pose3Upright inverse_proxy(const Pose3Upright& x1) { return x1.inverse(); }
124TEST( 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(H1: actualH1);
131 numericH1 = numericalDerivative11(h: inverse_proxy, x: x1, delta: 1e-5);
132 EXPECT(assert_equal(numericH1, actualH1, tol));
133}
134
135/* ************************************************************************* */
136int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
137/* ************************************************************************* */
138