update test for ISAM
parent
173919229f
commit
1f494fd1bd
|
@ -10,9 +10,6 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
|||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
import gtsam.utils.visual_data_generator as generator
|
||||
import gtsam.utils.visual_isam as visual_isam
|
||||
from gtsam import symbol
|
||||
|
@ -20,8 +17,9 @@ from gtsam.utils.test_case import GtsamTestCase
|
|||
|
||||
|
||||
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."""
|
||||
# Data Options
|
||||
options = generator.Options()
|
||||
options.triangle = False
|
||||
|
@ -39,19 +37,22 @@ class TestVisualISAMExample(GtsamTestCase):
|
|||
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(
|
||||
data, truth, 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)
|
||||
isam, result = visual_isam.step(data, isam, result, truth,
|
||||
currentPose)
|
||||
|
||||
for i in range(len(truth.cameras)):
|
||||
for i, _ in enumerate(truth.cameras):
|
||||
pose_i = result.atPose3(symbol('x', i))
|
||||
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
|
||||
|
||||
for j in range(len(truth.points)):
|
||||
for j, _ in enumerate(truth.points):
|
||||
point_j = result.atPoint3(symbol('l', j))
|
||||
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
Loading…
Reference in New Issue