ISAM2-impl.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/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/ISAM2.h>

namespace gtsam {

struct GTSAM_EXPORT ISAM2::Impl {

  struct GTSAM_EXPORT PartialSolveResult {
    ISAM2::sharedClique bayesTree;
  };

  struct GTSAM_EXPORT ReorderingMode {
    size_t nFullSystemVars;
    enum { /*AS_ADDED,*/ COLAMD } algorithm;
    enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
    boost::optional<FastMap<Key,int> > constrainedKeys;
  };

  static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
      VectorValues& deltaNewton, VectorValues& RgProd,
      const KeyFormatter& keyFormatter = DefaultKeyFormatter);

  static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
    NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices);
    
  static void RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
    Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
    VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes,
    KeySet& fixedVariables);

  static KeySet CheckRelinearizationFull(const VectorValues& delta,
      const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);

  static KeySet CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
    const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);

  static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask);

  static void ExpmapMasked(Values& values, const VectorValues& delta,
      const KeySet& mask,
      boost::optional<VectorValues&> invalidateIfDebug = boost::none,
      const KeyFormatter& keyFormatter = DefaultKeyFormatter);

  static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
      const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold);

  static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
      const VectorValues& gradAtZero, VectorValues& RgProd);
  
  static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero,
                                            const VectorValues& RgProd);

};

}