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 robot motion example, with prior and two odometry measurements
10 Author: Frank Dellaert
14 from __future__
import print_function
18 import matplotlib.pyplot
as plt
34 graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
40 graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
41 graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
55 result = optimizer.optimize()
62 marginals.marginalCovariance(i)))
65 gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5,
66 marginals.marginalCovariance(i))
71 if __name__ ==
"__main__":