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)
37 graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
42 graph.add(gtsam.BetweenFactorPose2(
44 graph.add(gtsam.BetweenFactorPose2(
45 2, 3,
gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
46 graph.add(gtsam.BetweenFactorPose2(
47 3, 4,
gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
48 graph.add(gtsam.BetweenFactorPose2(
49 4, 5,
gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
53 graph.add(gtsam.BetweenFactorPose2(5, 2,
gtsam.Pose2(2.0, 0.0, pi / 2), model))
57 initialEstimate.insert(1,
gtsam.Pose2(0.5, 0.0, 0.2))
58 initialEstimate.insert(2,
gtsam.Pose2(2.3, 0.1, -0.2))
59 initialEstimate.insert(3,
gtsam.Pose2(4.1, 0.1, pi / 2))
60 initialEstimate.insert(4,
gtsam.Pose2(4.0, 2.0, pi))
61 initialEstimate.insert(5,
gtsam.Pose2(2.1, 2.1, -pi / 2))
66 result = optimizer.optimizeSafely()
70 P = marginals.marginalCovariance(1)
72 pose_1 = result.atPose2(1)
77 if __name__ ==
"__main__":