chi_squared.cpp

#include <boost/math/distributions/chi_squared.hpp>


/// Chi squared 95th percentile table
std::map<int, double> chi_squared_table;

// Initialize the chi squared test table with confidence level 0.95
// https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221
for (int i = 1; i < 100; ++i) {
    boost::math::chi_squared chi_squared_dist(i);
    chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95);
}

// Compute the chi squared value with the covariance....
double min_distsq = 0;

// Add existing ID if it passes our threshold
// We should use 3 for 3dof on the plane
if(min_distsq < 10*chi_squared_table[3]) {
    ROS_INFO("[MATCHER]: SUCCESS mah_sq = %.2f (threshold = %.2f)",min_distsq,10*chi_squared_table[3]);
}