济南做网站设计,阿里巴巴关键词推广,网站建设技术部职责,汕头网站建站公司SLAM中后端优化的技术细节 本文档主要收集总结了一些SLAM大佬们讲解后端优化中偏理论的技术细节的博客和回答以及一些学习教材。 Written by wincent 位姿估计以及李群相关的概述 Lie theory is by no means simple ———— 谎言理论绝不简单doge 《A micro Lie theory for st…SLAM中后端优化的技术细节 本文档主要收集总结了一些SLAM大佬们讲解后端优化中偏理论的技术细节的博客和回答以及一些学习教材。 Written by wincent 位姿估计以及李群相关的概述 Lie theory is by no means simple ———— 谎言理论绝不简单doge 《A micro Lie theory for state estimate on robotics》 written by Joan Sola可以说这篇教材是为SLAM量身定做的李群李代数学习资料本热阅读过后茅塞顿开对后端优化和位姿估计的学习有巨大帮助强烈推荐。他的另一篇力作《quaternion kinematics for the error-state Kalman filter》则是VO、VIO位姿估计的必读经典。《Lie Groups 2D and 3D Tramsformations》《机器人学中的状态估计》三维空间运动机理部分。讲述了李群李代数的引入动机、基本运算并且引入概率论的思想介绍了高斯过程中的位姿融合、位姿插值以及采样。后续的应用部分介绍了如何利用lie theory做位姿估计。知乎shuyong.chen https://www.zhihu.com/people/shuyong-chen-31 。他讲解的关于位姿估计使用李群的动机和理论依据以及与卡尔曼滤波器结合的一系列文章非常细致深入。我认为这些阅读博客文章可以进一步加深对以上论文材料的学习理解。这里例举一篇文章姿态估计引入李群和流形的动机与直觉博客园JingeTUhttps://www.cnblogs.com/JingeTU 清华涂金戈 他有不少讲解位姿估计的博客比较偏理论和公式推导 可以自己跟着推到一边用来对应SLAM框架中的代码实现。例举一文:Adjoint of SE(3) - JingeTU - 博客园 (cnblogs.com) 这里引用shouyoung.chen的总结做结尾
对姿态估计问题的理解是一步步深入理解和发展的。最早当卡尔曼滤波器被发明出来后是直接估计的表示姿态的四元数的这类卡尔曼滤波器被称为加性滤波器。很快NASA里的聪明人发现不对旋转是群而不是向量于是他们发明了乘性滤波器就是误差状态卡尔曼滤波器又称间接卡尔曼滤波器。但是长久以来加性滤波器和乘性滤波器在实际项目中都工作正常。不得不说卡尔曼滤波器真是神奇的东西够能容纳异类的。
这也因此没人说得清到底谁对谁错直到本世纪初Dr. F. Landis Markley 这位白胡子老爷爷用几篇论文明确地揭示了问题所在才在姿态估计问题中确立了误差状态卡尔曼滤波器的正统地位。在姿态估计问题里又是乘法又是群的李群的应用自然浮出水面。过了一些年就有人用李群重新解释和推导了误差状态卡尔曼滤波器。再过些年Joan Solà 在写博士论文时顺带写了一份教材这就是有名的《Quaternion kinematics for the error-state Kalman filter》。如今他再战江湖挑战李理论难懂不会用的困难写出了最短小最易懂的入门教材就是参考文献[1]A micro Lie theory for state estimation in robotics。本文就是他的教材的学习笔记。
近些年对姿态估计问题的研究又有新的发展或者说有新的灌水田地。前面应用李群偏向应用群方面的知识现在开始偏向应用流形方面的知识。参考文献[4]就是代表。
但是姿态估计问题包括卡尔曼滤波器本质上是工程问题。工程上的一个小问题就能轻易地让数学上的努力变得毫无作用。而加性滤波器似乎也没那么不堪。PX4 项目里的 ECL2 姿态估计器就是对姿态四元数直接用EKF做估计估计精度不输于其它间接的估计器也成功地在成千上万个无人机里得到应用。卡尔曼滤波器既神奇又有黑暗的角落例如Q R矩阵调参似乎比数学更重要。所以在这个世界里始终还是调参侠的天下。
非线性最小二乘算法及滤波算法 正因有大千世界的不确定性才会产生优化 对非线性最小二乘法的综述《METHODS FOR NON-LINEAR LEAST SQUARES PROBLEMS》这篇博客比较通俗易懂得叙述了论文的内容《 METHODS FOR NON-LINEAR LEASTSQUARES PROBLEMS》论文学习_wincent嘻嘻哈哈的博客-CSDN博客《视觉SLAM十四讲》后端优化部分《机器人学中的状态估计》二三四章主要介绍了滤波方法。虽然是讲解不那么时髦的滤波方法但是书中的思想以及严格的数学证明是入门后端优化必不可少的学习资料。经过学习也会深刻认识到滤波方法和优化方法千丝万缕的联系以及最本质的区别。
我本人认为学习优化的算法时不仅仅是学习数值优化的方法更应该用概率的思想来理解各种各样的状态估计算法。比如最小二乘算法实际上就是去最大化我们估计的变量的后验概率
滑动窗口法中的稀疏化和边缘化 如何舍弃也是一门艺术 贺一佳论述稀疏化和边缘化SLAM中的marginalization 和 Schur complement_slam 边缘化_白巧克力亦唯心的博客-CSDN博客贺一佳以DSO为例讲解Sliding Window: DSO 中的Windowed Optimization_白巧克力亦唯心的博客-CSDN博客 。
个人认为以上两篇博客已经比较完整的介绍了marginalization和Schur complement包含了比较直观的解释以及数学推导。代码部分的话每个框架都有不同的边缘化策略以及先验能量的计算方法需要具体框架具体分析。
FEJ技术学习资料 SLAM可以不做好边缘化无非就是丢失一些先验信息导致优化精度不是那么高。而如果做不好FEJSLAM系统一定会随着运行逐渐崩溃。 知乎关于FEJ的讨论如何理解SLAM中的First-Estimates Jacobian - 知乎 (zhihu.com) 黄国权三篇基于EKF-SLAM论述FEJ技术以及分析证明SLAM系统一致性问题。他通过严谨的数学推导证明了不采用FEJ的SLAM系统存在不一致性不可观的变量可观了即优化增量会在实际上不存在的维度上产生更新并且不影响总的能量项。 《A First-Estimates Jacobian EKF for Improving SLAM Consistency》《Analysis and Improvement of the Consistency of Extended Kalman Filter based SLAM》《Generalized Analysis and Improvement of the Consistency of EKF-based SLAM》
事实证明虽然论述FEJ是在EKF-SLAM基础上进行的但是FEJ同样也适用于优化方法的SLAM。只是两者的Jacobian含义有所不同。
零空间正交化 指向前方真正的路 不是那么严谨的以DSO为例子讲解零空间正交化DSO(5)——零空间的计算与推导_a first-estimates jacobian ekf for improving slam _无人的回忆的博客-CSDN博客 也是一篇以DSO为例讲解零空间正交化的博客:DSO零空间与尺度漂移_林突破的博客-CSDN博客 零空间正交化的集大成博客一文看尽4种SLAM中零空间的维护方法_无人的回忆的博客-CSDN博客
和老师的交流 这一部分主要是记录我在学习Direct Sparse Odometry时向老师请教问题时老师的回答。由于是在微信上进行交流所以可能看起来不是那么优雅和严谨。我目前跟随Sen Zhang老师进行SLAM的学习老师前不久从悉尼大学博士毕业(师从dacheng Tao)2022年一年发表了五篇计算机视觉的顶会其中两篇为ECCV所以老师的回答还是有一定的参考价值的并且回答时非常认真负责。 1、零空间正交化和FEJ是怎么回事呢 老师回答是这样的零空间主要表示的是优化过程中我们没有唯一解有一些不可观测的变量会影响最终结果这些不可观测的变量值可以不断改变但是最终优化的cost function可以不变这种解的不确定性会带来问题。具体的说在对slam做BA的时候我们要求解变量的增量x通过HxbHxbHxb来解HJJTHJJ^THJJT, b−Jfb-Jfb−Jf 用Gauss-Newton method。如果H是不可逆的那么我们没有唯一解举个例子我们求出了一个xxx满足HxbHxbHxb但是在HHH不可逆的时候Hx′0Hx0Hx′0也有解假设求出一个解是x′xx′那么不止是我们求出的xxx可以满足HxbHxbHxb, 任意的xkx′x kxxkx′也可以满足H(xkx′)HxkHx′Hx0HxbH(xkx)Hx k Hx Hx 0 Hx bH(xkx′)HxkHx′Hx0Hxb.所以我们实际上有无穷多个解但优化结果只是随机pick了其中一个解根据我们给定的优化初始值。这无穷多个解是因为Hx′0Hx0Hx′0有非零解所以这个非零解乘以任意常数加到一个HxbHxbHxb的任意一个解里都还是HxbHxbHxb的一个解。所有Hx′0Hx0Hx′0的解就构成了所谓的零空间。 这儿就有一个问题我们每一次优化得到的kx′kxkx′可能是不一样的是random的取决于我们优化的初始值所以我们要消除这种randomness的影响具体有两个其中第一个就是FEJ就是我们在计算HHH和bbb的时候其实只需要计算当前state的Jacobiankx′kxkx′隐式包含在这个Jacobian里面所以我们在同一次优化的不同迭代里会固定这个Jacobian用初始的所以叫first-estimate)这样就能保证在这一次优化中我们所以是随机在无穷多个解中pick了一个xkx′xkxxkx′但我们保证了所有迭代用的都是同一个kx′kxkx′。 还有一个是在边缘化Marginalization的时候我们用要边缘化的变量来最后一次update要保留的变量的值这儿有一个问题是要边缘化的变量其实有自己的一个random的kx′kxkx′当被它的值被用来update其他变量以后在未来再度对被保留的变量进行优化的时候新的kx‘kx‘kx‘可能和当时被边缘化的变量的kx’kx’kx’是不一样的这也会造成误差这儿处理的方式是在计算update时我们先求解零空间所有可能的kx‘kx‘kx‘然后我们只update解空间里和零空间正交orthogonal的部分这样不管零空间怎么变都不会影响到这部分正交的部分。 然后这个零空间对应到slam里是有实际物理意义的就是为什么H不可逆。比方在单目slam里我们没有尺度的信息因为在从三维空间投影到相机的像素平面的时候我们对深度进行了归一化。来一个具体的例子这种造成的后果就是我们优化的深度和相机位移是没有尺度信息的比方在直接法里我们的cost function是光度损失。 位移ttt和深度ziz_izi在一个除法的分子分母里意味着分子分母同时乘以一个任意常数最后的cost function的值都是一样的。所以只要位移和深度的比例是固定的那么具体是几米几十米那从优化的角度都是一样的,具体的数值取决于你优化时候给的初始值这是random的。所以在同一次优化的不同迭代里状态量变了对应的深度的绝对值可能就变了原来一个点的深度给了1m后面的迭代可能对应1.5m再后面可能0.8m对于每一步自己来说都没问题但你不断基于这做迭代的时候前后就不一致了优化很容易出问题,或是边缘化的时候对于一个三维位置你丢掉的变量那时给的深度是1m后续来了新的变量它们在优化时随机得到深度是1.5m各自都是对的但是前后就不一致了放在一起优化就会出问题。单目slam除了尺度还有一组不可观测的量是相机的绝对位姿我们求的一般是关于第一张相机图像坐标系的相对位姿所以整个序列同时旋转平移这个相对位姿都不变,所以单目slam一共有7个不可观测量3个是旋转3个是平移1个是尺度。 2、SLAM中的不可观测量和零空间应该有紧密的联系吧 老师回答不管这个不可观测的变量可以有很多不同的取值但最后我们可以得到同样的cost。并且不同的不可观测变量的取值也会影响到可观测变量的取值因此每一次优化出来的可观测变量都会implicitly pick了一个随机的不可观测变量的值我们需要在优化过程中保证所有迭代步骤用的是同一个不可观测变量的值不然数值之间就没有直接的对应的关系了我们求的增量加上去就不对了。不可观测变量有点像多个自变量对应了一个因变量 不是一对一关系了 从矩阵上来说就是维度降低了 秩小了 如果不严谨的对应一下的话 我感觉这个就和零空间的表现很像。零空间不就是Hx0有非零解嘛 H不可逆说明H里有线性相关的成分,不是满秩的也就是维度会变低。 研究发展脉络是大家先研究slam系统里的可观性问题最早是在filter based system不需要优化但需要对每一次运动方程和观测方程进行Gaussian近似也需要Jacobian然后针对可观性不可观性提出FEJ。后来方法从filter变成了optimization这儿的不可观性就对应到Hxb的零空间然后类似的因为过程中用了GN或LM来优化只影响Jacobian所以依然可以用之前的FEJ方法只是在marginalization的时候需要额外考虑只更新orthogonal分量。 零空间问题本质就是状态量的可观性。这里可以参考现代控制理论中的状态空间方程。 3、边缘化怎么理解和Schur Complement的关系又是什么
额是这样的schur complementSC和marginalizationmarg不是一回事SC是一种求解Hxb的运算方法就像在不同的方法里我们都用到了微积分但微积分不是某一个方法的别名。
在优化里为了加速优化和利用H的稀疏性咱们把稀疏和不稀疏的部分分开来SC在这里的作用是可以把两个稀疏和不稀疏对应的x分开来求解这样稀疏的部分求解起来很快SC的主要作用是可以把含有多个变量的vector x中的变量给拆分开来分别计算在优化里是为了把x中稀疏和不稀疏的变量拆分开来。
在marg里所谓边缘化是随着相机不断运动我们会有越来越多的变量特征点相机位姿等系统里要优化的变量太多咱们机器就算不动了所以我们一般maintain一个sliding window固定变量的个数上限超过了就丢弃最早的一些变量这个变量丢弃的环节叫做边缘化把那些要丢的变量的给边缘化了但是我们不是直接丢弃因为变量之间是有correlation的要丢弃的这些变量里也含有我们要保留的变量的信息我们希望把这些信息给抽取出来加到我们要保留的变量上。所以我们求解Hxb但在marg里我们把x拆分成要丢弃marg的变量和要保留的变量然后分别计算他们的increment x其实只需要计算要保留的变量的increment然后更新要保留的变量的值相当于把要丢弃的变量的信息给加到要保留的变量里然后之后优化过程就不考虑这些被边缘化了的变量了。 所以SC只是一个求解Hxb的方法拆分x成两个部分然后分别求解其增量区分在于怎么根据我们不同目的来拆分x。1在正常优化过程中我们拆分x为稀疏和不稀疏的部分来加速运算2在边缘化过程中我们拆分x为要丢弃和要保留的部分从而用要丢弃的信息来最后一次更新要保留的变量。 这是SC的形式。从概率分布的角度来看marginalizaiton也有另一种理解要丢弃的变量b和要保留的变量a原本我们维护一个他们的joint distribution P(a, b)现在我们希望得到一个把b给marginalize掉之后a的marginal distribution P(a | b)。假设是multivariate Gaussian,我们可以得到 可以发现这个条件概率分布中协方差矩阵的形式和SC分解之后右半边式子形式是一样的如下图非常神奇。 协方差就是两个变量间的关系的一个度量SC是拆分两个变量然后先算一个再用之前的结果算另一个在后者的过程中肯定要利用两个变量间的关系才能使用起来之前的结果一般L2的损失都会有Gaussian distribution的对应关系btwL1对应Laplacian distribution所以这儿两个变量间的关系度量“happen to be” 协方差也说得通。 4、为什么L2损失下会有高斯分布的假设呢 在优化的时候对Gaussian distr求max其实等价exp里面的平方项求min也就是说最小二乘平方的L2形式实际上是从高斯分布的指数项为二次方推到过来的(详见机器人学中的状态估计)。所以很多形式能对应起来对用L2的优化问题基本都能找到基于Gaussian的概率分布下的解释和理解。
