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
• 三、獲取線性點(diǎn)和檢查值是否存在
• 四、計(jì)算估計(jì)值
• • 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成員函數(shù)
• • 8.1
  • 8.2
  • 8.3
  • 8.4
  • 8.5
  • 8.6 移除變量
  • 8.7 更新增量
• 九、private成員函數(shù)
• 十、關(guān)于iSAM2中邊緣化導(dǎo)致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_;

當(dāng)前的線性化點(diǎn)

0.2 variableIndex_

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

VariableIndex 讓我們通過涉及的變量查找因子并跟蹤維度

0.3 delta_

上一個(gè)線性解的線性delta，對(duì) theta 中估計(jì)的更新
這是可變的，因?yàn)樗且粋€(gè)“緩存”變量 - 在使用 getDelta() 或 calculateEstimate() 請(qǐng)求或在 update() 期間需要評(píng)估是否重新線性化變量之前，它不會(huì)更新。

/** 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_ 中已替換且尚未更新的變量的累積掩碼，僅在內(nèi)部使用，如有必要，當(dāng)使用 getDelta() 或 calculateEstimate() 請(qǐng)求時(shí)，將始終更新 delta。
這是可變的，因?yàn)樗趦?nèi)部用于在需要之前不更新 delta_。

0.7 nonlinearFactors_

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

所有原始非線性因子都存儲(chǔ)在此處以在重新線性化期間使用

0.8 linearFactors_

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

當(dāng)前的線性因子，僅根據(jù)需要更新

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_;

與來(lái)自邊緣化變量的線性因子相關(guān)的變量集，因此不能改變它們的線性化點(diǎn)

0.12 update_count_

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

計(jì)數(shù)器每次 update() 遞增，用于確定周期性重新線性化

一、update

1.1 重載版本1

添加新因素，根據(jù)需要更新解決方案并重新線性化。
可選地，此功能從系統(tǒng)中刪除現(xiàn)有因素，以啟用諸如將現(xiàn)有因素與新因素交換等行為。
向當(dāng)前系統(tǒng)添加新的測(cè)量值和可選的新變量。
這將運(yùn)行 ISAM2 算法的完整步驟，根據(jù)野火和重新線性化閾值，根據(jù)需要重新線性化和更新解決方案。

@param newFactors 要添加到系統(tǒng)中的新因子
@param newTheta 要添加到系統(tǒng)的新變量的初始化點(diǎn)。您必須在此處包括出現(xiàn)在 newFactors 中的所有新變量（系統(tǒng)中還沒有這些變量）。這里不能有任何變量沒有出現(xiàn)在 newFactors 中，另外，系統(tǒng)中已經(jīng)存在的變量也不能包含在此處。
@param removeFactorIndices 要從系統(tǒng)中刪除的因子的索引
@param force_relinearize 重新線性化增量幅度足夠大的任何變量（Params::relinearizeThreshold），而不管重新線性化間隔（Params::relinearizeSkip）。
@param constrainedKeys 是組標(biāo)簽的鍵的可選映射，這樣可以將變量約束到 BayesTree 中的特定分組
@param noRelinKeys 是一組可選的非線性鍵，iSAM2 將保持在一個(gè)恒定的線性化點(diǎn)，無(wú)論線性增量的大小如何
@param extraReelimKeys 是一組可選的非線性鍵，iSAM2 將重新消除，無(wú)論線性增量的大小如何。這允許重新排序提供的鍵。
@return 包含有關(guān)更新信息的 ISAM2Result 結(jié)構(gòu)

/*** 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);

翻譯

/** *添加新因素，根據(jù)需要更新解決方案并重新升級(jí)。 * *可選地，此功能從系統(tǒng)中刪除現(xiàn)有因素，以啟用諸如將現(xiàn)有因素與新因素交換等行為。 * *向當(dāng)前系統(tǒng)添加新的測(cè)量值和可選的新變量。 *這將運(yùn)行ISAM2算法的全部步驟，重新搜索和更新 *根據(jù)野火和重新啟動(dòng)閾值，根據(jù)需要提供解決方案。 * *@param newFactors要添加到系統(tǒng)中的新因素 *@param newTheta初始化指向要添加到系統(tǒng)中的新變量。 您必須在此處包含newFactors中出現(xiàn)的所有新變量（這些變量尚未在系統(tǒng)中）。 這里不能有newFactors中沒有出現(xiàn)的變量，另外，系統(tǒng)中已經(jīng)存在的變量也不能包含在這里。 *@param removeFactorIndices要從系統(tǒng)中刪除的因子索引 *@param force_relinealize重新線性化增量幅度足夠大的任何變量（Params:：relinearizeThreshold），而不管重新線性化間隔（param:：relinealizeSkip）。 *@param constrainedKeys是鍵到組標(biāo)簽的可選映射，這樣可以將變量約束到BayesTree中的特定組 *@param noRelinKeys是一組可選的非線性密鑰，無(wú)論線性增量的大小如何，iSAM2都將保持在恒定的線性化點(diǎn) *@param extraReelimKeys是一組可選的非線性密鑰，無(wú)論線性增量的大小如何，iSAM2都將重新消除。 這允許對(duì)提供的密鑰進(jìn)行重新排序。 *@return包含更新信息的ISAM2Result結(jié)構(gòu) */

1.2 重載版本2

添加新因素，根據(jù)需要更新解決方案并重新線性化。
update() 的替代簽名（參見上面的文檔），所有附加參數(shù)都在一個(gè)結(jié)構(gòu)中。 如果參數(shù)發(fā)生變化，這種形式更容易保持未來(lái)的 API/ABI 兼容性。
@param newFactors 要添加到系統(tǒng)中的新因子
@param newTheta 要添加到系統(tǒng)的新變量的初始化點(diǎn)。 您必須在此處包括出現(xiàn)在 newFactors 中的所有新變量（系統(tǒng)中還沒有這些變量）。 這里不能有任何變量沒有出現(xiàn)在 newFactors 中，另外，系統(tǒng)中已經(jīng)存在的變量也不能包含在此處。
@param updateParams 用于控制重新線性化、約束鍵等的附加參數(shù)。
@return 包含有關(guān)更新信息的 ISAM2Result 結(jié)構(gòu)
@note 沒有默認(rèn)參數(shù)以避免模棱兩可的調(diào)用錯(cuò)誤。

/*** 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);

翻譯

/* 添加新因素，根據(jù)需要更新解決方案并重新升級(jí)。 update（）的替代簽名（參見上面的文檔），在一個(gè)結(jié)構(gòu)中包含所有附加參數(shù)。 如果參數(shù)發(fā)生變化，此表單可以更容易地保持未來(lái)的API/ABI兼容性。 * *@param newFactors要添加到系統(tǒng)中的新因素 *@param newTheta初始化指向要添加到系統(tǒng)中的新變量。 您必須在此處包含newFactors中出現(xiàn)的所有新變量（這些變量尚未在系統(tǒng)中）。 這里不能有newFactors中沒有出現(xiàn)的變量，另外，系統(tǒng)中已經(jīng)存在的變量也不能包含在這里。 *@param updateParams控制重新排列、約束鍵等的其他參數(shù)。 *@return包含更新信息的ISAM2Result結(jié)構(gòu) @注意：無(wú)默認(rèn)參數(shù)可避免不明確的調(diào)用錯(cuò)誤。 */

對(duì)于參數(shù)constrainedKeys解決exception是有必要的
當(dāng)請(qǐng)求邊緣化來(lái)自 ISAM2 的非葉子變量時(shí)拋出。 要使您希望邊緣化的變量成為葉，應(yīng)使用 ISAM2::update() 的 constrainedKeys 參數(shù)來(lái)限制它們的順序。

“請(qǐng)求邊緣化變量” + formatter_(key_) + “，但該變量不是葉子。要使您想要邊緣化的變量成為葉子，應(yīng)使用 ISAM2::update( ).";

1.3

上述都是
添加新的因子(const NonlinearFactorGraph& newFactor),
添加新的優(yōu)化變量(const Values& newTheta)
這兩個(gè)添加的是有條件的
顯然因子可以與isam2中的舊的變量產(chǎn)生聯(lián)系,已經(jīng)存在的變量系統(tǒng)不能存在兩次，如果因子中不與某一個(gè)變量(假設(shè)為變量A)產(chǎn)生聯(lián)系，那么向系統(tǒng)中進(jìn)行變量A的添加必然是無(wú)效的

二、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 中的葉子。 如果請(qǐng)求邊緣化非葉子，則拋出 MarginalizeNonleafException。 邊際化在系統(tǒng)中留下了邊際的線性近似，并且該線性邊際中涉及的任何變量的線性化點(diǎn)變得固定。 設(shè)置的固定變量將包括原始因子中與邊緣化變量相關(guān)的任何鍵，以及可能由于填充而增加的鍵。如果提供，“marginalFactorsIndices”將增加在“marginalizeLeaves”調(diào)用期間添加的邊際因子的因子圖索引如果提供，“deletedFactorsIndices”將增加在“marginalizeLeaves”調(diào)用期間刪除的任何因子的因子圖索引 */

上述節(jié)點(diǎn)必須是葉子節(jié)點(diǎn)

三、獲取線性點(diǎn)和檢查值是否存在

/// 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); }

四、計(jì)算估計(jì)值

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;

翻譯：

根據(jù)上次更新期間計(jì)算的不完整線性增量計(jì)算估計(jì)值。此增量不完整，因?yàn)樗锤碌降陀趙ildfire_threshold。如果只需要一個(gè)變量，那么調(diào)用calculateEstimate（const KEY&）會(huì)更快

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);}

翻譯：
使用上次更新期間計(jì)算的不完整線性增量計(jì)算單個(gè)變量的估計(jì)值。這比調(diào)用calculateEstimate的無(wú)參數(shù)版本更快，該版本對(duì)所有變量進(jìn)行操作。

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;

使用上次更新期間計(jì)算的不完整線性增量計(jì)算單個(gè)變量的估計(jì)值。這比調(diào)用calculateEstimate的無(wú)參數(shù)版本更快，該版本對(duì)所有變量進(jìn)行操作。
這是一個(gè)非模板版本，它返回一個(gè)Value基類，用于MATLAB包裝。

4.4

上述是根據(jù)加入新因子圖和values對(duì)isam2構(gòu)建的貝葉斯樹進(jìn)行優(yōu)化，根據(jù)重載版本決定對(duì)所有變量更新還是單個(gè)變量更新

五

以協(xié)方差矩陣返回任何一個(gè)變量的邊際值

/** 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;

訪問上次調(diào)用更新時(shí)計(jì)算的當(dāng)前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
對(duì)于nonlinearFactors_ ，這是一個(gè)protected范圍的成員變量 the meaning of nonlinearFactors_
is
存儲(chǔ) original nonlinear factors在重新線性化的時(shí)候使用

/** 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.)與邊緣化變量的線性因素相關(guān)的變量集，因此不能改變其線性化點(diǎn)。

/** 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成員函數(shù)

八、
以上都是public函數(shù)

8.1

移除標(biāo)記的頂部，然后批量或增量重新計(jì)算

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

8.2

執(zhí)行批處理步驟-重新排序并重新排列所有變量

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

8.3

檢索僅包含受影響的變量的因子，(其余內(nèi)容存儲(chǔ)在緩存的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系統(tǒng)中添加變量

/*** 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成員函數(shù)

/** 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_);}

十、關(guān)于iSAM2中邊緣化導(dǎo)致exception

觸發(fā)條件邊緣化的變量不是一個(gè)葉子
參照
/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進(jìn)行update時(shí)
傳入?yún)?shù)

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);

上述三個(gè)參數(shù)分別是：
一個(gè)key到group labels的map，這樣可以在Bayes Tree將變量映射到特殊的組里
固定線性化點(diǎn)
重新消除和排序

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。