user can now also set the weights to initialize gnc!

gtsam/nonlinear/GncOptimizer.h

@@ -99,6 +99,10 @@ class GncOptimizer {
                   "that are not in the factor graph to be known outliers.");
       }
     }
+    // initialize weights (if we don't have prior knowledge of inliers/outliers
+    // the weights are all initialized to 1.
+    weights_ = initializeWeightsFromKnownInliersAndOutliers();
+
     // set default barcSq_ (inlier threshold)
     double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
     setInlierCostThresholdsAtProbability(alpha);
@@ -134,6 +138,17 @@ class GncOptimizer {
     }
   }
 
+  /** Set weights for each factor. This is typically not needed, but
+   * provides an extra interface for the user to initialize the weightst
+   * */
+  void setWeights(const Vector w) {
+    if(w.size() != nfg_.size()){
+      throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights"
+          " does not match the size of the factor graph.");
+    }
+    weights_ = w;
+  }
+
   /// Access a copy of the internal factor graph.
   const NonlinearFactorGraph& getFactors() const { return nfg_; }
 
@@ -167,8 +182,6 @@ class GncOptimizer {
 
   /// Compute optimal solution using graduated non-convexity.
   Values optimize() {
-    // start by assuming all measurements are inliers
-    weights_ = initializeWeightsFromKnownInliersAndOutliers();
     NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_);
     BaseOptimizer baseOptimizer(graph_initial, state_);
     Values result = baseOptimizer.optimize();

tests/testGncOptimizer.cpp

@@ -660,7 +660,7 @@ TEST(GncOptimizer, barcsq_heterogeneousFactors) {
 }
 
 /* ************************************************************************* */
-TEST(GncOptimizer, setWeights) {
+TEST(GncOptimizer, setInlierCostThresholds) {
   auto fg = example::sharedNonRobustFactorGraphWithOutliers();
 
   Point2 p0(1, 0);
@@ -839,6 +839,87 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
   }
 }
 
+/* ************************************************************************* */
+TEST(GncOptimizer, setWeights) {
+  auto fg = example::sharedNonRobustFactorGraphWithOutliers();
+
+  Point2 p0(1, 0);
+  Values initial;
+  initial.insert(X(1), p0);
+  // initialize weights to be the same
+  {
+    GncParams<GaussNewtonParams> gncParams;
+    gncParams.setLossType(GncLossType::TLS);
+
+    Vector weights = 0.5 * Vector::Ones(fg.size());
+    auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
+        gncParams);
+    gnc.setWeights(weights);
+    gnc.setInlierCostThresholds(1.0);
+    Values gnc_result = gnc.optimize();
+    CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
+
+    // check weights were actually fixed:
+    Vector finalWeights = gnc.getWeights();
+    DOUBLES_EQUAL(1.0, finalWeights[0], tol);
+    DOUBLES_EQUAL(1.0, finalWeights[1], tol);
+    DOUBLES_EQUAL(1.0, finalWeights[2], tol);
+    DOUBLES_EQUAL(0.0, finalWeights[3], tol);
+  }
+  // try a more challenging initialization
+  {
+    GncParams<GaussNewtonParams> gncParams;
+    gncParams.setLossType(GncLossType::TLS);
+
+    Vector weights = Vector::Zero(fg.size());
+    weights(2) = 1.0;
+    weights(3) = 1.0; // bad initialization: we say the outlier is inlier
+    // GNC can still recover (but if you omit weights(2) = 1.0, then it would fail)
+    auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
+        gncParams);
+    gnc.setWeights(weights);
+    gnc.setInlierCostThresholds(1.0);
+    Values gnc_result = gnc.optimize();
+    CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
+
+    // check weights were actually fixed:
+    Vector finalWeights = gnc.getWeights();
+    DOUBLES_EQUAL(1.0, finalWeights[0], tol);
+    DOUBLES_EQUAL(1.0, finalWeights[1], tol);
+    DOUBLES_EQUAL(1.0, finalWeights[2], tol);
+    DOUBLES_EQUAL(0.0, finalWeights[3], tol);
+  }
+  // initialize weights and also set known inliers/outliers
+  {
+    GncParams<GaussNewtonParams> gncParams;
+    std::vector<size_t> knownInliers;
+    knownInliers.push_back(2);
+    knownInliers.push_back(0);
+
+    std::vector<size_t> knownOutliers;
+    knownOutliers.push_back(3);
+    gncParams.setKnownInliers(knownInliers);
+    gncParams.setKnownOutliers(knownOutliers);
+
+    gncParams.setLossType(GncLossType::TLS);
+
+    Vector weights = 0.5 * Vector::Ones(fg.size());
+    auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial,
+        gncParams);
+    gnc.setWeights(weights);
+    gnc.setInlierCostThresholds(1.0);
+    Values gnc_result = gnc.optimize();
+    CHECK(assert_equal(Point2(0.0, 0.0), gnc_result.at<Point2>(X(1)), 1e-3));
+
+    // check weights were actually fixed:
+    Vector finalWeights = gnc.getWeights();
+    DOUBLES_EQUAL(1.0, finalWeights[0], tol);
+    DOUBLES_EQUAL(1.0, finalWeights[1], tol);
+    DOUBLES_EQUAL(1.0, finalWeights[2], tol);
+    DOUBLES_EQUAL(0.0, finalWeights[3], tol);
+  }
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;