def ImuPredict(self):
""" Predict NavState with IMU """
if self.current_time > self.last_opt_time: # when new optimized pose is available
# store state at measurement time
self.last_opt_time = self.current_time
# start new integration onwards from optimization time
self.imu_accum.resetIntegration()
#print("Old IMU, New IMU, opt: {}, {}, {}".format(imu_samples[0][0], imu_samples[-1][0], last_opt_time))
new_imu_samples = []
for sample in self.imu_samples:
if sample[0] > self.last_opt_time:
self.imu_accum.integrateMeasurement(sample[1], sample[2], sample[3])
new_imu_samples.append(sample)
self.imu_samples = new_imu_samples
# Extract new samples from queue
(time, linear_acceleration, angular_velocity, dt) = heapq.heappop(self.imu_meas_predict)
# Store sample for integration when new measurement is available
self.imu_samples.append((time, linear_acceleration, angular_velocity, dt))
# Integrate
self.imu_accum.integrateMeasurement(linear_acceleration, angular_velocity, dt)
# Compute NavState
predicted_nav_state = self.imu_accum.predict(gtsam.NavState(self.current_global_pose, self.current_global_vel), self.current_bias)
# extract position and orientation from NavState
pos, ori = gtsam_pose_to_numpy(predicted_nav_state.pose())
return (pos, ori, self.current_global_vel, self.current_bias.accelerometer(), self.current_bias.gyroscope())
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
# and you'll have to take dtᵢ from data.
params.dt = 1e-2
# IMU bias
params.accelerometer_bias = np.array([0, 0.1, 0])
params.gyroscope_bias = np.array([0, 0, 0])
params.IMU_bias = gtsam.imuBias_ConstantBias(params.accelerometer_bias,
params.gyroscope_bias)
# 'circle_gold.npy' simulates a loop with forward velocity 2 m/s,
# while pitching up with angular velocity 30 degrees/sec.
params.initial_velocity = np.array([2, 0, 0])
params.initial_angular_vel = np.array([0, -math.radians(30),
0]) # not used here
params.initial_pose = gtsam.Pose3()
params.initial_state = gtsam.NavState(params.initial_pose,
params.initial_velocity)
# IMU preintegration algorithm parameters
# "U" means "Z axis points up"; "MakeSharedD" would mean "Z axis points along the gravity"
preintegration_params = gtsam.PreintegrationParams.MakeSharedU(
10) # 10 is the gravity force
# Realistic noise parameters
kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW
kAccelSigma = 0.1 / 60 # 10 cm VRW
preintegration_params.setGyroscopeCovariance(kGyroSigma**2 *
np.identity(3, np.float))
preintegration_params.setAccelerometerCovariance(kAccelSigma**2 *
np.identity(3, np.float))
preintegration_params.setIntegrationCovariance(0.0000001**2 *
np.identity(3, np.float))
params.preintegration_params = preintegration_params
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 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()