2 A simple 2D pose slam example with "GPS" measurements
3 - The robot moves forward 2 meter each iteration
4 - The robot initially faces along the X axis (horizontal, to the right in 2D)
5 - We have full odometry between pose
6 - We have "GPS-like" measurements implemented with a custom factor
11 from gtsam
import BetweenFactorPose2, Pose2, noiseModel
12 from gtsam_unstable
import PartialPriorFactorPose2
21 odometryNoise = noiseModel.Diagonal.Sigmas(np.asarray([0.2, 0.2, 0.1]))
31 unaryNoise = noiseModel.Diagonal.Sigmas(np.array([0.1,
35 PartialPriorFactorPose2(1, [0, 1], np.asarray([0.0, 0.0]), unaryNoise))
37 PartialPriorFactorPose2(2, [0, 1], np.asarray([2.0, 0.0]), unaryNoise))
39 PartialPriorFactorPose2(3, [0, 1], np.asarray([4.0, 0.0]), unaryNoise))
40 graph.print(
"\nFactor Graph:\n")
45 initialEstimate.insert(1,
Pose2(0.5, 0.0, 0.2))
46 initialEstimate.insert(2,
Pose2(2.3, 0.1, -0.2))
47 initialEstimate.insert(3,
Pose2(4.1, 0.1, 0.1))
48 initialEstimate.print(
"\nInitial Estimate:\n")
57 result = optimizer.optimize()
58 result.print(
"Final Result:\n")
62 print(
"x1 covariance:\n", marginals.marginalCovariance(1))
63 print(
"x2 covariance:\n", marginals.marginalCovariance(2))
64 print(
"x3 covariance:\n", marginals.marginalCovariance(3))
67 if __name__ ==
"__main__":