12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #836 from borglab/fix/566

release/4.3a0
Varun Agrawal 2021-10-22 08:11:20 -04:00 committed by GitHub
parent db88546a0f b3e8bf2325
commit 791c7bdcab
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 663 additions and 272 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

191
examples/IMUKittiExampleGPS.cpp
View File

@ -11,21 +11,23 @@
/**
* @file IMUKittiExampleGPS
* @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE
* @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics
* @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI
* VISION BENCHMARK SUITE
* @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ
* Electronics
*/
// GTSAM related includes.
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <cstring>
#include <fstream>
@ -34,9 +36,9 @@
using namespace std;
using namespace gtsam;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
struct KittiCalibration {
double body_ptx;
@ -73,9 +75,11 @@ void loadKittiData(KittiCalibration& kitti_calibration,
string line;
// Read IMU metadata and compute relative sensor pose transforms
// BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma
// AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT
string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
// BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma
// GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma
// AverageDeltaT
string imu_metadata_file =
findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
ifstream imu_metadata(imu_metadata_file.c_str());
printf("-- Reading sensor metadata\n");
@ -85,12 +89,9 @@ void loadKittiData(KittiCalibration& kitti_calibration,
// Load Kitti calibration
getline(imu_metadata, line, '\n');
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
&kitti_calibration.body_ptx,
&kitti_calibration.body_pty,
&kitti_calibration.body_ptz,
&kitti_calibration.body_prx,
&kitti_calibration.body_pry,
&kitti_calibration.body_prz,
&kitti_calibration.body_ptx, &kitti_calibration.body_pty,
&kitti_calibration.body_ptz, &kitti_calibration.body_prx,
&kitti_calibration.body_pry, &kitti_calibration.body_prz,
&kitti_calibration.accelerometer_sigma,
&kitti_calibration.gyroscope_sigma,
&kitti_calibration.integration_sigma,
@ -98,15 +99,11 @@ void loadKittiData(KittiCalibration& kitti_calibration,
&kitti_calibration.gyroscope_bias_sigma,
&kitti_calibration.average_delta_t);
printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
kitti_calibration.body_ptx,
kitti_calibration.body_pty,
kitti_calibration.body_ptz,
kitti_calibration.body_prx,
kitti_calibration.body_pry,
kitti_calibration.body_prz,
kitti_calibration.body_ptx, kitti_calibration.body_pty,
kitti_calibration.body_ptz, kitti_calibration.body_prx,
kitti_calibration.body_pry, kitti_calibration.body_prz,
kitti_calibration.accelerometer_sigma,
kitti_calibration.gyroscope_sigma,
kitti_calibration.integration_sigma,
kitti_calibration.gyroscope_sigma, kitti_calibration.integration_sigma,
kitti_calibration.accelerometer_bias_sigma,
kitti_calibration.gyroscope_bias_sigma,
kitti_calibration.average_delta_t);
@ -119,13 +116,12 @@ void loadKittiData(KittiCalibration& kitti_calibration,
ifstream imu_data(imu_data_file.c_str());
getline(imu_data, line, '\n'); // ignore the first line
double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0;
double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0,
gyro_y = 0, gyro_z = 0;
while (!imu_data.eof()) {
getline(imu_data, line, '\n');
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf",
&time, &dt,
&acc_x, &acc_y, &acc_z,
&gyro_x, &gyro_y, &gyro_z);
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt,
&acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z);
ImuMeasurement measurement;
measurement.time = time;
@ -163,12 +159,16 @@ int main(int argc, char* argv[]) {
vector<GpsMeasurement> gps_measurements;
loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz,
kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz)
Vector6 BodyP =
(Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty,
kitti_calibration.body_ptz, kitti_calibration.body_prx,
kitti_calibration.body_pry, kitti_calibration.body_prz)
.finished();
auto body_T_imu = Pose3::Expmap(BodyP);
if (!body_T_imu.equals(Pose3(), 1e-5)) {
printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same");
printf(
"Currently only support IMUinBody is identity, i.e. IMU and body frame "
"are the same");
exit(-1);
}
@ -180,38 +180,45 @@ int main(int argc, char* argv[]) {
auto w_coriolis = Vector3::Zero(); // zero vector
// Configure noise models
auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
Vector3::Constant(1.0/0.07))
auto noise_model_gps = noiseModel::Diagonal::Precisions(
(Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07))
.finished());
// Set initial conditions for the estimated trajectory
// initial pose is the reference frame (navigation frame)
auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position);
auto current_pose_global =
Pose3(Rot3(), gps_measurements[first_gps_pose].position);
// the vehicle is stationary at the beginning at position 0,0,0
Vector3 current_velocity_global = Vector3::Zero();
auto current_bias = imuBias::ConstantBias(); // init with zero bias
auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
Vector3::Constant(1.0))
.finished());
auto sigma_init_x = noiseModel::Diagonal::Precisions(
(Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished());
auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100),
Vector3::Constant(5.00e-05))
auto sigma_init_b = noiseModel::Diagonal::Sigmas(
(Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05))
.finished());
// Set IMU preintegration parameters
Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
Matrix33 measured_acc_cov =
I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
Matrix33 measured_omega_cov =
I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
// error committed in integrating position from velocities
Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2);
Matrix33 integration_error_cov =
I_3x3 * pow(kitti_calibration.integration_sigma, 2);
auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
imu_params->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
imu_params->integrationCovariance = integration_error_cov; // integration uncertainty continuous
imu_params->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
imu_params->accelerometerCovariance =
measured_acc_cov; // acc white noise in continuous
imu_params->integrationCovariance =
integration_error_cov; // integration uncertainty continuous
imu_params->gyroscopeCovariance =
measured_omega_cov; // gyro white noise in continuous
imu_params->omegaCoriolis = w_coriolis;
std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement = nullptr;
std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement =
nullptr;
// Set ISAM2 parameters and create ISAM2 solver object
ISAM2Params isam_params;
@ -220,16 +227,22 @@ int main(int argc, char* argv[]) {
ISAM2 isam(isam_params);
// Create the factor graph and values object that will store new factors and values to add to the incremental graph
// Create the factor graph and values object that will store new factors and
// values to add to the incremental graph
NonlinearFactorGraph new_factors;
Values new_values; // values storing the initial estimates of new nodes in the factor graph
Values new_values; // values storing the initial estimates of new nodes in
// the factor graph
/// 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
printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n");
printf(
"-- Starting main loop: inference is performed at each time step, but we "
"plot trajectory every 10 steps\n");
size_t j = 0;
size_t included_imu_measurement_count = 0;
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
// At each non=IMU measurement we initialize a new node in the graph
auto current_pose_key = X(i);
@ -242,19 +255,24 @@ int main(int argc, char* argv[]) {
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.emplace_shared<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x);
new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);
new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b);
new_factors.emplace_shared<PriorFactor<Pose3>>(
current_pose_key, current_pose_global, sigma_init_x);
new_factors.emplace_shared<PriorFactor<Vector3>>(
current_vel_key, current_velocity_global, sigma_init_v);
new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(
current_bias_key, current_bias, sigma_init_b);
} else {
double t_previous = gps_measurements[i-1].time;
double t_previous = gps_measurements[i - 1].time;
// Summarize IMU data between the previous GPS measurement and now
current_summarized_measurement = std::make_shared<PreintegratedImuMeasurements>(imu_params, current_bias);
static size_t included_imu_measurement_count = 0;
current_summarized_measurement =
std::make_shared<PreintegratedImuMeasurements>(imu_params,
current_bias);
while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
if (imu_measurements[j].time >= t_previous) {
current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer,
imu_measurements[j].gyroscope,
current_summarized_measurement->integrateMeasurement(
imu_measurements[j].accelerometer, imu_measurements[j].gyroscope,
imu_measurements[j].dt);
included_imu_measurement_count++;
}
@ -262,38 +280,44 @@ int main(int argc, char* argv[]) {
}
// Create IMU factor
auto previous_pose_key = X(i-1);
auto previous_vel_key = V(i-1);
auto previous_bias_key = B(i-1);
auto previous_pose_key = X(i - 1);
auto previous_vel_key = V(i - 1);
auto previous_bias_key = B(i - 1);
new_factors.emplace_shared<ImuFactor>(previous_pose_key, previous_vel_key,
current_pose_key, current_vel_key,
previous_bias_key, *current_summarized_measurement);
new_factors.emplace_shared<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
auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() <<
Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma),
Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma))
auto sigma_between_b = noiseModel::Diagonal::Sigmas(
(Vector6() << Vector3::Constant(
sqrt(included_imu_measurement_count) *
kitti_calibration.accelerometer_bias_sigma),
Vector3::Constant(sqrt(included_imu_measurement_count) *
kitti_calibration.gyroscope_bias_sigma))
.finished());
new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(previous_bias_key,
current_bias_key,
imuBias::ConstantBias(),
new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(
previous_bias_key, current_bias_key, imuBias::ConstantBias(),
sigma_between_b);
// Create GPS factor
auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position);
auto gps_pose =
Pose3(current_pose_global.rotation(), gps_measurements[i].position);
if ((i % gps_skip) == 0) {
new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, gps_pose, noise_model_gps);
new_factors.emplace_shared<PriorFactor<Pose3>>(
current_pose_key, gps_pose, noise_model_gps);
new_values.insert(current_pose_key, gps_pose);
printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
printf("############ POSE INCLUDED AT TIME %.6lf ############\n",
t);
cout << gps_pose.translation();
printf("\n\n");
} else {
new_values.insert(current_pose_key, current_pose_global);
}
// Add initial values for velocity and bias based on the previous estimates
// 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);
@ -301,8 +325,9 @@ int main(int argc, char* argv[]) {
// =======================================================================
// 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)) {
printf("################ NEW FACTORS AT TIME %lf ################\n", t);
if (i > (first_gps_pose + 2 * gps_skip)) {
printf("############ NEW FACTORS AT TIME %.6lf ############\n",
t);
new_factors.print();
isam.update(new_factors, new_values);
@ -318,7 +343,7 @@ int main(int argc, char* argv[]) {
current_velocity_global = result.at<Vector3>(current_vel_key);
current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
printf("\n################ POSE AT TIME %lf ################\n", t);
printf("\n############ POSE AT TIME %lf ############\n", t);
current_pose_global.print();
printf("\n\n");
}
@ -328,7 +353,8 @@ int main(int argc, char* argv[]) {
// Save results to file
printf("\nWriting results to file...\n");
FILE* fp_out = fopen(output_filename.c_str(), "w+");
fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
fprintf(fp_out,
"#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
Values result = isam.calculateEstimate();
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
@ -349,10 +375,9 @@ int main(int argc, char* argv[]) {
cout << "Bias:" << endl << bias << endl;
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
gps_measurements[i].time,
pose.x(), pose.y(), pose.z(),
pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(),
gps(0), gps(1), gps(2));
gps_measurements[i].time, pose.x(), pose.y(), pose.z(),
pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0),
gps(1), gps(2));
}
fclose(fp_out);

366
python/gtsam/examples/IMUKittiExampleGPS.py Normal file
View File

@ -0,0 +1,366 @@
"""
Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE
Author: Varun Agrawal
"""
import argparse
from typing import List, Tuple
import gtsam
import numpy as np
from gtsam import ISAM2, Pose3, noiseModel
from gtsam.symbol_shorthand import B, V, X
GRAVITY = 9.8
class KittiCalibration:
"""Class to hold KITTI calibration info."""
def __init__(self, body_ptx: float, body_pty: float, body_ptz: float,
body_prx: float, body_pry: float, body_prz: float,
accelerometer_sigma: float, gyroscope_sigma: float,
integration_sigma: float, accelerometer_bias_sigma: float,
gyroscope_bias_sigma: float, average_delta_t: float):
self.bodyTimu = Pose3(gtsam.Rot3.RzRyRx(body_prx, body_pry, body_prz),
gtsam.Point3(body_ptx, body_pty, body_ptz))
self.accelerometer_sigma = accelerometer_sigma
self.gyroscope_sigma = gyroscope_sigma
self.integration_sigma = integration_sigma
self.accelerometer_bias_sigma = accelerometer_bias_sigma
self.gyroscope_bias_sigma = gyroscope_bias_sigma
self.average_delta_t = average_delta_t
class ImuMeasurement:
"""An instance of an IMU measurement."""
def __init__(self, time: float, dt: float, accelerometer: gtsam.Point3,
gyroscope: gtsam.Point3):
self.time = time
self.dt = dt
self.accelerometer = accelerometer
self.gyroscope = gyroscope
class GpsMeasurement:
"""An instance of a GPS measurement."""
def __init__(self, time: float, position: gtsam.Point3):
self.time = time
self.position = position
def loadImuData(imu_data_file: str) -> List[ImuMeasurement]:
"""Helper to load the IMU data."""
# Read IMU data
# Time dt accelX accelY accelZ omegaX omegaY omegaZ
imu_data_file = gtsam.findExampleDataFile(imu_data_file)
imu_measurements = []
print("-- Reading IMU measurements from file")
with open(imu_data_file, encoding='UTF-8') as imu_data:
data = imu_data.readlines()
for i in range(1, len(data)): # ignore the first line
time, dt, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z = map(
float, data[i].split(' '))
imu_measurement = ImuMeasurement(
time, dt, np.asarray([acc_x, acc_y, acc_z]),
np.asarray([gyro_x, gyro_y, gyro_z]))
imu_measurements.append(imu_measurement)
return imu_measurements
def loadGpsData(gps_data_file: str) -> List[GpsMeasurement]:
"""Helper to load the GPS data."""
# Read GPS data
# Time,X,Y,Z
gps_data_file = gtsam.findExampleDataFile(gps_data_file)
gps_measurements = []
print("-- Reading GPS measurements from file")
with open(gps_data_file, encoding='UTF-8') as gps_data:
data = gps_data.readlines()
for i in range(1, len(data)):
time, x, y, z = map(float, data[i].split(','))
gps_measurement = GpsMeasurement(time, np.asarray([x, y, z]))
gps_measurements.append(gps_measurement)
return gps_measurements
def loadKittiData(
imu_data_file: str = "KittiEquivBiasedImu.txt",
gps_data_file: str = "KittiGps_converted.txt",
imu_metadata_file: str = "KittiEquivBiasedImu_metadata.txt"
) -> Tuple[KittiCalibration, List[ImuMeasurement], List[GpsMeasurement]]:
"""
Load the KITTI Dataset.
"""
# Read IMU metadata and compute relative sensor pose transforms
# BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma
# GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma
# AverageDeltaT
imu_metadata_file = gtsam.findExampleDataFile(imu_metadata_file)
with open(imu_metadata_file, encoding='UTF-8') as imu_metadata:
print("-- Reading sensor metadata")
line = imu_metadata.readline() # Ignore the first line
line = imu_metadata.readline().strip()
data = list(map(float, line.split(' ')))
kitti_calibration = KittiCalibration(*data)
print("IMU metadata:", data)
imu_measurements = loadImuData(imu_data_file)
gps_measurements = loadGpsData(gps_data_file)
return kitti_calibration, imu_measurements, gps_measurements
def getImuParams(kitti_calibration: KittiCalibration):
"""Get the IMU parameters from the KITTI calibration data."""
w_coriolis = np.zeros(3)
# Set IMU preintegration parameters
measured_acc_cov = np.eye(3) * np.power(
kitti_calibration.accelerometer_sigma, 2)
measured_omega_cov = np.eye(3) * np.power(
kitti_calibration.gyroscope_sigma, 2)
# error committed in integrating position from velocities
integration_error_cov = np.eye(3) * np.power(
kitti_calibration.integration_sigma, 2)
imu_params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY)
# acc white noise in continuous
imu_params.setAccelerometerCovariance(measured_acc_cov)
# integration uncertainty continuous
imu_params.setIntegrationCovariance(integration_error_cov)
# gyro white noise in continuous
imu_params.setGyroscopeCovariance(measured_omega_cov)
imu_params.setOmegaCoriolis(w_coriolis)
return imu_params
def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int,
gps_measurements: List[GpsMeasurement]):
"""Write the results from `isam` to `output_filename`."""
# Save results to file
print("Writing results to file...")
with open(output_filename, 'w', encoding='UTF-8') as fp_out:
fp_out.write(
"#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n")
result = isam.calculateEstimate()
for i in range(first_gps_pose, len(gps_measurements)):
pose_key = X(i)
vel_key = V(i)
bias_key = B(i)
pose = result.atPose3(pose_key)
velocity = result.atVector(vel_key)
bias = result.atConstantBias(bias_key)
pose_quat = pose.rotation().toQuaternion()
gps = gps_measurements[i].position
print(f"State at #{i}")
print(f"Pose:\n{pose}")
print(f"Velocity:\n{velocity}")
print(f"Bias:\n{bias}")
fp_out.write("{},{},{},{},{},{},{},{},{},{},{}\n".format(
gps_measurements[i].time, pose.x(), pose.y(), pose.z(),
pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(),
gps[0], gps[1], gps[2]))
def parse_args() -> argparse.Namespace:
"""Parse command line arguments."""
parser = argparse.ArgumentParser()
parser.add_argument("--output_filename",
default="IMUKittiExampleGPSResults.csv")
return parser.parse_args()
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.setRelinearizeSkip(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 main():
"""Main runner."""
args = parse_args()
kitti_calibration, imu_measurements, gps_measurements = loadKittiData()
if not kitti_calibration.bodyTimu.equals(Pose3(), 1e-8):
raise ValueError(
"Currently only support IMUinBody is identity, i.e. IMU and body frame are the same"
)
# Configure different variables
first_gps_pose = 1
gps_skip = 10
# Configure noise models
noise_model_gps = noiseModel.Diagonal.Precisions(
np.asarray([0, 0, 0] + [1.0 / 0.07] * 3))
sigma_init_x = noiseModel.Diagonal.Precisions(
np.asarray([0, 0, 0, 1, 1, 1]))
sigma_init_v = noiseModel.Diagonal.Sigmas(np.ones(3) * 1000.0)
sigma_init_b = noiseModel.Diagonal.Sigmas(
np.asarray([0.1] * 3 + [5.00e-05] * 3))
isam = optimize(gps_measurements, imu_measurements, sigma_init_x,
sigma_init_v, sigma_init_b, noise_model_gps,
kitti_calibration, first_gps_pose, gps_skip)
save_results(isam, args.output_filename, first_gps_pose, gps_measurements)
if __name__ == "__main__":
main()