12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1569 from borglab/isam2-wrapper

release/4.3a0
Varun Agrawal 2023-07-06 06:08:10 -04:00 committed by GitHub
parent 2db2cca3d1 a14fb8db7d
commit 5dfaf4d0a4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 82 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/nonlinear/nonlinear.i
View File

@ -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>

87
python/gtsam/tests/test_VisualISAMExample.py
View File

@ -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__":

4
python/gtsam/utils/visual_isam.py
View File

@ -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.')