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 GTSAM_EXPORT GncParams {
 public:
  typedef typename BaseOptimizerParameters::OptimizerType OptimizerType;
 
  enum Verbosity {
    SILENT = 0,
    SUMMARY,
    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;  
  std::vector<size_t> knownInliers = std::vector<size_t>();  
  std::vector<size_t> knownOutliers = std::vector<size_t>();  
 
  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 std::vector<size_t>& knownIn) {
    for (size_t i = 0; i < knownIn.size(); i++){
      knownInliers.push_back(knownIn[i]);
    }
    std::sort(knownInliers.begin(), knownInliers.end());
  }
 
  void setKnownOutliers(const std::vector<size_t>& 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(str);
  }
};
 
}