update test for ISAM

release/4.3a0
Varun Agrawal 2022-03-24 21:05:27 -04:00
parent 173919229f
commit 1f494fd1bd
1 changed files with 9 additions and 8 deletions

View File

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