iSAM2 部分状态更新算法 (II - 源码阅读 - GTSAM)

2024-03-23 01:36
文章标签 算法 源码 更新 阅读 ii 状态 部分 gtsam isam2

本文主要是介绍iSAM2 部分状态更新算法 (II - 源码阅读 - GTSAM)，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

Title: iSAM2 部分状态更新算法 (II-源码阅读-GTSAM)

文章目录

  • I. 前言
  • II. GTSAM 中实现直接反向替代 (Back-Substitution)
    • 1. GTSAM 中实现全部反向替代计算 (Full Back-Substitution)
      • A. 高斯条件概率均值计算
      • B. 递归法实现直接反向替代
    • 2. GTSAM 中实现部分反向替代计算 (Partial Back-Substitution)
      • A. 深度优先法实现非完全递归优化计算
      • B. 部分状态更新中单个节点的优化结算
      • C. 部分状态更新中非完全递归的控制
  • III. GTSAM 中实现部分状态更新算法
    • 1. 基于高斯-牛顿法的部分状态更新
    • 2. GTSAM 中实现高斯-牛顿法
  • 参考文献

关联博客:

因子图、边缘化与消元算法的抽丝剥茧 —— Notes for “Factor Graphs for Robot Perception”

基于 GTSAM 的因子图简单实例

GTSAM 中的鲁棒噪声模型与 M-估计 (GTSAM Robust Noise Model and M-Estimator)

贝叶斯树定义与构建的寻行数墨 —— Notes for “The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping”

贝叶斯树增量式更新的细嚼慢咽 —— Notes for “The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping”

iSAM2 部分状态更新算法 (I - 原理解读)

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​−Tj​S^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 rhsconst Vector rhs = d() - S() * xS;// Solve matrix  解方程 R * \Delta = rhsconst Vector solution = R().triangularView<Eigen::Upper>().solve(rhs);// Check for indeterminant solution 异常检测if (solution.hasNaN()) {throw IndeterminantLinearSystemException(keys().front());}// Insert solution into a VectorValuesVectorValues 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 返回 Trueif (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();// 如果前端变量本身标注了要重新计算, 返回 Trueif (replaced.exists(frontals.front())) return true;// originalValues = 0Vector 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.001const 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 stepconst ISAM2DoglegParams& doglegParams =boost::get<ISAM2DoglegParams>(params_.optimizationParams);const double effectiveWildfireThreshold =forceFullSolve ? 0.0 : doglegParams.wildfireThreshold;// Do one Dogleg iterationgttic(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() 就是分离子对应的矩阵 TDeltaImpl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero,&RgProd_);  // Update RgProd// 为上述博文中式 (II-1-6)获得最降速法的最优步长 \alphaconst 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 stepdoglegDelta_ = doglegResult.delta;delta_ =doglegResult.dx_d;  // Copy the VectorValues containing with the linear solutiongttoc(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 wildfirelastBacksubVariableCount = 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

这篇关于iSAM2 部分状态更新算法 (II - 源码阅读 - GTSAM)的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/836725。

相关文章

Java 正则表达式URL 匹配与源码全解析

Java 正则表达式URL 匹配与源码全解析

《Java正则表达式URL匹配与源码全解析》在Web应用开发中,我们经常需要对URL进行格式验证,今天我们结合Java的Pattern和Matcher类,深入理解正则表达式在实际应用中... 目录1.正则表达式分解：2. 添加域名匹配 (2)3. 添加路径和查询参数匹配 (3) 4. 最终优化版本5.设计思
阅读更多...
一文详解如何在Python中从字符串中提取部分内容

一文详解如何在Python中从字符串中提取部分内容

《一文详解如何在Python中从字符串中提取部分内容》：本文主要介绍如何在Python中从字符串中提取部分内容的相关资料,包括使用正则表达式、Pyparsing库、AST（抽象语法树）、字符串操作... 目录前言解决方案方法一：使用正则表达式方法二：使用 Pyparsing方法三：使用 AST方法四：使用字
阅读更多...
openCV中KNN算法的实现

openCV中KNN算法的实现

《openCV中KNN算法的实现》KNN算法是一种简单且常用的分类算法,本文主要介绍了openCV中KNN算法的实现,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的... 目录KNN算法流程使用OpenCV实现KNNOpenCV 是一个开源的跨平台计算机视觉库，它提供了各
阅读更多...
SpringSecurity JWT基于令牌的无状态认证实现

SpringSecurity JWT基于令牌的无状态认证实现

《SpringSecurityJWT基于令牌的无状态认证实现》SpringSecurity中实现基于JWT的无状态认证是一种常见的做法,本文就来介绍一下SpringSecurityJWT基于令牌的无... 目录引言一、JWT基本原理与结构二、Spring Security JWT依赖配置三、JWT令牌生成与
阅读更多...
MySQL更新某个字段拼接固定字符串的实现

MySQL更新某个字段拼接固定字符串的实现

《MySQL更新某个字段拼接固定字符串的实现》在MySQL中,我们经常需要对数据库中的某个字段进行更新操作,本文就来介绍一下MySQL更新某个字段拼接固定字符串的实现,感兴趣的可以了解一下... 目录1. 查看字段当前值2. 更新字段拼接固定字符串3. 验证更新结果mysql更新某个字段拼接固定字符串 -
阅读更多...
Java调用C++动态库超详细步骤讲解(附源码)

Java调用C++动态库超详细步骤讲解(附源码)

《Java调用C++动态库超详细步骤讲解(附源码)》C语言因其高效和接近硬件的特性,时常会被用在性能要求较高或者需要直接操作硬件的场合,：本文主要介绍Java调用C++动态库的相关资料,文中通过代... 目录一、直接调用C++库第一步：动态库生成（vs2017+qt5.12.10）第二步：Java调用C++
阅读更多...
springboot+dubbo实现时间轮算法

springboot+dubbo实现时间轮算法

《springboot+dubbo实现时间轮算法》时间轮是一种高效利用线程资源进行批量化调度的算法,本文主要介绍了springboot+dubbo实现时间轮算法,文中通过示例代码介绍的非常详细,对大家... 目录前言一、参数说明二、具体实现1、HashedwheelTimer2、createWheel3、n
阅读更多...
关于WebSocket协议状态码解析

关于WebSocket协议状态码解析

《关于WebSocket协议状态码解析》：本文主要介绍关于WebSocket协议状态码的使用方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录WebSocket协议状态码解析1. 引言2. WebSocket协议状态码概述3. WebSocket协议状态码详解3
阅读更多...
Python实现无痛修改第三方库源码的方法详解

Python实现无痛修改第三方库源码的方法详解

《Python实现无痛修改第三方库源码的方法详解》很多时候,我们下载的第三方库是不会有需求不满足的情况,但也有极少的情况,第三方库没有兼顾到需求,本文将介绍几个修改源码的操作,大家可以根据需求进行选择... 目录需求不符合模拟示例 1. 修改源文件2. 继承修改3. 猴子补丁4. 追踪局部变量需求不符合很
阅读更多...
SpringBoot实现MD5加盐算法的示例代码

SpringBoot实现MD5加盐算法的示例代码

《SpringBoot实现MD5加盐算法的示例代码》加盐算法是一种用于增强密码安全性的技术,本文主要介绍了SpringBoot实现MD5加盐算法的示例代码,文中通过示例代码介绍的非常详细,对大家的学习... 目录一、什么是加盐算法二、如何实现加盐算法2.1 加盐算法代码实现2.2 注册页面中进行密码加盐2.
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2