done with new default noise thresholds!

gtsam/nonlinear/GncOptimizer.h

@@ -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_; }
 

tests/testGncOptimizer.cpp

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