70 lines
2.8 KiB
Python
70 lines
2.8 KiB
Python
import unittest
|
|
import gtsam
|
|
from gtsam import symbol
|
|
import numpy as np
|
|
|
|
|
|
class TestStereoVOExample(unittest.TestCase):
|
|
|
|
def test_StereoVOExample(self):
|
|
## Assumptions
|
|
# - For simplicity this example is in the camera's coordinate frame
|
|
# - X: right, Y: down, Z: forward
|
|
# - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis)
|
|
# - x1 is fixed with a constraint, x2 is initialized with noisy values
|
|
# - No noise on measurements
|
|
|
|
## Create keys for variables
|
|
x1 = symbol(ord('x'),1)
|
|
x2 = symbol(ord('x'),2)
|
|
l1 = symbol(ord('l'),1)
|
|
l2 = symbol(ord('l'),2)
|
|
l3 = symbol(ord('l'),3)
|
|
|
|
## Create graph container and add factors to it
|
|
graph = gtsam.NonlinearFactorGraph()
|
|
|
|
## add a constraint on the starting pose
|
|
first_pose = gtsam.Pose3()
|
|
graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
|
|
|
|
## Create realistic calibration and measurement noise model
|
|
# format: fx fy skew cx cy baseline
|
|
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]))
|
|
|
|
## Add measurements
|
|
# pose 1
|
|
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))
|
|
|
|
## Create initial estimate for camera poses and landmarks
|
|
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))
|
|
|
|
## optimize
|
|
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
|
|
result = optimizer.optimize()
|
|
|
|
## check equality for the first pose and point
|
|
pose_x1 = result.atPose3(x1)
|
|
self.assertTrue(pose_x1.equals(first_pose,1e-4))
|
|
|
|
point_l1 = result.atPoint3(l1)
|
|
self.assertTrue(point_l1.equals(expected_l1,1e-4))
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|