RISAMGraduationScheduler.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/base/FastVector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/nonlinear/internal/ChiSquaredInverse.h>
 
#include <memory>
#include <optional>
#include <stdexcept>
 
namespace gtsam {

class GTSAM_EXPORT GraduationScheduler {
 public:
  typedef std::shared_ptr<GraduationScheduler> shared_ptr;
  typedef std::function<double(double, double, size_t)> MuUpdateStrategy;

 protected:
  double muInit_;
  double convergenceThreshold_;
  MuUpdateStrategy muUpdateStrategy_;
  double muInitIncrement_;

 public:
  GraduationScheduler(MuUpdateStrategy muUpdateStrategy = MuUpdateStable,
                      double muInitIncrement = 0.2, double muInit = 0.0)
      : muInit_(muInit),
        convergenceThreshold_(1.0),
        muUpdateStrategy_(muUpdateStrategy),
        muInitIncrement_(muInitIncrement) {
    if (!(muInit >= 0.0 && muInit <= 1.0)) {
      throw std::invalid_argument(
          "GraduationScheduler: muInit must be in [0, 1].");
    }
  }

  double muInit() const { return muInit_; }

  double updateMu(const double& mu, const double& residual,
                  const size_t& updateCount) const;

  double updateMuInit(const double& muInit, const bool isInlier) const;

  bool isMuConverged(const double& mu) const;

 public:
  static double MuUpdateMcGann2023(const double& mu, const double& residual,
                                   const size_t& updateCount);
  static double MuUpdateStable(const double& mu, const double& residual,
                               const size_t& updateCount);
};
}  // namespace gtsam