diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index ebc77e2e3..a25019213 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -6,15 +6,18 @@ All Rights Reserved 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 +from gtsam import symbol + class TestVisualISAMExample(GtsamTestCase): """Test class for ISAM2 with visual landmarks.""" @@ -53,6 +56,51 @@ class TestVisualISAMExample(GtsamTestCase): point_j = result.atPoint3(symbol('l', j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + def test_isam2_update(self): + """ + Test for full version of ISAM2::update method + """ + # Data Options + options = generator.Options() + options.triangle = False + options.nrCameras = 20 + + # iSAM Options + isamOptions = visual_isam.Options() + isamOptions.hardConstraint = False + isamOptions.pointPriors = False + isamOptions.batchInitialization = True + isamOptions.reorderInterval = 10 + isamOptions.alwaysRelinearize = False + + # Generate data + data, truth = generator.generate_data(options) + + # Initialize iSAM with the first pose and points + isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions) + + remove_factor_indices = gtsam.FactorIndices() + 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, isamArgs) + + for i in range(len(truth.cameras)): + pose_i = result.atPose3(symbol(ord('x'), i)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) + + for j in range(len(truth.points)): + point_j = result.atPoint3(symbol(ord('l'), j)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py index 4ebd8accd..52ec8e32f 100644 --- a/python/gtsam/utils/visual_isam.py +++ b/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.')