diff --git a/examples/Data/ISAM2_SmartFactor_valgrind.txt b/examples/Data/ISAM2_SmartFactorStereo_IMU.txt similarity index 100% rename from examples/Data/ISAM2_SmartFactor_valgrind.txt rename to examples/Data/ISAM2_SmartFactorStereo_IMU.txt diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/examples/ISAM2_SmartFactorStereo_IMU.cpp new file mode 100644 index 000000000..25675abf8 --- /dev/null +++ b/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -0,0 +1,236 @@ +/** + * @file ISAM2_SmartFactorStereo_IMU.cpp + * @brief test of iSAM2 with smart stereo factors and IMU preintegration, + * originally used to debug valgrind invalid reads with Eigen + * @author Nghia Ho + * + * Setup is a stationary stereo camera with an IMU attached. + * The data file is at examples/Data/ISAM2_SmartFactorStereo_IMU.txt + * It contains 5 frames of stereo matches and IMU data. + */ +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; +using symbol_shorthand::V; +using symbol_shorthand::B; + +struct IMUHelper { + IMUHelper() { + { + auto gaussian = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3)) + .finished()); + + auto huber = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), gaussian); + + biasNoiseModel = huber; + } + + { + auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01); + auto huber = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), gaussian); + + velocityNoiseModel = huber; + } + + // expect IMU to be rotated in image space co-ords + auto p = boost::make_shared( + Vector3(0.0, 9.8, 0.0)); + + p->accelerometerCovariance = + I_3x3 * pow(0.0565, 2.0); // acc white noise in continuous + p->integrationCovariance = + I_3x3 * 1e-9; // integration uncertainty continuous + p->gyroscopeCovariance = + I_3x3 * pow(4.0e-5, 2.0); // gyro white noise in continuous + p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous + p->biasOmegaCovariance = + I_3x3 * pow(0.001, 2.0); // gyro bias in continuous + p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5; + + // body to IMU rotation + Rot3 iRb( + 0.036129, -0.998727, 0.035207, + 0.045417, -0.033553, -0.99840,4 + 0.998315, 0.037670, 0.044147); + + // body to IMU translation (meters) + Point3 iTb(0.03, -0.025, -0.06); + + // body in this example is the left camera + p->body_P_sensor = Pose3(iRb, iTb); + + Rot3 prior_rotation = Rot3(I_3x3); + Pose3 prior_pose(prior_rotation, Point3(0, 0, 0)); + + Vector3 acc_bias(0.0, -0.0942015, 0.0); // in camera frame + Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968); + + priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias); + + prevState = NavState(prior_pose, Vector3(0, 0, 0)); + propState = prevState; + prevBias = priorImuBias; + + preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias); + } + + imuBias::ConstantBias priorImuBias; // assume zero initial bias + noiseModel::Robust::shared_ptr velocityNoiseModel; + noiseModel::Robust::shared_ptr biasNoiseModel; + NavState prevState; + NavState propState; + imuBias::ConstantBias prevBias; + PreintegratedCombinedMeasurements* preintegrated; +}; + +int main(int argc, char* argv[]) { + if (argc != 2) { + cout << "./main [data.txt]\n"; + return 0; + } + + ifstream in(argv[1]); + + if (!in) { + cerr << "error opening: " << argv[1] << "\n"; + return 1; + } + + // Camera parameters + double fx = 822.37; + double fy = 822.37; + double cx = 538.73; + double cy = 579.10; + double baseline = 0.372; // meters + + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline)); + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.1; + ISAM2 isam(parameters); + + // Create a factor graph + std::map smartFactors; + NonlinearFactorGraph graph; + Values initialEstimate; + IMUHelper imu; + + // Pose prior - at identity + auto priorPoseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished()); + graph.emplace_shared>(X(1), Pose3::identity(), + priorPoseNoise); + initialEstimate.insert(X(0), Pose3::identity()); + + // Bias prior + graph.add(PriorFactor(B(1), imu.priorImuBias, + imu.biasNoiseModel)); + initialEstimate.insert(B(0), imu.priorImuBias); + + // Velocity prior - assume stationary + graph.add( + PriorFactor(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel)); + initialEstimate.insert(V(0), Vector3(0, 0, 0)); + + int lastFrame = 1; + int frame; + + while (true) { + char line[1024]; + + in.getline(line, sizeof(line)); + stringstream ss(line); + char type; + + ss >> type; + ss >> frame; + + if (frame != lastFrame || in.eof()) { + cout << "Running iSAM for frame: " << lastFrame << "\n"; + + initialEstimate.insert(X(lastFrame), Pose3::identity()); + initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0)); + initialEstimate.insert(B(lastFrame), imu.prevBias); + + CombinedImuFactor imuFactor(X(lastFrame - 1), V(lastFrame - 1), + X(lastFrame), V(lastFrame), B(lastFrame - 1), + B(lastFrame), *imu.preintegrated); + + graph.add(imuFactor); + + isam.update(graph, initialEstimate); + + Values currentEstimate = isam.calculateEstimate(); + + imu.propState = imu.preintegrated->predict(imu.prevState, imu.prevBias); + imu.prevState = NavState(currentEstimate.at(X(lastFrame)), + currentEstimate.at(V(lastFrame))); + imu.prevBias = currentEstimate.at(B(lastFrame)); + imu.preintegrated->resetIntegrationAndSetBias(imu.prevBias); + + graph.resize(0); + initialEstimate.clear(); + + if (in.eof()) { + break; + } + } + + if (type == 'i') { // Process IMU measurement + double ax, ay, az; + double gx, gy, gz; + double dt = 1 / 800.0; // IMU at ~800Hz + + ss >> ax; + ss >> ay; + ss >> az; + + ss >> gx; + ss >> gy; + ss >> gz; + + Vector3 acc(ax, ay, az); + Vector3 gyr(gx, gy, gz); + + imu.preintegrated->integrateMeasurement(acc, gyr, dt); + } else if (type == 's') { // Process stereo measurement + int landmark; + double xl, xr, y; + + ss >> landmark; + ss >> xl; + ss >> xr; + ss >> y; + + if (smartFactors.count(landmark) == 0) { + auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0); + + SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY); + + smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr( + new SmartStereoProjectionPoseFactor(gaussian, params)); + graph.push_back(smartFactors[landmark]); + } + + smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K); + } else { + throw runtime_error("unexpected data type: " + type); + } + + lastFrame = frame; + } + + return 0; +} diff --git a/examples/ISAM2_SmartFactor_valgrind.cpp b/examples/ISAM2_SmartFactor_valgrind.cpp deleted file mode 100644 index 2d5c6c15b..000000000 --- a/examples/ISAM2_SmartFactor_valgrind.cpp +++ /dev/null @@ -1,222 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using symbol_shorthand::X; -using symbol_shorthand::V; -using symbol_shorthand::B; - -struct IMUHelper -{ - IMUHelper() - { - gtsam::Matrix33 measured_acc_cov = gtsam::Matrix33::Identity(3,3) * pow(0.113, 2.0); - gtsam::Matrix33 measured_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(4.0e-5, 2.0); - gtsam::Matrix33 bias_acc_cov = gtsam::Matrix33::Identity(3,3) * pow( 0.00002, 2.0); - gtsam::Matrix33 bias_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(0.001, 2.0); - gtsam::Matrix33 integration_error_cov = gtsam::Matrix33::Identity(3,3)*1e-9; // error committed in integrating position from velocities - gtsam::Matrix bias_acc_omega_int = gtsam::Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration - - { - auto gaussian = gtsam::noiseModel::Diagonal::Sigmas( - (gtsam::Vector(6) << - gtsam::Vector3::Constant(5.0e-2), - gtsam::Vector3::Constant(5.0e-3)).finished()); - - auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian); - - bias_noise_model = huber; - } - - { - auto gaussian = gtsam::noiseModel::Isotropic::Sigma(3,0.01); - auto huber = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(1.345), gaussian); - - velocity_noise_model = huber; - } - - // expect IMU to be rotated in image space co-ords - auto p = boost::make_shared(gtsam::Vector3(0.0, 9.8, 0.0)); - - p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous - p->integrationCovariance = integration_error_cov; // integration uncertainty continuous - p->gyroscopeCovariance = measured_omega_cov; // gyro white noise 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; - - // from SVD comparing gyro and vision estimates, no bias modelled - gtsam::Rot3 body_to_imu_rot( - 3.612861008216737e-02, -9.987267865568920e-01, 3.520695026944293e-02, - 4.541686330383411e-02, -3.355264881270600e-02, -9.984044913186698e-01, - 9.983145957368207e-01, 3.766995581886975e-02, 4.414682737675374e-02); - - gtsam::Point3 body_to_imu_trans(0.03, -0.025, -0.06); - - p->body_P_sensor = gtsam::Pose3(body_to_imu_rot, body_to_imu_trans); - - gtsam::Rot3 prior_rotation = gtsam::Rot3(gtsam::Matrix33::Identity()); - gtsam::Pose3 prior_pose(prior_rotation, gtsam::Point3(0,0,0)); - - gtsam::Vector3 acc_bias(2.05998e-18, -0.0942015, 1.17663e-16); - gtsam::Vector3 gyro_bias(0,0,0); - - prior_imu_bias = gtsam::imuBias::ConstantBias(acc_bias, gyro_bias); - - prev_state = gtsam::NavState(prior_pose, gtsam::Vector3(0,0,0)); - prop_state = prev_state; - prev_bias = prior_imu_bias; - - preintegrated = new gtsam::PreintegratedCombinedMeasurements(p, prior_imu_bias); - } - - gtsam::imuBias::ConstantBias prior_imu_bias; // assume zero initial bias - gtsam::noiseModel::Robust::shared_ptr velocity_noise_model; - gtsam::noiseModel::Robust::shared_ptr bias_noise_model; - gtsam::NavState prev_state; - gtsam::NavState prop_state; - gtsam::imuBias::ConstantBias prev_bias; - gtsam::PreintegratedCombinedMeasurements *preintegrated; -}; - -int main(int argc, char* argv[]) { - if (argc != 2) { - cout << "./main [data.txt]\n"; - return 0; - } - - double fx = 8.2237229542137766e+02; - double fy = fx; - double cx = 5.3872524261474609e+02; - double cy = 5.7909587860107422e+02; - double baseline = 7.4342307430851651e-01; - - Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline)); - - ISAM2Params parameters; - parameters.relinearizeThreshold = 0.1; - ISAM2 isam(parameters); - - // Create a factor graph - std::map smartFactors; - NonlinearFactorGraph graph; - Values initialEstimate; - IMUHelper imu; - - // Pose prior - auto priorPoseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished()); - graph.emplace_shared>(X(1), Pose3::identity(), priorPoseNoise); - initialEstimate.insert(X(0), Pose3::identity()); - - // Bias prior - graph.add(PriorFactor(B(1), imu.prior_imu_bias, imu.bias_noise_model)); - initialEstimate.insert(B(0), imu.prior_imu_bias); - - // Velocity prior - graph.add(PriorFactor(V(1), Vector3(0,0,0), imu.velocity_noise_model)); - initialEstimate.insert(V(0), Vector3(0,0,0)); - - ifstream in(argv[1]); - - if (!in) { - cerr << "error opening: " << argv[1] << "\n"; - return 1; - } - - int last_frame = 1; - int frame; - - while (true) { - char line[1024]; - - in.getline(line, sizeof(line)); - stringstream ss(line); - char type; - - ss >> type; - ss >> frame; - - if (frame != last_frame || in.eof()) { - cout << "Running isam for frame: " << last_frame << "\n"; - - initialEstimate.insert(X(last_frame), Pose3::identity()); - initialEstimate.insert(V(last_frame), Vector3(0,0,0)); - initialEstimate.insert(B(last_frame), imu.prev_bias); - - CombinedImuFactor imu_factor( - X(last_frame-1), V(last_frame-1), - X(last_frame), V(last_frame), - B(last_frame-1), B(last_frame), - *imu.preintegrated); - - graph.add(imu_factor); - - isam.update(graph, initialEstimate); - - Values currentEstimate = isam.calculateEstimate(); - //currentEstimate.print("Current estimate: "); - - imu.prop_state = imu.preintegrated->predict(imu.prev_state, imu.prev_bias); - imu.prev_state = NavState(currentEstimate.at(X(last_frame)), currentEstimate.at(V(last_frame))); - imu.prev_bias = currentEstimate.at(B(last_frame)); - imu.preintegrated->resetIntegrationAndSetBias(imu.prev_bias); - - graph.resize(0); - initialEstimate.clear(); - - if (in.eof()) { - break; - } - } - - if (type == 'i') { - double ax, ay, az; - double gx, gy, gz; - double dt = 1/800.0; - - ss >> ax; - ss >> ay; - ss >> az; - - ss >> gx; - ss >> gy; - ss >> gz; - - Vector3 acc(ax, ay, az); - Vector3 gyr(gx, gy, gz); - - imu.preintegrated->integrateMeasurement(acc, gyr, dt); - } else if (type == 's') { - int landmark; - double xl, xr, y; - - ss >> landmark; - ss >> xl; - ss >> xr; - ss >> y; - - if (smartFactors.count(landmark) == 0) { - auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0); - - SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY); - - smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(new SmartStereoProjectionPoseFactor(gaussian, params)); - graph.push_back(smartFactors[landmark]); - } - - smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K); - } - - last_frame = frame; - } - - return 0; -}