diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index 5d44dab64..b9777a07c 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -9,9 +9,6 @@ import numpy as np import gtsam import gtsam_unstable -# Create noise models -PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2])) def _timestamp_key_value(key, value): return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( @@ -35,8 +32,9 @@ def BatchFixedLagSmootherExample(): # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) X1 = 0 - new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, PRIOR_NOISE)) + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) new_timestamps.insert(_timestamp_key_value(X1, 0.0)) diff --git a/cython/gtsam_unstable/examples/__init__.py b/cython/gtsam_unstable/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/__init__.py b/cython/gtsam_unstable/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py new file mode 100644 index 000000000..64895f4d3 --- /dev/null +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -0,0 +1,94 @@ +import unittest +import gtsam +import gtsam_unstable +import numpy as np + +def _timestamp_key_value(key, value): + return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( + key, value + ) +class TestFixedLagSmootherExample(unittest.TestCase): + # Simple test that checks for equality between C++ example + # file and the Python implementation + def test_FixedLagSmootherExample(self): + # Define a batch fixed lag smoother, which uses + # Levenberg-Marquardt to perform the nonlinear optimization + lag = 2.0 + smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + + + # Create containers to store the factors and linearization points + # that will be sent to the smoothers + new_factors = gtsam.NonlinearFactorGraph() + new_values = gtsam.Values() + new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + + + # Create a prior on the first pose, placing it at the origin + prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + X1 = 0 + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) + new_values.insert(X1, prior_mean) + new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + + delta_time = 0.25 + time = 0.25 + + i = 0 + + ground_truth = [ + gtsam.Pose2(0.49792, 0.007802, 0.015), + gtsam.Pose2(0.99547, 0.023019, 0.03), + gtsam.Pose2(1.4928, 0.045725, 0.045), + gtsam.Pose2(1.9898, 0.075888, 0.06), + gtsam.Pose2(2.4863, 0.1135, 0.075), + gtsam.Pose2(2.9821, 0.15856, 0.09), + gtsam.Pose2(3.4772, 0.21105, 0.105), + gtsam.Pose2(3.9715, 0.27096, 0.12), + gtsam.Pose2(4.4648, 0.33827, 0.135), + gtsam.Pose2(4.957, 0.41298, 0.15), + gtsam.Pose2(5.4481, 0.49506, 0.165), + gtsam.Pose2(5.9379, 0.5845, 0.18), + ] + while time <= 3.0: + previous_key = 1000 * (time - delta_time) + current_key = 1000 * time + + # assign current key to the current timestamp + new_timestamps.insert(_timestamp_key_value(current_key, time)) + + # Add a guess for this pose to the new values + # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] + current_pose = gtsam.Pose2(time * 2, 0, 0) + new_values.insert(current_key, current_pose) + + # Add odometry factors from two different sources with different error stats + odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_1, odometry_noise_1 + )) + + odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.05, 0.05, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_2, odometry_noise_2 + )) + + # Update the smoothers with the new factors + smoother_batch.update(new_factors, new_values, new_timestamps) + + estimate = smoother_batch.calculateEstimatePose2(current_key) + self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + print("PASS") + + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) + + time += delta_time + i += 1 + +if __name__ == "__main__": + unittest.main()