GncParams.h

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 
namespace gtsam {
 
/* ************************************************************************* */
enum GncLossType {
  GM /*Geman McClure*/,
  TLS /*Truncated least squares*/
};

enum class GncScheduler {
  Linear,
  SuperLinear
};
 
template<class BaseOptimizerParameters>
class GncParams {
 public:
  typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;

  enum Verbosity {
    SILENT = 0,
    SUMMARY,
    LAMBDA,
    WEIGHTS,
    VALUES,
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
    MU = LAMBDA,
#endif
  };

  GncParams(const BaseOptimizerParameters& baseOptimizerParams)
      : baseOptimizerParams(baseOptimizerParams) {
  }

  GncParams()
      : baseOptimizerParams() {
  }

  BaseOptimizerParameters baseOptimizerParams;  
  GncLossType lossType = TLS;  
  GncScheduler scheduler = GncScheduler::Linear;  
  size_t maxIterations = 100;  
  double lambdaStep = 1.4;  
  double relativeCostTol = 1e-5;  
  double weightsTol = 1e-4;  
  double lambdaMax = 1e16;  
  Verbosity verbosity = SILENT;  
  bool allowNonNoiseModelFactors = false;  

  using IndexVector = FastVector<uint64_t>;
  IndexVector knownInliers;
  IndexVector knownOutliers;

  void setLossType(const GncLossType type) {
    lossType = type;
  }

  void setScheduler(const GncScheduler s) {
    scheduler = s;
  }

  void setMaxIterations(const size_t maxIter) {
    std::cout
        << "setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended! "
        << std::endl;
    maxIterations = maxIter;
  }

  void setLambdaStep(const double step) {
    lambdaStep = step;
  }

  void setRelativeCostTol(double value) {
    relativeCostTol = value;
  }

  void setWeightsTol(double value) {
    weightsTol = value;
  }

  void setVerbosityGNC(const Verbosity value) {
    verbosity = value;
  }

  void setKnownInliers(const IndexVector& knownIn) {
    for (size_t i = 0; i < knownIn.size(); i++){
      knownInliers.push_back(knownIn[i]);
    }
    std::sort(knownInliers.begin(), knownInliers.end());
  }

  void setKnownOutliers(const IndexVector& knownOut) {
    for (size_t i = 0; i < knownOut.size(); i++){
      knownOutliers.push_back(knownOut[i]);
    }
    std::sort(knownOutliers.begin(), knownOutliers.end());
  }
 
  void setAllowNonNoiseModelFactors(bool allow) {
    allowNonNoiseModelFactors = allow;
  }
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
  void setMuStep(const double step) { setLambdaStep(step); }
#endif

  bool equals(const GncParams& other, double tol = 1e-9) const {
    return baseOptimizerParams.equals(other.baseOptimizerParams)
        && lossType == other.lossType && maxIterations == other.maxIterations
        && std::fabs(lambdaStep - other.lambdaStep) <= tol
        && scheduler == other.scheduler
        && verbosity == other.verbosity && knownInliers == other.knownInliers
        && knownOutliers == other.knownOutliers
        && allowNonNoiseModelFactors == other.allowNonNoiseModelFactors;
  }

  void print(const std::string& str) const {
    std::cout << str << "\n";
    switch (lossType) {
      case GM:
        std::cout << "lossType: Geman McClure" << "\n";
        break;
      case TLS:
        std::cout << "lossType: Truncated Least-squares" << "\n";
        break;
      default:
        throw std::runtime_error("GncParams::print: unknown loss type.");
    }
    switch (scheduler) {
      case GncScheduler::Linear:
        std::cout << "scheduler: Linear" << "\n";
        break;
      case GncScheduler::SuperLinear:
        std::cout << "scheduler: SuperLinear" << "\n";
        break;
      default:
        throw std::runtime_error("GncParams::print: unknown scheduler type.");
    }
    std::cout << "maxIterations: " << maxIterations << "\n";
    std::cout << "lambdaStep: " << lambdaStep << "\n";
    std::cout << "relativeCostTol: " << relativeCostTol << "\n";
    std::cout << "weightsTol: " << weightsTol << "\n";
    std::cout << "verbosity: " << verbosity << "\n";
    for (size_t i = 0; i < knownInliers.size(); i++)
      std::cout << "knownInliers: " << knownInliers[i] << "\n";
    for (size_t i = 0; i < knownOutliers.size(); i++)
      std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
    std::cout << "allowNonNoiseModelFactors: " << allowNonNoiseModelFactors << "\n";
    baseOptimizerParams.print("Base optimizer params: ");
  }
};
 
}