2 GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 See LICENSE for the license information
9 Simple robotics example using odometry measurements and bearing-range (laser) measurements
10 Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
14 from __future__
import print_function
41 gtsam.PriorFactorPose2(X1,
gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
45 gtsam.BetweenFactorPose2(X1, X2,
gtsam.Pose2(2.0, 0.0, 0.0),
48 gtsam.BetweenFactorPose2(X2, X3,
gtsam.Pose2(2.0, 0.0, 0.0),
54 np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))
67 initial_estimate.insert(X1,
gtsam.Pose2(-0.25, 0.20, 0.15))
68 initial_estimate.insert(X2,
gtsam.Pose2(2.30, 0.10, -0.20))
69 initial_estimate.insert(X3,
gtsam.Pose2(4.10, 0.10, 0.10))
74 print(
"Initial Estimate:\n{}".
format(initial_estimate))
85 result = optimizer.optimize()
90 for (key, s)
in [(X1,
"X1"), (X2,
"X2"), (X3,
"X3"), (L1,
"L1"),
93 marginals.marginalCovariance(key)))
96 if __name__ ==
"__main__":