added gnc loop

tests/testGncOptimizer.cpp

@@ -29,21 +29,70 @@ using namespace gtsam;
 
 using symbol_shorthand::X;
 using symbol_shorthand::L;
+static double tol = 1e-7;
 
 /* ************************************************************************* */
 template <class BaseOptimizerParameters>
 class GncParams {
 public:
 
-  // using BaseOptimizer = BaseOptimizerParameters::OptimizerType;
+  /** See NonlinearOptimizerParams::verbosity */
-  GncParams(const BaseOptimizerParameters& baseOptimizerParams): baseOptimizerParams(baseOptimizerParams) {}
+  enum RobustLossType {
+    GM /*Geman McClure*/, TLS /*Truncated least squares*/
+  };
+
+  // using BaseOptimizer = GaussNewtonOptimizer; // BaseOptimizerParameters::OptimizerType;
+
+  GncParams(const BaseOptimizerParameters& baseOptimizerParams):
+    baseOptimizerParams(baseOptimizerParams),
+    lossType(GM),      /* default loss*/
+    maxIterations(100), /* maximum number of iterations*/
+    barcSq(1.0), /* a factor is considered an inlier if factor.error() < barcSq. Note that factor.error() whitens by the covariance*/
+    muStep(1.4){}/* multiplicative factor to reduce/increase the mu in gnc */
 
   // default constructor
   GncParams(): baseOptimizerParams() {}
 
   BaseOptimizerParameters baseOptimizerParams;
-
   /// any other specific GNC parameters:
+  RobustLossType lossType;
+  size_t maxIterations;
+  double barcSq;
+  double muStep;
+
+  void setLossType(RobustLossType type){ lossType = type; }
+  void setMaxIterations(size_t maxIter){
+    std::cout
+    << "setMaxIterations: changing the max number of iterations might lead to less accurate solutions and is not recommended! "
+    << std::endl;
+    maxIterations = maxIter;
+  }
+  void setInlierThreshold(double inth){ barcSq = inth; }
+  void setMuStep(double step){ muStep = step; }
+
+  /// equals
+  bool equals(const GncParams& other, double tol = 1e-9) const {
+    return baseOptimizerParams.equals(other.baseOptimizerParams)
+        && lossType == other.lossType
+        && maxIterations == other.maxIterations
+        && std::fabs(barcSq - other.barcSq) <= tol
+        && std::fabs(muStep - other.muStep) <= tol;
+  }
+
+  /// print function
+  void print(const std::string& str) const {
+    std::cout << str << "\n";
+    switch(lossType) {
+    case GM: std::cout << "lossType: Geman McClure" << "\n"; break;
+    default:
+      throw std::runtime_error(
+          "GncParams::print: unknown loss type.");
+    }
+    std::cout << "maxIterations: " << maxIterations << "\n";
+    std::cout << "barcSq: " << barcSq << "\n";
+    std::cout << "muStep: " << muStep << "\n";
+    baseOptimizerParams.print(str);
+  }
 };
 
 /* ************************************************************************* */
@@ -64,33 +113,88 @@ public:
     // TODO: Check that all noise models are Gaussian
   }
 
-//  Values optimize() const {
+  NonlinearFactorGraph getFactors() const { return NonlinearFactorGraph(nfg_); }
-//    NonlinearFactorGraph currentGraph = graph_;
+  Values getState() const { return Values(state_); }
-//  for (i : {1, 2, 3}) {
+  GncParameters getParams() const { return GncParameters(params_); }
-//    BaseOptimizer::Optimizer baseOptimizer(currentGraph, initial);
-//    VALUES currentSolution = baseOptimizer.optimize();
-//    if (converged) {
-//      return currentSolution;
-//    }
-//    graph_i = this->makeGraph(currentSolution);
-//  }
-//}
 
-//NonlinearFactorGraph makeGraph(const Values& currentSolution) const {
+  /// implement GNC main loop, including graduating nonconvexity with mu
-//  // calculate some weights
+  Values optimize() {
-//  this->calculateWeights();
+    // start by assuming all measurements are inliers
-//  // copy the graph with new weights
+    Vector weights = Vector::Ones(nfg_.size());
-//
+    GaussNewtonOptimizer baseOptimizer(nfg_,state_);
-//}
+    Values result = baseOptimizer.optimize();
+    double mu = initializeMu();
+    for(size_t iter=0; iter < params_.maxIterations; iter++){
+      // weights update
+      weights = calculateWeights(result, mu);
+
+      // variable/values update
+      NonlinearFactorGraph graph_iter = this->makeGraph(weights);
+      GaussNewtonOptimizer baseOptimizer_iter(graph_iter, state_);
+      Values result = baseOptimizer.optimize();
+
+      // stopping condition
+      if( checkMuConvergence(mu) ) { break; }
+
+      // otherwise update mu
+      mu = updateMu(mu);
+    }
+    return result;
+  }
+
+  /// initialize the gnc parameter mu such that loss is approximately convex
+  double initializeMu() const {
+    // compute largest error across all factors
+    double rmax_sq = 0.0;
+    for (size_t i = 0; i < nfg_.size(); i++) {
+      if(nfg_[i]){
+        rmax_sq = std::max(rmax_sq, nfg_[i]->error(state_));
+      }
+    }
+    // set initial mu
+    switch(params_.lossType) {
+    case GncParameters::GM:
+      return  2*rmax_sq / params_.barcSq; // initial mu
+    default:
+      throw std::runtime_error(
+          "GncOptimizer::initializeMu: called with unknown loss type.");
+    }
+  }
+
+  /// update the gnc parameter mu to gradually increase nonconvexity
+  double updateMu(const double mu) const {
+    switch(params_.lossType) {
+    case GncParameters::GM:
+      return  std::max(1.0 , mu / params_.muStep); // reduce mu, but saturate at 1
+    default:
+      throw std::runtime_error(
+          "GncOptimizer::updateMu: called with unknown loss type.");
+    }
+  }
+
+  /// check if we have reached the value of mu for which the surrogate loss matches the original loss
+  bool checkMuConvergence(const double mu) const {
+    switch(params_.lossType) {
+    case GncParameters::GM:
+      return std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function
+    default:
+      throw std::runtime_error(
+          "GncOptimizer::checkMuConvergence: called with unknown loss type.");
+    }
+  }
+
+  /// create a graph where each factor is weighted by the gnc weights
+  NonlinearFactorGraph makeGraph(const Vector& weights) const {
+   return NonlinearFactorGraph(nfg_);
+  }
+
+  /// calculate gnc weights
+  Vector calculateWeights(const Values currentEstimate, const double mu){
+    Vector weights = Vector::Ones(nfg_.size());
+    return weights;
+  }
 };
 
-///* ************************************************************************* */
-//TEST(GncOptimizer, calculateWeights) {
-//}
-//
-///* ************************************************************************* */
-//TEST(GncOptimizer, copyGraph) {
-//}
 
 /* ************************************************************************* */
 TEST(GncOptimizer, gncParamsConstructor) {
@@ -106,7 +210,7 @@ TEST(GncOptimizer, gncParamsConstructor) {
 
   // and check params become different if we change lmParams
   lmParams.setVerbosity("DELTA");
-  CHECK(!lmParams.equals(gncParams1.baseOptimizerParams));
+  CHECK(! lmParams.equals(gncParams1.baseOptimizerParams));
 
   // and same for GN
   GaussNewtonParams gnParams;
@@ -116,9 +220,44 @@ TEST(GncOptimizer, gncParamsConstructor) {
   // check default constructor
   GncParams<GaussNewtonParams> gncParams2b;
   CHECK(gnParams.equals(gncParams2b.baseOptimizerParams));
+
+  // change something at the gncParams level
+  GncParams<GaussNewtonParams> gncParams2c(gncParams2b);
+  gncParams2c.setLossType(GncParams<GaussNewtonParams>::RobustLossType::TLS);
+  CHECK(! gncParams2c.equals(gncParams2b.baseOptimizerParams));
 }
 
 /* ************************************************************************* */
+TEST(GncOptimizer, gncConstructor) {
+  // has to have Gaussian noise models !
+  auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point
+
+  Point2 p0(3, 3);
+  Values initial;
+  initial.insert(X(1), p0);
+
+  LevenbergMarquardtParams lmParams;
+  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
+  auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
+
+  CHECK(gnc.getFactors().equals(fg));
+  CHECK(gnc.getState().equals(initial));
+  CHECK(gnc.getParams().equals(gncParams));
+}
+
+///* ************************************************************************* */
+//TEST(GncOptimizer, calculateWeights) {
+//}
+//
+///* ************************************************************************* */
+//TEST(GncOptimizer, calculateWeights) {
+//}
+//
+///* ************************************************************************* */
+//TEST(GncOptimizer, copyGraph) {
+//}
+
+/* ************************************************************************* *
 TEST(GncOptimizer, makeGraph) {
   // has to have Gaussian noise models !
   auto fg = example::createReallyNonlinearFactorGraph(); // just a unary factor on a 2D point
@@ -134,7 +273,7 @@ TEST(GncOptimizer, makeGraph) {
 //  NonlinearFactorGraph actual = gnc.makeGraph(initial);
 }
 
-/* ************************************************************************* *
+/* ************************************************************************* */
 TEST(GncOptimizer, optimize) {
   // has to have Gaussian noise models !
   auto fg = example::createReallyNonlinearFactorGraph();
@@ -144,10 +283,13 @@ TEST(GncOptimizer, optimize) {
   initial.insert(X(1), p0);
 
   LevenbergMarquardtParams lmParams;
-  GncParams gncParams(lmParams);
+  GncParams<LevenbergMarquardtParams> gncParams(lmParams);
-  auto gnc = GncOptimizer(fg, initial, gncParams);
+  auto gnc = GncOptimizer<GncParams<LevenbergMarquardtParams>>(fg, initial, gncParams);
+
+  gncParams.print("");
+
   Values actual = gnc.optimize();
-  DOUBLES_EQUAL(0, fg.error(actual2), tol);
+  DOUBLES_EQUAL(0, fg.error(actual), tol);
 }
 
 /* ************************************************************************* */