diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 4c423574d..fe0d7fc2b 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -13,14 +13,25 @@ Author: Frank Dellaert import unittest import gtsam -from gtsam import ShonanAveraging3, ShonanAveragingParameters3 +from gtsam import ( + BetweenFactorPose2, + LevenbergMarquardtParams, + Rot2, + Pose2, + ShonanAveraging2, + ShonanAveragingParameters2, + ShonanAveraging3, + ShonanAveragingParameters3 +) from gtsam.utils.test_case import GtsamTestCase DEFAULT_PARAMS = ShonanAveragingParameters3( gtsam.LevenbergMarquardtParams.CeresDefaults()) -def fromExampleName(name: str, parameters=DEFAULT_PARAMS): +def fromExampleName( + name: str, parameters: ShonanAveragingParameters3 = DEFAULT_PARAMS +) -> ShonanAveraging3: g2oFile = gtsam.findExampleDataFile(name) return ShonanAveraging3(g2oFile, parameters) @@ -133,6 +144,57 @@ class TestShonanAveraging(GtsamTestCase): self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3) result, _lambdaMin = shonan.run(initial, 3, 3) self.assertAlmostEqual(0.0015, shonan.cost(result), places=3) + + def test_constructorBetweenFactorPose2s(self) -> None: + """Check if ShonanAveraging2 constructor works when not initialized from g2o file.""" + # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1 + i2Ri1_dict = { + (1, 2): -43.86342, + (1, 5): -135.06351, + (2, 4): 72.64527, + (3, 5): 117.75967, + (6, 7): -31.73934, + (7, 10): 177.45901, + (6, 9): -133.58713, + (7, 8): -90.58668, + (0, 3): 127.02978, + (8, 10): -92.16361, + (4, 8): 51.63781, + (4, 6): 173.96384, + (4, 10): 139.59445, + (2, 3): 151.04022, + (3, 4): -78.39495, + (1, 4): 28.78185, + (6, 8): -122.32602, + (0, 2): -24.01045, + (5, 7): -53.93014, + (4, 5): -163.84535, + (2, 5): -91.20009, + (1, 3): 107.17680, + (7, 9): -102.35615, + (0, 1): 19.85297, + (5, 8): -144.51682, + (5, 6): -22.19079, + (5, 10): -56.56016, + } + i2Ri1_dict = {(i1,i2): Rot2.fromDegrees(theta_deg) for (i1,i2), theta_deg in i2Ri1_dict.items()} + lm_params = LevenbergMarquardtParams.CeresDefaults() + shonan_params = ShonanAveragingParameters2(lm_params) + shonan_params.setUseHuber(False) + shonan_params.setCertifyOptimality(True) + + noise_model = gtsam.noiseModel.Unit.Create(3) + between_factors = gtsam.BetweenFactorPose2s() + for (i1, i2), i2Ri1 in i2Ri1_dict.items(): + i2Ti1 = Pose2(i2Ri1, np.zeros(2)) + between_factors.append(BetweenFactorPose2(i2, i1, i2Ti1, noise_model)) + + obj = ShonanAveraging2(between_factors, shonan_params) + initial = obj.initializeRandomly() + result_values, _ = obj.run(initial, p_min, self._p_max) + + for i in range(11): + wRi = result_values.atRot2(i) if __name__ == '__main__':