diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 1a269468b..db9e780e2 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/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(); diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 7160c32fd..a3d1e4e9b 100644 --- a/tests/testGncOptimizer.cpp +++ b/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 gncParams; + gncParams.setLossType(GncLossType::TLS); + + Vector weights = 0.5 * Vector::Ones(fg.size()); + auto gnc = GncOptimizer>(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(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 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>(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(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 gncParams; + std::vector knownInliers; + knownInliers.push_back(2); + knownInliers.push_back(0); + + std::vector 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>(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(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;