diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 6d101af98..4d0d847c1 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/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 diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index ebc77e2e3..39f563a35 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -6,24 +6,29 @@ 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 +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 @@ class TestVisualISAMExample(GtsamTestCase): 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, 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) - for i, _ in enumerate(truth.cameras): + for i, true_camera in enumerate(self.truth.cameras): pose_i = result.atPose3(symbol('x', i)) - self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) + self.gtsamAssertEquals(pose_i, true_camera.pose(), 1e-5) - for j, _ in enumerate(truth.points): + for j, expected_point in enumerate(self.truth.points): point_j = result.atPoint3(symbol('l', j)) - self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + 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, self.options.nrCameras): + isam, result = visual_isam.step(self.data, isam, result, + self.truth, currentPose, isamArgs) + + for i in range(len(self.truth.cameras)): + pose_i = result.atPose3(symbol('x', i)) + self.gtsamAssertEquals(pose_i, self.truth.cameras[i].pose(), 1e-5) + + for j in range(len(self.truth.points)): + point_j = result.atPoint3(symbol('l', j)) + self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5) if __name__ == "__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.')