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 testLie.cpp
14 * @date May, 2015
15 * @author Frank Dellaert
16 * @brief unit tests for Lie group type machinery
17 */
18
19#include <gtsam/base/ProductLieGroup.h>
20
21#include <gtsam/geometry/Pose2.h>
22#include <gtsam/geometry/Point2.h>
23#include <gtsam/base/testLie.h>
24
25#include <CppUnitLite/TestHarness.h>
26
27using namespace std;
28using namespace gtsam;
29
30static const double tol = 1e-9;
31
32//******************************************************************************
33typedef ProductLieGroup<Point2, Pose2> Product;
34
35// Define any direct product group to be a model of the multiplicative Group concept
36namespace gtsam {
37template<> struct traits<Product> : internal::LieGroupTraits<Product> {
38 static void Print(const Product& m, const string& s = "") {
39 cout << s << "(" << m.first << "," << m.second.translation() << "/"
40 << m.second.theta() << ")" << endl;
41 }
42 static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) {
43 return traits<Point2>::Equals(v1: m1.first, v2: m2.first, tol) && m1.second.equals(pose: m2.second, tol);
44 }
45};
46}
47
48//******************************************************************************
49TEST(Lie, ProductLieGroup) {
50 GTSAM_CONCEPT_ASSERT(IsGroup<Product>);
51 GTSAM_CONCEPT_ASSERT(IsManifold<Product>);
52 GTSAM_CONCEPT_ASSERT(IsLieGroup<Product>);
53 Product pair1;
54 Vector5 d;
55 d << 1, 2, 0.1, 0.2, 0.3;
56 Product expected(Point2(1, 2), Pose2::Expmap(xi: Vector3(0.1, 0.2, 0.3)));
57 Product pair2 = pair1.expmap(v: d);
58 EXPECT(assert_equal(expected, pair2, 1e-9));
59 EXPECT(assert_equal(d, pair1.logmap(pair2), 1e-9));
60 const auto adj = pair1.AdjointMap();
61 EXPECT_LONGS_EQUAL(5, adj.rows());
62 EXPECT_LONGS_EQUAL(5, adj.cols());
63}
64
65/* ************************************************************************* */
66Product compose_proxy(const Product& A, const Product& B) {
67 return A.compose(g: B);
68}
69TEST( testProduct, compose ) {
70 Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1;
71
72 Matrix actH1, actH2;
73 state1.compose(other: state2, H1: actH1, H2: actH2);
74 Matrix numericH1 = numericalDerivative21(h: compose_proxy, x1: state1, x2: state2);
75 Matrix numericH2 = numericalDerivative22(h: compose_proxy, x1: state1, x2: state2);
76 EXPECT(assert_equal(numericH1, actH1, tol));
77 EXPECT(assert_equal(numericH2, actH2, tol));
78}
79
80/* ************************************************************************* */
81Product between_proxy(const Product& A, const Product& B) {
82 return A.between(g: B);
83}
84TEST( testProduct, between ) {
85 Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1;
86
87 Matrix actH1, actH2;
88 state1.between(other: state2, H1: actH1, H2: actH2);
89 Matrix numericH1 = numericalDerivative21(h: between_proxy, x1: state1, x2: state2);
90 Matrix numericH2 = numericalDerivative22(h: between_proxy, x1: state1, x2: state2);
91 EXPECT(assert_equal(numericH1, actH1, tol));
92 EXPECT(assert_equal(numericH2, actH2, tol));
93}
94
95/* ************************************************************************* */
96Product inverse_proxy(const Product& A) {
97 return A.inverse();
98}
99TEST( testProduct, inverse ) {
100 Product state1(Point2(1, 2), Pose2(3, 4, 5));
101
102 Matrix actH1;
103 state1.inverse(D: actH1);
104 Matrix numericH1 = numericalDerivative11(h: inverse_proxy, x: state1);
105 EXPECT(assert_equal(numericH1, actH1, tol));
106}
107
108/* ************************************************************************* */
109Product expmap_proxy(const Vector5& vec) {
110 return Product::Expmap(v: vec);
111}
112TEST( testProduct, Expmap ) {
113 Vector5 vec;
114 vec << 1, 2, 0.1, 0.2, 0.3;
115
116 Matrix actH;
117 Product::Expmap(v: vec, Hv: actH);
118 Matrix numericH = numericalDerivative11(h: expmap_proxy, x: vec);
119 EXPECT(assert_equal(numericH, actH, tol));
120}
121
122/* ************************************************************************* */
123Vector5 logmap_proxy(const Product& p) {
124 return Product::Logmap(p);
125}
126TEST( testProduct, Logmap ) {
127 Product state(Point2(1, 2), Pose2(3, 4, 5));
128
129 Matrix actH;
130 Product::Logmap(p: state, Hp: actH);
131 Matrix numericH = numericalDerivative11(h: logmap_proxy, x: state);
132 EXPECT(assert_equal(numericH, actH, tol));
133}
134
135/* ************************************************************************* */
136Product interpolate_proxy(const Product& x, const Product& y, double t) {
137 return interpolate<Product>(X: x, Y: y, t);
138}
139
140TEST(Lie, Interpolate) {
141 Product x(Point2(1, 2), Pose2(3, 4, 5));
142 Product y(Point2(6, 7), Pose2(8, 9, 0));
143
144 double t;
145 Matrix actH1, numericH1, actH2, numericH2, actH3, numericH3;
146
147 t = 0.0;
148 interpolate<Product>(X: x, Y: y, t, Hx: actH1, Hy: actH2, Ht: actH3);
149 numericH1 = numericalDerivative31<Product, Product, Product, double>(
150 h: interpolate_proxy, x1: x, x2: y, x3: t);
151 EXPECT(assert_equal(numericH1, actH1, tol));
152 numericH2 = numericalDerivative32<Product, Product, Product, double>(
153 h: interpolate_proxy, x1: x, x2: y, x3: t);
154 EXPECT(assert_equal(numericH2, actH2, tol));
155 numericH3 = numericalDerivative33<Product, Product, Product, double>(
156 h: interpolate_proxy, x1: x, x2: y, x3: t);
157 EXPECT(assert_equal(numericH3, actH3, tol));
158
159 t = 0.5;
160 interpolate<Product>(X: x, Y: y, t, Hx: actH1, Hy: actH2);
161 numericH1 = numericalDerivative31<Product, Product, Product, double>(
162 h: interpolate_proxy, x1: x, x2: y, x3: t);
163 EXPECT(assert_equal(numericH1, actH1, tol));
164 numericH2 = numericalDerivative32<Product, Product, Product, double>(
165 h: interpolate_proxy, x1: x, x2: y, x3: t);
166 EXPECT(assert_equal(numericH2, actH2, tol));
167 numericH3 = numericalDerivative33<Product, Product, Product, double>(
168 h: interpolate_proxy, x1: x, x2: y, x3: t);
169 EXPECT(assert_equal(numericH3, actH3, tol));
170
171 t = 1.0;
172 interpolate<Product>(X: x, Y: y, t, Hx: actH1, Hy: actH2);
173 numericH1 = numericalDerivative31<Product, Product, Product, double>(
174 h: interpolate_proxy, x1: x, x2: y, x3: t);
175 EXPECT(assert_equal(numericH1, actH1, tol));
176 numericH2 = numericalDerivative32<Product, Product, Product, double>(
177 h: interpolate_proxy, x1: x, x2: y, x3: t);
178 EXPECT(assert_equal(numericH2, actH2, tol));
179 numericH3 = numericalDerivative33<Product, Product, Product, double>(
180 h: interpolate_proxy, x1: x, x2: y, x3: t);
181 EXPECT(assert_equal(numericH3, actH3, tol));
182}
183
184//******************************************************************************
185int main() {
186 TestResult tr;
187 return TestRegistry::runAllTests(result&: tr);
188}
189//******************************************************************************
190
191