RISAM.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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/ISAM2.h>
#include <gtsam/nonlinear/internal/ChiSquaredInverse.h>
#include <gtsam/sam/RISAMGraduatedFactor.h>
 
namespace gtsam {

struct RISAMParams {
  RISAMParams(ISAM2Params isam2Params = ISAM2Params(),
              bool incrementOutlierMu = true,
              double outlierMuChiSquaredUpperBound = 0.95,
              double outlierMuChiSquaredLowerBound = 0.25,
              double outlierMuAverageVariableConvergenceThreshold = 0.01,
              size_t numberExtraIterations = 1)
      : isam2Params(isam2Params),
        incrementOutlierMu(incrementOutlierMu),
        outlierMuChiSquaredUpperBound(outlierMuChiSquaredUpperBound),
        outlierMuChiSquaredLowerBound(outlierMuChiSquaredLowerBound),
        outlierMuAverageVariableConvergenceThreshold(
            outlierMuAverageVariableConvergenceThreshold),
        numberExtraIterations(numberExtraIterations) {}

  ISAM2Params isam2Params;

  // See RISAM::incrementMuInits for details.
  bool incrementOutlierMu;
  double outlierMuChiSquaredUpperBound;
  double outlierMuChiSquaredLowerBound;
  double outlierMuAverageVariableConvergenceThreshold;
  size_t numberExtraIterations;
};

struct RISAMUpdateResult {
  ISAM2Result isam2Result;

  std::set<Key> involvedVariables;
  std::set<Key> affectedVariables;
  std::set<FactorIndex> convexifiedFactors;
};

class GTSAM_EXPORT RISAM {
 public:
  typedef RISAMParams Parameters;
  typedef RISAMUpdateResult UpdateResult;

 protected:
  Parameters params_;
  std::unique_ptr<ISAM2> solver_;
  FastVector<std::shared_ptr<double>> mu_;
  FastVector<std::shared_ptr<double>> muInits_;
  FastSet<size_t> factorsToCheckStatus_;

  VariableIndex variableIndex_;
  NonlinearFactorGraph factors_;

 public:
  RISAM(const Parameters& params) : params_(params) {
    // We must have relinearization skip as 1.
    params_.isam2Params.relinearizeSkip = 1;
    // We must not use cached factors.
    params_.isam2Params.cacheLinearizedFactors = false;
 
    // Construct the encapsulated solver using the modified parameters.
    solver_ = std::make_unique<ISAM2>(params_.isam2Params);
  }

  UpdateResult update(
      const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
      const Values& newTheta = Values(),
      const std::optional<std::set<Key>> extraGncInvolvedKeys = std::nullopt,
      const FactorIndices& removeFactorIndices = FactorIndices(),
      const std::optional<FastMap<Key, int>>& constrainedKeys = std::nullopt,
      const std::optional<FastList<Key>>& noRelinKeys = std::nullopt,
      const std::optional<FastList<Key>>& extraReelimKeys = std::nullopt,
      bool force_relinearize = false);

  UpdateResult update(const NonlinearFactorGraph& newFactors,
                      const Values& newTheta,
                      const std::optional<std::set<Key>> extraGncInvolvedKeys,
                      const ISAM2UpdateParams& updateParams);

  Values calculateEstimate();

  const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; }

  std::set<size_t> getOutliers(double chiSquaredOutlierThreshold);

 protected:
  UpdateResult updateRobust(
      const NonlinearFactorGraph& newFactors, const Values& newTheta,
      const std::optional<std::set<Key>>& extraGncInvolvedKeys,
      const ISAM2UpdateParams& updateParams);

  FactorIndices runRobustIteration(
      const FactorIndices& convexFactors,
      std::map<FactorIndex, size_t>& muUpdateCount);

  void updateConvexFactorMu(const Values& currentEstimate, const size_t fidx,
                            std::map<FactorIndex, size_t>& muUpdateCount,
                            FastList<Key>& convexKeys);

  FactorIndices updateConvexFactors(const FactorIndices& convexFactors) const;

  FactorIndices convexifyInvolvedFactors(
      const NonlinearFactorGraph& newFactors, const Values& newTheta,
      const std::optional<std::set<Key>>& extraGncInvolvedKeys,
      ISAM2UpdateParams& internalUpdateParams, UpdateResult& updateResult);

  KeySet accumulateInvolvedKeys(
      const NonlinearFactorGraph& newFactors,
      const std::optional<std::set<Key>>& extraGncInvolvedKeys,
      ISAM2UpdateParams& updateParams) const;

  void convexifyFactorIfInvolved(const FactorIndex fidx,
                                 const KeySet& involvedKeys,
                                 const KeySet& affectedKeys,
                                 const bool isBatchUpdate,
                                 std::set<FactorIndex>& convexFactors);

  void updateHousekeeping(const NonlinearFactorGraph& newFactors,
                          const ISAM2UpdateParams& updateParams);

  void augmentMu(const NonlinearFactorGraph& newFactors,
                 const FactorIndices& newFactorIndices);

  void incrementMuInits();

 public:
  template <class FACTOR_TYPE>
  static auto MakeGraduated(GraduatedFactor::RobustLoss::shared_ptr loss,
                            GraduationScheduler::shared_ptr scheduler) {
    return [loss = std::move(loss),
            scheduler = std::move(scheduler)](auto&&... factorArgs) ->
           typename GenericGraduatedFactor<FACTOR_TYPE>::shared_ptr {
             return std::make_shared<GenericGraduatedFactor<FACTOR_TYPE>>(
                 loss, scheduler,
                 std::forward<decltype(factorArgs)>(factorArgs)...);
           };
  }

};
 
}  // namespace gtsam