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

only format c++ file (no code changes)

release/4.3a0
Varun Agrawal 2021-08-20 13:33:57 -04:00
parent f6a432961a
commit a4a58cf803
1 changed files with 297 additions and 274 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

571
examples/IMUKittiExampleGPS.cpp
View File

@ -11,21 +11,23 @@
/** /**
* @file IMUKittiExampleGPS * @file IMUKittiExampleGPS
* @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI
* @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics * VISION BENCHMARK SUITE
* @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ
* Electronics
*/ */
// GTSAM related includes. // GTSAM related includes.
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h> #include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.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/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Params.h> #include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.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 <cstring>
#include <fstream> #include <fstream>
@ -34,35 +36,35 @@
using namespace std; using namespace std;
using namespace gtsam; 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::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 { struct KittiCalibration {
double body_ptx; double body_ptx;
double body_pty; double body_pty;
double body_ptz; double body_ptz;
double body_prx; double body_prx;
double body_pry; double body_pry;
double body_prz; double body_prz;
double accelerometer_sigma; double accelerometer_sigma;
double gyroscope_sigma; double gyroscope_sigma;
double integration_sigma; double integration_sigma;
double accelerometer_bias_sigma; double accelerometer_bias_sigma;
double gyroscope_bias_sigma; double gyroscope_bias_sigma;
double average_delta_t; double average_delta_t;
}; };
struct ImuMeasurement { struct ImuMeasurement {
double time; double time;
double dt; double dt;
Vector3 accelerometer; Vector3 accelerometer;
Vector3 gyroscope; // omega Vector3 gyroscope; // omega
}; };
struct GpsMeasurement { struct GpsMeasurement {
double time; double time;
Vector3 position; // x,y,z Vector3 position; // x,y,z
}; };
const string output_filename = "IMUKittiExampleGPSResults.csv"; const string output_filename = "IMUKittiExampleGPSResults.csv";
@ -70,290 +72,311 @@ const string output_filename = "IMUKittiExampleGPSResults.csv";
void loadKittiData(KittiCalibration& kitti_calibration, void loadKittiData(KittiCalibration& kitti_calibration,
vector<ImuMeasurement>& imu_measurements, vector<ImuMeasurement>& imu_measurements,
vector<GpsMeasurement>& gps_measurements) { vector<GpsMeasurement>& gps_measurements) {
string line; string line;
// Read IMU metadata and compute relative sensor pose transforms // Read IMU metadata and compute relative sensor pose transforms
// BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma
// AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT // GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma
string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); // AverageDeltaT
ifstream imu_metadata(imu_metadata_file.c_str()); string imu_metadata_file =
findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
ifstream imu_metadata(imu_metadata_file.c_str());
printf("-- Reading sensor metadata\n"); printf("-- Reading sensor metadata\n");
getline(imu_metadata, line, '\n'); // ignore the first line getline(imu_metadata, line, '\n'); // ignore the first line
// Load Kitti calibration // Load Kitti calibration
getline(imu_metadata, line, '\n'); getline(imu_metadata, line, '\n');
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
&kitti_calibration.body_ptx, &kitti_calibration.body_ptx, &kitti_calibration.body_pty,
&kitti_calibration.body_pty, &kitti_calibration.body_ptz, &kitti_calibration.body_prx,
&kitti_calibration.body_ptz, &kitti_calibration.body_pry, &kitti_calibration.body_prz,
&kitti_calibration.body_prx, &kitti_calibration.accelerometer_sigma,
&kitti_calibration.body_pry, &kitti_calibration.gyroscope_sigma,
&kitti_calibration.body_prz, &kitti_calibration.integration_sigma,
&kitti_calibration.accelerometer_sigma, &kitti_calibration.accelerometer_bias_sigma,
&kitti_calibration.gyroscope_sigma, &kitti_calibration.gyroscope_bias_sigma,
&kitti_calibration.integration_sigma, &kitti_calibration.average_delta_t);
&kitti_calibration.accelerometer_bias_sigma, printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
&kitti_calibration.gyroscope_bias_sigma, kitti_calibration.body_ptx, kitti_calibration.body_pty,
&kitti_calibration.average_delta_t); kitti_calibration.body_ptz, kitti_calibration.body_prx,
printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", kitti_calibration.body_pry, kitti_calibration.body_prz,
kitti_calibration.body_ptx, kitti_calibration.accelerometer_sigma,
kitti_calibration.body_pty, kitti_calibration.gyroscope_sigma, kitti_calibration.integration_sigma,
kitti_calibration.body_ptz, kitti_calibration.accelerometer_bias_sigma,
kitti_calibration.body_prx, kitti_calibration.gyroscope_bias_sigma,
kitti_calibration.body_pry, kitti_calibration.average_delta_t);
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);
// Read IMU data // Read IMU data
// Time dt accelX accelY accelZ omegaX omegaY omegaZ // Time dt accelX accelY accelZ omegaX omegaY omegaZ
string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt");
printf("-- Reading IMU measurements from file\n"); printf("-- Reading IMU measurements from file\n");
{ {
ifstream imu_data(imu_data_file.c_str()); ifstream imu_data(imu_data_file.c_str());
getline(imu_data, line, '\n'); // ignore the first line 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,
while (!imu_data.eof()) { gyro_y = 0, gyro_z = 0;
getline(imu_data, line, '\n'); while (!imu_data.eof()) {
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", getline(imu_data, line, '\n');
&time, &dt, sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt,
&acc_x, &acc_y, &acc_z, &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z);
&gyro_x, &gyro_y, &gyro_z);
ImuMeasurement measurement; ImuMeasurement measurement;
measurement.time = time; measurement.time = time;
measurement.dt = dt; measurement.dt = dt;
measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); measurement.accelerometer = Vector3(acc_x, acc_y, acc_z);
measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z);
imu_measurements.push_back(measurement); imu_measurements.push_back(measurement);
}
} }
}
// Read GPS data // Read GPS data
// Time,X,Y,Z // Time,X,Y,Z
string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); string gps_data_file = findExampleDataFile("KittiGps_converted.txt");
printf("-- Reading GPS measurements from file\n"); printf("-- Reading GPS measurements from file\n");
{ {
ifstream gps_data(gps_data_file.c_str()); ifstream gps_data(gps_data_file.c_str());
getline(gps_data, line, '\n'); // ignore the first line getline(gps_data, line, '\n'); // ignore the first line
double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; double time = 0, gps_x = 0, gps_y = 0, gps_z = 0;
while (!gps_data.eof()) { while (!gps_data.eof()) {
getline(gps_data, line, '\n'); getline(gps_data, line, '\n');
sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
GpsMeasurement measurement; GpsMeasurement measurement;
measurement.time = time; measurement.time = time;
measurement.position = Vector3(gps_x, gps_y, gps_z); measurement.position = Vector3(gps_x, gps_y, gps_z);
gps_measurements.push_back(measurement); gps_measurements.push_back(measurement);
}
} }
}
} }
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
KittiCalibration kitti_calibration; KittiCalibration kitti_calibration;
vector<ImuMeasurement> imu_measurements; vector<ImuMeasurement> imu_measurements;
vector<GpsMeasurement> gps_measurements; vector<GpsMeasurement> gps_measurements;
loadKittiData(kitti_calibration, imu_measurements, gps_measurements); loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz, Vector6 BodyP =
kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz) (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty,
.finished(); kitti_calibration.body_ptz, kitti_calibration.body_prx,
auto body_T_imu = Pose3::Expmap(BodyP); kitti_calibration.body_pry, kitti_calibration.body_prz)
if (!body_T_imu.equals(Pose3(), 1e-5)) { .finished();
printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same"); auto body_T_imu = Pose3::Expmap(BodyP);
exit(-1); 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);
}
// Configure different variables // Configure different variables
// double t_offset = gps_measurements[0].time; // double t_offset = gps_measurements[0].time;
size_t first_gps_pose = 1; size_t first_gps_pose = 1;
size_t gps_skip = 10; // Skip this many GPS measurements each time size_t gps_skip = 10; // Skip this many GPS measurements each time
double g = 9.8; double g = 9.8;
auto w_coriolis = Vector3::Zero(); // zero vector auto w_coriolis = Vector3::Zero(); // zero vector
// Configure noise models // Configure noise models
auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), auto noise_model_gps = noiseModel::Diagonal::Precisions(
Vector3::Constant(1.0/0.07)) (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07))
.finished()); .finished());
// Set initial conditions for the estimated trajectory // Set initial conditions for the estimated trajectory
// initial pose is the reference frame (navigation frame) // initial pose is the reference frame (navigation frame)
auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position); auto current_pose_global =
// the vehicle is stationary at the beginning at position 0,0,0 Pose3(Rot3(), gps_measurements[first_gps_pose].position);
Vector3 current_velocity_global = Vector3::Zero(); // the vehicle is stationary at the beginning at position 0,0,0
auto current_bias = imuBias::ConstantBias(); // init with zero bias 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), auto sigma_init_x = noiseModel::Diagonal::Precisions(
Vector3::Constant(1.0)) (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished());
.finished()); auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0)); auto sigma_init_b = noiseModel::Diagonal::Sigmas(
auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100), (Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05))
Vector3::Constant(5.00e-05)) .finished());
.finished());
// Set IMU preintegration parameters // Set IMU preintegration parameters
Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2); Matrix33 measured_acc_cov =
Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2); I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
// error committed in integrating position from velocities Matrix33 measured_omega_cov =
Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2); 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);
auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g); auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
imu_params->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous imu_params->accelerometerCovariance =
imu_params->integrationCovariance = integration_error_cov; // integration uncertainty continuous measured_acc_cov; // acc white noise in continuous
imu_params->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous imu_params->integrationCovariance =
imu_params->omegaCoriolis = w_coriolis; 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 // Set ISAM2 parameters and create ISAM2 solver object
ISAM2Params isam_params; ISAM2Params isam_params;
isam_params.factorization = ISAM2Params::CHOLESKY; isam_params.factorization = ISAM2Params::CHOLESKY;
isam_params.relinearizeSkip = 10; isam_params.relinearizeSkip = 10;
ISAM2 isam(isam_params); 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
NonlinearFactorGraph new_factors; // values to add to the incremental graph
Values new_values; // 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: /// Main loop:
/// (1) we read the measurements /// (1) we read the measurements
/// (2) we create the corresponding factors in the graph /// (2) we create the corresponding factors in the graph
/// (3) we solve the graph to obtain and optimal estimate of robot trajectory /// (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(
size_t j = 0; "-- Starting main loop: inference is performed at each time step, but we "
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { "plot trajectory every 10 steps\n");
// At each non=IMU measurement we initialize a new node in the graph size_t j = 0;
auto current_pose_key = X(i); for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
auto current_vel_key = V(i); // At each non=IMU measurement we initialize a new node in the graph
auto current_bias_key = B(i); auto current_pose_key = X(i);
double t = gps_measurements[i].time; auto current_vel_key = V(i);
auto current_bias_key = B(i);
double t = gps_measurements[i].time;
if (i == first_gps_pose) { if (i == first_gps_pose) {
// Create initial estimate and prior on initial pose, velocity, and biases // Create initial estimate and prior on initial pose, velocity, and biases
new_values.insert(current_pose_key, current_pose_global); new_values.insert(current_pose_key, current_pose_global);
new_values.insert(current_vel_key, current_velocity_global); new_values.insert(current_vel_key, current_velocity_global);
new_values.insert(current_bias_key, current_bias); 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<Pose3>>(
new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v); current_pose_key, current_pose_global, sigma_init_x);
new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b); new_factors.emplace_shared<PriorFactor<Vector3>>(
} else { current_vel_key, current_velocity_global, sigma_init_v);
double t_previous = gps_measurements[i-1].time; new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(
current_bias_key, current_bias, sigma_init_b);
} else {
double t_previous = gps_measurements[i - 1].time;
// Summarize IMU data between the previous GPS measurement and now // Summarize IMU data between the previous GPS measurement and now
current_summarized_measurement = std::make_shared<PreintegratedImuMeasurements>(imu_params, current_bias); current_summarized_measurement =
static size_t included_imu_measurement_count = 0; std::make_shared<PreintegratedImuMeasurements>(imu_params,
while (j < imu_measurements.size() && imu_measurements[j].time <= t) { current_bias);
if (imu_measurements[j].time >= t_previous) { static size_t included_imu_measurement_count = 0;
current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer, while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
imu_measurements[j].gyroscope, if (imu_measurements[j].time >= t_previous) {
imu_measurements[j].dt); current_summarized_measurement->integrateMeasurement(
included_imu_measurement_count++; imu_measurements[j].accelerometer, imu_measurements[j].gyroscope,
} imu_measurements[j].dt);
j++; included_imu_measurement_count++;
}
// Create IMU factor
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);
// 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))
.finished());
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);
if ((i % gps_skip) == 0) {
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);
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
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)) {
printf("################ NEW FACTORS AT TIME %lf ################\n", t);
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
Values result = isam.calculateEstimate();
current_pose_global = result.at<Pose3>(current_pose_key);
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);
current_pose_global.print();
printf("\n\n");
}
} }
j++;
}
// Create IMU factor
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);
// 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))
.finished());
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);
if ((i % gps_skip) == 0) {
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);
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
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)) {
printf("################ NEW FACTORS AT TIME %lf ################\n",
t);
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
Values result = isam.calculateEstimate();
current_pose_global = result.at<Pose3>(current_pose_key);
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);
current_pose_global.print();
printf("\n\n");
}
} }
}
// Save results to file // Save results to file
printf("\nWriting results to file...\n"); printf("\nWriting results to file...\n");
FILE* fp_out = fopen(output_filename.c_str(), "w+"); 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(); Values result = isam.calculateEstimate();
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
auto pose_key = X(i); auto pose_key = X(i);
auto vel_key = V(i); auto vel_key = V(i);
auto bias_key = B(i); auto bias_key = B(i);
auto pose = result.at<Pose3>(pose_key); auto pose = result.at<Pose3>(pose_key);
auto velocity = result.at<Vector3>(vel_key); auto velocity = result.at<Vector3>(vel_key);
auto bias = result.at<imuBias::ConstantBias>(bias_key); auto bias = result.at<imuBias::ConstantBias>(bias_key);
auto pose_quat = pose.rotation().toQuaternion(); auto pose_quat = pose.rotation().toQuaternion();
auto gps = gps_measurements[i].position; auto gps = gps_measurements[i].position;
cout << "State at #" << i << endl; cout << "State at #" << i << endl;
cout << "Pose:" << endl << pose << endl; cout << "Pose:" << endl << pose << endl;
cout << "Velocity:" << endl << velocity << endl; cout << "Velocity:" << endl << velocity << endl;
cout << "Bias:" << endl << bias << endl; cout << "Bias:" << endl << bias << endl;
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
gps_measurements[i].time, gps_measurements[i].time, pose.x(), pose.y(), pose.z(),
pose.x(), pose.y(), pose.z(), pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0),
pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(1), gps(2));
gps(0), gps(1), gps(2)); }
}
fclose(fp_out); fclose(fp_out);
} }