From 906d0277e93b43f5160c7a18df6e83a5990d1ce9 Mon Sep 17 00:00:00 2001 From: Thomas Jespersen Date: Mon, 16 Mar 2020 00:48:36 +0800 Subject: [PATCH 1/2] Added ported C++ version of ISAM2 Kitti example --- examples/IMUKittiExampleGPS.cpp | 293 ++++++++++++++++++++++++++++++++ 1 file changed, 293 insertions(+) create mode 100644 examples/IMUKittiExampleGPS.cpp diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp new file mode 100644 index 000000000..9f302ab2f --- /dev/null +++ b/examples/IMUKittiExampleGPS.cpp @@ -0,0 +1,293 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @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 + */ + +// GTSAM related includes. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace std; + +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) + +typedef struct { + double Time; + double dt; + Vector3 Accelerometer; + Vector3 Gyroscope; // omega +} imuMeasurement_t; + +typedef struct { + double Time; + Vector3 Position; // x,y,z +} gpsMeasurement_t; + +const string output_filename = "IMUKittyExampleGPSResults.csv"; + +int main(int argc, char* argv[]) +{ + string line; + + // Read IMU metadata and compute relative sensor pose transforms + // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT + // 0 0 0 0 0 0 0.01 0.000175 0 0.000167 2.91e-006 0.0100395199348279 + string IMU_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); + ifstream IMU_metadata(IMU_metadata_file.c_str()); + + printf("-- Reading sensor metadata\n"); + + getline(IMU_metadata, line, '\n'); // ignore the first line + + double BodyPtx, BodyPty, BodyPtz, BodyPrx, BodyPry, BodyPrz, AccelerometerSigma, GyroscopeSigma, IntegrationSigma, AccelerometerBiasSigma, GyroscopeBiasSigma, AverageDeltaT; + getline(IMU_metadata, line, '\n'); + sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", &BodyPtx, &BodyPty, &BodyPtz, &BodyPrx, &BodyPry, &BodyPrz, &AccelerometerSigma, &GyroscopeSigma, &IntegrationSigma, &AccelerometerBiasSigma, &GyroscopeBiasSigma, &AverageDeltaT); + printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", BodyPtx, BodyPty, BodyPtz, BodyPrx, BodyPry, BodyPrz, AccelerometerSigma, GyroscopeSigma, IntegrationSigma, AccelerometerBiasSigma, GyroscopeBiasSigma, AverageDeltaT); + + Vector6 BodyP = (Vector(6) << BodyPtx, BodyPty, BodyPtz, BodyPrx, BodyPry, BodyPrz).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"); + exit(-1); + } + + // Read IMU data + // Time dt accelX accelY accelZ omegaX omegaY omegaZ + // 46534.47837579 46534.47837579 1.7114864219577 0.1717911743144 9.80533438749 -0.0032006241515747 0.031231284764596 -0.0063569265706488 + vector IMU_measurements; + string IMU_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); + + printf("-- Reading IMU measurements from file\n"); + { + 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; + 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); + + imuMeasurement_t measurement; + measurement.Time = time; + measurement.dt = dt; + measurement.Accelerometer = Vector3(acc_x, acc_y, acc_z); + measurement.Gyroscope = Vector3(gyro_x, gyro_y, gyro_z); + IMU_measurements.push_back(measurement); + } + } + + // Read GPS data + // Time,X,Y,Z + // 46534.478375790000428,-6.8269361350059405424,-11.868164241239471224,0.040306091310000624617 + vector GPS_measurements; + string GPS_data_file = findExampleDataFile("KittiGps_converted.txt"); + + printf("-- Reading GPS measurements from file\n"); + { + ifstream GPS_data(GPS_data_file.c_str()); + getline(GPS_data, line, '\n'); // ignore the first line + + double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; + while (!GPS_data.eof()) { + getline(GPS_data, line, '\n'); + sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); + + gpsMeasurement_t measurement; + measurement.Time = time; + measurement.Position = Vector3(gps_x, gps_y, gps_z); + GPS_measurements.push_back(measurement); + } + } + + // Configure different variables + double tOffset = GPS_measurements[0].Time; + size_t firstGPSPose = 1; + size_t GPSskip = 10; // Skip this many GPS measurements each time + double g = 9.8; + auto w_coriolis = Vector3(); // zero vector + + // Configure noise models + noiseModel::Diagonal::shared_ptr noiseModelGPS = noiseModel::Diagonal::Precisions((Vector(6) << Vector3::Constant(0), Vector3::Constant(1.0/0.07)).finished()); + + // Set initial conditions for the estimated trajectory + auto currentPoseGlobal = Pose3(Rot3(), GPS_measurements[firstGPSPose].Position); // initial pose is the reference frame (navigation frame) + auto currentVelocityGlobal = Vector3(); // the vehicle is stationary at the beginning at position 0,0,0 + auto currentBias = imuBias::ConstantBias(); // init with zero bias + + noiseModel::Diagonal::shared_ptr sigma_init_x = noiseModel::Isotropic::Precisions((Vector(6) << Vector3::Constant(0), Vector3::Constant(1.0)).finished()); + noiseModel::Diagonal::shared_ptr sigma_init_v = noiseModel::Isotropic::Sigma(3, 1000.0); + noiseModel::Diagonal::shared_ptr sigma_init_b = noiseModel::Isotropic::Sigmas((Vector(6) << Vector3::Constant(0.100), Vector3::Constant(5.00e-05)).finished()); + + // Set IMU preintegration parameters + Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(AccelerometerSigma,2); + Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(GyroscopeSigma,2); + Matrix33 integration_error_cov = Matrix33::Identity(3,3) * pow(IntegrationSigma,2); // error committed in integrating position from velocities + + boost::shared_ptr 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->omegaCoriolis = w_coriolis; + + std::shared_ptr currentSummarizedMeasurement = nullptr; + + // Set ISAM2 parameters and create ISAM2 solver object + ISAM2Params isamParams; + isamParams.factorization = ISAM2Params::CHOLESKY; + isamParams.relinearizeSkip = 10; + + ISAM2 isam(isamParams); + + // Create the factor graph and values object that will store new factors and values to add to the incremental graph + NonlinearFactorGraph newFactors; + Values newValues; // 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"); + size_t imuMeasurementIndex = 0; + for (size_t gpsMeasurementIndex = firstGPSPose; gpsMeasurementIndex < GPS_measurements.size() - 1; gpsMeasurementIndex++) { + // At each non=IMU measurement we initialize a new node in the graph + auto currentPoseKey = X(gpsMeasurementIndex); + auto currentVelKey = V(gpsMeasurementIndex); + auto currentBiasKey = B(gpsMeasurementIndex); + double t = GPS_measurements[gpsMeasurementIndex].Time; + + if (gpsMeasurementIndex == firstGPSPose) { + // Create initial estimate and prior on initial pose, velocity, and biases + newValues.insert(currentPoseKey, currentPoseGlobal); + newValues.insert(currentVelKey, currentVelocityGlobal); + newValues.insert(currentBiasKey, currentBias); + newFactors.add(PriorFactor(currentPoseKey, currentPoseGlobal, sigma_init_x)); + newFactors.add(PriorFactor(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactor(currentBiasKey, currentBias, sigma_init_b)); + } else { + double t_previous = GPS_measurements[gpsMeasurementIndex-1].Time; + + // Summarize IMU data between the previous GPS measurement and now + currentSummarizedMeasurement = std::make_shared(IMU_params, currentBias); + static size_t includedIMUmeasurementCount = 0; + while (imuMeasurementIndex < IMU_measurements.size() && IMU_measurements[imuMeasurementIndex].Time <= t) { + if (IMU_measurements[imuMeasurementIndex].Time >= t_previous) { + currentSummarizedMeasurement->integrateMeasurement(IMU_measurements[imuMeasurementIndex].Accelerometer, IMU_measurements[imuMeasurementIndex].Gyroscope, IMU_measurements[imuMeasurementIndex].dt); + includedIMUmeasurementCount++; + } + imuMeasurementIndex++; + } + + // Create IMU factor + auto previousPoseKey = X(gpsMeasurementIndex-1); + auto previousVelKey = V(gpsMeasurementIndex-1); + auto previousBiasKey = B(gpsMeasurementIndex-1); + + newFactors.add(ImuFactor( + previousPoseKey, previousVelKey, + currentPoseKey, currentVelKey, + previousBiasKey, *currentSummarizedMeasurement)); + + // Bias evolution as given in the IMU metadata + noiseModel::Diagonal::shared_ptr sigma_between_b = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(sqrt(includedIMUmeasurementCount) * AccelerometerBiasSigma), Vector3::Constant(sqrt(includedIMUmeasurementCount) * GyroscopeBiasSigma)).finished()); + newFactors.add(BetweenFactor(previousBiasKey, currentBiasKey, imuBias::ConstantBias(), sigma_between_b)); + + // Create GPS factor + auto GPSPose = Pose3(currentPoseGlobal.rotation(), GPS_measurements[gpsMeasurementIndex].Position); + if ((gpsMeasurementIndex % GPSskip) == 0) { + newFactors.add(PriorFactor(currentPoseKey, GPSPose, noiseModelGPS)); + newValues.insert(currentPoseKey, GPSPose); + + printf("################ POSE INCLUDED AT TIME %lf ################\n", t); + GPSPose.translation().print(); + printf("\n\n"); + } else { + newValues.insert(currentPoseKey, currentPoseGlobal); + } + + // Add initial values for velocity and bias based on the previous estimates + newValues.insert(currentVelKey, currentVelocityGlobal); + newValues.insert(currentBiasKey, currentBias); + + // Update solver + // ======================================================================= + // We accumulate 2*GPSskip GPS measurements before updating the solver at + //first so that the heading becomes observable. + if (gpsMeasurementIndex > (firstGPSPose + 2*GPSskip)) { + printf("################ NEW FACTORS AT TIME %lf ################\n", t); + newFactors.print(); + + isam.update(newFactors, newValues); + + // Reset the newFactors and newValues list + newFactors.resize(0); + newValues.clear(); + + // Extract the result/current estimates + Values result = isam.calculateEstimate(); + + currentPoseGlobal = result.at(currentPoseKey); + currentVelocityGlobal = result.at(currentVelKey); + currentBias = result.at(currentBiasKey); + + printf("\n################ POSE AT TIME %lf ################\n", t); + currentPoseGlobal.print(); + printf("\n\n"); + } + } + } + + // 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"); + + Values result = isam.calculateEstimate(); + for (size_t gpsMeasurementIndex = firstGPSPose; gpsMeasurementIndex < GPS_measurements.size() - 1; gpsMeasurementIndex++) { + auto poseKey = X(gpsMeasurementIndex); + auto velKey = V(gpsMeasurementIndex); + auto biasKey = B(gpsMeasurementIndex); + + auto pose = result.at(poseKey); + auto velocity = result.at(velKey); + auto bias = result.at(biasKey); + + auto pose_quat = pose.rotation().toQuaternion(); + auto gps = GPS_measurements[gpsMeasurementIndex].Position; + + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + GPS_measurements[gpsMeasurementIndex].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); +} \ No newline at end of file From e3712772cbdfa2f0e851e80241d46236c901c7b9 Mon Sep 17 00:00:00 2001 From: Thomas Jespersen Date: Wed, 8 Jul 2020 00:05:38 +0800 Subject: [PATCH 2/2] ISAM2 Kitti example: Addressed review comments --- examples/IMUKittiExampleGPS.cpp | 380 ++++++++++++++++++-------------- 1 file changed, 220 insertions(+), 160 deletions(-) diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp index 9f302ab2f..7cfccbc11 100644 --- a/examples/IMUKittiExampleGPS.cpp +++ b/examples/IMUKittiExampleGPS.cpp @@ -24,241 +24,301 @@ #include #include #include -#include #include #include + #include #include #include -using namespace gtsam; 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::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) -typedef struct { - double Time; +struct KittiCalibration { + double body_ptx; + double body_pty; + double body_ptz; + double body_prx; + double body_pry; + double body_prz; + double accelerometer_sigma; + double gyroscope_sigma; + double integration_sigma; + double accelerometer_bias_sigma; + double gyroscope_bias_sigma; + double average_delta_t; +}; + +struct ImuMeasurement { + double time; double dt; - Vector3 Accelerometer; - Vector3 Gyroscope; // omega -} imuMeasurement_t; + Vector3 accelerometer; + Vector3 gyroscope; // omega +}; -typedef struct { - double Time; - Vector3 Position; // x,y,z -} gpsMeasurement_t; +struct GpsMeasurement { + double time; + Vector3 position; // x,y,z +}; -const string output_filename = "IMUKittyExampleGPSResults.csv"; +const string output_filename = "IMUKittiExampleGPSResults.csv"; -int main(int argc, char* argv[]) -{ +void loadKittiData(KittiCalibration& kitti_calibration, + vector& imu_measurements, + vector& gps_measurements) { string line; // Read IMU metadata and compute relative sensor pose transforms - // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT - // 0 0 0 0 0 0 0.01 0.000175 0 0.000167 2.91e-006 0.0100395199348279 - string IMU_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); - ifstream IMU_metadata(IMU_metadata_file.c_str()); + // 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"); - getline(IMU_metadata, line, '\n'); // ignore the first line + getline(imu_metadata, line, '\n'); // ignore the first line - double BodyPtx, BodyPty, BodyPtz, BodyPrx, BodyPry, BodyPrz, AccelerometerSigma, GyroscopeSigma, IntegrationSigma, AccelerometerBiasSigma, GyroscopeBiasSigma, AverageDeltaT; - getline(IMU_metadata, line, '\n'); - sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", &BodyPtx, &BodyPty, &BodyPtz, &BodyPrx, &BodyPry, &BodyPrz, &AccelerometerSigma, &GyroscopeSigma, &IntegrationSigma, &AccelerometerBiasSigma, &GyroscopeBiasSigma, &AverageDeltaT); - printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", BodyPtx, BodyPty, BodyPtz, BodyPrx, BodyPry, BodyPrz, AccelerometerSigma, GyroscopeSigma, IntegrationSigma, AccelerometerBiasSigma, GyroscopeBiasSigma, AverageDeltaT); + // 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.accelerometer_sigma, + &kitti_calibration.gyroscope_sigma, + &kitti_calibration.integration_sigma, + &kitti_calibration.accelerometer_bias_sigma, + &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.accelerometer_sigma, + kitti_calibration.gyroscope_sigma, + kitti_calibration.integration_sigma, + kitti_calibration.accelerometer_bias_sigma, + kitti_calibration.gyroscope_bias_sigma, + kitti_calibration.average_delta_t); - Vector6 BodyP = (Vector(6) << BodyPtx, BodyPty, BodyPtz, BodyPrx, BodyPry, BodyPrz).finished(); + // Read IMU data + // Time dt accelX accelY accelZ omegaX omegaY omegaZ + string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); + printf("-- Reading IMU measurements from file\n"); + { + 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; + 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); + + ImuMeasurement measurement; + measurement.time = time; + measurement.dt = dt; + measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); + measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); + imu_measurements.push_back(measurement); + } + } + + // Read GPS data + // Time,X,Y,Z + string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); + printf("-- Reading GPS measurements from file\n"); + { + ifstream gps_data(gps_data_file.c_str()); + getline(gps_data, line, '\n'); // ignore the first line + + double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; + while (!gps_data.eof()) { + getline(gps_data, line, '\n'); + sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); + + GpsMeasurement measurement; + measurement.time = time; + measurement.position = Vector3(gps_x, gps_y, gps_z); + gps_measurements.push_back(measurement); + } + } +} + +int main(int argc, char* argv[]) { + KittiCalibration kitti_calibration; + vector imu_measurements; + vector gps_measurements; + loadKittiData(kitti_calibration, imu_measurements, gps_measurements); + + Vector6 BodyP = (Vector(6) << 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"); exit(-1); } - // Read IMU data - // Time dt accelX accelY accelZ omegaX omegaY omegaZ - // 46534.47837579 46534.47837579 1.7114864219577 0.1717911743144 9.80533438749 -0.0032006241515747 0.031231284764596 -0.0063569265706488 - vector IMU_measurements; - string IMU_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); - - printf("-- Reading IMU measurements from file\n"); - { - 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; - 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); - - imuMeasurement_t measurement; - measurement.Time = time; - measurement.dt = dt; - measurement.Accelerometer = Vector3(acc_x, acc_y, acc_z); - measurement.Gyroscope = Vector3(gyro_x, gyro_y, gyro_z); - IMU_measurements.push_back(measurement); - } - } - - // Read GPS data - // Time,X,Y,Z - // 46534.478375790000428,-6.8269361350059405424,-11.868164241239471224,0.040306091310000624617 - vector GPS_measurements; - string GPS_data_file = findExampleDataFile("KittiGps_converted.txt"); - - printf("-- Reading GPS measurements from file\n"); - { - ifstream GPS_data(GPS_data_file.c_str()); - getline(GPS_data, line, '\n'); // ignore the first line - - double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; - while (!GPS_data.eof()) { - getline(GPS_data, line, '\n'); - sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); - - gpsMeasurement_t measurement; - measurement.Time = time; - measurement.Position = Vector3(gps_x, gps_y, gps_z); - GPS_measurements.push_back(measurement); - } - } - // Configure different variables - double tOffset = GPS_measurements[0].Time; - size_t firstGPSPose = 1; - size_t GPSskip = 10; // Skip this many GPS measurements each time + double t_offset = gps_measurements[0].time; + size_t first_gps_pose = 1; + size_t gps_skip = 10; // Skip this many GPS measurements each time double g = 9.8; - auto w_coriolis = Vector3(); // zero vector + auto w_coriolis = Vector3(); // zero vector // Configure noise models - noiseModel::Diagonal::shared_ptr noiseModelGPS = noiseModel::Diagonal::Precisions((Vector(6) << Vector3::Constant(0), Vector3::Constant(1.0/0.07)).finished()); + auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector(6) << Vector3::Constant(0), + Vector3::Constant(1.0/0.07)) + .finished()); // Set initial conditions for the estimated trajectory - auto currentPoseGlobal = Pose3(Rot3(), GPS_measurements[firstGPSPose].Position); // initial pose is the reference frame (navigation frame) - auto currentVelocityGlobal = Vector3(); // the vehicle is stationary at the beginning at position 0,0,0 - auto currentBias = imuBias::ConstantBias(); // init with zero bias + // initial pose is the reference frame (navigation frame) + auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position); + auto current_velocity_global = Vector3(); // the vehicle is stationary at the beginning at position 0,0,0 + auto current_bias = imuBias::ConstantBias(); // init with zero bias - noiseModel::Diagonal::shared_ptr sigma_init_x = noiseModel::Isotropic::Precisions((Vector(6) << Vector3::Constant(0), Vector3::Constant(1.0)).finished()); - noiseModel::Diagonal::shared_ptr sigma_init_v = noiseModel::Isotropic::Sigma(3, 1000.0); - noiseModel::Diagonal::shared_ptr sigma_init_b = noiseModel::Isotropic::Sigmas((Vector(6) << Vector3::Constant(0.100), Vector3::Constant(5.00e-05)).finished()); + auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector(6) << 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((Vector(6) << Vector3::Constant(0.100), + Vector3::Constant(5.00e-05)) + .finished()); // Set IMU preintegration parameters - Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(AccelerometerSigma,2); - Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(GyroscopeSigma,2); - Matrix33 integration_error_cov = Matrix33::Identity(3,3) * pow(IntegrationSigma,2); // error committed in integrating position from velocities + 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); - boost::shared_ptr 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->omegaCoriolis = w_coriolis; + 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->omegaCoriolis = w_coriolis; - std::shared_ptr currentSummarizedMeasurement = nullptr; + std::shared_ptr current_summarized_measurement = nullptr; // Set ISAM2 parameters and create ISAM2 solver object - ISAM2Params isamParams; - isamParams.factorization = ISAM2Params::CHOLESKY; - isamParams.relinearizeSkip = 10; + ISAM2Params isam_params; + isam_params.factorization = ISAM2Params::CHOLESKY; + isam_params.relinearizeSkip = 10; - ISAM2 isam(isamParams); + ISAM2 isam(isam_params); // Create the factor graph and values object that will store new factors and values to add to the incremental graph - NonlinearFactorGraph newFactors; - Values newValues; // values storing the initial estimates of new nodes in the factor graph + NonlinearFactorGraph new_factors; + 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"); - size_t imuMeasurementIndex = 0; - for (size_t gpsMeasurementIndex = firstGPSPose; gpsMeasurementIndex < GPS_measurements.size() - 1; gpsMeasurementIndex++) { + size_t j = 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 currentPoseKey = X(gpsMeasurementIndex); - auto currentVelKey = V(gpsMeasurementIndex); - auto currentBiasKey = B(gpsMeasurementIndex); - double t = GPS_measurements[gpsMeasurementIndex].Time; + auto current_pose_key = X(i); + auto current_vel_key = V(i); + auto current_bias_key = B(i); + double t = gps_measurements[i].time; - if (gpsMeasurementIndex == firstGPSPose) { + if (i == first_gps_pose) { // Create initial estimate and prior on initial pose, velocity, and biases - newValues.insert(currentPoseKey, currentPoseGlobal); - newValues.insert(currentVelKey, currentVelocityGlobal); - newValues.insert(currentBiasKey, currentBias); - newFactors.add(PriorFactor(currentPoseKey, currentPoseGlobal, sigma_init_x)); - newFactors.add(PriorFactor(currentVelKey, currentVelocityGlobal, sigma_init_v)); - newFactors.add(PriorFactor(currentBiasKey, currentBias, sigma_init_b)); + 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>(current_pose_key, current_pose_global, sigma_init_x); + new_factors.emplace_shared>(current_vel_key, current_velocity_global, sigma_init_v); + new_factors.emplace_shared>(current_bias_key, current_bias, sigma_init_b); } else { - double t_previous = GPS_measurements[gpsMeasurementIndex-1].Time; + double t_previous = gps_measurements[i-1].time; // Summarize IMU data between the previous GPS measurement and now - currentSummarizedMeasurement = std::make_shared(IMU_params, currentBias); - static size_t includedIMUmeasurementCount = 0; - while (imuMeasurementIndex < IMU_measurements.size() && IMU_measurements[imuMeasurementIndex].Time <= t) { - if (IMU_measurements[imuMeasurementIndex].Time >= t_previous) { - currentSummarizedMeasurement->integrateMeasurement(IMU_measurements[imuMeasurementIndex].Accelerometer, IMU_measurements[imuMeasurementIndex].Gyroscope, IMU_measurements[imuMeasurementIndex].dt); - includedIMUmeasurementCount++; + current_summarized_measurement = std::make_shared(imu_params, current_bias); + static size_t included_imu_measurement_count = 0; + 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, + imu_measurements[j].dt); + included_imu_measurement_count++; } - imuMeasurementIndex++; + j++; } // Create IMU factor - auto previousPoseKey = X(gpsMeasurementIndex-1); - auto previousVelKey = V(gpsMeasurementIndex-1); - auto previousBiasKey = B(gpsMeasurementIndex-1); + auto previous_pose_key = X(i-1); + auto previous_vel_key = V(i-1); + auto previous_bias_key = B(i-1); - newFactors.add(ImuFactor( - previousPoseKey, previousVelKey, - currentPoseKey, currentVelKey, - previousBiasKey, *currentSummarizedMeasurement)); + new_factors.emplace_shared(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 - noiseModel::Diagonal::shared_ptr sigma_between_b = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(sqrt(includedIMUmeasurementCount) * AccelerometerBiasSigma), Vector3::Constant(sqrt(includedIMUmeasurementCount) * GyroscopeBiasSigma)).finished()); - newFactors.add(BetweenFactor(previousBiasKey, currentBiasKey, imuBias::ConstantBias(), sigma_between_b)); + auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector(6) << + 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>(previous_bias_key, + current_bias_key, + imuBias::ConstantBias(), + sigma_between_b); // Create GPS factor - auto GPSPose = Pose3(currentPoseGlobal.rotation(), GPS_measurements[gpsMeasurementIndex].Position); - if ((gpsMeasurementIndex % GPSskip) == 0) { - newFactors.add(PriorFactor(currentPoseKey, GPSPose, noiseModelGPS)); - newValues.insert(currentPoseKey, GPSPose); + auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position); + if ((i % gps_skip) == 0) { + new_factors.emplace_shared>(current_pose_key, gps_pose, noise_model_gps); + new_values.insert(current_pose_key, gps_pose); printf("################ POSE INCLUDED AT TIME %lf ################\n", t); - GPSPose.translation().print(); + gps_pose.translation().print(); printf("\n\n"); } else { - newValues.insert(currentPoseKey, currentPoseGlobal); + new_values.insert(current_pose_key, current_pose_global); } // Add initial values for velocity and bias based on the previous estimates - newValues.insert(currentVelKey, currentVelocityGlobal); - newValues.insert(currentBiasKey, currentBias); + 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 (gpsMeasurementIndex > (firstGPSPose + 2*GPSskip)) { + // first so that the heading becomes observable. + if (i > (first_gps_pose + 2*gps_skip)) { printf("################ NEW FACTORS AT TIME %lf ################\n", t); - newFactors.print(); + new_factors.print(); - isam.update(newFactors, newValues); + isam.update(new_factors, new_values); // Reset the newFactors and newValues list - newFactors.resize(0); - newValues.clear(); + new_factors.resize(0); + new_values.clear(); // Extract the result/current estimates Values result = isam.calculateEstimate(); - currentPoseGlobal = result.at(currentPoseKey); - currentVelocityGlobal = result.at(currentVelKey); - currentBias = result.at(currentBiasKey); + current_pose_global = result.at(current_pose_key); + current_velocity_global = result.at(current_vel_key); + current_bias = result.at(current_bias_key); printf("\n################ POSE AT TIME %lf ################\n", t); - currentPoseGlobal.print(); + current_pose_global.print(); printf("\n\n"); } } @@ -270,24 +330,24 @@ int main(int argc, char* argv[]) 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 gpsMeasurementIndex = firstGPSPose; gpsMeasurementIndex < GPS_measurements.size() - 1; gpsMeasurementIndex++) { - auto poseKey = X(gpsMeasurementIndex); - auto velKey = V(gpsMeasurementIndex); - auto biasKey = B(gpsMeasurementIndex); + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { + auto pose_key = X(i); + auto vel_key = V(i); + auto bias_key = B(i); - auto pose = result.at(poseKey); - auto velocity = result.at(velKey); - auto bias = result.at(biasKey); + auto pose = result.at(pose_key); + auto velocity = result.at(vel_key); + auto bias = result.at(bias_key); auto pose_quat = pose.rotation().toQuaternion(); - auto gps = GPS_measurements[gpsMeasurementIndex].Position; + auto gps = gps_measurements[i].position; fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", - GPS_measurements[gpsMeasurementIndex].Time, + 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); -} \ No newline at end of file +}