Merge pull request #1569 from borglab/isam2-wrapper

gtsam/nonlinear/nonlinear.i

@@ -507,26 +507,22 @@ class ISAM2 {
   gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
                             const gtsam::Values& newTheta,
                             const gtsam::FactorIndices& removeFactorIndices,
-                            gtsam::KeyGroupMap& constrainedKeys,
+                            const gtsam::KeyGroupMap& constrainedKeys,
                             const gtsam::KeyList& noRelinKeys);
-  gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
-                            const gtsam::Values& newTheta,
-                            const gtsam::FactorIndices& removeFactorIndices,
-                            gtsam::KeyGroupMap& constrainedKeys,
-                            const gtsam::KeyList& noRelinKeys,
-                            const gtsam::KeyList& extraReelimKeys);
   gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
                             const gtsam::Values& newTheta,
                             const gtsam::FactorIndices& removeFactorIndices,
                             gtsam::KeyGroupMap& constrainedKeys,
                             const gtsam::KeyList& noRelinKeys,
                             const gtsam::KeyList& extraReelimKeys,
-                            bool force_relinearize);
+                            bool force_relinearize = false);
 
   gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
                             const gtsam::Values& newTheta,
                             const gtsam::ISAM2UpdateParams& updateParams);
 
+  double error(const gtsam::VectorValues& values) const;
+
   gtsam::Values getLinearizationPoint() const;
   bool valueExists(gtsam::Key key) const;
   gtsam::Values calculateEstimate() const;
@@ -552,9 +548,8 @@ class ISAM2 {
 
   string dot(const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
-  void saveGraph(string s,
-                const gtsam::KeyFormatter& keyFormatter =
-                 gtsam::DefaultKeyFormatter) const;
+  void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter =
+                               gtsam::DefaultKeyFormatter) const;
 };
 
 #include <gtsam/nonlinear/NonlinearISAM.h>

python/gtsam/tests/test_VisualISAMExample.py

@@ -6,24 +6,29 @@
 See LICENSE for the license information
 
 visual_isam unit tests.
-Author: Frank Dellaert & Duy Nguyen Ta (Python)
+Author: Frank Dellaert & Duy Nguyen Ta & Varun Agrawal (Python)
 """
+# pylint: disable=maybe-no-member,invalid-name
+
 import unittest
 
 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
 
+import gtsam
+from gtsam import symbol
+
 
 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."""
+
+    def setUp(self):
         # Data Options
         options = generator.Options()
         options.triangle = False
         options.nrCameras = 20
+        self.options = options
 
         # iSAM Options
         isamOptions = visual_isam.Options()
@@ -32,26 +37,82 @@ def test_VisualISAMExample(self):
         isamOptions.batchInitialization = True
         isamOptions.reorderInterval = 10
         isamOptions.alwaysRelinearize = False
+        self.isamOptions = isamOptions
 
         # Generate data
-        data, truth = generator.generate_data(options)
+        self.data, self.truth = generator.generate_data(options)
 
+    def test_VisualISAMExample(self):
+        """Test to see if ISAM works as expected for a simple visual SLAM example."""
         # Initialize iSAM with the first pose and points
         isam, result, nextPose = visual_isam.initialize(
-            data, truth, isamOptions)
+            self.data, self.truth, self.isamOptions)
+
+        # Main loop for iSAM: stepping through all poses
+        for currentPose in range(nextPose, self.options.nrCameras):
+            isam, result = visual_isam.step(self.data, isam, result,
+                                            self.truth, currentPose)
+
+        for i, true_camera in enumerate(self.truth.cameras):
+            pose_i = result.atPose3(symbol('x', i))
+            self.gtsamAssertEquals(pose_i, true_camera.pose(), 1e-5)
+
+        for j, expected_point in enumerate(self.truth.points):
+            point_j = result.atPoint3(symbol('l', j))
+            self.gtsamAssertEquals(point_j, expected_point, 1e-5)
+
+    def test_isam2_error(self):
+        """Test for isam2 error() method."""
+        # Initialize iSAM with the first pose and points
+        isam, result, nextPose = visual_isam.initialize(
+            self.data, self.truth, self.isamOptions)
+
+        # Main loop for iSAM: stepping through all poses
+        for currentPose in range(nextPose, self.options.nrCameras):
+            isam, result = visual_isam.step(self.data, isam, result,
+                                            self.truth, currentPose)
+
+        values = gtsam.VectorValues()
+
+        estimate = isam.calculateBestEstimate()
+
+        for key in estimate.keys():
+            try:
+                v = gtsam.Pose3.Logmap(estimate.atPose3(key))
+            except RuntimeError:
+                v = estimate.atPoint3(key)
+
+            values.insert(key, v)
+
+        self.assertAlmostEqual(isam.error(values), 34212421.14731998)
+
+    def test_isam2_update(self):
+        """
+        Test for full version of ISAM2::update method
+        """
+        # Initialize iSAM with the first pose and points
+        isam, result, nextPose = visual_isam.initialize(
+            self.data, self.truth, self.isamOptions)
+
+        remove_factor_indices = []
+        constrained_keys = gtsam.KeyGroupMap()
+        no_relin_keys = gtsam.KeyList()
+        extra_reelim_keys = gtsam.KeyList()
+        isamArgs = (remove_factor_indices, constrained_keys, no_relin_keys,
+                    extra_reelim_keys, False)
 
         # 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)
+        for currentPose in range(nextPose, self.options.nrCameras):
+            isam, result = visual_isam.step(self.data, isam, result,
+                                            self.truth, currentPose, isamArgs)
 
-        for i, _ in enumerate(truth.cameras):
+        for i in range(len(self.truth.cameras)):
             pose_i = result.atPose3(symbol('x', i))
-            self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
+            self.gtsamAssertEquals(pose_i, self.truth.cameras[i].pose(), 1e-5)
 
-        for j, _ in enumerate(truth.points):
+        for j in range(len(self.truth.points)):
             point_j = result.atPoint3(symbol('l', j))
-            self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
+            self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5)
 
 
 if __name__ == "__main__":

python/gtsam/utils/visual_isam.py

@@ -79,7 +79,7 @@ def initialize(data, truth, options):
     return isam, result, nextPoseIndex
 
 
-def step(data, isam, result, truth, currPoseIndex):
+def step(data, isam, result, truth, currPoseIndex, isamArgs=()):
     '''
     Do one step isam update
     @param[in] data: measurement data (odometry and visual measurements and their noiseModels)
@@ -123,7 +123,7 @@ def step(data, isam, result, truth, currPoseIndex):
 
     # Update ISAM
     # figure(1)tic
-    isam.update(newFactors, initialEstimates)
+    isam.update(newFactors, initialEstimates, *isamArgs)
     # t=toc plot(frame_i,t,'r.') tic
     newResult = isam.calculateEstimate()
     # t=toc plot(frame_i,t,'g.')