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