ISAM2.h/ISAM2.cpp

xlsemmmmmm

已于 2022-11-08 15:02:53 修改

阅读量455

点赞数

分类专栏：因子图优化文章标签： c++

于 2022-11-02 20:33:55 首次发布

本文链接：https://blog.csdn.net/weixin_43848456/article/details/127649720

版权

因子图优化专栏收录该内容

27 篇文章 3 订阅

订阅专栏

0、成员变量

 protected:
  /** The current linearization point */
  Values theta_;

  /** VariableIndex lets us look up factors by involved variable and keeps track
   * of dimensions */
  VariableIndex variableIndex_;

  /** The linear delta from the last linear solution, an update to the estimate
   * in theta
   *
   * This is \c mutable because it is a "cached" variable - it is not updated
   * until either requested with getDelta() or calculateEstimate(), or needed
   * during update() to evaluate whether to relinearize variables.
   */
  mutable VectorValues delta_;

  mutable VectorValues deltaNewton_;  // Only used when using Dogleg - stores
                                      // the Gauss-Newton update
  mutable VectorValues RgProd_;  // Only used when using Dogleg - stores R*g and
                                 // is updated incrementally

  /** A cumulative mask for the variables that were replaced and have not yet
   * been updated in the linear solution delta_, this is only used internally,
   * delta will always be updated if necessary when requested with getDelta()
   * or calculateEstimate().
   *
   * This is \c mutable because it is used internally to not update delta_
   * until it is needed.
   */
  mutable KeySet deltaReplacedMask_;  // TODO(dellaert): Make sure accessed in
                                      // the right way

  /** All original nonlinear factors are stored here to use during
   * relinearization */
  NonlinearFactorGraph nonlinearFactors_;

  /** The current linear factors, which are only updated as needed */
  mutable GaussianFactorGraph linearFactors_;

  /** The current parameters */
  ISAM2Params params_;

  /** The current Dogleg Delta (trust region radius) */
  mutable boost::optional<double> doglegDelta_;

  /** Set of variables that are involved with linear factors from marginalized
   * variables and thus cannot have their linearization points changed. */
  KeySet fixedVariables_;

  int update_count_;  ///< Counter incremented every update(), used to determine
                      ///< periodic relinearization

 public:
  using This = ISAM2;                       ///< This class
  using Base = BayesTree<ISAM2Clique>;      ///< The BayesTree base class
  using Clique = Base::Clique;              ///< A clique
  using sharedClique = Base::sharedClique;  ///< Shared pointer to a clique
  using Cliques = Base::Cliques;            ///< List of Cliques

0.1 theta_

  /** The current linearization point */
  Values theta_;

当前的线性化点

0.2 variableIndex_

  /** VariableIndex lets us look up factors by involved variable and keeps track
   * of dimensions */
  VariableIndex variableIndex_;

VariableIndex 让我们通过涉及的变量查找因子并跟踪维度

0.3 delta_

上一个线性解的线性delta，对 theta 中估计的更新
这是可变的，因为它是一个“缓存”变量 - 在使用 getDelta() 或 calculateEstimate() 请求或在 update() 期间需要评估是否重新线性化变量之前，它不会更新。

  /** The linear delta from the last linear solution, an update to the estimate
   * in theta
   *
   * This is \c mutable because it is a "cached" variable - it is not updated
   * until either requested with getDelta() or calculateEstimate(), or needed
   * during update() to evaluate whether to relinearize variables.
   */
  mutable VectorValues delta_;

0.4 deltaNewton_

  mutable VectorValues deltaNewton_;  // Only used when using Dogleg - stores
                                      // the Gauss-Newton update

0.5 RgProd_

  mutable VectorValues RgProd_;  // Only used when using Dogleg - stores R*g and
                                 // is updated incrementally

0.6 deltaReplacedMask_

  /** A cumulative mask for the variables that were replaced and have not yet
   * been updated in the linear solution delta_, this is only used internally,
   * delta will always be updated if necessary when requested with getDelta()
   * or calculateEstimate().
   *
   * This is \c mutable because it is used internally to not update delta_
   * until it is needed.
   */
  mutable KeySet deltaReplacedMask_;  // TODO(dellaert): Make sure accessed in
                                      // the right way

线性解 delta_ 中已替换且尚未更新的变量的累积掩码，仅在内部使用，如有必要，当使用 getDelta() 或 calculateEstimate() 请求时，将始终更新 delta。
这是可变的，因为它在内部用于在需要之前不更新 delta_。

0.7 nonlinearFactors_

  /** All original nonlinear factors are stored here to use during
   * relinearization */
  NonlinearFactorGraph nonlinearFactors_;

所有原始非线性因子都存储在此处以在重新线性化期间使用

0.8 linearFactors_

  /** The current linear factors, which are only updated as needed */
  mutable GaussianFactorGraph linearFactors_;

当前的线性因子，仅根据需要更新

0.9 params_

  /** The current parameters */
  ISAM2Params params_;

0.10 doglegDelta_

  /** The current Dogleg Delta (trust region radius) */
  mutable boost::optional<double> doglegDelta_;

0.11 fixedVariables_

  /** Set of variables that are involved with linear factors from marginalized
   * variables and thus cannot have their linearization points changed. */
  KeySet fixedVariables_;

与来自边缘化变量的线性因子相关的变量集，因此不能改变它们的线性化点

0.12 update_count_

  int update_count_;  ///< Counter incremented every update(), used to determine
                      ///< periodic relinearization

计数器每次 update() 递增，用于确定周期性重新线性化

一、update

1.1 重载版本1

添加新因素，根据需要更新解决方案并重新线性化。
可选地，此功能从系统中删除现有因素，以启用诸如将现有因素与新因素交换等行为。
向当前系统添加新的测量值和可选的新变量。
这将运行 ISAM2 算法的完整步骤，根据野火和重新线性化阈值，根据需要重新线性化和更新解决方案。

@param newFactors 要添加到系统中的新因子
@param newTheta 要添加到系统的新变量的初始化点。您必须在此处包括出现在 newFactors 中的所有新变量（系统中还没有这些变量）。这里不能有任何变量没有出现在 newFactors 中，另外，系统中已经存在的变量也不能包含在此处。
@param removeFactorIndices 要从系统中删除的因子的索引
@param force_relinearize 重新线性化增量幅度足够大的任何变量（Params::relinearizeThreshold），而不管重新线性化间隔（Params::relinearizeSkip）。
@param constrainedKeys 是组标签的键的可选映射，这样可以将变量约束到 BayesTree 中的特定分组
@param noRelinKeys 是一组可选的非线性键，iSAM2 将保持在一个恒定的线性化点，无论线性增量的大小如何
@param extraReelimKeys 是一组可选的非线性键，iSAM2 将重新消除，无论线性增量的大小如何。这允许重新排序提供的键。
@return 包含有关更新信息的 ISAM2Result 结构

  /**
   * Add new factors, updating the solution and relinearizing as needed.
   *
   * Optionally, this function remove existing factors from the system to enable
   * behaviors such as swapping existing factors with new ones.
   *
   * Add new measurements, and optionally new variables, to the current system.
   * This runs a full step of the ISAM2 algorithm, relinearizing and updating
   * the solution as needed, according to the wildfire and relinearize
   * thresholds.
   *
   * @param newFactors The new factors to be added to the system
   * @param newTheta Initialization points for new variables to be added to the
   * system. You must include here all new variables occuring in newFactors
   * (which were not already in the system).  There must not be any variables
   * here that do not occur in newFactors, and additionally, variables that were
   * already in the system must not be included here.
   * @param removeFactorIndices Indices of factors to remove from system
   * @param force_relinearize Relinearize any variables whose delta magnitude is
   * sufficiently large (Params::relinearizeThreshold), regardless of the
   * relinearization interval (Params::relinearizeSkip).
   * @param constrainedKeys is an optional map of keys to group labels, such
   * that a variable can be constrained to a particular grouping in the
   * BayesTree
   * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will
   * hold at a constant linearization point, regardless of the size of the
   * linear delta
   * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will
   * re-eliminate, regardless of the size of the linear delta. This allows the
   * provided keys to be reordered.
   * @return An ISAM2Result struct containing information about the update
   */
  virtual ISAM2Result update(
      const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
      const Values& newTheta = Values(),
      const FactorIndices& removeFactorIndices = FactorIndices(),
      const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,
      const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
      const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
      bool force_relinearize = false);

翻译

/**
*添加新因素，根据需要更新解决方案并重新升级。
*
*可选地，此功能从系统中删除现有因素，以启用诸如将现有因素与新因素交换等行为。
*
*向当前系统添加新的测量值和可选的新变量。
*这将运行ISAM2算法的全部步骤，重新搜索和更新
*根据野火和重新启动阈值，根据需要提供解决方案。
*
*@param newFactors要添加到系统中的新因素
*@param newTheta初始化指向要添加到系统中的新变量。
您必须在此处包含newFactors中出现的所有新变量（这些变量尚未在系统中）。
这里不能有newFactors中没有出现的变量，另外，系统中已经存在的变量也不能包含在这里。
*@param removeFactorIndices要从系统中删除的因子索引
*@param force_relinealize重新线性化增量幅度足够大的任何变量（Params:：relinearizeThreshold），而不管重新线性化间隔（param:：relinealizeSkip）。
*@param constrainedKeys是键到组标签的可选映射，这样可以将变量约束到BayesTree中的特定组
*@param noRelinKeys是一组可选的非线性密钥，无论线性增量的大小如何，iSAM2都将保持在恒定的线性化点
*@param extraReelimKeys是一组可选的非线性密钥，无论线性增量的大小如何，iSAM2都将重新消除。
这允许对提供的密钥进行重新排序。
*@return包含更新信息的ISAM2Result结构
*/

1.2 重载版本2

添加新因素，根据需要更新解决方案并重新线性化。
update() 的替代签名（参见上面的文档），所有附加参数都在一个结构中。如果参数发生变化，这种形式更容易保持未来的 API/ABI 兼容性。
@param newFactors 要添加到系统中的新因子
@param newTheta 要添加到系统的新变量的初始化点。您必须在此处包括出现在 newFactors 中的所有新变量（系统中还没有这些变量）。这里不能有任何变量没有出现在 newFactors 中，另外，系统中已经存在的变量也不能包含在此处。
@param updateParams 用于控制重新线性化、约束键等的附加参数。
@return 包含有关更新信息的 ISAM2Result 结构
@note 没有默认参数以避免模棱两可的调用错误。

  /**
   * Add new factors, updating the solution and relinearizing as needed.
   *
   * Alternative signature of update() (see its documentation above), with all
   * additional parameters in one structure. This form makes easier to keep
   * future API/ABI compatibility if parameters change.
   *
   * @param newFactors The new factors to be added to the system
   * @param newTheta Initialization points for new variables to be added to the
   * system. You must include here all new variables occuring in newFactors
   * (which were not already in the system).  There must not be any variables
   * here that do not occur in newFactors, and additionally, variables that were
   * already in the system must not be included here.
   * @param updateParams Additional parameters to control relinearization,
   * constrained keys, etc.
   * @return An ISAM2Result struct containing information about the update
   * @note No default parameters to avoid ambiguous call errors.
   */
  virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,
                             const Values& newTheta,
                             const ISAM2UpdateParams& updateParams);

翻译

/*
添加新因素，根据需要更新解决方案并重新升级。
update（）的替代签名（参见上面的文档），在一个结构中包含所有附加参数。
如果参数发生变化，此表单可以更容易地保持未来的API/ABI兼容性。
*
*@param newFactors要添加到系统中的新因素
*@param newTheta初始化指向要添加到系统中的新变量。
您必须在此处包含newFactors中出现的所有新变量（这些变量尚未在系统中）。
这里不能有newFactors中没有出现的变量，另外，系统中已经存在的变量也不能包含在这里。
*@param updateParams控制重新排列、约束键等的其他参数。
*@return包含更新信息的ISAM2Result结构
@注意：无默认参数可避免不明确的调用错误。
*/

对于参数constrainedKeys解决exception是有必要的
当请求边缘化来自 ISAM2 的非叶子变量时抛出。要使您希望边缘化的变量成为叶，应使用 ISAM2::update() 的 constrainedKeys 参数来限制它们的顺序。

“请求边缘化变量” + formatter_(key_) + “，但该变量不是叶子。要使您想要边缘化的变量成为叶子，应使用 ISAM2::update( ).";

1.3

上述都是
添加新的因子(const NonlinearFactorGraph& newFactor),
添加新的优化变量(const Values& newTheta)
这两个添加的是有条件的
显然因子可以与isam2中的旧的变量产生联系,已经存在的变量系统不能存在两次，如果因子中不与某一个变量(假设为变量A)产生联系，那么向系统中进行变量A的添加必然是无效的

二、marginalizeLeaves

  /** Marginalize out variables listed in leafKeys.  These keys must be leaves
   * in the BayesTree.  Throws MarginalizeNonleafException if non-leaves are
   * requested to be marginalized.  Marginalization leaves a linear
   * approximation of the marginal in the system, and the linearization points
   * of any variables involved in this linear marginal become fixed.  The set
   * fixed variables will include any key involved with the marginalized
   * variables in the original factors, and possibly additional ones due to
   * fill-in.
   *
   * If provided, 'marginalFactorsIndices' will be augmented with the factor
   * graph indices of the marginal factors added during the 'marginalizeLeaves'
   * call
   *
   * If provided, 'deletedFactorsIndices' will be augmented with the factor
   * graph indices of any factor that was removed during the 'marginalizeLeaves'
   * call
   */
  void marginalizeLeaves(
      const FastList<Key>& leafKeys,
      boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
      boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);

翻译：

/**
将leafKeys中列出的变量边缘化。 这些键必须是 BayesTree 中的叶子。 
如果请求边缘化非叶子，则抛出 MarginalizeNonleafException。
边际化在系统中留下了边际的线性近似，并且该线性边际中涉及的任何变量的线性化点变得固定。
设置的固定变量将包括原始因子中与边缘化变量相关的任何键，以及可能由于填充而增加的键。

如果提供，“marginalFactorsIndices”将增加在“marginalizeLeaves”调用期间添加的边际因子的因子图索引

如果提供，“deletedFactorsIndices”将增加在“marginalizeLeaves”调用期间删除的任何因子的因子图索引
*/

上述节点必须是叶子节点

三、获取线性点和检查值是否存在

  /// Access the current linearization point
  const Values& getLinearizationPoint() const { return theta_; }

  /// Check whether variable with given key exists in linearization point
  bool valueExists(Key key) const { return theta_.exists(key); }  /// Access the current linearization point
  const Values& getLinearizationPoint() const { return theta_; }

  /// Check whether variable with given key exists in linearization point
  bool valueExists(Key key) const { return theta_.exists(key); }

四、计算估计值

4.1 重载1

  /** Compute an estimate from the incomplete linear delta computed during the
   * last update. This delta is incomplete because it was not updated below
   * wildfire_threshold.  If only a single variable is needed, it is faster to
   * call calculateEstimate(const KEY&).
   */
  Values calculateEstimate() const;

翻译：

根据上次更新期间计算的不完整线性增量计算估计值。此增量不完整，因为它未更新到低于wildfire_threshold。如果只需要一个变量，那么调用calculateEstimate（const KEY&）会更快

4.2 模板

  /** Compute an estimate for a single variable using its incomplete linear
   * delta computed during the last update.  This is faster than calling the
   * no-argument version of calculateEstimate, which operates on all variables.
   * @param key
   * @return
   */
  template <class VALUE>
  VALUE calculateEstimate(Key key) const {
    const Vector& delta = getDelta()[key];
    return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
  }

翻译：
使用上次更新期间计算的不完整线性增量计算单个变量的估计值。这比调用calculateEstimate的无参数版本更快，该版本对所有变量进行操作。

4.3 重载2

  /** Compute an estimate for a single variable using its incomplete linear
   * delta computed during the last update.  This is faster than calling the
   * no-argument version of calculateEstimate, which operates on all variables.
   * This is a non-templated version that returns a Value base class for use
   * with the MATLAB wrapper.
   * @param key
   * @return
   */
  const Value& calculateEstimate(Key key) const;

使用上次更新期间计算的不完整线性增量计算单个变量的估计值。这比调用calculateEstimate的无参数版本更快，该版本对所有变量进行操作。
这是一个非模板版本，它返回一个Value基类，用于MATLAB包装。

4.4

上述是根据加入新因子图和values对isam2构建的贝叶斯树进行优化，根据重载版本决定对所有变量更新还是单个变量更新

五

以协方差矩阵返回任何一个变量的边际值

  /** Return marginal on any variable as a covariance matrix */
  Matrix marginalCovariance(Key key) const;

六

非典型用法

  /// @name Public members for non-typical usage
  /// @{

  /** Compute an estimate using a complete delta computed by a full
   * back-substitution.
   */
  Values calculateBestEstimate() const;

七、获取值

7.1 delta

  /** Access the current delta, computed during the last call to update */
  const VectorValues& getDelta() const;

访问上次调用更新时计算的当前delta

7.2 linear error

  /** Compute the linear error */
  double error(const VectorValues& x) const;

7.3 返回no-linear factor graph

  /** Access the set of nonlinear factors */
  const NonlinearFactorGraph& getFactorsUnsafe() const {
    return nonlinearFactors_;
  }

返回值no-linear factor graph
对于nonlinearFactors_ ，这是一个protected范围的成员变量 the meaning of nonlinearFactors_
is
存储 original nonlinear factors在重新线性化的时候使用

  /** All original nonlinear factors are stored here to use during relinearization */

7.4

  /** Access the nonlinear variable index */
  const VariableIndex& getVariableIndex() const { return variableIndex_; }

variableIndex_(VariableIndex lets us look up factors by involved variable and keeps track of dimensions)允许通变量查找因子并跟踪维度

7.5 fixed Variables

fixedVariables_(Set of variables that are involved with linear factors from marginalized variables and thus cannot have their linearization points changed.)与边缘化变量的线性因素相关的变量集，因此不能改变其线性化点。

  /** Access the nonlinear variable index */
  const KeySet& getFixedVariables() const { return fixedVariables_; }

7.6 梯度向量

//TODO
$f(\nabla_{x=0} \Vert \Sigma^{-1} R x - d \Vert^2)$
$f(-R^T d)$

  /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert
   * \Sigma^{-1} R x - d \right\Vert^2 \f$, centered around zero. The gradient
   * about zero is \f$ -R^T d \f$.  See also gradient(const GaussianBayesNet&,
   * const VectorValues&).
   *
   * @return A VectorValues storing the gradient.
   */
  VectorValues gradientAtZero() const;

八、protected成员函数

八、
以上都是public函数

8.1

移除标记的顶部，然后批量或增量重新计算

  /// Remove marked top and either recalculate in batch or incrementally.
  void recalculate(const ISAM2UpdateParams& updateParams,
                   const KeySet& relinKeys, ISAM2Result* result);

8.2

执行批处理步骤-重新排序并重新排列所有变量

  // Do a batch step - reorder and relinearize all variables
  void recalculateBatch(const ISAM2UpdateParams& updateParams,
                        KeySet* affectedKeysSet, ISAM2Result* result);

8.3

检索仅包含受影响的变量的因子，(其余内容存储在缓存的factors)

  // retrieve all factors that ONLY contain the affected variables
  // (note that the remaining stuff is summarized in the cached factors)
  GaussianFactorGraph relinearizeAffectedFactors(
      const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
      const KeySet& relinKeys);

8.4

  void recalculateIncremental(const ISAM2UpdateParams& updateParams,
                              const KeySet& relinKeys,
                              const FastList<Key>& affectedKeys,
                              KeySet* affectedKeysSet, Cliques* orphans,
                              ISAM2Result* result);

8.5

向ISAM2系统中添加变量

  /**
   * Add new variables to the ISAM2 system.
   * @param newTheta Initial values for new variables
   * @param variableStatus optional detailed result structure
   */
  void addVariables(const Values& newTheta,
                    ISAM2Result::DetailedResults* detail = 0);

8.6 移除变量

移除变量

  /**
   * Remove variables from the ISAM2 system.
   */
  void removeVariables(const KeySet& unusedKeys);

8.7 更新增量

  void updateDelta(bool forceFullSolve = false) const;

九、private成员函数

  /** Serialization function */
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
      ar & BOOST_SERIALIZATION_NVP(theta_);
      ar & BOOST_SERIALIZATION_NVP(variableIndex_);
      ar & BOOST_SERIALIZATION_NVP(delta_);
      ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
      ar & BOOST_SERIALIZATION_NVP(RgProd_);
      ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
      ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
      ar & BOOST_SERIALIZATION_NVP(linearFactors_);
      ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
      ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
      ar & BOOST_SERIALIZATION_NVP(update_count_);
  }

十、关于iSAM2中边缘化导致exception

触发条件边缘化的变量不是一个叶子
参照
/gtsam/gtsam/nonlinear/nonlinearExceptions.h

namespace gtsam {

  /**
  Thrown when requesting to marginalize out variables from ISAM2 that are not
  leaves.  To make the variables you would like to marginalize be leaves, their
  ordering should be constrained using the constrainedKeys argument to
  ISAM2::update().
  */
  class MarginalizeNonleafException : public std::exception {
    Key key_;
    KeyFormatter formatter_;
    mutable std::string what_;
  public:
    MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) noexcept :
      key_(key), formatter_(formatter) {}
    virtual ~MarginalizeNonleafException() noexcept {}
    Key key() const { return key_; }
    const char* what() const noexcept override {
      if(what_.empty())
        what_ =
"\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\
is not a leaf.  To make the variables you would like to marginalize be leaves,\n\
their ordering should be constrained using the constrainedKeys argument to\n\
ISAM2::update().\n";
      return what_.c_str();
    }
  };
}

在iSAM2进行update时
传入参数

  virtual ISAM2Result update(
      const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
      const Values& newTheta = Values(),
      const FactorIndices& removeFactorIndices = FactorIndices(),
      const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,
      const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
      const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
      bool force_relinearize = false);

上述三个参数分别是：
一个key到group labels的map，这样可以在Bayes Tree将变量映射到特殊的组里
固定线性化点
重新消除和排序

  virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,
                             const Values& newTheta,
                             const ISAM2UpdateParams& updateParams);