2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 Cal3Unified unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
21 Tests the fixed lag smoother wrapper
26 Simple test that checks for equality between C++ example
27 file and the Python implementation. See
28 gtsam_unstable/examples/FixedLagSmootherExample.cpp
44 np.array([0.3, 0.3, 0.1]))
46 new_factors.push_back(
47 gtsam.PriorFactorPose2(
48 X1, prior_mean, prior_noise))
49 new_values.insert(X1, prior_mean)
50 new_timestamps[X1] = 0.0
76 previous_key =
int(1000 * (time - delta_time))
77 current_key =
int(1000 * time)
80 new_timestamps[current_key] = time
86 new_values.insert(current_key, current_pose)
90 odometry_measurement_1 =
gtsam.Pose2(0.61, -0.08, 0.02)
92 np.array([0.1, 0.1, 0.05]))
93 new_factors.push_back(
94 gtsam.BetweenFactorPose2(
97 odometry_measurement_1,
100 odometry_measurement_2 =
gtsam.Pose2(0.47, 0.03, 0.01)
102 np.array([0.05, 0.05, 0.05]))
103 new_factors.push_back(
104 gtsam.BetweenFactorPose2(
107 odometry_measurement_2,
114 smoother_batch.update(new_factors, new_values, new_timestamps)
116 estimate = smoother_batch.calculateEstimatePose2(current_key)
117 self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
120 new_timestamps.clear()
122 new_factors.resize(0)
127 if __name__ ==
"__main__":