Title: iSAM2 部分状态更新算法 (II-源码阅读-GTSAM)
文章目录
关联博客:
因子图、边缘化与消元算法的抽丝剥茧 —— Notes for “Factor Graphs for Robot Perception”
GTSAM 中的鲁棒噪声模型与 M-估计 (GTSAM Robust Noise Model and M-Estimator)
iSAM2 部分状态更新算法 (II - 源码阅读 - GTSAM) ⇐ \quad\Leftarrow\quad ⇐ 本篇
I. 前言
上一篇博文中, 我们尝试解读了 iSAM2 算法中的部分状态更新子算法的原理.
这里我们看一下 GTSAM (https://github.com/borglab/gtsam) 中源码如何实现 “部分状态更新” 的.
II. GTSAM 中实现直接反向替代 (Back-Substitution)
1. GTSAM 中实现全部反向替代计算 (Full Back-Substitution)
A. 高斯条件概率均值计算
在 /gtsam/gtsam/linear/GaussianConditional.h 中声明了高斯条件均值的计算函数
/**
* Solves a conditional Gaussian and writes the solution into the entries of
* \c x for each frontal variable of the conditional. The parents are
* assumed to have already been solved in and their values are read from \c x.
* This function works for multiple frontal variables.
*
* Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2 \f$,
* where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
* variables of this conditional, this solve function computes
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
*
* @param parents VectorValues containing solved parents \f$ x_s \f$.
*/
VectorValues solve(const VectorValues& parents) const;
在 /gtsam/gtsam/linear/GaussianConditional.cpp 中实现了高斯条件概率均值计算函数 (即直接反向替代中的一步计算), 即
Δ
j
=
R
j
−
1
(
d
j
−
T
j
S
^
j
)
\Delta_j = R^{-1}_{j} (d_j - T_j \hat{S}_j)
Δj=Rj−1(dj−TjS^j)
/* ************************************************************************* */
VectorValues GaussianConditional::solve(const VectorValues& x) const {
// Concatenate all vector values that correspond to parent variables 分离子偏差
const Vector xS = x.vector(KeyVector(beginParents(), endParents()));
// Update right-hand-side rhs
const Vector rhs = d() - S() * xS;
// Solve matrix 解方程 R * \Delta = rhs
const Vector solution = R().triangularView<Eigen::Upper>().solve(rhs);
// Check for indeterminant solution 异常检测
if (solution.hasNaN()) {
throw IndeterminantLinearSystemException(keys().front());
}
// Insert solution into a VectorValues
VectorValues result;
DenseIndex vectorPosition = 0;
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
result.emplace(*frontal, solution.segment(vectorPosition, getDim(frontal)));
vectorPosition += getDim(frontal);
}
return result;
}
B. 递归法实现直接反向替代
在 /gtsam/gtsam/nonlinear/ISAM2-impl.cpp 中, 递归地调用高斯条件概率均值计算函数, 实现了直接反向替代算法.
namespace internal {
inline static void optimizeInPlace(const ISAM2::sharedClique& clique,
VectorValues* result) {
// parents are assumed to already be solved and available in result
// 调用了上面提到的 "高斯条件概率均值计算函数" —— "A. 高斯条件概率均值计算"
result->update(clique->conditional()->solve(*result));
// starting from the root, call optimize on each conditional 递归，反向替代
for (const ISAM2::sharedClique& child : clique->children)
optimizeInPlace(child, result);
}
} // namespace internal
2. GTSAM 中实现部分反向替代计算 (Partial Back-Substitution)
A. 深度优先法实现非完全递归优化计算
在 /gtsam/gtsam/nonlinear/ISAM2Clique.h 中, 对 optimizeWildfireNonRecursive 函数的声明.
/**
* Optimize the BayesTree, starting from the root.
* @param threshold The maximum change against the PREVIOUS delta for
* non-replaced variables that can be ignored, ie. the old delta entry is kept
* and recursive backsubstitution might eventually stop if none of the changed
* variables are contained in the subtree.
* @param replaced Needs to contain all variables that are contained in the top
* of the Bayes tree that has been redone.
* @return The number of variables that were solved for.
* @param delta The current solution, an offset from the linearization point.
*/
size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold,
const KeySet& replaced, VectorValues* delta);
size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root,
double threshold, const KeySet& replaced,
VectorValues* delta);
在 /gtsam/gtsam/nonlinear/ISAM2Clique.cpp 中, 对 optimizeWildfireNonRecursive 函数的实现. 这是一个深度优先方法的实现.
size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root,
double threshold, const KeySet& keys,
VectorValues* delta) {
KeySet changed;
size_t count = 0;
if (root) {
std::stack<ISAM2Clique::shared_ptr> travStack; // 栈数据结构, 存储贝叶斯树上需要更新计算的节点
travStack.push(root);
ISAM2Clique::shared_ptr currentNode = root;
// 可以看出来这是一个深度优先算法, 用来遍历需要优化计算的部分节点(或部分子节点)
while (!travStack.empty()) {
currentNode = travStack.top();
travStack.pop(); // 出栈
// "B. 部分状态更新中单个节点的优化结算"
bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed,
delta, &count);
// 当前节点中是否有其状态偏差大于阈值的状态变量, 如有将更新计算推进到当前节点的子节点上
if (dirty) {
for (const auto& child : currentNode->children) {
travStack.push(child); // 入栈
}
}
}
}
return count;
}
B. 部分状态更新中单个节点的优化结算
在 /gtsam/gtsam/nonlinear/ISAM2Clique.cpp 中, 对 optimizeWildfireNode 函数的实现.
/* ************************************************************************* */
bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold,
KeySet* changed, VectorValues* delta,
size_t* count) const {
// TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst,
// potentially refactor
// 判断是否受到污染, 需要更新计算 —— 参看下文 "C. 部分状态更新中非完全递归的控制"
bool dirty = isDirty(replaced, *changed);
if (dirty) {
// Temporary copy of the original values, to check how much they change
// 临时拷贝保存当前节点(团)的前端变量, 即 \Delta 的原始值, 此 originalValues 应该为 0 向量
auto originalValues = delta->vector(conditional_->frontals());
// Back-substitute
// 实现当前节点(团)上的反向替代计算 (单步反向替代计算)
fastBackSubstitute(delta);
*count += conditional_->nrFrontals();
// 1. 如果状态更新显著
// 2. 该状态本身就需要重新计算（如加入新测量后 Bayes tree 需要更新, 该节点在受影响路径上等情况）
// 以上两种情况, 则 valuesChanged 返回 True
if (valuesChanged(replaced, originalValues, *delta, threshold)) {
// 标记前端变量需要修正 (需要更新)
markFrontalsAsChanged(changed);
} else {
// 如果状态更新不显著, 就不需要标记, 恢复原来的 \Delta 值, 即 0 向量
restoreFromOriginals(originalValues, delta);
}
}
return dirty;
}
在 /gtsam/gtsam/nonlinear/ISAM2Clique.cpp 中, 实现一个节点上的反向替代计算 (单步反向替代计算)
/**
* Back-substitute - special version stores solution pointers in cliques for
* fast access.
*/
void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const {
#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE
...
#else
// 调用高斯条件概率均值计算函数 —— "A. 高斯条件概率均值计算"
delta->update(conditional_->solve(*delta));
#endif
}
在 /gtsam/gtsam/nonlinear/ISAM2Clique.cpp 中, 实现了判断状态更新是否显著的函数.
/* ************************************************************************* */
bool ISAM2Clique::valuesChanged(const KeySet& replaced,
const Vector& originalValues,
const VectorValues& delta,
double threshold) const {
auto frontals = conditional_->frontals();
// 如果前端变量本身标注了要重新计算, 返回 True
if (replaced.exists(frontals.front())) return true;
// originalValues = 0
Vector diff = originalValues - delta.vector(frontals);
// 判断状态更新是否显著 (超过阈值), \Delta >= 阈值？
return diff.lpNorm<Eigen::Infinity>() >= threshold;
}
C. 部分状态更新中非完全递归的控制
在 /gtsam/gtsam/nonlinear/ISAM2Clique.h 中, 对 isDirty 函数进行了声明.
部分更行算法中有 “如果状态更新 Δ k \Delta_k Δk 中的变量 Δ k j \Delta_{k_j} Δkj 大于阈值 α \alpha α, 则对包含该变量的子团递归地执行本算法” 这条语句, isDirty 函数就是把这些满足条件的子团全标记出来 (污染标注).
/**
* Check if clique was replaced, or if any parents were changed above the
* threshold or themselves replaced.
*/
bool isDirty(const KeySet& replaced, const KeySet& changed) const;
在 /gtsam/gtsam/nonlinear/ISAM2Clique.cpp 中, 对 isDirty 函数进行了实现.
/* ************************************************************************* */
bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const {
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
// cliques in the children need to be processed
// 如果一个团中的所有变量的改变 (即 \Delta) 都不是显著的, 这些变量包括前端变量和分离子变量
// 则基于路径包含性 (running intersection property), 没有任何子团需要处理
// 因为可以预测子图中变量更新也不会显著
// Are any clique variables part of the tree that has been redone?
// 如果团的条件概率中的前端变量集合中有需要重新计算的变量, 侧该团标记为受污染(需要计算更新)
bool dirty = replaced.exists(conditional_->frontals().front());
#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for (Key frontal : conditional_->frontals()) {
assert(dirty == replaced.exists(frontal));
}
#endif
// If not, then has one of the separator variables changed significantly?
// 如果团的前端变量中不需要更新计算, 则继续查看该团的条件概率中描述的父团(即分离子集合)中是否存在需要重新计算的变量
// 如果条件概率的分离子集合中有需要修正的变量, 则本团也标记为受污染, 需要更新计算
if (!dirty) {
for (Key parent : conditional_->parents()) {
if (changed.exists(parent)) {
dirty = true;
break;
}
}
}
return dirty;
}
III. GTSAM 中实现部分状态更新算法
1. 基于高斯-牛顿法的部分状态更新
直接反向替代算法中, 递归地实现了整个贝叶斯树中变量偏差 Δ \Delta Δ 的更新计算.
而 “部分状态更新算法” 通过阈值 α \alpha α (源码中的 effectiveWildfireThreshold) 来控制是否将变量偏差 Δ j \Delta_j Δj 的计算递归地推进执行到每一个节点.
在 /gtsam/gtsam/nonlinear/ISAM2.cpp 中实现了部分状态更新算法.
/* ************************************************************************* */
// Marked const but actually changes mutable delta
// 参数 forceFullSolve 用于选择 "更新全部状态偏差" 还是 "更新部分状态偏差"
void ISAM2::updateDelta(bool forceFullSolve) const {
gttic(updateDelta);
// 如果选择高斯-牛顿法作为最优化问题求解器
if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) {
// If using Gauss-Newton, update with wildfireThreshold
// 因为经过线性化处理, 利用"高斯-牛顿方法对整个贝叶斯树的最优估计"转化为"直接反向替代中的线性方程求解"来实现
const ISAM2GaussNewtonParams& gaussNewtonParams =
boost::get<ISAM2GaussNewtonParams>(params_.optimizationParams);
// 如果设定为 "更新*整个*贝叶斯树的状态偏差", 则阈值 effectiveWildfireThreshold 设为 0
// 如果设定为 "更新*部分*贝叶斯树的状态偏差", 则阈值 effectiveWildfireThreshold 设为人为设置的阈值, 默认值 0.001
const double effectiveWildfireThreshold =
forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold;
gttic(Wildfire_update);
// 利用高斯-牛顿法解最优问题, 其实还是执行 "直接反向替代"
// effectiveWildfireThreshold 作为阈值来控制 "全部更新" 还是 "部分更新"
// 参看下文 "2. GTSAM 中实现高斯-牛顿法"
// 从根节点开始
DeltaImpl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_,
effectiveWildfireThreshold, &delta_);
deltaReplacedMask_.clear();
gttoc(Wildfire_update);
} else if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) {
// If using Dogleg, do a Dogleg step
const ISAM2DoglegParams& doglegParams =
boost::get<ISAM2DoglegParams>(params_.optimizationParams);
const double effectiveWildfireThreshold =
forceFullSolve ? 0.0 : doglegParams.wildfireThreshold;
// Do one Dogleg iteration
gttic(Dogleg_Iterate);
// Compute Newton's method step
// 狗腿法由高斯-牛顿法和最速降法融合而成, 故也要计算高斯-牛顿更新值
gttic(Wildfire_update);
DeltaImpl::UpdateGaussNewtonDelta(
roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_);
gttoc(Wildfire_update);
// Compute steepest descent step
// gradientAtZero() 参见博文 "非线性最小二乘问题的数值方法 —— 狗腿法 Powell‘s Dog Leg Method (I - 原理与算法)"
// 博文中式 (II-1-3)
// 梯度计算中 前端变量 \Delta 和 分离子变量 \hat{S} 都作为自变量
// 狗腿法中会先遍历每个团, 把所有零点梯度都加到一起
// 狗腿法是对整棵贝叶斯树的优化目标进行迭代优化, 不同于直接反向替代法, 更像是批处理方法
const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient
// RgProd_ 为 R * \Delta + T * \hat{S} = J_r * h
// 为上述博文中式 (II-1-6) 的分母范数符号内的部分
// R() 就是上三角矩阵 R, S() 就是分离子对应的矩阵 T
DeltaImpl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero,
&RgProd_); // Update RgProd
// 为上述博文中式 (II-1-6)获得最降速法的最优步长 \alpha
const VectorValues dx_u = DeltaImpl::ComputeGradientSearch(
gradAtZero, RgProd_); // Compute gradient search point
// Clear replaced keys mask because now we've updated deltaNewton_ and
// RgProd_
deltaReplacedMask_.clear();
// Compute dogleg point
// 执行狗腿法
DoglegOptimizerImpl::IterationResult doglegResult(
DoglegOptimizerImpl::Iterate(
*doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_,
*this, nonlinearFactors_, theta_, nonlinearFactors_.error(theta_),
doglegParams.verbose));
gttoc(Dogleg_Iterate);
gttic(Copy_dx_d);
// Update Delta and linear step
doglegDelta_ = doglegResult.delta;
delta_ =
doglegResult
.dx_d; // Copy the VectorValues containing with the linear solution
gttoc(Copy_dx_d);
} else {
throw std::runtime_error("iSAM2: unknown ISAM2Params type");
}
}
注: 狗腿法中没看出 “部分更新” 特性, 故不作为此处源码阅读重点. 此处狗腿法更像是批处理方法.
2. GTSAM 中实现高斯-牛顿法
因子图进过线性化后, 求解整个系统的二次最优解就等价于求解上面直接反向替代法中线性方程组.
在 /gtsam/gtsam/nonlinear/ISAM2-impl.h 中实现了线性二乘问题的牛顿-高斯算法 (直接反向替代法).
这里主要关注源码如何实现对部分变量偏差的更新计算的控制.
/* ************************************************************************* */
size_t DeltaImpl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
const KeySet& replacedKeys,
double wildfireThreshold,
VectorValues* delta) {
size_t lastBacksubVariableCount;
if (wildfireThreshold <= 0.0) {
// Threshold is zero or less, so do a full recalculation
// 阈值小于等于 0, 直接调用"直接反向替代算法", 更新全部状态变量
for (const ISAM2::sharedClique& root : roots)
// 上文 "II.1.B. 递归法实现直接反向替代"
internal::optimizeInPlace(root, delta);
lastBacksubVariableCount = delta->size();
} else {
// Optimize with wildfire
lastBacksubVariableCount = 0;
// 从根节点开始, 以消元逆序的反方向推进执行
for (const ISAM2::sharedClique& root : roots)
// 上文 "II.2.A. 深度优先法实现非完全递归优化计算" —— 实现部分状态更新计算
lastBacksubVariableCount += optimizeWildfireNonRecursive(
root, wildfireThreshold, replacedKeys, delta); // modifies delta
#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for (VectorValues::const_iterator key_delta = delta->begin();
key_delta != delta->end(); ++key_delta) {
assert((*delta)[key_delta->first].allFinite());
}
#endif
}
return lastBacksubVariableCount;
}
参考文献
[1] Kaess M, Johannsson H, Roberts R, Ila V, Leonard JJ, Dellaert F. iSAM2: Incremental smoothing and mapping using the Bayes tree. The International Journal of Robotics Research. 31(2):216-235. 2012
[2] Frank Dellaert, Michael Kaess, Factor Graphs for Robot Perception, Foundations and Trends in Robotics, Vol. 6, No. 1-2 (2017) 1–139
8265
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
