RISAMGraduatedFactor.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/NonlinearFactor.h>
#include <gtsam/sam/RISAMGraduationScheduler.h>
 
#include <cmath>
 
namespace gtsam {

class GTSAM_EXPORT GraduatedFactor {
 public:
  typedef std::shared_ptr<GraduatedFactor> shared_ptr;
  typedef noiseModel::mEstimator::Base RobustLoss;

 protected:
  RobustLoss::shared_ptr robustLoss_;
  GraduationScheduler::shared_ptr scheduler_;

  std::shared_ptr<double> mu_;

  friend class RISAM;

 public:
  GraduatedFactor(RobustLoss::shared_ptr loss,
                  GraduationScheduler::shared_ptr scheduler)
      : robustLoss_(loss), scheduler_(scheduler) {
    mu_ = std::make_shared<double>(scheduler_->muInit());
  }

  GraduatedFactor(const GraduatedFactor& other)
      : robustLoss_(other.robustLoss_), scheduler_(other.scheduler_) {
    mu_ = std::make_shared<double>(*(other.mu_));
  }
 
  virtual ~GraduatedFactor() = default;

  virtual GaussianFactor::shared_ptr linearizeGraduated(
      const Values& currentEstimate) const = 0;

  virtual double residual(const Values& currentEstimate) const = 0;

  virtual double robustLoss(const Values& currentEstimate) const = 0;

  const RobustLoss::shared_ptr& loss() const { return robustLoss_; }

  const GraduationScheduler::shared_ptr& scheduler() const {
    return scheduler_;
  }

  virtual NonlinearFactor::shared_ptr cloneUngraduated() const = 0;
};

template <class FACTOR_TYPE>
class GenericGraduatedFactor : public FACTOR_TYPE, public GraduatedFactor {
  static_assert(std::is_base_of<NonlinearFactor, FACTOR_TYPE>::value,
                "GraduatedFactor Must be instantiated with a Factor Derived "
                "from NonlinearFactor.");

 public:
  typedef std::shared_ptr<GenericGraduatedFactor<FACTOR_TYPE>> shared_ptr;

 public:
  template <class... Args>
  GenericGraduatedFactor(RobustLoss::shared_ptr loss,
                         GraduationScheduler::shared_ptr scheduler,
                         Args&&... args)
      : FACTOR_TYPE(std::forward<Args>(args)...),
        GraduatedFactor(loss, scheduler) {}

  GenericGraduatedFactor(const GenericGraduatedFactor<FACTOR_TYPE>& other)
      : FACTOR_TYPE(other), GraduatedFactor(other) {}

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(
            new GenericGraduatedFactor<FACTOR_TYPE>(*this)));
  }

  GaussianFactor::shared_ptr linearizeGraduated(
      const Values& currentEstimate) const override {
    double residual = this->residual(currentEstimate);
 
    // Use base factor to linearize
    auto whitenedLinearSystem = FACTOR_TYPE::linearize(currentEstimate);
    auto [A, b] = whitenedLinearSystem->jacobian();
    const size_t outputDim = b.size();
 
    // Extract the non-dense linear system
    auto keys = whitenedLinearSystem->keys();
    std::vector<Matrix> Ablocks;
    size_t indexStart = 0;
    for (const auto& key : keys) {
      size_t d = currentEstimate.at(key).dim();
      Ablocks.push_back(A.block(0, indexStart, outputDim, d));
      indexStart += d;
    }
 
    // Weight the Linearized Blocks
    const double sqrtWeight =
        std::sqrt(robustLoss_->graduatedWeight(residual, *mu_));
    for (Matrix& Aj : Ablocks) {
      Aj *= sqrtWeight;
    }
    b *= sqrtWeight;
 
    // Construct a jacobian factor from the weighted system
    FastMap<Key, Matrix> AblockMap;
    for (size_t i = 0; i < Ablocks.size(); i++) {
      AblockMap[keys[i]] = Ablocks[i];
    }
    return std::make_shared<JacobianFactor>(AblockMap, b);
  }

  GaussianFactor::shared_ptr linearize(
      const Values& currentEstimate) const override {
    // Delegate to linearizeGraduated which is required by the GraduatedFactor
    // Interface
    return linearizeGraduated(currentEstimate);
  }

  double error(const Values& values) const override {
    return robustLoss(values);
  }


  double residual(const Values& currentEstimate) const override {
    return std::sqrt(2.0 * FACTOR_TYPE::error(currentEstimate));
  }

  double robustLoss(const Values& currentEstimate) const override {
    return robustLoss_->graduatedLoss(residual(currentEstimate), *mu_);
  }

  NonlinearFactor::shared_ptr cloneUngraduated() const override {
    return FACTOR_TYPE::clone();
  }

};
}  // namespace gtsam