diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 8ace9d98c..7c8b07f37 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -894,6 +894,9 @@ template std::pair ShonanAveraging::run(const Values &initialEstimate, size_t pMin, size_t pMax) const { + if (pMin < d) { + throw std::runtime_error("pMin is smaller than the base dimension d"); + } Values Qstar; Values initialSOp = LiftTo(pMin, initialEstimate); // lift to pMin! for (size_t p = pMin; p <= pMax; p++) { diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index d398a2a87..dd4759daa 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -415,6 +415,20 @@ TEST(ShonanAveraging3, PriorWeights) { auto result = shonan.run(initial, 3, 3); EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4); } + +/* ************************************************************************* */ +// Check a small graph created using binary measurements +TEST(ShonanAveraging3, BinaryMeasurements) { + std::vector> measurements; + auto unit3 = noiseModel::Unit::Create(3); + measurements.emplace_back(0, 1, Rot3::Yaw(M_PI_2), unit3); + measurements.emplace_back(1, 2, Rot3::Yaw(M_PI_2), unit3); + ShonanAveraging3 shonan(measurements); + Values initial = shonan.initializeRandomly(); + auto result = shonan.run(initial, 3, 5); + EXPECT_DOUBLES_EQUAL(0.0, shonan.cost(result.first), 1e-4); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 845f6cf1c..fc0943772 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -10,14 +10,16 @@ Author: Frank Dellaert """ # pylint: disable=invalid-name, no-name-in-module, no-member +import math import unittest import numpy as np from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2, - ShonanAveraging2, ShonanAveraging3, +from gtsam import (BetweenFactorPose2, BetweenFactorPose3, + BinaryMeasurementRot3, LevenbergMarquardtParams, Pose2, + Pose3, Rot2, Rot3, ShonanAveraging2, ShonanAveraging3, ShonanAveragingParameters2, ShonanAveragingParameters3) DEFAULT_PARAMS = ShonanAveragingParameters3( @@ -197,6 +199,19 @@ class TestShonanAveraging(GtsamTestCase): expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) + def test_measurements3(self): + """Create from Measurements.""" + measurements = [] + unit3 = gtsam.noiseModel.Unit.Create(3) + m01 = BinaryMeasurementRot3(0, 1, Rot3.Yaw(math.radians(90)), unit3) + m12 = BinaryMeasurementRot3(1, 2, Rot3.Yaw(math.radians(90)), unit3) + measurements.append(m01) + measurements.append(m12) + obj = ShonanAveraging3(measurements) + self.assertIsInstance(obj, ShonanAveraging3) + initial = obj.initializeRandomly() + _, cost = obj.run(initial, min_p=3, max_p=5) + self.assertAlmostEqual(cost, 0) if __name__ == "__main__": unittest.main()