ISAM2.h/ISAM2.cpp

• 0、成員變量
• • 0.1 theta_
  • 0.2 variableIndex_
  • 0.3 delta_
  • 0.4 deltaNewton_
  • 0.5 RgProd_
  • 0.6 deltaReplacedMask_
  • 0.7 nonlinearFactors_
  • 0.8 linearFactors_
  • 0.9 params_
  • 0.10 doglegDelta_
  • 0.11 fixedVariables_
  • 0.12 update_count_
• 一、update
• • 1.1 重載版本1
  • 1.2 重載版本2
  • 1.3
• 二、marginalizeLeaves
• 三、獲取線性點和檢查值是否存在
• 四、計算估計值
• • 4.1 重載1
  • 4.2 模板
  • 4.3 重載2
  • 4.4
• 五
• 六
• 七、獲取值
• • 7.1 delta
  • 7.2 linear error
  • 7.3 返回no-linear factor graph
  • 7.4
  • 7.5 fixed Variables
  • 7.6 梯度向量
• 八、protected成員函數
• • 8.1
  • 8.2
  • 8.3
  • 8.4
  • 8.5
  • 8.6 移除變量
  • 8.7 更新增量
• 九、private成員函數
• 十、關于iSAM2中邊緣化導致exception

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 updatemutable 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 relinearizationpublic:using This = ISAM2; ///< This classusing Base = BayesTree<ISAM2Clique>; ///< The BayesTree base classusing Clique = Base::Clique; ///< A cliqueusing sharedClique = Base::sharedClique; ///< Shared pointer to a cliqueusing 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 pointconst Values& getLinearizationPoint() const { return theta_; }/// Check whether variable with given key exists in linearization pointbool valueExists(Key key) const { return theta_.exists(key); } /// Access the current linearization pointconst Values& getLinearizationPoint() const { return theta_; }/// Check whether variable with given key exists in linearization pointbool 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({?}_{x=0}\Vert {\Sigma }^{?1}Rx?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 variablesvoid 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 notleaves. To make the variables you would like to marginalize be leaves, theirordering should be constrained using the constrainedKeys argument toISAM2::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);

總結

以上是生活随笔為你收集整理的ISAM2.h/ISAM2.cpp的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。