1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------1------------------------------------------- */
11
12/**
13 * @file testManifold.cpp
14 * @date September 18, 2014
15 * @author Frank Dellaert
16 * @author Paul Furgale
17 * @brief unit tests for Manifold type machinery
18 */
19
20#include <gtsam/base/Manifold.h>
21#include <gtsam/geometry/PinholeCamera.h>
22#include <gtsam/geometry/Pose2.h>
23#include <gtsam/geometry/Cal3_S2.h>
24#include <gtsam/geometry/Cal3Bundler.h>
25#include <gtsam/base/VectorSpace.h>
26#include <gtsam/base/testLie.h>
27#include <gtsam/base/Testable.h>
28
29#undef CHECK
30#include <CppUnitLite/TestHarness.h>
31
32using namespace std;
33using namespace gtsam;
34
35// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector
36typedef PinholeCamera<Cal3Bundler> Camera;
37
38//******************************************************************************
39TEST(Manifold, SomeManifoldsGTSAM) {
40 //GTSAM_CONCEPT_ASSERT(IsManifold<int>); // integer is not a manifold
41 GTSAM_CONCEPT_ASSERT(IsManifold<Camera>);
42 GTSAM_CONCEPT_ASSERT(IsManifold<Cal3_S2>);
43 GTSAM_CONCEPT_ASSERT(IsManifold<Cal3Bundler>);
44 GTSAM_CONCEPT_ASSERT(IsManifold<Camera>);
45}
46
47//******************************************************************************
48TEST(Manifold, SomeLieGroupsGTSAM) {
49 GTSAM_CONCEPT_ASSERT(IsLieGroup<Rot2>);
50 GTSAM_CONCEPT_ASSERT(IsLieGroup<Pose2>);
51 GTSAM_CONCEPT_ASSERT(IsLieGroup<Rot3>);
52 GTSAM_CONCEPT_ASSERT(IsLieGroup<Pose3>);
53}
54
55//******************************************************************************
56TEST(Manifold, SomeVectorSpacesGTSAM) {
57 GTSAM_CONCEPT_ASSERT(IsVectorSpace<double>);
58 GTSAM_CONCEPT_ASSERT(IsVectorSpace<float>);
59 GTSAM_CONCEPT_ASSERT(IsVectorSpace<Point2>);
60 GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix24>);
61}
62
63//******************************************************************************
64// dimension
65TEST(Manifold, _dimension) {
66 //using namespace traits;
67 EXPECT_LONGS_EQUAL(2, traits<Point2>::dimension);
68 EXPECT_LONGS_EQUAL(8, traits<Matrix24>::dimension);
69 EXPECT_LONGS_EQUAL(1, traits<double>::dimension);
70}
71
72//******************************************************************************
73TEST(Manifold, Identity) {
74 EXPECT_DOUBLES_EQUAL(0.0, traits<double>::Identity(), 0.0);
75 EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits<Matrix24>::Identity())));
76 EXPECT(assert_equal(Pose3(), traits<Pose3>::Identity()));
77 EXPECT(assert_equal(Point2(0,0), traits<Point2>::Identity()));
78}
79
80//******************************************************************************
81// charts
82TEST(Manifold, DefaultChart) {
83
84 //DefaultChart<Point2> chart1;
85 EXPECT(traits<Point2>::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0));
86 EXPECT(traits<Point2>::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0));
87
88 Vector v2(2);
89 v2 << 1, 0;
90 //DefaultChart<Vector2> chart2;
91 EXPECT(assert_equal(v2, traits<Vector2>::Local(Vector2(0, 0), Vector2(1, 0))));
92 EXPECT(traits<Vector2>::Retract(Vector2(0, 0), v2) == Vector2(1, 0));
93
94 {
95 typedef Matrix2 ManifoldPoint;
96 ManifoldPoint m;
97 //DefaultChart<ManifoldPoint> chart;
98 m << 1, 3,
99 2, 4;
100 // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)!
101 EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
102 EXPECT(traits<ManifoldPoint>::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m);
103 }
104
105 {
106 typedef Eigen::Matrix<double, 1, 2> ManifoldPoint;
107 ManifoldPoint m;
108 //DefaultChart<ManifoldPoint> chart;
109 m << 1, 2;
110 EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
111 EXPECT(traits<ManifoldPoint>::Retract(m, Vector2(1, 2)) == 2 * m);
112 }
113
114 {
115 typedef Eigen::Matrix<double, 1, 1> ManifoldPoint;
116 ManifoldPoint m;
117 //DefaultChart<ManifoldPoint> chart;
118 m << 1;
119 EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
120 EXPECT(traits<ManifoldPoint>::Retract(m, ManifoldPoint::Ones()) == 2 * m);
121 }
122
123 //DefaultChart<double> chart3;
124 Vector v1(1);
125 v1 << 1;
126 EXPECT(assert_equal(v1, traits<double>::Local(0, 1)));
127 EXPECT_DOUBLES_EQUAL(traits<double>::Retract(0, v1), 1, 1e-9);
128
129 // Dynamic does not work yet !
130 Vector z = Z_2x1, v(2);
131 v << 1, 0;
132 //DefaultChart<Vector> chart4;
133// EXPECT(assert_equal(traits<Vector>::Local(z, v), v));
134// EXPECT(assert_equal(traits<Vector>::Retract(z, v), v));
135
136 Vector v3(3);
137 v3 << 1, 1, 1;
138 Rot3 I = Rot3::Identity();
139 Rot3 R = I.retract(v: v3);
140 //DefaultChart<Rot3> chart5;
141 EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));
142 EXPECT(assert_equal(traits<Rot3>::Retract(I, v3), R));
143 // Check zero vector
144 //DefaultChart<Rot3> chart6;
145 EXPECT(assert_equal((Vector) Z_3x1, traits<Rot3>::Local(R, R)));
146}
147
148//******************************************************************************
149int main() {
150 TestResult tr;
151 return TestRegistry::runAllTests(result&: tr);
152}
153//******************************************************************************
154
155