2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
16 from gtsam
import symbol
42 graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
51 graph.add(gtsam.GenericStereoFactor3D(
gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
52 graph.add(gtsam.GenericStereoFactor3D(
gtsam.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K))
53 graph.add(gtsam.GenericStereoFactor3D(
gtsam.StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))
56 graph.add(gtsam.GenericStereoFactor3D(
gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
57 graph.add(gtsam.GenericStereoFactor3D(
gtsam.StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K))
58 graph.add(gtsam.GenericStereoFactor3D(
gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K))
62 initialEstimate.insert(x1, first_pose)
66 initialEstimate.insert(l1, expected_l1)
72 result = optimizer.optimize()
75 pose_x1 = result.atPose3(x1)
78 point_l1 = result.atPoint3(l1)
81 if __name__ ==
"__main__":