2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 Localization unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
30 np.array([0.2, 0.2, 0.1]))
31 graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise))
32 graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
42 graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model))
46 initialEstimate.insert(0,
gtsam.Pose2(0.5, 0.0, 0.2))
47 initialEstimate.insert(1,
gtsam.Pose2(2.3, 0.1, -0.2))
48 initialEstimate.insert(2,
gtsam.Pose2(4.1, 0.1, 0.1))
53 result = optimizer.optimizeSafely()
57 P = [
None] * result.size()
58 for i
in range(0, result.size()):
59 pose_i = result.atPose2(i)
61 P[i] = marginals.marginalCovariance(i)
63 if __name__ ==
"__main__":