added tests for newly wrapped isam2 methods
Varun Agrawal
parent
6992f0d64c
commit
42e8f498e7
|
|
@ -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__":
|
||||
|
|
|
|||
Loading…
Reference in New Issue