diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py index 141162044..786701e0f 100644 --- a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -19,7 +19,7 @@ import gtsam_unstable def _timestamp_key_value(key, value): """ - Creates a key value pair for a FixedLagSmootherKeyTimeStampMap + """ return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( key, value @@ -29,8 +29,7 @@ def _timestamp_key_value(key, value): def BatchFixedLagSmootherExample(): """ Runs a batch fixed smoother on an agent with two odometry - sensors that is simply moving along the x axis in constant - speed. + sensors that is simply moving to the """ # Define a batch fixed lag smoother, which uses @@ -38,14 +37,12 @@ def BatchFixedLagSmootherExample(): 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])) @@ -57,9 +54,6 @@ def BatchFixedLagSmootherExample(): delta_time = 0.25 time = 0.25 - # Iterates from 0.25s to 3.0s, adding 0.25s each loop - # In each iteration, the agent moves at a constant speed - # and its two odometers measure the change. while time <= 3.0: previous_key = 1000 * (time - delta_time) current_key = 1000 * time @@ -72,32 +66,37 @@ def BatchFixedLagSmootherExample(): 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 + # 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])) + 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])) + 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) - - print("Timestamp = " + str(time) + ", Key = " + str(current_key)) - print(smoother_batch.calculateEstimatePose2(current_key)) - - new_timestamps.clear() - new_values.clear() - new_factors.resize(0) + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) + print("Timestamp = " + str(time) + ", Key = " + str(current_key)) + print(smoother_batch.calculateEstimatePose2(current_key)) + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) time += delta_time + if __name__ == '__main__': BatchFixedLagSmootherExample() print("Example complete") diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py index e26c450a0..90c4e436b 100644 --- a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -3,10 +3,13 @@ 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): ''' Tests the fixed lag smoother wrapper @@ -23,19 +26,20 @@ class TestFixedLagSmootherExample(unittest.TestCase): 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])) + 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)) @@ -45,19 +49,19 @@ class TestFixedLagSmootherExample(unittest.TestCase): 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), + gtsam.Pose2(0.995821, 0.0231012, 0.0300001), + gtsam.Pose2(1.49284, 0.0457247, 0.045), + gtsam.Pose2(1.98981, 0.0758879, 0.06), + gtsam.Pose2(2.48627, 0.113502, 0.075), + gtsam.Pose2(2.98211, 0.158558, 0.09), + gtsam.Pose2(3.47722, 0.211047, 0.105), + gtsam.Pose2(3.97149, 0.270956, 0.12), + gtsam.Pose2(4.4648, 0.338272, 0.135), + gtsam.Pose2(4.95705, 0.41298, 0.15), + gtsam.Pose2(5.44812, 0.495063, 0.165), + gtsam.Pose2(5.9379, 0.584503, 0.18), ] + # Iterates from 0.25s to 3.0s, adding 0.25s each loop # In each iteration, the agent moves at a constant speed # and its two odometers measure the change. The smoothed @@ -70,35 +74,49 @@ class TestFixedLagSmootherExample(unittest.TestCase): 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] + # 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 + # 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_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 - )) + 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) + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) - estimate = smoother_batch.calculateEstimatePose2(current_key) - self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + estimate = smoother_batch.calculateEstimatePose2(current_key) + self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + i += 1 - new_timestamps.clear() - new_values.clear() - new_factors.resize(0) + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) time += delta_time - i += 1 + if __name__ == "__main__": unittest.main() diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 931e85c1a..8ccc9cc2c 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -111,23 +111,27 @@ int main(int argc, char** argv) { noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); - // Update the smoothers with the new factors - smootherBatch.update(newFactors, newValues, newTimestamps); - smootherISAM2.update(newFactors, newValues, newTimestamps); - for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations - smootherISAM2.update(); + // Update the smoothers with the new factors. + // In this example, Levenberg-Marquadt needs one iteration + // to pass to accurately estimate. + if (time >= 0.50) { + smootherBatch.update(newFactors, newValues, newTimestamps); + smootherISAM2.update(newFactors, newValues, newTimestamps); + for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations + smootherISAM2.update(); + } + + // Print the optimized current pose + cout << setprecision(5) << "Timestamp = " << time << endl; + smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); + smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); + cout << endl; + + // Clear contains for the next iteration + newTimestamps.clear(); + newValues.clear(); + newFactors.resize(0); } - - // Print the optimized current pose - cout << setprecision(5) << "Timestamp = " << time << endl; - smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); - smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); - cout << endl; - - // Clear contains for the next iteration - newTimestamps.clear(); - newValues.clear(); - newFactors.resize(0); } // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds