From d3bcfddfb5269cff43e5fcbc5ad988e8e3c8758c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 30 Jan 2020 15:17:04 -0500 Subject: [PATCH 1/5] ISAM2: wrap full update method and error method --- gtsam/nonlinear/nonlinear.i | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 6d101af98..a24bcbb75 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -507,18 +507,18 @@ 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::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::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); @@ -527,6 +527,8 @@ class ISAM2 { 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; From 6992f0d64cafbedea3ec12580bf0c9094ffa55f4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 Feb 2020 16:50:32 -0500 Subject: [PATCH 2/5] added test for full ISAM2 update method --- python/gtsam/tests/test_VisualISAMExample.py | 52 +++++++++++++++++++- python/gtsam/utils/visual_isam.py | 4 +- 2 files changed, 52 insertions(+), 4 deletions(-) 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.') From 42e8f498e7adaac369e5f9ad21305f37d123094d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Feb 2020 17:57:44 -0500 Subject: [PATCH 3/5] added tests for newly wrapped isam2 methods --- python/gtsam/tests/test_VisualISAMExample.py | 95 ++++++++++++-------- 1 file changed, 56 insertions(+), 39 deletions(-) diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index a25019213..5f30e83ea 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -21,12 +21,13 @@ 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() @@ -35,71 +36,87 @@ 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) + + @unittest.skip( + "Need to understand how to calculate error using VectorValues correctly" + ) + def test_isam2_error(self): + #TODO(Varun) fix + # 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() + + keys = estimate.keys() + + for k in range(keys.size()): + key = keys.at(k) + try: + v = estimate.atPose3(key).matrix() + + except RuntimeError: + v = estimate.atPoint3(key).vector() + values.insert(key, v) + # print(isam.error(values)) 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) + isam, result, nextPose = visual_isam.initialize( + self.data, self.truth, self.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) + 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 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(truth.cameras)): + for i in range(len(self.truth.cameras)): pose_i = result.atPose3(symbol(ord('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 range(len(truth.points)): + for j in range(len(self.truth.points)): point_j = result.atPoint3(symbol(ord('l'), j)) - self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5) if __name__ == "__main__": From 93ed850c6c01b7c5f66e6db291f00769b46ad95b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 5 Jul 2023 13:14:16 -0400 Subject: [PATCH 4/5] get tests working --- gtsam/nonlinear/nonlinear.i | 10 ++----- python/gtsam/tests/test_VisualISAMExample.py | 29 ++++++++++---------- 2 files changed, 17 insertions(+), 22 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index a24bcbb75..3721e27e8 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -512,16 +512,10 @@ class ISAM2 { gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, - const 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, - const gtsam::KeyGroupMap& constrainedKeys, + 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, diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index 5f30e83ea..c7e72ff8b 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -16,6 +16,7 @@ import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam from gtsam.utils.test_case import GtsamTestCase +import gtsam from gtsam import symbol @@ -60,10 +61,8 @@ class TestVisualISAMExample(GtsamTestCase): point_j = result.atPoint3(symbol('l', j)) self.gtsamAssertEquals(point_j, expected_point, 1e-5) - @unittest.skip( - "Need to understand how to calculate error using VectorValues correctly" - ) def test_isam2_error(self): + """Test for isam2 error() method.""" #TODO(Varun) fix # Initialize iSAM with the first pose and points isam, result, nextPose = visual_isam.initialize( @@ -78,17 +77,19 @@ class TestVisualISAMExample(GtsamTestCase): estimate = isam.calculateBestEstimate() - keys = estimate.keys() - - for k in range(keys.size()): - key = keys.at(k) + for key in estimate.keys(): try: - v = estimate.atPose3(key).matrix() - + v = gtsam.Pose3.Logmap(estimate.atPose3(key)) except RuntimeError: - v = estimate.atPoint3(key).vector() + v = estimate.atPoint3(key) + + print(key) + print(type(v)) + print(v) values.insert(key, v) - # print(isam.error(values)) + print(isam.error(values)) + + self.assertEqual(isam.error(values), 34212421.14731998) def test_isam2_update(self): """ @@ -98,7 +99,7 @@ class TestVisualISAMExample(GtsamTestCase): isam, result, nextPose = visual_isam.initialize( self.data, self.truth, self.isamOptions) - remove_factor_indices = gtsam.FactorIndices() + remove_factor_indices = [] constrained_keys = gtsam.KeyGroupMap() no_relin_keys = gtsam.KeyList() extra_reelim_keys = gtsam.KeyList() @@ -111,11 +112,11 @@ class TestVisualISAMExample(GtsamTestCase): self.truth, currentPose, isamArgs) for i in range(len(self.truth.cameras)): - pose_i = result.atPose3(symbol(ord('x'), i)) + 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(ord('l'), j)) + point_j = result.atPoint3(symbol('l', j)) self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5) From a14fb8db7d55da85e3d5f860572cae7bab7ee20f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 6 Jul 2023 04:12:39 -0400 Subject: [PATCH 5/5] formatting and fix --- gtsam/nonlinear/nonlinear.i | 7 +++---- python/gtsam/tests/test_VisualISAMExample.py | 7 +------ 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3721e27e8..4d0d847c1 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -515,7 +515,7 @@ class ISAM2 { gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, - bool force_relinearize=false); + bool force_relinearize = false); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, @@ -548,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 c7e72ff8b..39f563a35 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -63,7 +63,6 @@ class TestVisualISAMExample(GtsamTestCase): def test_isam2_error(self): """Test for isam2 error() method.""" - #TODO(Varun) fix # Initialize iSAM with the first pose and points isam, result, nextPose = visual_isam.initialize( self.data, self.truth, self.isamOptions) @@ -83,13 +82,9 @@ class TestVisualISAMExample(GtsamTestCase): except RuntimeError: v = estimate.atPoint3(key) - print(key) - print(type(v)) - print(v) values.insert(key, v) - print(isam.error(values)) - self.assertEqual(isam.error(values), 34212421.14731998) + self.assertAlmostEqual(isam.error(values), 34212421.14731998) def test_isam2_update(self): """