done with new default noise thresholds!

release/4.3a0
lcarlone 2021-01-22 23:24:28 -05:00
parent 28b0f0ac8e
commit a59a12245c
2 changed files with 72 additions and 5 deletions

View File

@ -65,10 +65,6 @@ class GncOptimizer {
// make sure all noiseModels are Gaussian or convert to Gaussian
nfg_.resize(graph.size());
barcSq_ = Vector::Ones(graph.size());
double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
for (size_t i = 0; i < graph.size(); i++) {
if (graph[i]) {
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
@ -77,9 +73,12 @@ class GncOptimizer {
noiseModel::Robust>(factor->noiseModel());
// if the factor has a robust loss, we remove the robust loss
nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
barcSq_[i] = 0.5 * Chi2inv(alpha, nfg_[i]->dim()); // 0.5 derives from the error definition in gtsam
}
}
// set default barcSq_ (inlier threshold)
double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
setInlierCostThresholdsAtProbability(alpha);
}
/** Set the maximum weighted residual error for an inlier (same for all factors). For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega,
@ -102,6 +101,18 @@ class GncOptimizer {
barcSq_ = inthVec;
}
/** Set the maximum weighted residual error threshold by specifying the probability
* alpha that the inlier residuals are smaller than that threshold
* */
void setInlierCostThresholdsAtProbability(const double alpha) {
barcSq_ = Vector::Ones(nfg_.size());
for (size_t k = 0; k < nfg_.size(); k++) {
if (nfg_[k]) {
barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the error definition in gtsam
}
}
}
/// Access a copy of the internal factor graph.
const NonlinearFactorGraph& getFactors() const { return nfg_; }

View File

@ -33,6 +33,9 @@
#include <gtsam/slam/dataset.h>
#include <tests/smallExample.h>
#include <gtsam/sam/BearingFactor.h>
#include <gtsam/geometry/Pose2.h>
using namespace std;
using namespace gtsam;
@ -603,6 +606,59 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
}
}
/* ************************************************************************* */
TEST(GncOptimizer, chi2inv) {
DOUBLES_EQUAL(8.807468393511950, Chi2inv(0.997, 1), tol); // from MATLAB: chi2inv(0.997, 1) = 8.807468393511950
DOUBLES_EQUAL(13.931422665512077, Chi2inv(0.997, 3), tol); // from MATLAB: chi2inv(0.997, 3) = 13.931422665512077
}
/* ************************************************************************* */
TEST(GncOptimizer, barcsq) {
auto fg = example::sharedNonRobustFactorGraphWithOutliers();
Point2 p0(1, 0);
Values initial;
initial.insert(X(1), p0);
std::vector<size_t> knownInliers;
knownInliers.push_back(0);
knownInliers.push_back(1);
knownInliers.push_back(2);
GncParams<GaussNewtonParams> gncParams;
gncParams.setKnownInliers(knownInliers);
gncParams.setLossType(GncLossType::GM);
//gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
gncParams);
// expected: chi2inv(0.99, 2)/2
CHECK(assert_equal(4.605170185988091 * Vector::Ones(fg.size()), gnc.getInlierCostThresholds(), 1e-3));
}
/* ************************************************************************* */
TEST(GncOptimizer, barcsq_heterogeneousFactors) {
NonlinearFactorGraph fg;
// specify noise model, otherwise it segfault if we leave default noise model
SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
fg.add( PriorFactor<Pose2>( 0, Pose2(0.0, 0.0, 0.0) , model3D )); // size 3
SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
fg.add( PriorFactor<Point2>( 1, Point2(0.0,0.0), model2D )); // size 2
SharedNoiseModel model1D(noiseModel::Isotropic::Sigma(1, 0.5));
fg.add( BearingFactor<Pose2, Point2>( 0, 1, 1.0, model1D) ); // size 1
Values initial;
initial.insert(0, Pose2(0.0, 0.0, 0.0));
initial.insert(1, Point2(0.0,0.0));
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial);
CHECK(assert_equal(Vector3(5.672433365072185, 4.605170185988091, 3.317448300510607),
gnc.getInlierCostThresholds(), 1e-3));
// extra test:
// fg.add( PriorFactor<Pose2>( 0, Pose2(0.0, 0.0, 0.0) )); // works if we add model3D as noise model
// std::cout << "fg[3]->dim() " << fg[3]->dim() << std::endl; // this segfaults?
}
/* ************************************************************************* */
TEST(GncOptimizer, setWeights) {
auto fg = example::sharedNonRobustFactorGraphWithOutliers();