test_StereoVOExample.py

Go to the documentation of this file.

"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
 
See LICENSE for the license information
 
Stereo VO unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
import unittest
 
import numpy as np
 
import gtsam
from gtsam import symbol
from gtsam.utils.test_case import GtsamTestCase
 
 
class TestStereoVOExample(GtsamTestCase):
 
    def test_StereoVOExample(self):
        
 
        
        x1 = symbol('x',1) 
        x2 = symbol('x',2) 
        l1 = symbol('l',1) 
        l2 = symbol('l',2) 
        l3 = symbol('l',3)
 
        
        graph = gtsam.NonlinearFactorGraph()
 
        
        first_pose = gtsam.Pose3()
        graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
 
        
        K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
        stereo_model = gtsam.noiseModel.Diagonal.Sigmas(np.array([1.0, 1.0, 1.0]))
 
        
        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120,  80, 440), stereo_model, x1, l2, K))
        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))
 
        #pose 2
        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2( 70,  20, 490), stereo_model, x2, l2, K))
        graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K))
 
        
        initialEstimate = gtsam.Values()
        initialEstimate.insert(x1, first_pose)
        # noisy estimate for pose 2
        initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1)))
        expected_l1 = gtsam.Point3( 1,  1, 5)
        initialEstimate.insert(l1, expected_l1)
        initialEstimate.insert(l2, gtsam.Point3(-1,  1, 5))
        initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5))
 
        
        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
        result = optimizer.optimize()
 
        
        pose_x1 = result.atPose3(x1)
        self.gtsamAssertEquals(pose_x1, first_pose,1e-4)
 
        point_l1 = result.atPoint3(l1)
        self.gtsamAssertEquals(point_l1, expected_l1,1e-4)
 
if __name__ == "__main__":
    unittest.main()