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*/
};

template<class BaseOptimizerParameters>
class GncParams {
 public:
  typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;

  enum Verbosity {
    SILENT = 0,
    SUMMARY,
    MU,
    WEIGHTS,
    VALUES
  };

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

  GncParams()
      : baseOptimizerParams() {
  }

  BaseOptimizerParameters baseOptimizerParams;  
  GncLossType lossType = TLS;  
  size_t maxIterations = 100;  
  double muStep = 1.4;  
  double relativeCostTol = 1e-5;  
  double weightsTol = 1e-4;  
  Verbosity verbosity = SILENT;  

  //TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled.
  using IndexVector = FastVector<uint64_t>;
  IndexVector knownInliers = IndexVector();
  IndexVector knownOutliers = IndexVector();

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

  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 setMuStep(const double step) {
    muStep = 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());
  }

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

  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.");
    }
    std::cout << "maxIterations: " << maxIterations << "\n";
    std::cout << "muStep: " << muStep << "\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";
    baseOptimizerParams.print("Base optimizer params: ");
  }
};

}