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)
26 p0 = hexagon.atPose3(0)
27 p1 = hexagon.atPose3(1)
31 fg.add(gtsam.NonlinearEqualityPose3(0, p0))
32 delta = p0.between(p1)
34 np.array([0.05, 0.05, 0.05, np.deg2rad(5.), np.deg2rad(5.), np.deg2rad(5.)]))
35 fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
36 fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))
37 fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance))
38 fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance))
39 fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance))
40 fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance))
46 initial.insert(1, hexagon.atPose3(1).
retract(s * np.random.randn(6, 1)))
47 initial.insert(2, hexagon.atPose3(2).
retract(s * np.random.randn(6, 1)))
48 initial.insert(3, hexagon.atPose3(3).
retract(s * np.random.randn(6, 1)))
49 initial.insert(4, hexagon.atPose3(4).
retract(s * np.random.randn(6, 1)))
50 initial.insert(5, hexagon.atPose3(5).
retract(s * np.random.randn(6, 1)))
54 result = optimizer.optimizeSafely()
56 pose_1 = result.atPose3(1)
59 if __name__ ==
"__main__":