def ImuUpdate(self, imu_samples): """ Update NavState using IMU factor """ # Reset integration imu_accum = gtsam.PreintegratedImuMeasurements(self.imu_preint_params) # Integrate IMU measurements at measurement time for sample in imu_samples: imu_accum.integrateMeasurement(sample[1], sample[2], sample[3]) # Compute NavState predicted_nav_state = imu_accum.predict(gtsam.NavState(self.current_global_pose, self.current_global_vel), self.current_bias) # Add IMU factor imu_factor = gtsam.ImuFactor( self.poseKey-1, self.velKey-1, self.poseKey, self.velKey, self.biasKey, imu_accum) self.new_factors.add(imu_factor) return predicted_nav_state
imu_factor_pairs = zip(preintegration_steps[:-1], preintegration_steps[1:])
current_imu_factor_pair = next(imu_factor_pairs)
# Clear the accumulated value
current_preintegrated_IMU.resetIntegration()
for i, imu_measurement in enumerate(imu_measurements):
measured_acceleration, measured_angular_vel = imu_measurement[:
3], imu_measurement[
3:]
if i == current_imu_factor_pair[1]:
# Add IMU factor
factor = gtsam.ImuFactor(symbol('x', current_imu_factor_pair[0]),
symbol('v', current_imu_factor_pair[0]),
symbol('x', current_imu_factor_pair[1]),
symbol('v', current_imu_factor_pair[1]),
symbol('b', 0), current_preintegrated_IMU)
factor_graph.push_back(factor)
# Start accumulating from scratch
current_preintegrated_IMU.resetIntegration()
# Get the next pair of indices
try:
current_imu_factor_pair = next(imu_factor_pairs)
except StopIteration:
assert i == len(imu_measurements) - 1
# Accumulate the current measurement
current_preintegrated_IMU.integrateMeasurement(measured_acceleration,
def run(self):
graph = gtsam.NonlinearFactorGraph()
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
H9 = gtsam.OptionalJacobian9()
T = 12
num_poses = T + 1 # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses):
state_i = self.scenario.navState(float(i))
initial.insert(X(i), state_i.pose())
initial.insert(V(i), state_i.velocity())
# simulate the loop
i = 0 # state index
actual_state_i = self.scenario.navState(0)
for k, t in enumerate(np.arange(0, T, self.dt)):
# get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
# Plot IMU many times
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
# Plot every second
if k % int(1 / self.dt) == 0:
self.plotGroundTruthPose(t)
# create IMU factor every second
if (k + 1) % int(1 / self.dt) == 0:
factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1),
BIAS_KEY, pim)
graph.push_back(factor)
if True:
print(factor)
H2 = gtsam.OptionalJacobian96()
print(pim.predict(actual_state_i, self.actualBias, H9, H2))
pim.resetIntegration()
actual_state_i = self.scenario.navState(t + self.dt)
i += 1
# add priors on beginning and end
self.addPrior(0, graph)
self.addPrior(num_poses - 1, graph)
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
# Calculate and print marginal covariances
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
# Plot resulting poses
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plotPose3(POSES_FIG, pose_i, 0.1)
i += 1
print(result.atConstantBias(BIAS_KEY))
plt.ioff()
plt.show()
def optimize(gps_measurements: List[GpsMeasurement],
imu_measurements: List[ImuMeasurement],
sigma_init_x: gtsam.noiseModel.Diagonal,
sigma_init_v: gtsam.noiseModel.Diagonal,
sigma_init_b: gtsam.noiseModel.Diagonal,
noise_model_gps: gtsam.noiseModel.Diagonal,
kitti_calibration: KittiCalibration, first_gps_pose: int,
gps_skip: int) -> gtsam.ISAM2:
"""Run ISAM2 optimization on the measurements."""
# Set initial conditions for the estimated trajectory
# initial pose is the reference frame (navigation frame)
current_pose_global = Pose3(gtsam.Rot3(),
gps_measurements[first_gps_pose].position)
# the vehicle is stationary at the beginning at position 0,0,0
current_velocity_global = np.zeros(3)
current_bias = gtsam.imuBias.ConstantBias() # init with zero bias
imu_params = getImuParams(kitti_calibration)
# Set ISAM2 parameters and create ISAM2 solver object
isam_params = gtsam.ISAM2Params()
isam_params.setFactorization("CHOLESKY")
isam_params.relinearizeSkip = 10
isam = gtsam.ISAM2(isam_params)
# Create the factor graph and values object that will store new factors and
# values to add to the incremental graph
new_factors = gtsam.NonlinearFactorGraph()
# values storing the initial estimates of new nodes in the factor graph
new_values = gtsam.Values()
# Main loop:
# (1) we read the measurements
# (2) we create the corresponding factors in the graph
# (3) we solve the graph to obtain and optimal estimate of robot trajectory
print("-- Starting main loop: inference is performed at each time step, "
"but we plot trajectory every 10 steps")
j = 0
included_imu_measurement_count = 0
for i in range(first_gps_pose, len(gps_measurements)):
# At each non=IMU measurement we initialize a new node in the graph
current_pose_key = X(i)
current_vel_key = V(i)
current_bias_key = B(i)
t = gps_measurements[i].time
if i == first_gps_pose:
# Create initial estimate and prior on initial pose, velocity, and biases
new_values.insert(current_pose_key, current_pose_global)
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
new_factors.addPriorPose3(current_pose_key, current_pose_global,
sigma_init_x)
new_factors.addPriorVector(current_vel_key,
current_velocity_global, sigma_init_v)
new_factors.addPriorConstantBias(current_bias_key, current_bias,
sigma_init_b)
else:
t_previous = gps_measurements[i - 1].time
# Summarize IMU data between the previous GPS measurement and now
current_summarized_measurement = gtsam.PreintegratedImuMeasurements(
imu_params, current_bias)
while (j < len(imu_measurements)
and imu_measurements[j].time <= t):
if imu_measurements[j].time >= t_previous:
current_summarized_measurement.integrateMeasurement(
imu_measurements[j].accelerometer,
imu_measurements[j].gyroscope, imu_measurements[j].dt)
included_imu_measurement_count += 1
j += 1
# Create IMU factor
previous_pose_key = X(i - 1)
previous_vel_key = V(i - 1)
previous_bias_key = B(i - 1)
new_factors.push_back(
gtsam.ImuFactor(previous_pose_key, previous_vel_key,
current_pose_key, current_vel_key,
previous_bias_key,
current_summarized_measurement))
# Bias evolution as given in the IMU metadata
sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas(
np.asarray([
np.sqrt(included_imu_measurement_count) *
kitti_calibration.accelerometer_bias_sigma
] * 3 + [
np.sqrt(included_imu_measurement_count) *
kitti_calibration.gyroscope_bias_sigma
] * 3))
new_factors.push_back(
gtsam.BetweenFactorConstantBias(previous_bias_key,
current_bias_key,
gtsam.imuBias.ConstantBias(),
sigma_between_b))
# Create GPS factor
gps_pose = Pose3(current_pose_global.rotation(),
gps_measurements[i].position)
if (i % gps_skip) == 0:
new_factors.addPriorPose3(current_pose_key, gps_pose,
noise_model_gps)
new_values.insert(current_pose_key, gps_pose)
print(f"############ POSE INCLUDED AT TIME {t} ############")
print(gps_pose.translation(), "\n")
else:
new_values.insert(current_pose_key, current_pose_global)
# Add initial values for velocity and bias based on the previous
# estimates
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
# Update solver
# =======================================================================
# We accumulate 2*GPSskip GPS measurements before updating the solver at
# first so that the heading becomes observable.
if i > (first_gps_pose + 2 * gps_skip):
print(f"############ NEW FACTORS AT TIME {t:.6f} ############")
new_factors.print()
isam.update(new_factors, new_values)
# Reset the newFactors and newValues list
new_factors.resize(0)
new_values.clear()
# Extract the result/current estimates
result = isam.calculateEstimate()
current_pose_global = result.atPose3(current_pose_key)
current_velocity_global = result.atVector(current_vel_key)
current_bias = result.atConstantBias(current_bias_key)
print(f"############ POSE AT TIME {t} ############")
current_pose_global.print()
print("\n")
return isam
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses
angular_velocity = math.radians(180) # rad/sec
delta_t = 1.0/18 # makes for 10 degrees per step
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph
newgraph = gtsam.NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = gtsam.ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = gtsam.Values()
# Add a prior on pose x0. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = gtsam.symbol(ord('b'), 0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise)
newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
# Simulate poses and imu measurements, adding them to the factor graph
for i in range(80):
t = i * delta_t # simulation time
if i == 0: # First time add two poses
pose_1 = scenario.pose(delta_t)
initialEstimate.insert(X(0), pose_0.compose(DELTA))
initialEstimate.insert(X(1), pose_1.compose(DELTA))
elif i >= 2: # Add more poses as necessary
pose_i = scenario.pose(t)
initialEstimate.insert(X(i), pose_i.compose(DELTA))
if i > 0:
# Add Bias variables periodically
if i % 5 == 0:
biasKey += 1
factor = gtsam.BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
bRn = np.transpose(nRb)
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
measuredOmega = scenario.omega_b(t)
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor
imufac = gtsam.ImuFactor(
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
# insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity)
accum.resetIntegration()
# Incremental solution
isam.update(newgraph, initialEstimate)
result = isam.calculateEstimate()
ISAM2_plot(result)
# reset
newgraph = gtsam.NonlinearFactorGraph()
initialEstimate.clear()
def run(self,
T: int = 12,
compute_covariances: bool = False,
verbose: bool = True):
"""
Main runner.
Args:
T: Total trajectory time.
compute_covariances: Flag indicating whether to compute marginal covariances.
verbose: Flag indicating if printing should be verbose.
"""
graph = gtsam.NonlinearFactorGraph()
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
num_poses = T # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
# simulate the loop
i = 0 # state index
initial_state_i = self.scenario.navState(0)
initial.insert(X(i), initial_state_i.pose())
initial.insert(V(i), initial_state_i.velocity())
# add prior on beginning
self.addPrior(0, graph)
for k, t in enumerate(np.arange(0, T, self.dt)):
# get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
# Plot IMU many times
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
if (k + 1) % int(1 / self.dt) == 0:
# Plot every second
self.plotGroundTruthPose(t, scale=1)
plt.title("Ground Truth Trajectory")
# create IMU factor every second
factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1),
BIAS_KEY, pim)
graph.push_back(factor)
if verbose:
print(factor)
print(pim.predict(initial_state_i, self.actualBias))
pim.resetIntegration()
rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1)
translationNoise = gtsam.Point3(*np.random.randn(3) * 1)
poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
actual_state_i = self.scenario.navState(t + self.dt)
print("Actual state at {0}:\n{1}".format(
t + self.dt, actual_state_i))
noisy_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3) * 0.1)
initial.insert(X(i + 1), noisy_state_i.pose())
initial.insert(V(i + 1), noisy_state_i.velocity())
i += 1
# add priors on end
self.addPrior(num_poses - 1, graph)
initial.print("Initial values:")
result = self.optimize(graph, initial)
result.print("Optimized values:")
print("------------------")
print(graph.error(initial))
print(graph.error(result))
print("------------------")
if compute_covariances:
# Calculate and print marginal covariances
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n",
marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
self.plot(result, show=True)
def add_imu_measurements(self,
measured_poses,
measured_acc,
measured_omega,
measured_vel,
delta_t,
n_skip,
initial_poses=None):
n_frames = measured_poses.shape[0]
# Check if sizes are correct
assert measured_poses.shape[0] == n_frames
assert measured_acc.shape[0] == n_frames
assert measured_vel.shape[0] == n_frames
# Pose prior
pose_key = X(0)
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.2, 0.2, 0.2, 0.2, 0.2, 0.2]))
pose_0 = gtsam.Pose3(measured_poses[0])
self.graph.push_back(
gtsam.PriorFactorPose3(pose_key, pose_0, pose_noise))
self.initial_estimate.insert(pose_key, gtsam.Pose3(measured_poses[0]))
# IMU prior
bias_key = B(0)
bias_noise = gtsam.noiseModel.Isotropic.Sigma(6, 0.5)
self.graph.push_back(
gtsam.PriorFactorConstantBias(bias_key,
gtsam.imuBias.ConstantBias(),
bias_noise))
self.initial_estimate.insert(bias_key, gtsam.imuBias.ConstantBias())
# Velocity prior
velocity_key = V(0)
velocity_noise = gtsam.noiseModel.Isotropic.Sigma(3, .5)
velocity_0 = measured_vel[0]
self.graph.push_back(
gtsam.PriorFactorVector(velocity_key, velocity_0, velocity_noise))
self.initial_estimate.insert(velocity_key, velocity_0)
# Preintegrator
accum = gtsam.PreintegratedImuMeasurements(self.IMU_PARAMS)
# Add measurements to factor graph
for i in range(1, n_frames):
accum.integrateMeasurement(measured_acc[i], measured_omega[i],
delta_t[i - 1])
if i % n_skip == 0:
pose_key += 1
DELTA = gtsam.Pose3(
gtsam.Rot3.Rodrigues(0, 0, 0.1 * np.random.randn()),
gtsam.Point3(4 * np.random.randn(), 4 * np.random.randn(),
4 * np.random.randn()))
self.initial_estimate.insert(
pose_key,
gtsam.Pose3(measured_poses[i]).compose(DELTA))
velocity_key += 1
self.initial_estimate.insert(velocity_key, measured_vel[i])
bias_key += 1
self.graph.add(
gtsam.BetweenFactorConstantBias(
bias_key - 1, bias_key, gtsam.imuBias.ConstantBias(),
self.BIAS_COVARIANCE))
self.initial_estimate.insert(bias_key,
gtsam.imuBias.ConstantBias())
# Add IMU Factor
self.graph.add(
gtsam.ImuFactor(pose_key - 1, velocity_key - 1, pose_key,
velocity_key, bias_key, accum))
# Reset preintegration
accum.resetIntegration()
def run(self, T=12, compute_covariances=False, verbose=True):
graph = gtsam.NonlinearFactorGraph()
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
# T = 12
num_poses = T # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
# simulate the loop
i = 0 # state index
initial_state_i = self.scenario.navState(0)
initial.insert(X(i), initial_state_i.pose())
initial.insert(V(i), initial_state_i.velocity())
# add prior on beginning
self.addPrior(0, graph)
gtNavStates = []
predictedNavStates = []
integrationTime = []
for k, t in enumerate(np.arange(0, T, self.dt)):
# get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
start = time()
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
integrationTime.append(time() - start)
# Plot IMU many times
if k % 10 == 0 and not DISABLE_VISUAL:
self.plotImu(t, measuredOmega, measuredAcc)
if (k + 1) % int(1 / self.dt) == 0:
# Plot every second
gtNavState = self.plotGroundTruthPose(t, scale=0.3)
gtNavStates.append(gtNavState)
plt.title("Ground Truth + Estimated Trajectory")
# create IMU factor every second
factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1),
BIAS_KEY, pim)
graph.push_back(factor)
if verbose:
print(factor)
print(pim.predict(initial_state_i, self.actualBias))
pim.resetIntegration()
rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1 * 1)
translationNoise = gtsam.Point3(*np.random.randn(3) * 1 * 1)
poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
actual_state_i = self.scenario.navState(t + self.dt)
if not DISABLE_VISUAL:
print("Actual state at {0}:\n{1}".format(
t + self.dt, actual_state_i))
noisy_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3) * 0.1)
initial.insert(X(i + 1), noisy_state_i.pose())
initial.insert(V(i + 1), noisy_state_i.velocity())
i += 1
# add priors on end
self.addPrior(num_poses - 1, graph)
initial.print_("Initial values:")
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
result.print_("Optimized values:")
if compute_covariances:
# Calculate and print marginal covariances
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n",
marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
# Plot resulting poses
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
predictedNavStates.append(pose_i)
if not DISABLE_VISUAL:
plot_pose3(POSES_FIG, pose_i, 1)
i += 1
# plt.title("Estimated Trajectory")
if not DISABLE_VISUAL:
gtsam.utils.plot.set_axes_equal(POSES_FIG)
print("Bias Values", result.atConstantBias(BIAS_KEY))
ATE = []
# import ipdb; ipdb.set_trace()
for gt, pred in zip(gtNavStates, predictedNavStates[1:]):
delta = gt.inverse().compose(pred)
ATE.append(np.linalg.norm(delta.Logmap(delta))**2)
print("ATE={}".format(np.sqrt(np.mean(ATE))))
print("Run time={}".format(np.median(integrationTime)))
plt.ioff()
plt.show()