testLie.h
Go to the documentation of this file.
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  * -------------------------------------------------------------------------- */
11 
12 /*
13  * @file testLie.h
14  * @brief Test utilities for Lie groups
15  * @date November, 2014
16  * @author Paul Furgale
17  */
18 
19 #pragma once
20 
21 #include <gtsam/base/Lie.h>
23 
24 #include <CppUnitLite/TestResult.h>
25 #include <CppUnitLite/Test.h>
26 #include <CppUnitLite/Failure.h>
27 
28 namespace gtsam {
29 
30 // Do a comprehensive test of Lie Group derivatives
31 template<typename G>
32 void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
33  const G& t1, const G& t2) {
34 
35  Matrix H1, H2;
36  typedef traits<G> T;
38 
39  // Inverse
40  OJ none;
41  EXPECT(assert_equal<G>(t1.inverse(),T::Inverse(t1, H1)));
42  EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
43 
44  EXPECT(assert_equal<G>(t2.inverse(),T::Inverse(t2, H1)));
45  EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
46 
47  // Compose
48  EXPECT(assert_equal<G>(t1 * t2,T::Compose(t1, t2, H1, H2)));
49  EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
50  EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
51 
52  // Between
53  EXPECT(assert_equal<G>(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
54  EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
55  EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
56 }
57 
58 // Do a comprehensive test of Lie Group Chart derivatives
59 template<typename G>
60 void testChartDerivatives(TestResult& result_, const std::string& name_,
61  const G& t1, const G& t2) {
62 
63  Matrix H1, H2;
64  typedef traits<G> T;
65  typedef typename T::TangentVector V;
67 
68  // Retract
69  OJ none;
70  V w12 = T::Local(t1, t2);
71  EXPECT(assert_equal<G>(t2, T::Retract(t1,w12, H1, H2)));
72  EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
73  EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
74 
75  // Local
76  EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2)));
77  EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
78  EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
79 }
80 } // namespace gtsam
81 
82 #define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \
83  { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); }
84 
85 #define CHECK_CHART_DERIVATIVES(t1,t2) \
86  { gtsam::testChartDerivatives(result_, name_, t1, t2); }
void testChartDerivatives(TestResult &result_, const std::string &name_, const G &t1, const G &t2)
Definition: testLie.h:60
JacobiRotation< float > G
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Some functions to compute numerical derivatives.
Definition: pytypes.h:1614
void testLieGroupDerivatives(TestResult &result_, const std::string &name_, const G &t1, const G &t2)
Definition: testLie.h:32
#define EXPECT(condition)
Definition: Test.h:150
Eigen::Triplet< double > T
BetweenFactor< Rot3 > Between
Base class and basic functions for Lie types.
traits
Definition: chartTesting.h:28


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:35