wrap measured and add tests

gtsam/sam/sam.i

@@ -20,6 +20,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
 
   // enabling serialization functionality
   void serialize() const;
+
+  const double measured() const;
 };
 
 // between points:
@@ -54,6 +56,9 @@ virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor {
 
   // enabling serialization functionality
   void serialize() const;
+
+  // Use `double` instead of template since that is all we need.
+  const double measured() const;
 };
 
 typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2>
@@ -73,6 +78,8 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
 
   // enabling serialization functionality
   void serialize() const;
+
+  const BEARING& measured() const;
 };
 
 typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2>

python/gtsam/tests/test_sam.py

@@ -0,0 +1,55 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Basis unit tests.
+Author: Frank Dellaert & Varun Agrawal (Python)
+"""
+import unittest
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestSam(GtsamTestCase):
+    """
+    Tests python binding for classes/functions in `sam.i`.
+    """
+    def test_RangeFactor2D(self):
+        """
+        Test that `measured` works as expected for RangeFactor2D.
+        """
+        measurement = 10.0
+        factor = gtsam.RangeFactor2D(1, 2, measurement,
+                                     gtsam.noiseModel.Isotropic.Sigma(1, 1))
+        self.assertEqual(measurement, factor.measured())
+
+    def test_BearingFactor2D(self):
+        """
+        Test that `measured` works as expected for BearingFactor2D.
+        """
+        measurement = gtsam.Rot2(.3)
+        factor = gtsam.BearingFactor2D(1, 2, measurement,
+                                       gtsam.noiseModel.Isotropic.Sigma(1, 1))
+        self.gtsamAssertEquals(measurement, factor.measured())
+
+    def test_BearingRangeFactor2D(self):
+        """
+        Test that `measured` works as expected for BearingRangeFactor2D.
+        """
+        range_measurement = 10.0
+        bearing_measurement = gtsam.Rot2(0.3)
+        factor = gtsam.BearingRangeFactor2D(
+            1, 2, bearing_measurement, range_measurement,
+            gtsam.noiseModel.Isotropic.Sigma(2, 1))
+        measurement = factor.measured()
+
+        self.assertEqual(range_measurement, measurement.range())
+        self.gtsamAssertEquals(bearing_measurement, measurement.bearing())
+
+
+if __name__ == "__main__":
+    unittest.main()