Fix warning message in the unit tests/examples

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
+    sensors that is simply moving to the
-    speed.
     """
 
     # 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
+        # Update the smoothers with the new factors. In this case,
-        smoother_batch.update(new_factors, new_values, new_timestamps)
+        # one iteration must pass for Levenberg-Marquardt to accurately
-
+        # estimate
-        print("Timestamp = " + str(time) + ", Key = " + str(current_key))
+        if time >= 0.50:
-        print(smoother_batch.calculateEstimatePose2(current_key))
+            smoother_batch.update(new_factors, new_values, new_timestamps)
-
+            print("Timestamp = " + str(time) + ", Key = " + str(current_key))
-        new_timestamps.clear()
+            print(smoother_batch.calculateEstimatePose2(current_key))
-        new_values.clear()
-        new_factors.resize(0)
 
+            new_timestamps.clear()
+            new_values.clear()
+            new_factors.resize(0)
 
         time += delta_time
 
+
 if __name__ == '__main__':
     BatchFixedLagSmootherExample()
     print("Example complete")

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.995821, 0.0231012, 0.0300001),
-            gtsam.Pose2(0.99547, 0.023019, 0.03),
+            gtsam.Pose2(1.49284, 0.0457247, 0.045),
-            gtsam.Pose2(1.4928, 0.045725, 0.045),
+            gtsam.Pose2(1.98981, 0.0758879, 0.06),
-            gtsam.Pose2(1.9898, 0.075888, 0.06),
+            gtsam.Pose2(2.48627, 0.113502, 0.075),
-            gtsam.Pose2(2.4863, 0.1135, 0.075),
+            gtsam.Pose2(2.98211, 0.158558, 0.09),
-            gtsam.Pose2(2.9821, 0.15856, 0.09),
+            gtsam.Pose2(3.47722, 0.211047, 0.105),
-            gtsam.Pose2(3.4772, 0.21105, 0.105),
+            gtsam.Pose2(3.97149, 0.270956, 0.12),
-            gtsam.Pose2(3.9715, 0.27096, 0.12),
+            gtsam.Pose2(4.4648, 0.338272, 0.135),
-            gtsam.Pose2(4.4648, 0.33827, 0.135),
+            gtsam.Pose2(4.95705, 0.41298, 0.15),
-            gtsam.Pose2(4.957, 0.41298, 0.15),
+            gtsam.Pose2(5.44812, 0.495063, 0.165),
-            gtsam.Pose2(5.4481, 0.49506, 0.165),
+            gtsam.Pose2(5.9379, 0.584503, 0.18),
-            gtsam.Pose2(5.9379, 0.5845, 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]))
+            odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
-            new_factors.push_back(gtsam.BetweenFactorPose2(
+                np.array([0.1, 0.1, 0.05]))
-                previous_key, current_key, odometry_measurement_1, odometry_noise_1
+            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(
-            new_factors.push_back(gtsam.BetweenFactorPose2(
+                np.array([0.05, 0.05, 0.05]))
-                previous_key, current_key, odometry_measurement_2, odometry_noise_2
+            new_factors.push_back(
-            ))
+                gtsam.BetweenFactorPose2(
+                    previous_key,
+                    current_key,
+                    odometry_measurement_2,
+                    odometry_noise_2))
 
-            # Update the smoothers with the new factors
+            # Update the smoothers with the new factors. In this case,
-            smoother_batch.update(new_factors, new_values, new_timestamps)
+            # 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)
+                estimate = smoother_batch.calculateEstimatePose2(current_key)
-            self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
+                self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
+                i += 1
 
-            new_timestamps.clear()
+                new_timestamps.clear()
-            new_values.clear()
+                new_values.clear()
-            new_factors.resize(0)
+                new_factors.resize(0)
 
             time += delta_time
-            i += 1
+
 
 if __name__ == "__main__":
     unittest.main()

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<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
 
-    // Update the smoothers with the new factors
+    // Update the smoothers with the new factors.
-    smootherBatch.update(newFactors, newValues, newTimestamps);
+    // In this example, Levenberg-Marquadt needs one iteration
-    smootherISAM2.update(newFactors, newValues, newTimestamps);
+    // to pass to accurately estimate.
-    for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
+    if (time >= 0.50) {
-      smootherISAM2.update();
+      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<Pose2>(currentKey).print("Batch Estimate:");
+      smootherISAM2.calculateEstimate<Pose2>(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<Pose2>(currentKey).print("Batch Estimate:");
-    smootherISAM2.calculateEstimate<Pose2>(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