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

get tests working

release/4.3a0
Varun Agrawal 2023-07-05 13:14:16 -04:00
parent 42e8f498e7
commit 93ed850c6c
2 changed files with 17 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/nonlinear/nonlinear.i
View File

@ -512,16 +512,10 @@ class ISAM2 {
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
const gtsam::Values& newTheta, const gtsam::Values& newTheta,
const gtsam::FactorIndices& removeFactorIndices, const gtsam::FactorIndices& removeFactorIndices,
const gtsam::KeyGroupMap& constrainedKeys, 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,
const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& noRelinKeys,
const gtsam::KeyList& extraReelimKeys, const gtsam::KeyList& extraReelimKeys,
bool force_relinearize); bool force_relinearize=false);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
const gtsam::Values& newTheta, const gtsam::Values& newTheta,

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

@ -16,6 +16,7 @@ import gtsam.utils.visual_data_generator as generator
import gtsam.utils.visual_isam as visual_isam import gtsam.utils.visual_isam as visual_isam
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import symbol from gtsam import symbol
@ -60,10 +61,8 @@ class TestVisualISAMExample(GtsamTestCase):
point_j = result.atPoint3(symbol('l', j)) point_j = result.atPoint3(symbol('l', j))
self.gtsamAssertEquals(point_j, expected_point, 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): def test_isam2_error(self):
"""Test for isam2 error() method."""
#TODO(Varun) fix #TODO(Varun) fix
# Initialize iSAM with the first pose and points # Initialize iSAM with the first pose and points
isam, result, nextPose = visual_isam.initialize( isam, result, nextPose = visual_isam.initialize(
@ -78,17 +77,19 @@ class TestVisualISAMExample(GtsamTestCase):
estimate = isam.calculateBestEstimate() estimate = isam.calculateBestEstimate()
keys = estimate.keys() for key in estimate.keys():
for k in range(keys.size()):
key = keys.at(k)
try: try:
v = estimate.atPose3(key).matrix() v = gtsam.Pose3.Logmap(estimate.atPose3(key))
except RuntimeError: except RuntimeError:
v = estimate.atPoint3(key).vector() v = estimate.atPoint3(key)
print(key)
print(type(v))
print(v)
values.insert(key, 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): def test_isam2_update(self):
""" """
@ -98,7 +99,7 @@ class TestVisualISAMExample(GtsamTestCase):
isam, result, nextPose = visual_isam.initialize( isam, result, nextPose = visual_isam.initialize(
self.data, self.truth, self.isamOptions) self.data, self.truth, self.isamOptions)
remove_factor_indices = gtsam.FactorIndices() remove_factor_indices = []
constrained_keys = gtsam.KeyGroupMap() constrained_keys = gtsam.KeyGroupMap()
no_relin_keys = gtsam.KeyList() no_relin_keys = gtsam.KeyList()
extra_reelim_keys = gtsam.KeyList() extra_reelim_keys = gtsam.KeyList()
@ -111,11 +112,11 @@ class TestVisualISAMExample(GtsamTestCase):
self.truth, currentPose, isamArgs) self.truth, currentPose, isamArgs)
for i in range(len(self.truth.cameras)): 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) self.gtsamAssertEquals(pose_i, self.truth.cameras[i].pose(), 1e-5)
for j in range(len(self.truth.points)): 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) self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5)