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 Demonstration of the fixed-lag smoothers using a planar robot example
10 and multiple odometry-like sensors
11 Author: Frank Dellaert (C++), Jeremy Aguilon (Python)
22 Runs a batch fixed smoother on an agent with two odometry
23 sensors that is simply moving to the
41 new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
42 new_values.insert(X1, prior_mean)
43 new_timestamps[X1] = 0.0
49 previous_key =
int(1000 * (time - delta_time))
50 current_key =
int(1000 * time)
53 new_timestamps[current_key] = time
58 new_values.insert(current_key, current_pose)
62 odometry_measurement_1 =
gtsam.Pose2(0.61, -0.08, 0.02)
64 np.array([0.1, 0.1, 0.05]))
65 new_factors.push_back(gtsam.BetweenFactorPose2(
66 previous_key, current_key, odometry_measurement_1, odometry_noise_1
69 odometry_measurement_2 =
gtsam.Pose2(0.47, 0.03, 0.01)
71 np.array([0.05, 0.05, 0.05]))
72 new_factors.push_back(gtsam.BetweenFactorPose2(
73 previous_key, current_key, odometry_measurement_2, odometry_noise_2
80 smoother_batch.update(new_factors, new_values, new_timestamps)
81 print(
"Timestamp = " +
str(time) +
", Key = " +
str(current_key))
82 print(smoother_batch.calculateEstimatePose2(current_key))
84 new_timestamps.clear()
91 if __name__ ==
'__main__':
93 print(
"Example complete")