2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 Unit tests for Shonan Rotation Averaging.
20 from gtsam
import (BetweenFactorPose2, BetweenFactorPose3,
21 BinaryMeasurementRot3, LevenbergMarquardtParams, Pose2,
22 Pose3, Rot2, Rot3, ShonanAveraging2, ShonanAveraging3,
23 ShonanAveragingParameters2, ShonanAveragingParameters3)
31 name: str, parameters: ShonanAveragingParameters3 = DEFAULT_PARAMS
32 ) -> ShonanAveraging3:
38 """Tests for Shonan Rotation Averaging."""
41 """Set up common variables."""
45 self.assertEqual(5, self.
shonan.nrUnknowns())
48 self.assertEqual((15, 15), D.shape)
51 self.assertEqual((15, 15), Q.shape)
54 self.assertEqual((15, 15), L.shape)
57 graph = self.
shonan.buildGraphAt(5)
58 self.assertEqual(7, graph.size())
61 random = self.
shonan.initializeRandomlyAt(4)
62 lambdaMin = self.
shonan.computeMinEigenValue(random)
63 self.assertAlmostEqual(-414.87376657555996,
65 self.assertFalse(self.
shonan.checkOptimality(random))
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,
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)
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,
87 SO3Values = self.
shonan.roundSolution(result)
88 self.assertAlmostEqual(0, self.
shonan.cost(SO3Values), places=3)
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())
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,
112 self.assertAlmostEqual(4, shonan.nrUnknowns())
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)
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)
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)
147 """Check if ShonanAveraging2 constructor works when not initialized from g2o file.
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])),
169 edges = [(0, 1), (1, 2), (0, 2)]
172 for (i1, i2)
in edges
175 lm_params = LevenbergMarquardtParams.CeresDefaults()
177 shonan_params.setUseHuber(
False)
178 shonan_params.setCertifyOptimality(
True)
182 for (i1, i2), i2Ri1
in i2Ri1_dict.items():
183 i2Ti1 =
Pose2(i2Ri1, np.zeros(2))
184 between_factors.append(
189 initial = obj.initializeRandomly()
190 result_values, _ = obj.run(initial, min_p=2, max_p=100)
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])
196 thetas_deg = thetas_deg % 360
197 thetas_deg -= thetas_deg[0]
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)
203 """Create from 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)
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)
216 if __name__ ==
"__main__":