Program Crashed using quaternions

[klissanzmp]

Hi, I'm trying to use IMU factor to make predictions of robot poses and I want to enable quaternions. I added #define GTSAM_USE_QUATERNIONS in config.h under gtsam folder, but my program crashed, displaying the following information:

*** stack smashing detected ***: terminated
Aborted (core dumped)

The data i'm using is euroc MH_01_esay. The code i'm using is ImuFactorsExample.cpp. I modified the code to remove GPS factors. I was wondering if I was using gtsam correctly or is it a bug? Sorry for bothering. Thank you.

[varunagrawal]

@klissanzmp can you please share your changes? I am able to run the vanilla program with #define GTSAM_USE_QUATERNIONS no problem. If I can reproduce this issue, I can help debug and fix it.

[klissanzmp]

@varunagrawal Yes. The code is as follows. Basically, i removed GPS factor and read imu measurements from a file as the example did. Thank you.

// GTSAM related includes.
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>

#include <cstring>
#include <fstream>
#include <iostream>

#define INTERVAL 1

#define accel_noise_sigma 0.002
#define gyro_noise_sigma 0.00016968
#define accel_bias_rw_sigma 0.003
#define gyro_bias_rw_sigma 0.000019393

using namespace gtsam;
using namespace std;

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)

boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
  // We use the sensor specs to build the noise model for the IMU factor.
//   double accel_noise_sigma = 0.002;
//   double gyro_noise_sigma = 0.00016968;
//   double accel_bias_rw_sigma = 0.003;
//   double gyro_bias_rw_sigma = 0.000019393;
  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

  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(9.81);
  // PreintegrationBase params:
  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
  // PreintegrationCombinedMeasurements params:
  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
  p->biasAccOmegaInt = bias_acc_omega_int;

  return p;
}

int main(int argc, char* argv[]) {
    string imu_file_name(argv[1]), output_filename = "result.txt";

    FILE* fp_out = fopen(output_filename.c_str(), "w+");
    ifstream file(imu_file_name.c_str());
    string value;

    // [x,y,z,qx,qy,qz,qw,vx,vy,vz]
    Vector10 initial_state;
    initial_state(6) = 1;
    initial_state(3) = initial_state(4) = initial_state(5) = 0;
    cout << "initial state:\n" << initial_state.transpose() << "\n\n";

    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

    Values initial_values;
    int correction_count = 0;
    initial_values.insert(X(correction_count), prior_pose);
    initial_values.insert(V(correction_count), prior_velocity);
    initial_values.insert(B(correction_count), prior_imu_bias);

    auto pose_noise_model = noiseModel::Diagonal::Sigmas(
        (Vector(6) << gyro_noise_sigma, gyro_noise_sigma, gyro_noise_sigma, accel_noise_sigma, accel_noise_sigma, accel_noise_sigma)
            .finished());  // rad,rad,rad,m, m, m
    auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.0001);  // m/s
    auto bias_noise_model = noiseModel::Diagonal::Sigmas(
        (Vector(6) << gyro_bias_rw_sigma, gyro_bias_rw_sigma, gyro_bias_rw_sigma, accel_bias_rw_sigma, accel_bias_rw_sigma, accel_bias_rw_sigma)
            .finished());

    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);
    auto p = imuParams();

    std::shared_ptr<PreintegrationType> preintegrated =
        std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);

    NavState prev_state(prior_pose, prior_velocity);
    NavState prop_state = prev_state;
    imuBias::ConstantBias prev_bias = prior_imu_bias;

    // 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;
    double dt = 0.0005;

    getline(file, value, '\n');
    cout << value << "\n";
    int inc = 0;

    while (file.peek() != EOF ) {
        getline(file, value, ',');
        string time = value;
        //cout << time << " ";
        Vector6 imu;
        for (int i = 0; i < 5; ++i) {
            getline(file, value, ',');
            imu(i) = stof(value.c_str());
            //cout << imu(i) << " ";
        }
        getline(file, value, '\n');
        imu(5) = stof(value.c_str());
        //cout << imu(5) << endl;

        preintegrated->integrateMeasurement(imu.tail<3>(), imu.head<3>(), dt);
        inc++;
        if(inc == INTERVAL) {
            inc = 0;
            correction_count++;
            auto preint_imu =
            dynamic_cast<const PreintegratedImuMeasurements&>(*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<imuBias::ConstantBias>(
            B(correction_count - 1), B(correction_count), zero_bias,
            bias_noise_model));

            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);

            Values result;
            LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
            result = optimizer.optimize();
            // Overwrite the beginning of the preintegration for the next step.
            prev_state = NavState(result.at<Pose3>(X(correction_count)),
                            result.at<Vector3>(V(correction_count)));
            prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
            // Reset the preintegration object.
            preintegrated->resetIntegrationAndSetBias(prev_bias);

            Vector3 gtsam_position = prev_state.pose().translation();
            Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
            fprintf(fp_out, "%s,%f,%f,%f,%f,%f,%f,%f\n",
                time.c_str(), gtsam_position(0), gtsam_position(1),
                gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
                gtsam_quat.w());
        }
    }
    fclose(fp_out);
    cout << "Done" << endl;
    return 0;
}

[ProfFan]

wait, we have fprintf in our examples? @klissanzmp Could you run valgrind on your code?

[klissanzmp]

@ProfFan valgrind result is attached. error.txt

[varunagrawal]

@klissanzmp I am not able to reproduce the error. I advise you to set GTSAM_USE_QUATERNIONS in the CMake options and perhaps try again?

[ProfFan]

No wonder this will crash, as GTSAM is not compiled to use quaternions. You cannot just change config.h and pretend :)

[catproof]

@ProfFan where and how are you supposed to set this option? I don't see it in 'CMakeLists.txt'