From 15478bf2783fbf4bac53e570bcd3bb61376a7d68 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:33:33 -0400 Subject: [PATCH 01/17] Update ShonanAveraging.h --- gtsam/sfm/ShonanAveraging.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index f00109caee..bff5ab471a 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -430,6 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> { const Parameters ¶meters = Parameters()); explicit ShonanAveraging2(std::string g2oFile, const Parameters ¶meters = Parameters()); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); }; class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> { From 3c1823349b6b90a7fce71bfeca229a56e26bed70 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:38:10 -0400 Subject: [PATCH 02/17] add interface in C++, and helper extractRot2Measurements() --- gtsam/sfm/ShonanAveraging.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index a982ef7da9..f933f368d8 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -944,6 +944,20 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) parameters.getUseHuber()), parameters) {} +static ShonanAveraging2::Measurements extractRot2Measurements( + const BetweenFactorPose2s &factors) { + ShonanAveraging2::Measurements result; + result.reserve(factors.size()); + for (auto f : factors) result.push_back(convert(f)); + return result; +} + +ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors, + const Parameters ¶meters) + : ShonanAveraging<3>(maybeRobust(extractRot2Measurements(factors), + parameters.getUseHuber()), + parameters) {} + /* ************************************************************************* */ // Explicit instantiation for d=3 template class ShonanAveraging<3>; From 0e0d630c917ec6f80a8a037f61bd030842f7feb0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:39:38 -0400 Subject: [PATCH 03/17] fix typo --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index f933f368d8..0de37be1e6 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -954,7 +954,7 @@ static ShonanAveraging2::Measurements extractRot2Measurements( ShonanAveraging2::ShonanAveraging2(const BetweenFactorPose2s &factors, const Parameters ¶meters) - : ShonanAveraging<3>(maybeRobust(extractRot2Measurements(factors), + : ShonanAveraging<2>(maybeRobust(extractRot2Measurements(factors), parameters.getUseHuber()), parameters) {} From 3c8cdb4eeee70f0a95a8ccea54373262ea24b618 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:42:59 -0400 Subject: [PATCH 04/17] add ShonanAveraging2 constructor to wrapper, that accepts BetweenFactorPose2s as input --- gtsam/gtsam.i | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 94d10953bb..f17679739f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -3123,6 +3123,8 @@ class ShonanAveraging2 { ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile, const gtsam::ShonanAveragingParameters2 ¶meters); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); // Query properties size_t nrUnknowns() const; From 4bf2308ec595a96270952d81ce25e8d2d2d447fa Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:48:26 -0400 Subject: [PATCH 05/17] add conversion function for Pose2 -> BinaryMeasurement --- gtsam/sfm/ShonanAveraging.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 0de37be1e6..7810d0d470 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -944,11 +944,27 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) parameters.getUseHuber()), parameters) {} +// Extract Rot2 measurement from Pose2 betweenfactors +// Modeled after similar function in dataset.cpp +static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( + const BetweenFactor::shared_ptr &f) { + auto gaussian = + boost::dynamic_pointer_cast(f->noiseModel()); + if (!gaussian) + throw std::invalid_argument( + "parseMeasurements can only convert Pose2 measurements " + "with Gaussian noise models."); + const Matrix6 M = gaussian->covariance(); + auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2)); + return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), + model); +} + static ShonanAveraging2::Measurements extractRot2Measurements( const BetweenFactorPose2s &factors) { ShonanAveraging2::Measurements result; result.reserve(factors.size()); - for (auto f : factors) result.push_back(convert(f)); + for (auto f : factors) result.push_back(convertPose2ToBinaryMeasurementRot2(f)); return result; } From 641a01c72620c6736d917c470e85db26ae471980 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 17:54:14 -0400 Subject: [PATCH 06/17] fix typo on 3x3 matrix def --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 7810d0d470..88c393f16d 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -954,7 +954,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( throw std::invalid_argument( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); - const Matrix6 M = gaussian->covariance(); + const Matrix3 M = gaussian->covariance(); auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); From 7fc8f23367eeba02c7ac20464eaeee78ce9d5702 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Fri, 9 Jul 2021 23:34:55 -0400 Subject: [PATCH 07/17] use default parameters if none provided, and remove gtsam namespace prefix in .h file --- gtsam/sfm/ShonanAveraging.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index bff5ab471a..de12de4782 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -430,8 +430,8 @@ class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> { const Parameters ¶meters = Parameters()); explicit ShonanAveraging2(std::string g2oFile, const Parameters ¶meters = Parameters()); - ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, - const gtsam::ShonanAveragingParameters2 ¶meters); + ShonanAveraging2(const BetweenFactorPose2s &factors, + const Parameters ¶meters = Parameters()); }; class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> { From 64514387b42e04249b467c9d2b5bbb6807ded4b5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 00:57:01 -0400 Subject: [PATCH 08/17] check in python unit test for new functionality --- python/gtsam/tests/test_ShonanAveraging.py | 66 +++++++++++++++++++++- 1 file changed, 64 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 4c423574d8..fe0d7fc2b0 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -13,14 +13,25 @@ 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 @@ def test_PriorWeights(self): 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__': From db801f164d2ff7c6c65d79058959c6dfb7832ef4 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 10:31:27 -0400 Subject: [PATCH 09/17] add missing import to python unit test --- python/gtsam/tests/test_ShonanAveraging.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index fe0d7fc2b0..a873a64306 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -13,6 +13,7 @@ import unittest import gtsam +import numpy as np from gtsam import ( BetweenFactorPose2, LevenbergMarquardtParams, From aebb90573ab688287cf703f6ed4af97f5a2be962 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sat, 10 Jul 2021 11:41:46 -0400 Subject: [PATCH 10/17] set pmin and pmax in unit test --- python/gtsam/tests/test_ShonanAveraging.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index a873a64306..69e7055453 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -148,6 +148,7 @@ def test_PriorWeights(self): def test_constructorBetweenFactorPose2s(self) -> None: """Check if ShonanAveraging2 constructor works when not initialized from g2o file.""" + num_images = 11 # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1 i2Ri1_dict = { (1, 2): -43.86342, @@ -192,9 +193,9 @@ def test_constructorBetweenFactorPose2s(self) -> None: obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - result_values, _ = obj.run(initial, p_min, self._p_max) + result_values, _ = obj.run(initial, min_p=2, max_p=10) - for i in range(11): + for i in range(num_images): wRi = result_values.atRot2(i) From ddfb45efb03544636f69268f85a703816a730c57 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sun, 11 Jul 2021 01:02:36 -0600 Subject: [PATCH 11/17] fix typo in block indexing, 3x3 covariance for Pose2 should have just 1x1 block for theta --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 88c393f16d..76fd1bfc70 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -955,7 +955,7 @@ static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( "parseMeasurements can only convert Pose2 measurements " "with Gaussian noise models."); const Matrix3 M = gaussian->covariance(); - auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2)); + auto model = noiseModel::Gaussian::Covariance(M.block<1, 1>(2, 2)); return BinaryMeasurement(f->key1(), f->key2(), f->measured().rotation(), model); } From 61c5e89de3bf628dccd98e95c0f955bf908ca8a9 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sun, 11 Jul 2021 22:46:54 -0400 Subject: [PATCH 12/17] try increasing pmax to pass test --- python/gtsam/tests/test_ShonanAveraging.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 69e7055453..d07d840991 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -193,7 +193,8 @@ def test_constructorBetweenFactorPose2s(self) -> None: obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - result_values, _ = obj.run(initial, min_p=2, max_p=10) + min + result_values, _ = obj.run(initial, min_p=2, max_p=40) for i in range(num_images): wRi = result_values.atRot2(i) From 690300124c5f7a162601298fca115c8f01cc531a Mon Sep 17 00:00:00 2001 From: John Lambert Date: Sun, 11 Jul 2021 22:47:14 -0400 Subject: [PATCH 13/17] fix typo --- python/gtsam/tests/test_ShonanAveraging.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index d07d840991..c109ce6b0a 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -193,7 +193,6 @@ def test_constructorBetweenFactorPose2s(self) -> None: obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - min result_values, _ = obj.run(initial, min_p=2, max_p=40) for i in range(num_images): From cce952fbb3fc266e89bd2599ec58de7fb92622be Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 12 Jul 2021 19:35:34 -0400 Subject: [PATCH 14/17] use simple example for unit test --- python/gtsam/tests/test_ShonanAveraging.py | 84 +++++++++++----------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index c109ce6b0a..08bbe5191d 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -22,12 +22,13 @@ ShonanAveraging2, ShonanAveragingParameters2, ShonanAveraging3, - ShonanAveragingParameters3 + ShonanAveragingParameters3, ) from gtsam.utils.test_case import GtsamTestCase DEFAULT_PARAMS = ShonanAveragingParameters3( - gtsam.LevenbergMarquardtParams.CeresDefaults()) + gtsam.LevenbergMarquardtParams.CeresDefaults() +) def fromExampleName( @@ -146,58 +147,59 @@ def test_PriorWeights(self): 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.""" - num_images = 11 - # map (i1,i2) pair to theta in degrees that parameterizes Rot2 object i2Ri1 + """Check if ShonanAveraging2 constructor works when not initialized from g2o file. + + GT pose graph: + + | cam 1 = (0,4) + --o + | . + . . + . . + | | + o-- ... o-- + cam 0 cam 2 = (4,0) + (0,0) + """ + num_images = 3 + + wTi_list = [ + Pose2(Rot2.fromDegrees(0), np.array([0, 0])), + Pose2(Rot2.fromDegrees(90), np.array([0, 4])), + Pose2(Rot2.fromDegrees(0), np.array([4, 0])), + ] + + edges = [(0, 1), (1, 2), (0, 2)] 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, + (i1, i2): wTi_list[i2].inverse().compose(wTi_list[i1]).rotation() + for (i1, i2) in edges } - 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)) - + between_factors.append( + BetweenFactorPose2(i2, i1, i2Ti1, noise_model) + ) + obj = ShonanAveraging2(between_factors, shonan_params) initial = obj.initializeRandomly() - result_values, _ = obj.run(initial, min_p=2, max_p=40) - - for i in range(num_images): - wRi = result_values.atRot2(i) + result_values, _ = obj.run(initial, min_p=2, max_p=100) + + wRi_list = [result_values.atRot2(i) for i in range(num_images)] + thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) + thetas_deg -= max(thetas_deg) + expected_thetas_deg = np.array([0.0, -270.0, 0.0]) + np.testing.assert_allclose(thetas_deg, expected_thetas_deg) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() From d7151ed28410e57526cf5286d820fc4ae089cf05 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 12 Jul 2021 20:52:36 -0400 Subject: [PATCH 15/17] use mod when comparing angles --- python/gtsam/tests/test_ShonanAveraging.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 08bbe5191d..8c70df5dff 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -197,7 +197,9 @@ def test_constructorBetweenFactorPose2s(self) -> None: wRi_list = [result_values.atRot2(i) for i in range(num_images)] thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) thetas_deg -= max(thetas_deg) - expected_thetas_deg = np.array([0.0, -270.0, 0.0]) + # map all angles to [0,360) + thetas_deg = thetas_deg % 360 + expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg) From c4a4e13196be2fdf738a173ca4e70b2a4eb43590 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 13 Jul 2021 00:16:24 -0600 Subject: [PATCH 16/17] fix assert on angles --- python/gtsam/tests/test_ShonanAveraging.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 8c70df5dff..29a14b1b6c 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -196,11 +196,13 @@ def test_constructorBetweenFactorPose2s(self) -> None: wRi_list = [result_values.atRot2(i) for i in range(num_images)] thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) - thetas_deg -= max(thetas_deg) + # map all angles to [0,360) thetas_deg = thetas_deg % 360 + thetas_deg -= max(thetas_deg) + expected_thetas_deg = np.array([0.0, 90.0, 0.0]) - np.testing.assert_allclose(thetas_deg, expected_thetas_deg) + np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) if __name__ == "__main__": From 54d34711214a438c89f173eb9878b43036876d22 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 13 Jul 2021 08:18:45 -0600 Subject: [PATCH 17/17] update logic in angular error comparison --- python/gtsam/tests/test_ShonanAveraging.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 29a14b1b6c..19b9f8dc1a 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -199,7 +199,7 @@ def test_constructorBetweenFactorPose2s(self) -> None: # map all angles to [0,360) thetas_deg = thetas_deg % 360 - thetas_deg -= max(thetas_deg) + thetas_deg -= thetas_deg[0] expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)