• Uncategorized
  • 0

Analysis of the cause of GTSAM crash

Hits: 0

1. IndeterminantLinearSystemException

Mathematically, the following conditions cause this problem:

  • Underdetermined system: This occurs when the variables are not completely constrained by factors. Even if all variables are involved in factors, the variables can still be numerically underconstrained, (for example, if a landmark is observed by only one ProjectionFactor). Mathematically in this case, the rank of the linear Jacobian or Hessian is less than the number of scalars in the system.
  • Indefinite system: This condition occurs when the system Hessian is indefinite, i.e. non-positive-semidefinite. Note that this condition can also indicate an underdetermined system, but when solving with Cholesky, accumulation of numerical errors can cause the system to become indefinite (have some negative eigenvalues) even if it is in reality positive semi-definite (has some near-zero positive eigenvalues). Alternatively, if the system contains some HessianFactor‘s with negative eigenvalues, these can create a truly indefinite system, whose quadratic error function has negative curvature in some directions.
  • Poorly-conditioned system: A system that is positive-definite (has a unique solution) but is numerically ill-conditioned can accumulate numerical errors during solving that cause the system to become indefinite later during the elimination process. Note that poorly- conditioned systems will usually have inaccurate solutions, even if they do not always trigger this error. Systems with almost- unconstrained variables or vastly different measurement uncertainties or variable units can be poorly-conditioned.

Resolving this problem:

  • This exception contains the variable at which the problem was discovered (IndeterminantLinearSystemException::nearbyVariable()). Note, however, that this is not necessarily the variable where the problem originates. For example, in the case that a prior on the first camera was forgotten, it may only be another camera or landmark where the problem can be detected. Where the problem is detected depends on the graph structure and variable elimination ordering.
  • MATLAB (or similar software) is a useful tool to diagnose this problem. Use GaussianFactorGraph::sparseJacobian_()GaussianFactorGraph::sparseJacobian() GaussianFactorGraph::denseJacobian(), and GaussianFactorGraph::denseHessian() to output the linear graph in matrix format. If using the MATLAB wrapper, the returned matrices can be immediately inspected and analyzed using standard methods. If not using the MATLAB wrapper, the output of these functions may be written to text files and then loaded into MATLAB.
  • When outputting linear graphs as Jacobian matrices, rows are ordered in the same order as factors in the graph, which each factor occupying the number of rows indicated by its JacobianFactor::rows() function. Each column appears in elimination ordering, as in the Ordering class. Each variable occupies the number of columns indicated by the JacobianFactor::getDim() function.
  • When outputting linear graphs as Hessian matrices, rows and columns are ordered in elimination order and occupy scalars in the same way as described for Jacobian columns in the previous bullet.

Inheritance diagram for gtsam::IndeterminantLinearSystemException:

1.1. Reasons for compilation

GTSAM_BUILD_WITH_MARCH_NATIVE is ON by default at compile time

In the GtsamBuildTypes.cmake file, when GTSAM_BUILD_WITH_MARCH_NATIVE is ON, add -march= [native to the compilation options]

Set GTSAM_BUILD_WITH_MARCH_NATIVE to OFF to solve the crash

Cause Analysis

Adding -march=native means optimizing instructions according to CPU characteristics, which will make your program faster, but it requires your dependent libraries to also use -march=native when compiling

The actual situation is that we generally do not use -march=native when compiling other libraries, and sometimes even install it directly by apt-get

Solution

use when cmake

#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <fstream>
#include <iostream>

using namespace std;
using namespace gtsam;

using gtsam::symbol_shorthand::X; // gtsam::Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)

gtsam::Key computeKey(double time) {
  double period = time;
  double key = period / 0.01;
  return gtsam::Key(key);
}

gtsam::Key getGravityKey(const gtsam::Key& key) {
  return gtsam::Symbol('g', key);
}

// This will either be PreintegratedImuMeasurements (for ImuFactor) or
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
PreintegrationType *imu_preintegrated_;

int main()
{
  // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
  gtsam::Rot3 prior_rotation;
  gtsam::Point3 prior_point;
  gtsam::Pose3 prior_pose(prior_rotation, prior_point);
  gtsam::Vector3 prior_velocity;
  gtsam::imuBias::ConstantBias prior_imu_bias; // assume zero initial bias

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

  // Assemble prior noise model and add it to the graph. 
  gtsam::noiseModel::Diagonal:: shared_ptr pose_noise_model = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector( 6 ) << 0.01 , 0.01 , 0.01 , 0.5 , 0.5 , 0.5 ).finished()) ; // rad,rad,rad,m, m, m 
  gtsam::noiseModel::Diagonal:: shared_ptr velocity_noise_model = gtsam::noiseModel::Isotropic::Sigma( 3 , 0.1 ); // m/s 
  gtsam::noiseModel::Diagonal:: shared_ptr bias_noise_model = gtsam::noiseModel::Isotropic::Sigma( 6, 1e-3 );
  gtsam::noiseModel::Diagonal:: shared_ptr gravity_noise_model = gtsam::noiseModel::Isotropic::Sigma( 3 , 1e-9 );

  // Add all prior factors (pose, velocity, bias) to the graph.
  gtsam::NonlinearFactorGraph *graph = new gtsam::NonlinearFactorGraph();
  graph->add(gtsam::PriorFactor<gtsam::Pose3>(X(correction_count), prior_pose, pose_noise_model));
  graph->add(gtsam::PriorFactor<gtsam::Vector3>(V(correction_count), prior_velocity, velocity_noise_model));
  graph->add(gtsam::PriorFactor<gtsam::imuBias::ConstantBias>(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;
  gtsam::Matrix33 measured_acc_cov = gtsam::Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
  gtsam::Matrix33 measured_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
  gtsam::Matrix33 integration_error_cov = gtsam::Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
  gtsam::Matrix33 bias_acc_cov = gtsam::Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
  gtsam::Matrix33 bias_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
  gtsam::Matrix66 bias_acc_omega_int = gtsam::Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration

  gtsam::Vector3 gravity_inertial(0, 0, -9.81);
  boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p(new PreintegratedCombinedMeasurements::Params(gravity_inertial));
  // 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;

  imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);

  // Store previous state for the imu integration and the latest predicted outcome.
  NavState prev_state(prior_pose, prior_velocity);
  NavState prop_state = prev_state;
  gtsam::imuBias::ConstantBias prev_bias = prior_imu_bias;

  double dt = 0.005;  // The real system has noise, but here, results are nearly
  // exactly the same, so keeping this for simplicity.

  gtsam::ISAM2Params parameters = gtsam::ISAM2Params();
  std::shared_ptr<gtsam::ISAM2> isam_(new gtsam::ISAM2(parameters));

  // All priors have been set up, now iterate through the data file.
  ++correction_count;
  for ( ; correction_count < 10000; ++correction_count) {
    gtsam::Vector3 acceleration(0.0001, 0, 0);
    gtsam::Vector3 angular_velocity(0, 0, 0);

    // Adding the IMU preintegration.
    imu_preintegrated_->integrateMeasurement(acceleration, angular_velocity, dt);

    // Adding IMU factor and GPS factor and optimizing.
    PreintegratedCombinedMeasurements *preint_imu = dynamic_cast<PreintegratedCombinedMeasurements*>(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) ;

    graph->add(imu_factor);

    // Now optimize and compare results.
    prop_state = imu_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), prior_imu_bias);

    isam_->update(*graph, initial_values);
    isam_->update();

    initial_values.clear();
    graph->resize(0);

    gtsam::Values currentEstimate = isam_->calculateEstimate();
    std::string str = "Current estimate " + std::to_string(correction_count) + ": ";
    currentEstimate.at<gtsam::Pose3>(X(correction_count)).translation().print(str);

    // Reset the preintegration object.
    imu_preintegrated_->resetIntegrationAndSetBias(prior_imu_bias);
  }

  return 0;
}

1.2. Parameter reasons

The following example crashes after running for a while

#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <fstream>
#include <iostream>

using namespace std;
using namespace gtsam;

using gtsam::symbol_shorthand::X; // gtsam::Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)

gtsam::Key computeKey(double time) {
  double period = time;
  double key = period / 0.01;
  return gtsam::Key(key);
}

gtsam::Key getGravityKey(const gtsam::Key& key) {
  return gtsam::Symbol('g', key);
}

// This will either be PreintegratedImuMeasurements (for ImuFactor) or
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
PreintegrationType *imu_preintegrated_;

int main()
{
  // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
  gtsam::Rot3 prior_rotation;
  gtsam::Point3 prior_point;
  gtsam::Pose3 prior_pose(prior_rotation, prior_point);
  gtsam::Vector3 prior_velocity;
  gtsam::imuBias::ConstantBias prior_imu_bias; // assume zero initial bias

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

  // Assemble prior noise model and add it to the graph. 
  gtsam::noiseModel::Diagonal:: shared_ptr pose_noise_model = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector( 6 ) << 0.01 , 0.01 , 0.01 , 0.5 , 0.5 , 0.5 ).finished()) ; // rad,rad,rad,m, m, m 
  gtsam::noiseModel::Diagonal:: shared_ptr velocity_noise_model = gtsam::noiseModel::Isotropic::Sigma( 3 , 0.1 ); // m/s 
  gtsam::noiseModel::Diagonal:: shared_ptr bias_noise_model = gtsam::noiseModel::Isotropic::Sigma( 6, 1e-3 );
  gtsam::noiseModel::Diagonal:: shared_ptr gravity_noise_model = gtsam::noiseModel::Isotropic::Sigma( 3 , 1e-9 );

  // Add all prior factors (pose, velocity, bias) to the graph.
  gtsam::NonlinearFactorGraph *graph = new gtsam::NonlinearFactorGraph();
  graph->add(gtsam::PriorFactor<gtsam::Pose3>(X(correction_count), prior_pose, pose_noise_model));
  graph->add(gtsam::PriorFactor<gtsam::Vector3>(V(correction_count), prior_velocity, velocity_noise_model));
  graph->add(gtsam::PriorFactor<gtsam::imuBias::ConstantBias>(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;
  gtsam::Matrix33 measured_acc_cov = gtsam::Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
  gtsam::Matrix33 measured_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
  gtsam::Matrix33 integration_error_cov = gtsam::Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
  gtsam::Matrix33 bias_acc_cov = gtsam::Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
  gtsam::Matrix33 bias_omega_cov = gtsam::Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
  gtsam::Matrix66 bias_acc_omega_int = gtsam::Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration

  gtsam::Vector3 gravity_inertial(0, 0, -9.81);
  boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p(new PreintegratedCombinedMeasurements::Params(gravity_inertial));
  // 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;

  imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);

  // Store previous state for the imu integration and the latest predicted outcome.
  NavState prev_state(prior_pose, prior_velocity);
  NavState prop_state = prev_state;
  gtsam::imuBias::ConstantBias prev_bias = prior_imu_bias;

  double dt = 0.005;  // The real system has noise, but here, results are nearly
  // exactly the same, so keeping this for simplicity.

  gtsam::ISAM2Params parameters = gtsam::ISAM2Params();
  std::shared_ptr<gtsam::ISAM2> isam_(new gtsam::ISAM2(parameters));

  // All priors have been set up, now iterate through the data file.
  ++correction_count;
  for ( ; correction_count < 10000; ++correction_count) {
    gtsam::Vector3 acceleration(0.0001, 0, 0);
    gtsam::Vector3 angular_velocity(0, 0, 0);

    // Adding the IMU preintegration.
    imu_preintegrated_->integrateMeasurement(acceleration, angular_velocity, dt);

    // Adding IMU factor and GPS factor and optimizing.
    PreintegratedCombinedMeasurements *preint_imu = dynamic_cast<PreintegratedCombinedMeasurements*>(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) ;

    graph->add(imu_factor);

    // Now optimize and compare results.
    prop_state = imu_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), prior_imu_bias);

    isam_->update(*graph, initial_values);
    isam_->update();

    initial_values.clear();
    graph->resize(0);

    gtsam::Values currentEstimate = isam_->calculateEstimate();
    std::string str = "Current estimate " + std::to_string(correction_count) + ": ";
    currentEstimate.at<gtsam::Pose3>(X(correction_count)).translation().print(str);

    // Reset the preintegration object.
    imu_preintegrated_->resetIntegrationAndSetBias(prior_imu_bias);
  }

  return 0;
}

The reason is that matrix decomposition uses LLT decomposition by default. Although it is fast, it is sometimes numerically unstable. This problem can be solved using QR decomposition. Using QR is slow but stable.

parameters.factorization = ISAM2Params::QR;

references

gtsam: gtsam::IndeterminantLinearSystemException Class Reference

You may also like...

Leave a Reply

Your email address will not be published.