def init_smoother(request): """ Runs a batch fixed smoother on an agent with two odometry sensors that is simply moving along the x axis in constant speed. """ global SMOOTHER_BATCH # Define a batch fixed lag smoother, which uses # Levenberg-Marquardt to perform the nonlinear optimization lag = request.lag smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() prior_mean = request.prior_mean prior_noise = request.prior_noise 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)) SMOOTHER_BATCH = smoother_batch SMOOTHER_BATCH.update(new_factors, new_values, new_timestamps) return X1
def record_observation(observation): global SMOOTHER_BATCH factor_graph = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() time = observation.time previous_key = observation.previous_key current_key = observation.current_key current_pose = observation.current_pose new_timestamps.insert(_timestamp_key_value(current_key, time)) new_values.insert(current_key, current_pose) # for measurement, noise in zip( # observation.odometry_measurements, observation.odometry_noise): # factor_graph.push_back(gtsam.BetweenFactorPose2( # previous_key, current_key, measurement, noise import numpy as np 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])) factor_graph.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])) factor_graph.push_back( gtsam.BetweenFactorPose2(previous_key, current_key, odometry_measurement_2, odometry_noise_2)) # Update the smoothers with the new factors SMOOTHER_BATCH.update(factor_graph, new_values, new_timestamps) pose = SMOOTHER_BATCH.calculateEstimatePose2(current_key) output = FixedLagSmootherOutput(current_key, pose) return output
def test_FixedLagSmootherExample(self): ''' Simple test that checks for equality between C++ example file and the Python implementation. See gtsam_unstable/examples/FixedLagSmootherExample.cpp ''' # 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.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 # result is then compared to the ground truth 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. 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)) i += 1 new_timestamps.clear() new_values.clear() new_factors.resize(0) time += delta_time
def BatchFixedLagSmootherExample(): """ Runs a batch fixed smoother on an agent with two odometry sensors that is simply moving to the """ # 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 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. 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