45 lines
1.5 KiB
Python
45 lines
1.5 KiB
Python
import unittest
|
|
from gtsam import *
|
|
from math import *
|
|
import numpy as np
|
|
from gtsam_utils import Vector, Matrix
|
|
import gtsam_utils.visual_data_generator as generator
|
|
import gtsam_utils.visual_isam as visual_isam
|
|
|
|
class TestVisualISAMExample(unittest.TestCase):
|
|
|
|
def test_VisualISAMExample(self):
|
|
# 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)
|
|
|
|
# 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 i in range(len(truth.cameras)):
|
|
pose_i = result.atPose3(symbol(ord('x'), i))
|
|
self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5))
|
|
|
|
for j in range(len(truth.points)):
|
|
point_j = result.atPoint3(symbol(ord('l'), j))
|
|
self.assertTrue(point_j.equals(truth.points[j], 1e-5))
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|