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)
29 np.array([0.3, 0.3, 0.1]))
31 graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
37 np.array([0.2, 0.2, 0.1]))
38 graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
39 graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
43 initialEstimate.insert(1,
gtsam.Pose2(0.5, 0.0, 0.2))
44 initialEstimate.insert(2,
gtsam.Pose2(2.3, 0.1, -0.2))
45 initialEstimate.insert(3,
gtsam.Pose2(4.1, 0.1, 0.1))
50 result = optimizer.optimizeSafely()
52 marginals.marginalCovariance(1)
55 pose_1 = result.atPose2(1)
58 if __name__ ==
"__main__":