2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 Unit tests for 3D SLAM initialization, using rotation relaxation.
9 Author: Luca Carlone and Frank Dellaert (Python)
17 from gtsam
import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
20 x0, x1, x2, x3 = 0, 1, 2, 3
40 self.
R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
42 self.
R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
44 self.
R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
46 self.
R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
54 g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
55 g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
56 g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
57 g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
58 g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
59 g.add(gtsam.PriorFactorPose3(x0, pose0, model))
80 inputGraph.add(gtsam.PriorFactorPose3(0,
Pose3(), priorModel))
87 if __name__ ==
"__main__":