testKarcherMeanFactor.cpp
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 testKarcherMeanFactor.cpp
14  * @author Frank Dellaert
15  */
16 
17 #include <gtsam/geometry/Rot3.h>
18 #include <gtsam/geometry/SO4.h>
24 
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 /* ************************************************************************* */
31 // Rot3 version
32 /* ************************************************************************* */
33 static const Rot3 R = Rot3::Expmap(Vector3(0.1, 0, 0));
34 
35 /* ************************************************************************* */
36 // Check that optimizing for Karcher mean (which minimizes Between distance)
37 // gets correct result.
38 TEST(KarcherMean, FindRot3) {
39  std::vector<Rot3> rotations = {R, R.inverse()};
40  Rot3 expected;
41  auto actual = FindKarcherMean(rotations);
42  EXPECT(assert_equal(expected, actual));
43 }
44 
45 /* ************************************************************************* */
46 // Check that the InnerConstraint factor leaves the mean unchanged.
47 TEST(KarcherMean, FactorRot3) {
48  // Make a graph with two variables, one between, and one InnerConstraint
49  // The optimal result should satisfy the between, while moving the other
50  // variable to make the mean the same as before.
51  // Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
52  // R*R*R, i.e. geodesic length is 3 rather than 2.
54  graph.emplace_shared<BetweenFactor<Rot3>>(1, 2, R * R * R);
55  std::vector<Key> keys{1, 2};
57 
59  initial.insert<Rot3>(1, R.inverse());
60  initial.insert<Rot3>(2, R);
61  const auto expected = FindKarcherMean<Rot3>({R, R.inverse()});
62 
63  auto result = GaussNewtonOptimizer(graph, initial).optimize();
64  const auto actual =
65  FindKarcherMean<Rot3>({result.at<Rot3>(1), result.at<Rot3>(2)});
66  EXPECT(assert_equal(expected, actual));
67  EXPECT(
68  assert_equal(R * R * R, result.at<Rot3>(1).between(result.at<Rot3>(2))));
69 }
70 
71 /* ************************************************************************* */
72 // SO(4) version
73 /* ************************************************************************* */
74 static const SO4 Q = SO4::Expmap((Vector6() << 1, 2, 3, 4, 5, 6).finished());
75 
76 /* ************************************************************************* */
77 TEST(KarcherMean, FindSO4) {
78  std::vector<SO4, Eigen::aligned_allocator<SO4>> rotations = {Q, Q.inverse()};
79  auto expected = SO4(); //::ChordalMean(rotations);
80  auto actual = FindKarcherMean(rotations);
81  EXPECT(assert_equal(expected, actual));
82 }
83 
84 /* ************************************************************************* */
85 TEST(KarcherMean, FactorSO4) {
87  graph.emplace_shared<BetweenFactor<SO4>>(1, 2, Q * Q * Q);
88  std::vector<Key> keys{1, 2};
90 
92  initial.insert<SO4>(1, Q.inverse());
93  initial.insert<SO4>(2, Q);
94 
95  std::vector<SO4, Eigen::aligned_allocator<SO4> > rotations = {Q, Q.inverse()};
96  const auto expected = FindKarcherMean<SO4>(rotations);
97 
98  auto result = GaussNewtonOptimizer(graph, initial).optimize();
99  const auto actual =
100  FindKarcherMean<SO4>({result.at<SO4>(1), result.at<SO4>(2)});
101  EXPECT(assert_equal(expected, actual));
102  EXPECT(assert_equal((Matrix)(Q * Q * Q).matrix(),
103  result.at<SO4>(1).between(result.at<SO4>(2)).matrix()));
104 }
105 
106 /* ************************************************************************* */
107 int main() {
108  TestResult tr;
109  return TestRegistry::runAllTests(tr);
110 }
111 /* ************************************************************************* */
int main()
virtual const Values & optimize()
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
SO< 4 > SO4
Definition: SO4.h:34
Factor Graph consisting of non-linear factors.
SO inverse() const
inverse of a rotation = transpose
Definition: SOn.h:195
TEST(KarcherMean, FindRot3)
T FindKarcherMean(const std::vector< T > &rotations)
Matrix expected
Definition: testMatrix.cpp:971
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
4*4 matrix representation of SO(4)
Pose2_ Expmap(const Vector3_ &xi)
Values initial
Definition: BFloat16.h:88
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Definition: SOn.h:54
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
Values result
#define EXPECT(condition)
Definition: Test.h:150
static const Rot3 R
traits
Definition: chartTesting.h:28
static const SO4 Q
The quaternion class used to represent 3D orientations and rotations.
Class between(const Class &g) const
Definition: Lie.h:52
const KeyVector keys
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
3D rotation represented as a rotation matrix or quaternion


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