testShonanGaugeFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, 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 
20 
22 
23 #include <iostream>
24 #include <map>
25 
26 using namespace std;
27 using namespace gtsam;
28 
29 /* ************************************************************************* */
30 // Check dimensions of all low-dim GaugeFactors
31 TEST(ShonanAveraging, GaugeFactorLows) {
32  constexpr Key key(123);
33  EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 2, 2).dim());
34  EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 3, 2).dim());
35  EXPECT_LONGS_EQUAL(1, ShonanGaugeFactor(key, 4, 2).dim()); // SO(4-2) -> 1
36  EXPECT_LONGS_EQUAL(3, ShonanGaugeFactor(key, 5, 2).dim()); // SO(5-2) -> 3
37 
38  EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 3, 3).dim());
39  EXPECT_LONGS_EQUAL(0, ShonanGaugeFactor(key, 4, 3).dim());
40  EXPECT_LONGS_EQUAL(1, ShonanGaugeFactor(key, 5, 3).dim()); // SO(5-3) -> 1
41 }
42 
43 /* ************************************************************************* */
44 // Check ShonanGaugeFactor for SO(6)
45 TEST(ShonanAveraging, GaugeFactorSO6) {
46  constexpr Key key(666);
47  ShonanGaugeFactor factor(key, 6); // For SO(6)
48  Matrix A = Matrix::Zero(3, 15); // SO(6-3) = SO(3) == 3-dimensional gauge
49  A(0, 0) = 1; // first 2 of 6^th skew column, which has 5 non-zero entries
50  A(1, 1) = 1; // then we skip 3 tangent dimensions
51  A(2, 5) = 1; // first of 5th skew colum, which has 4 non-zero entries above
52  // diagonal.
53  JacobianFactor linearized(key, A, Vector::Zero(3));
54  Values values;
55  EXPECT_LONGS_EQUAL(3, factor.dim());
56  EXPECT(assert_equal(linearized, *std::dynamic_pointer_cast<JacobianFactor>(
57  factor.linearize(values))));
58 }
59 
60 /* ************************************************************************* */
61 // Check ShonanGaugeFactor for SO(7)
62 TEST(ShonanAveraging, GaugeFactorSO7) {
63  constexpr Key key(777);
64  ShonanGaugeFactor factor(key, 7); // For SO(7)
65  Matrix A = Matrix::Zero(6, 21); // SO(7-3) = SO(4) == 6-dimensional gauge
66  A(0, 0) = 1; // first 3 of 7^th skew column, which has 6 non-zero entries
67  A(1, 1) = 1;
68  A(2, 2) = 1; // then we skip 3 tangent dimensions
69  A(3, 6) = 1; // first 2 of 6^th skew column, which has 5 non-zero entries
70  A(4, 7) = 1; // then we skip 3 tangent dimensions
71  A(5, 11) = 1; // first of 5th skew colum, which has 4 non-zero entries above
72  // diagonal.
73  JacobianFactor linearized(key, A, Vector::Zero(6));
74  Values values;
75  EXPECT_LONGS_EQUAL(6, factor.dim());
76  EXPECT(assert_equal(linearized, *std::dynamic_pointer_cast<JacobianFactor>(
77  factor.linearize(values))));
78 }
79 
80 /* ************************************************************************* */
81 // Check ShonanGaugeFactor for SO(6), with base SO(2)
82 TEST(ShonanAveraging, GaugeFactorSO6over2) {
83  constexpr Key key(602);
84  double gamma = 4;
85  ShonanGaugeFactor factor(key, 6, 2, gamma); // For SO(6), base SO(2)
86  Matrix A = Matrix::Zero(6, 15); // SO(6-2) = SO(4) == 6-dimensional gauge
87  A(0, 0) = 2; // first 3 of 6^th skew column, which has 5 non-zero entries
88  A(1, 1) = 2;
89  A(2, 2) = 2; // then we skip only 2 tangent dimensions
90  A(3, 5) = 2; // first 2 of 5^th skew column, which has 4 non-zero entries
91  A(4, 6) = 2; // then we skip only 2 tangent dimensions
92  A(5, 9) = 2; // first of 4th skew colum, which has 3 non-zero entries above
93  // diagonal.
94  JacobianFactor linearized(key, A, Vector::Zero(6));
95  Values values;
96  EXPECT_LONGS_EQUAL(6, factor.dim());
97  EXPECT(assert_equal(linearized, *std::dynamic_pointer_cast<JacobianFactor>(
98  factor.linearize(values))));
99 }
100 
101 /* ************************************************************************* */
102 int main() {
103  TestResult tr;
104  return TestRegistry::runAllTests(tr);
105 }
106 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::ShonanGaugeFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: ShonanGaugeFactor.h:97
main
int main()
Definition: testShonanGaugeFactor.cpp:102
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::ShonanGaugeFactor
Definition: ShonanGaugeFactor.h:45
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
ShonanGaugeFactor.h
Factor used in Shonan Averaging to clamp down gauge freedom.
TEST
TEST(ShonanAveraging, GaugeFactorLows)
Definition: testShonanGaugeFactor.cpp:31
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
gamma
#define gamma
Definition: mconf.h:85
TestResult
Definition: TestResult.h:26
key
const gtsam::Symbol key('X', 0)
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::ShonanGaugeFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition: ShonanGaugeFactor.h:100
gtsam::ShonanAveraging
Definition: ShonanAveraging.h:123
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


gtsam
Author(s):
autogenerated on Thu Nov 28 2024 04:09:04