Wrapping Updates

gtsam/nonlinear/ISAM2Result.h

@@ -175,6 +175,7 @@ struct ISAM2Result {
   /** Getters and Setters */
   size_t getVariablesRelinearized() const { return variablesRelinearized; }
   size_t getVariablesReeliminated() const { return variablesReeliminated; }
+  FactorIndices getNewFactorsIndices() const { return newFactorsIndices; }
   size_t getCliques() const { return cliques; }
   double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); }
   double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); }

gtsam/nonlinear/nonlinear.i

@@ -682,6 +682,7 @@ class ISAM2Result {
   /** Getters and Setters for all properties */
   size_t getVariablesRelinearized() const;
   size_t getVariablesReeliminated() const;
+  FactorIndices getNewFactorsIndices() const;
   size_t getCliques() const;
   double getErrorBefore() const;
   double getErrorAfter() const;

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_VisualISAMExample.py

@@ -10,18 +10,16 @@
 """
 import unittest
 
-import numpy as np
-
-import gtsam
 import gtsam.utils.visual_data_generator as generator
 import gtsam.utils.visual_isam as visual_isam
 from gtsam import symbol
 from gtsam.utils.test_case import GtsamTestCase
 
 
 class TestVisualISAMExample(GtsamTestCase):
-
+    """Test class for ISAM2 with visual landmarks."""
     def test_VisualISAMExample(self):
+        """Test to see if ISAM works as expected for a simple visual SLAM example."""
         # Data Options
         options = generator.Options()
         options.triangle = False
@@ -39,19 +37,22 @@ def test_VisualISAMExample(self):
         data, truth = generator.generate_data(options)
 
         # Initialize iSAM with the first pose and points
-        isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions)
+        isam, result, nextPose = visual_isam.initialize(
+            data, truth, isamOptions)
 
         # Main loop for iSAM: stepping through all poses
         for currentPose in range(nextPose, options.nrCameras):
-            isam, result = visual_isam.step(data, isam, result, truth, currentPose)
+            isam, result = visual_isam.step(data, isam, result, truth,
+                                            currentPose)
 
-        for i in range(len(truth.cameras)):
+        for i, _ in enumerate(truth.cameras):
             pose_i = result.atPose3(symbol('x', i))
             self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
 
-        for j in range(len(truth.points)):
+        for j, _ in enumerate(truth.points):
             point_j = result.atPoint3(symbol('l', j))
             self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
 
+
 if __name__ == "__main__":
     unittest.main()

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()