test_KarcherMeanFactor.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 KarcherMeanFactor unit tests.
9 Author: Frank Dellaert
10 """
11 
12 # pylint: disable=invalid-name, no-name-in-module, no-member
13 
14 import unittest
15 
16 import numpy as np
17 from gtsam.utils.test_case import GtsamTestCase
18 
19 import gtsam
20 from gtsam import Rot3
21 
22 KEY = 0
24 
25 # Rot3 version
26 R = Rot3.Expmap(np.array([0.1, 0, 0]))
27 
28 
30 
31  def test_find(self):
32  """
33  Check that optimizing for Karcher mean (which minimizes Between distance)
34  gets correct result.
35  """
36  rotations = [R, R.inverse()]
37  expected = Rot3()
38  actual = gtsam.FindKarcherMean(rotations)
39  self.gtsamAssertEquals(expected, actual)
40 
42  """Averaging 3 identity rotations should yield the identity."""
43  a1Rb1 = Rot3()
44  a2Rb2 = Rot3()
45  a3Rb3 = Rot3()
46 
47  aRb_list = [a1Rb1, a2Rb2, a3Rb3]
48  aRb_expected = Rot3()
49 
50  aRb = gtsam.FindKarcherMean(aRb_list)
51  self.gtsamAssertEquals(aRb, aRb_expected)
52 
53  def test_factor(self):
54  """Check that the InnerConstraint factor leaves the mean unchanged."""
55  # Make a graph with two variables, one between, and one InnerConstraint
56  # The optimal result should satisfy the between, while moving the other
57  # variable to make the mean the same as before.
58  # Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
59  # R*R*R, i.e. geodesic length is 3 rather than 2.
61  R12 = R.compose(R.compose(R))
62  graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
63  keys = [1, 2]
64  graph.add(gtsam.KarcherMeanFactorRot3(keys))
65 
66  initial = gtsam.Values()
67  initial.insert(1, R.inverse())
68  initial.insert(2, R)
69  expected = Rot3()
70 
71  result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
72  actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)])
73  self.gtsamAssertEquals(expected, actual)
74  self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2)))
75 
76 
77 if __name__ == "__main__":
78  unittest.main()
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
test_KarcherMeanFactor.TestKarcherMean
Definition: test_KarcherMeanFactor.py:29
gtsam::utils.test_case.GtsamTestCase.gtsamAssertEquals
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
gtsam::FindKarcherMean
T FindKarcherMean(std::initializer_list< T > &&rotations)
Definition: KarcherMeanFactor-inl.h:55
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
test_KarcherMeanFactor.TestKarcherMean.test_find_karcher_mean_identity
def test_find_karcher_mean_identity(self)
Definition: test_KarcherMeanFactor.py:41
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
test_KarcherMeanFactor.TestKarcherMean.test_factor
def test_factor(self)
Definition: test_KarcherMeanFactor.py:53
gtsam::utils.test_case
Definition: test_case.py:1
gtsam::Values
Definition: Values.h:65
test_KarcherMeanFactor.TestKarcherMean.test_find
def test_find(self)
Definition: test_KarcherMeanFactor.py:31
gtsam::testing::between
T between(const T &t1, const T &t2)
Definition: lieProxies.h:36
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:27