added examples and a test on pMin

borglab · Jul 19, 2023 · 57291e1 · 57291e1
1 parent 73eb405
commit 57291e1
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 2 deletions.
diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
@@ -894,6 +894,9 @@ template <size_t d>
 std::pair<Values, double> ShonanAveraging<d>::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<Rot>(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
@@ -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<BinaryMeasurement<Rot3>> 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
@@ -10,14 +10,16 @@
 """
 # 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 @@ def test_constructorBetweenFactorPose2s(self) -> None:
         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()