From ff5a14831b11131526b6eadebc7be4e6a1ede284 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Sat, 9 May 2020 19:08:17 -0400 Subject: [PATCH] Reformatted and refactored --- examples/FisheyeExample.cpp | 113 ++++++++++---------- examples/ImuFactorsExample.cpp | 190 +++++++++++++++++---------------- 2 files changed, 155 insertions(+), 148 deletions(-) diff --git a/examples/FisheyeExample.cpp b/examples/FisheyeExample.cpp index b03c9a71d..223149299 100644 --- a/examples/FisheyeExample.cpp +++ b/examples/FisheyeExample.cpp @@ -12,7 +12,7 @@ /** * @file FisheyeExample.cpp * @brief A visualSLAM example for the structure-from-motion problem on a - * simulated dataset. This version uses a fisheye camera model and a GaussNewton + * simulated dataset. This version uses a fisheye camera model and a GaussNewton * solver to solve the graph in one batch * @author ghaggin * @Date Apr 9,2020 @@ -55,75 +55,76 @@ using namespace std; using namespace gtsam; +using symbol_shorthand::L; // for landmarks +using symbol_shorthand::X; // for poses + /* ************************************************************************* */ -int main(int argc, char *argv[]) -{ - // Define the camera calibration parameters - // Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - boost::shared_ptr K(new Cal3Fisheye( - 278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625)); +int main(int argc, char *argv[]) { + // Define the camera calibration parameters + auto K = boost::make_shared( + 278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035, + 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625); - // Define the camera observation noise model, 1 pixel stddev - auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + // Define the camera observation noise model, 1 pixel stddev + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); - // Create the set of ground-truth landmarks - vector points = createPoints(); + // Create the set of ground-truth landmarks + const vector points = createPoints(); - // Create the set of ground-truth poses - vector poses = createPoses(); + // Create the set of ground-truth poses + const vector poses = createPoses(); - // Create a Factor Graph and Values to hold the new data - NonlinearFactorGraph graph; - Values initialEstimate; + // Create a Factor Graph and Values to hold the new data + NonlinearFactorGraph graph; + Values initialEstimate; - // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw - static auto kPosePrior = noiseModel::Diagonal::Sigmas( - (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); - graph.emplace_shared>(Symbol('x', 0), poses[0], kPosePrior); + // Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z + auto posePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); + graph.emplace_shared>(X(0), poses[0], posePrior); - // Add a prior on landmark l0 - static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared>(Symbol('l', 0), points[0], kPointPrior); + // Add a prior on landmark l0 + auto pointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared>(L(0), points[0], pointPrior); - // Add initial guesses to all observed landmarks - // Intentionally initialize the variables off from the ground truth - static Point3 kDeltaPoint(-0.25, 0.20, 0.15); - for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j] + kDeltaPoint); + // Add initial guesses to all observed landmarks + // Intentionally initialize the variables off from the ground truth + static const Point3 kDeltaPoint(-0.25, 0.20, 0.15); + for (size_t j = 0; j < points.size(); ++j) + initialEstimate.insert(L(j), points[j] + kDeltaPoint); - // Loop over the poses, adding the observations to the graph - for (size_t i = 0; i < poses.size(); ++i) - { - // Add factors for each landmark observation - for (size_t j = 0; j < points.size(); ++j) - { - PinholeCamera camera(poses[i], *K); - Point2 measurement = camera.project(points[j]); - graph.emplace_shared>( - measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); - } - - // Add an initial guess for the current pose - // Intentionally initialize the variables off from the ground truth - static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); - initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); + // Loop over the poses, adding the observations to the graph + for (size_t i = 0; i < poses.size(); ++i) { + // Add factors for each landmark observation + for (size_t j = 0; j < points.size(); ++j) { + PinholeCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + graph.emplace_shared>( + measurement, measurementNoise, X(i), L(j), K); } - GaussNewtonParams params; - params.setVerbosity("TERMINATION"); - params.maxIterations = 10000; + // Add an initial guess for the current pose + // Intentionally initialize the variables off from the ground truth + static const Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), + Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(X(i), poses[i] * kDeltaPose); + } - std::cout << "Optimizing the factor graph" << std::endl; - GaussNewtonOptimizer optimizer(graph, initialEstimate, params); - Values result = optimizer.optimize(); - std::cout << "Optimization complete" << std::endl; + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); + params.maxIterations = 10000; - std::cout << "initial error=" << graph.error(initialEstimate) << std::endl; - std::cout << "final error=" << graph.error(result) << std::endl; + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; - std::ofstream os("examples/vio_batch.dot"); - graph.saveGraph(os, result); + std::cout << "initial error=" << graph.error(initialEstimate) << std::endl; + std::cout << "final error=" << graph.error(result) << std::endl; - return 0; + std::ofstream os("examples/vio_batch.dot"); + graph.saveGraph(os, result); + + return 0; } /* ************************************************************************* */ diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index f9ac31bed..a4707ea46 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -11,12 +11,14 @@ /** * @file imuFactorsExample - * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code. + * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor + * navigation code. * @author Garrett (ghemann@gmail.com), Luca Carlone */ /** - * Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS + * Example of use of the imuFactors (imuFactor and combinedImuFactor) in + * conjunction with GPS * - imuFactor is used by default. You can test combinedImuFactor by * appending a `-c` flag at the end (see below for example command). * - we read IMU and GPS data from a CSV file, with the following format: @@ -26,8 +28,8 @@ * linAccN, linAccE, linAccD, angVelN, angVelE, angVelD * A row starting with "1" is a gps correction formatted with * N, E, D, qX, qY, qZ, qW - * Note that for GPS correction, we're only using the position not the rotation. The - * rotation is provided in the file for ground truth comparison. + * Note that for GPS correction, we're only using the position not the + * rotation. The rotation is provided in the file for ground truth comparison. * * Usage: ./ImuFactorsExample [data_csv_path] [-c] * optional arguments: @@ -36,14 +38,15 @@ */ // GTSAM related includes. +#include #include #include #include -#include -#include #include #include -#include +#include +#include + #include #include #include @@ -51,26 +54,21 @@ 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) +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) -const string output_filename = "imuFactorExampleResults.csv"; -const string use_combined_imu_flag = "-c"; +static const char output_filename[] = "imuFactorExampleResults.csv"; +static const char use_combined_imu_flag[3] = "-c"; -// This will either be PreintegratedImuMeasurements (for ImuFactor) or -// PreintegratedCombinedMeasurements (for CombinedImuFactor). -PreintegrationType *imu_preintegrated_; - -int main(int argc, char* argv[]) -{ +int main(int argc, char* argv[]) { string data_filename; bool use_combined_imu = false; if (argc < 2) { printf("using default CSV file\n"); data_filename = findExampleDataFile("imuAndGPSdata.csv"); - } else if (argc < 3){ - if (strcmp(argv[1], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match + } else if (argc < 3) { + if (strcmp(argv[1], use_combined_imu_flag) == 0) { printf("using CombinedImuFactor\n"); use_combined_imu = true; printf("using default CSV file\n"); @@ -80,15 +78,17 @@ int main(int argc, char* argv[]) } } else { data_filename = argv[1]; - if (strcmp(argv[2], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match + if (strcmp(argv[2], use_combined_imu_flag) == 0) { printf("using CombinedImuFactor\n"); use_combined_imu = true; } } // Set up output file for plotting errors - 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),gt_qx,gt_qy,gt_qz,gt_qw\n"); + FILE* fp_out = fopen(output_filename, "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),gt_qx," + "gt_qy,gt_qz,gt_qw\n"); // Begin parsing the CSV file. Input the first line for initialization. // From there, we'll iterate through the file and we'll preintegrate the IMU @@ -97,9 +97,9 @@ int main(int argc, char* argv[]) string value; // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD) - Eigen::Matrix initial_state = Eigen::Matrix::Zero(); - getline(file, value, ','); // i - for (int i=0; i<9; i++) { + Vector10 initial_state; + getline(file, value, ','); // i + for (int i = 0; i < 9; i++) { getline(file, value, ','); initial_state(i) = atof(value.c_str()); } @@ -107,13 +107,14 @@ int main(int argc, char* argv[]) initial_state(9) = atof(value.c_str()); cout << "initial state:\n" << initial_state.transpose() << "\n\n"; - // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z); + // Assemble initial quaternion through GTSAM constructor + // ::quaternion(w,x,y,z); Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), initial_state(4), initial_state(5)); Point3 prior_point(initial_state.head<3>()); Pose3 prior_pose(prior_rotation, prior_point); Vector3 prior_velocity(initial_state.tail<3>()); - imuBias::ConstantBias prior_imu_bias; // assume zero initial bias + imuBias::ConstantBias prior_imu_bias; // assume zero initial bias Values initial_values; int correction_count = 0; @@ -121,57 +122,64 @@ int main(int argc, char* argv[]) initial_values.insert(V(correction_count), prior_velocity); initial_values.insert(B(correction_count), prior_imu_bias); - // Assemble prior noise model and add it the graph. - noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m - noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s - noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3); + // Assemble prior noise model and add it the graph.` + auto pose_noise_model = noiseModel::Diagonal::Sigmas( + (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5) + .finished()); // rad,rad,rad,m, m, m + auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1); // m/s + auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3); // Add all prior factors (pose, velocity, bias) to the graph. - NonlinearFactorGraph *graph = new NonlinearFactorGraph(); + NonlinearFactorGraph* graph = new NonlinearFactorGraph(); graph->addPrior(X(correction_count), prior_pose, pose_noise_model); - graph->addPrior(V(correction_count), prior_velocity,velocity_noise_model); - graph->addPrior(B(correction_count), prior_imu_bias,bias_noise_model); + graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model); + graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model); // We use the sensor specs to build the noise model for the IMU factor. double accel_noise_sigma = 0.0003924; double gyro_noise_sigma = 0.000205689024915; double accel_bias_rw_sigma = 0.004905; double gyro_bias_rw_sigma = 0.000001454441043; - Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2); - Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2); - Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities - Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2); - Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2); - Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration + Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); + Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); + Matrix33 integration_error_cov = + I_3x3 * 1e-8; // error committed in integrating position from velocities + Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); + Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); + Matrix66 bias_acc_omega_int = + I_6x6 * 1e-5; // error in the bias used for preintegration - boost::shared_ptr p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); // PreintegrationBase params: - p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous - p->integrationCovariance = integration_error_cov; // integration uncertainty continuous + p->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + p->integrationCovariance = + integration_error_cov; // integration uncertainty continuous // should be using 2nd order integration // PreintegratedRotation params: - p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous + p->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous // PreintegrationCombinedMeasurements params: - p->biasAccCovariance = bias_acc_cov; // acc bias in continuous - p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous + p->biasAccCovariance = bias_acc_cov; // acc bias in continuous + p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous p->biasAccOmegaInt = bias_acc_omega_int; - std::shared_ptr imu_preintegrated_ = nullptr; + std::shared_ptr preintegrated = nullptr; if (use_combined_imu) { - imu_preintegrated_ = + preintegrated = std::make_shared(p, prior_imu_bias); } else { - imu_preintegrated_ = + preintegrated = std::make_shared(p, prior_imu_bias); } - assert(imu_preintegrated_); + assert(preintegrated); - // Store previous state for the imu integration and the latest predicted outcome. + // Store previous state for imu integration and latest predicted outcome. NavState prev_state(prior_pose, prior_velocity); NavState prop_state = prev_state; imuBias::ConstantBias prev_bias = prior_imu_bias; - // Keep track of the total error over the entire run for a simple performance metric. + // Keep track of total error over the entire run as simple performance metric. double current_position_error = 0.0, current_orientation_error = 0.0; double output_time = 0.0; @@ -180,14 +188,13 @@ int main(int argc, char* argv[]) // All priors have been set up, now iterate through the data file. while (file.good()) { - // Parse out first value getline(file, value, ','); int type = atoi(value.c_str()); - if (type == 0) { // IMU measurement - Eigen::Matrix imu = Eigen::Matrix::Zero(); - for (int i=0; i<5; ++i) { + if (type == 0) { // IMU measurement + Vector6 imu; + for (int i = 0; i < 5; ++i) { getline(file, value, ','); imu(i) = atof(value.c_str()); } @@ -195,11 +202,11 @@ int main(int argc, char* argv[]) imu(5) = atof(value.c_str()); // Adding the IMU preintegration. - imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); + preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); - } else if (type == 1) { // GPS measurement - Eigen::Matrix gps = Eigen::Matrix::Zero(); - for (int i=0; i<6; ++i) { + } else if (type == 1) { // GPS measurement + Vector7 gps; + for (int i = 0; i < 6; ++i) { getline(file, value, ','); gps(i) = atof(value.c_str()); } @@ -210,39 +217,37 @@ int main(int argc, char* argv[]) // Adding IMU factor and GPS factor and optimizing. if (use_combined_imu) { - const PreintegratedCombinedMeasurements& preint_imu_combined = + auto preint_imu_combined = dynamic_cast( - *imu_preintegrated_); - CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1), - X(correction_count ), V(correction_count ), - B(correction_count-1), B(correction_count ), - preint_imu_combined); + *preintegrated); + CombinedImuFactor imu_factor( + X(correction_count - 1), V(correction_count - 1), + X(correction_count), V(correction_count), B(correction_count - 1), + B(correction_count), preint_imu_combined); graph->add(imu_factor); } else { - const PreintegratedImuMeasurements& preint_imu = - dynamic_cast( - *imu_preintegrated_); - ImuFactor imu_factor(X(correction_count-1), V(correction_count-1), - X(correction_count ), V(correction_count ), - B(correction_count-1), - preint_imu); + auto preint_imu = + dynamic_cast(*preintegrated); + ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1), + X(correction_count), V(correction_count), + B(correction_count - 1), preint_imu); graph->add(imu_factor); imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - graph->add(BetweenFactor(B(correction_count-1), - B(correction_count ), - zero_bias, bias_noise_model)); + graph->add(BetweenFactor( + B(correction_count - 1), B(correction_count), zero_bias, + bias_noise_model)); } - noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0); + auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0); GPSFactor gps_factor(X(correction_count), - Point3(gps(0), // N, - gps(1), // E, - gps(2)), // D, + Point3(gps(0), // N, + gps(1), // E, + gps(2)), // D, correction_noise); graph->add(gps_factor); // Now optimize and compare results. - prop_state = imu_preintegrated_->predict(prev_state, prev_bias); + prop_state = preintegrated->predict(prev_state, prev_bias); initial_values.insert(X(correction_count), prop_state.pose()); initial_values.insert(V(correction_count), prop_state.v()); initial_values.insert(B(correction_count), prev_bias); @@ -256,7 +261,7 @@ int main(int argc, char* argv[]) prev_bias = result.at(B(correction_count)); // Reset the preintegration object. - imu_preintegrated_->resetIntegrationAndSetBias(prev_bias); + preintegrated->resetIntegrationAndSetBias(prev_bias); // Print out the position and orientation error for comparison. Vector3 gtsam_position = prev_state.pose().translation(); @@ -267,19 +272,19 @@ int main(int argc, char* argv[]) Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5)); Quaternion quat_error = gtsam_quat * gps_quat.inverse(); quat_error.normalize(); - Vector3 euler_angle_error(quat_error.x()*2, - quat_error.y()*2, - quat_error.z()*2); + Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2, + quat_error.z() * 2); current_orientation_error = euler_angle_error.norm(); // display statistics - cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n"; + cout << "Position error:" << current_position_error << "\t " + << "Angular error:" << current_orientation_error << "\n"; fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", - output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2), - gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(), - gps(0), gps(1), gps(2), - gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w()); + output_time, gtsam_position(0), gtsam_position(1), + gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), + gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(), + gps_quat.y(), gps_quat.z(), gps_quat.w()); output_time += 1.0; @@ -289,6 +294,7 @@ int main(int argc, char* argv[]) } } fclose(fp_out); - cout << "Complete, results written to " << output_filename << "\n\n";; + cout << "Complete, results written to " << output_filename << "\n\n"; + return 0; }