test_ShonanAveraging.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 Unit tests for Shonan Rotation Averaging.
9 Author: Frank Dellaert
10 """
11 # pylint: disable=invalid-name, no-name-in-module, no-member
12 
13 import math
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 (BetweenFactorPose2, BetweenFactorPose3,
21  BinaryMeasurementRot3, LevenbergMarquardtParams, Pose2,
22  Pose3, Rot2, Rot3, ShonanAveraging2, ShonanAveraging3,
23  ShonanAveragingParameters2, ShonanAveragingParameters3)
24 
25 DEFAULT_PARAMS = ShonanAveragingParameters3(
27 )
28 
29 
31  name: str, parameters: ShonanAveragingParameters3 = DEFAULT_PARAMS
32 ) -> ShonanAveraging3:
33  g2oFile = gtsam.findExampleDataFile(name)
34  return ShonanAveraging3(g2oFile, parameters)
35 
36 
38  """Tests for Shonan Rotation Averaging."""
39 
40  def setUp(self):
41  """Set up common variables."""
42  self.shonan = fromExampleName("toyExample.g2o")
43 
45  self.assertEqual(5, self.shonan.nrUnknowns())
46 
47  D = self.shonan.denseD()
48  self.assertEqual((15, 15), D.shape)
49 
50  Q = self.shonan.denseQ()
51  self.assertEqual((15, 15), Q.shape)
52 
53  L = self.shonan.denseL()
54  self.assertEqual((15, 15), L.shape)
55 
56  def test_buildGraphAt(self):
57  graph = self.shonan.buildGraphAt(5)
58  self.assertEqual(7, graph.size())
59 
61  random = self.shonan.initializeRandomlyAt(4)
62  lambdaMin = self.shonan.computeMinEigenValue(random)
63  self.assertAlmostEqual(-414.87376657555996,
64  lambdaMin, places=3) # Regression test
65  self.assertFalse(self.shonan.checkOptimality(random))
66 
68  initial = self.shonan.initializeRandomlyAt(3)
69  self.assertFalse(self.shonan.checkOptimality(initial))
70  result = self.shonan.tryOptimizingAt(3, initial)
71  self.assertTrue(self.shonan.checkOptimality(result))
72  lambdaMin = self.shonan.computeMinEigenValue(result)
73  self.assertAlmostEqual(-5.427688831332745e-07,
74  lambdaMin, places=3) # Regression test
75  self.assertAlmostEqual(0, self.shonan.costAt(3, result), places=3)
76  SO3Values = self.shonan.roundSolution(result)
77  self.assertAlmostEqual(0, self.shonan.cost(SO3Values), places=3)
78 
80  random = self.shonan.initializeRandomlyAt(4)
81  result = self.shonan.tryOptimizingAt(4, random)
82  self.assertTrue(self.shonan.checkOptimality(result))
83  self.assertAlmostEqual(0, self.shonan.costAt(4, result), places=2)
84  lambdaMin = self.shonan.computeMinEigenValue(result)
85  self.assertAlmostEqual(-5.427688831332745e-07,
86  lambdaMin, places=3) # Regression test
87  SO3Values = self.shonan.roundSolution(result)
88  self.assertAlmostEqual(0, self.shonan.cost(SO3Values), places=3)
89 
91  random = self.shonan.initializeRandomlyAt(3)
92  Qstar3 = self.shonan.tryOptimizingAt(3, random)
93  lambdaMin, minEigenVector = self.shonan.computeMinEigenVector(Qstar3)
94  initialQ4 = self.shonan.initializeWithDescent(
95  4, Qstar3, minEigenVector, lambdaMin)
96  self.assertAlmostEqual(5, initialQ4.size())
97 
98  def test_run(self):
99  initial = self.shonan.initializeRandomly()
100  result, lambdaMin = self.shonan.run(initial, 5, 10)
101  self.assertAlmostEqual(0, self.shonan.cost(result), places=2)
102  self.assertAlmostEqual(-5.427688831332745e-07,
103  lambdaMin, places=3) # Regression test
104 
106  # Load 2D toy example
108  # lmParams.setVerbosityLM("SUMMARY")
109  g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt")
110  parameters = gtsam.ShonanAveragingParameters2(lmParams)
111  shonan = gtsam.ShonanAveraging2(g2oFile, parameters)
112  self.assertAlmostEqual(4, shonan.nrUnknowns())
113 
114  # Check graph building
115  graph = shonan.buildGraphAt(2)
116  self.assertAlmostEqual(6, graph.size())
117  initial = shonan.initializeRandomly()
118  result, lambdaMin = shonan.run(initial, 2, 10)
119  self.assertAlmostEqual(0.0008211, shonan.cost(result), places=5)
120  self.assertAlmostEqual(0, lambdaMin, places=9) # certificate!
121 
122  # Test alpha/beta/gamma prior weighting.
123  def test_PriorWeights(self):
125  params = ShonanAveragingParameters3(lmParams)
126  self.assertAlmostEqual(0, params.getAnchorWeight(), 1e-9)
127  self.assertAlmostEqual(1, params.getKarcherWeight(), 1e-9)
128  self.assertAlmostEqual(0, params.getGaugesWeight(), 1e-9)
129  alpha, beta, gamma = 100.0, 200.0, 300.0
130  params.setAnchorWeight(alpha)
131  params.setKarcherWeight(beta)
132  params.setGaugesWeight(gamma)
133  self.assertAlmostEqual(alpha, params.getAnchorWeight(), 1e-9)
134  self.assertAlmostEqual(beta, params.getKarcherWeight(), 1e-9)
135  self.assertAlmostEqual(gamma, params.getGaugesWeight(), 1e-9)
136  params.setKarcherWeight(0)
137  shonan = fromExampleName("Klaus3.g2o", params)
138 
139  initial = gtsam.Values()
140  for i in range(3):
141  initial.insert(i, gtsam.Rot3())
142  self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3)
143  result, _lambdaMin = shonan.run(initial, 3, 3)
144  self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
145 
147  """Check if ShonanAveraging2 constructor works when not initialized from g2o file.
148 
149  GT pose graph:
150 
151  | cam 1 = (0,4)
152  --o
153  | .
154  . .
155  . .
156  | |
157  o-- ... o--
158  cam 0 cam 2 = (4,0)
159  (0,0)
160  """
161  num_images = 3
162 
163  wTi_list = [
164  Pose2(Rot2.fromDegrees(0), np.array([0, 0])),
165  Pose2(Rot2.fromDegrees(90), np.array([0, 4])),
166  Pose2(Rot2.fromDegrees(0), np.array([4, 0])),
167  ]
168 
169  edges = [(0, 1), (1, 2), (0, 2)]
170  i2Ri1_dict = {
171  (i1, i2): wTi_list[i2].inverse().compose(wTi_list[i1]).rotation()
172  for (i1, i2) in edges
173  }
174 
175  lm_params = LevenbergMarquardtParams.CeresDefaults()
176  shonan_params = ShonanAveragingParameters2(lm_params)
177  shonan_params.setUseHuber(False)
178  shonan_params.setCertifyOptimality(True)
179 
180  noise_model = gtsam.noiseModel.Unit.Create(3)
181  between_factors = []
182  for (i1, i2), i2Ri1 in i2Ri1_dict.items():
183  i2Ti1 = Pose2(i2Ri1, np.zeros(2))
184  between_factors.append(
185  BetweenFactorPose2(i2, i1, i2Ti1, noise_model)
186  )
187 
188  obj = ShonanAveraging2(between_factors, shonan_params)
189  initial = obj.initializeRandomly()
190  result_values, _ = obj.run(initial, min_p=2, max_p=100)
191 
192  wRi_list = [result_values.atRot2(i) for i in range(num_images)]
193  thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
194 
195  # map all angles to [0,360)
196  thetas_deg = thetas_deg % 360
197  thetas_deg -= thetas_deg[0]
198 
199  expected_thetas_deg = np.array([0.0, 90.0, 0.0])
200  np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
201 
203  """Create from Measurements."""
204  measurements = []
206  m01 = BinaryMeasurementRot3(0, 1, Rot3.Yaw(math.radians(90)), unit3)
207  m12 = BinaryMeasurementRot3(1, 2, Rot3.Yaw(math.radians(90)), unit3)
208  measurements.append(m01)
209  measurements.append(m12)
210  obj = ShonanAveraging3(measurements)
211  self.assertIsInstance(obj, ShonanAveraging3)
212  initial = obj.initializeRandomly()
213  _, cost = obj.run(initial, min_p=3, max_p=5)
214  self.assertAlmostEqual(cost, 0)
215 
216 if __name__ == "__main__":
217  unittest.main()
test_ShonanAveraging.TestShonanAveraging.test_run
def test_run(self)
Definition: test_ShonanAveraging.py:98
gtsam::internal::rotation
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Definition: slam/expressions.h:80
test_ShonanAveraging.TestShonanAveraging
Definition: test_ShonanAveraging.py:37
test_ShonanAveraging.TestShonanAveraging.test_initializeWithDescent
def test_initializeWithDescent(self)
Definition: test_ShonanAveraging.py:90
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
test_ShonanAveraging.TestShonanAveraging.test_buildGraphAt
def test_buildGraphAt(self)
Definition: test_ShonanAveraging.py:56
test_ShonanAveraging.fromExampleName
ShonanAveraging3 fromExampleName(str name, ShonanAveragingParameters3 parameters=DEFAULT_PARAMS)
Definition: test_ShonanAveraging.py:30
test_ShonanAveraging.TestShonanAveraging.setUp
def setUp(self)
Definition: test_ShonanAveraging.py:40
test_ShonanAveraging.TestShonanAveraging.test_runKlausKarcher
def test_runKlausKarcher(self)
Definition: test_ShonanAveraging.py:105
test_ShonanAveraging.TestShonanAveraging.test_tryOptimizingAt4
def test_tryOptimizingAt4(self)
Definition: test_ShonanAveraging.py:79
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
test_ShonanAveraging.TestShonanAveraging.test_tryOptimizingAt3
def test_tryOptimizingAt3(self)
Definition: test_ShonanAveraging.py:67
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::ShonanAveragingParameters
Parameters governing optimization etc.
Definition: ShonanAveraging.h:46
gtsam::ShonanAveraging3
Definition: ShonanAveraging.h:438
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
BetweenFactorPose2
BetweenFactor< Pose2 > BetweenFactorPose2
Definition: serialization.cpp:49
test_ShonanAveraging.TestShonanAveraging.test_PriorWeights
def test_PriorWeights(self)
Definition: test_ShonanAveraging.py:123
gtsam.examples.DogLegOptimizerExample.run
def run(args)
Definition: DogLegOptimizerExample.py:21
gtsam::utils.test_case
Definition: test_case.py:1
test_ShonanAveraging.TestShonanAveraging.test_checkOptimality
def test_checkOptimality(self)
Definition: test_ShonanAveraging.py:60
gtsam::LevenbergMarquardtParams::CeresDefaults
static LevenbergMarquardtParams CeresDefaults()
Definition: LevenbergMarquardtParams.h:106
gtsam::ShonanAveragingParameters3
ShonanAveragingParameters< 3 > ShonanAveragingParameters3
Definition: ShonanAveraging.h:105
gtsam::Values
Definition: Values.h:65
gtsam::testing::compose
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
test_ShonanAveraging.TestShonanAveraging.test_constructorBetweenFactorPose2s
None test_constructorBetweenFactorPose2s(self)
Definition: test_ShonanAveraging.py:146
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::ShonanAveragingParameters2
ShonanAveragingParameters< 2 > ShonanAveragingParameters2
Definition: ShonanAveraging.h:104
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
test_ShonanAveraging.TestShonanAveraging.shonan
shonan
Definition: test_ShonanAveraging.py:42
test_ShonanAveraging.TestShonanAveraging.test_measurements3
def test_measurements3(self)
Definition: test_ShonanAveraging.py:202
test_ShonanAveraging.TestShonanAveraging.test_checkConstructor
def test_checkConstructor(self)
Definition: test_ShonanAveraging.py:44
gtsam::Pose2
Definition: Pose2.h:39
gtsam::ShonanAveraging2
Definition: ShonanAveraging.h:428


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