Square Root SAM论文原理

文章目录

    • Square Root SAM论文原理
      • 核心原理
      • SLAM问题的3种表示
        • 贝叶斯网络
        • 因子图(Factor graph)
        • 马尔科夫随机场(Markov Random Field, MRF)
      • SLAM最小二乘问题&线性化
      • 因式分解 factorization
      • 矩阵与图(Matrices ⇔ Graphs)
      • 因式分解&变量消元(Factorization ⇔ Variable Elimination )
        • Cholesky分解(或LDL分解)
          • 变量消除在Cholesky分解中的作用
          • 步骤
        • QR分解
          • 变量消除在QR分解中的作用
          • 步骤
      • 变量计算(回代Back-substitution)
        • 基本概念
        • 回代算法
        • 具体步骤
        • 例子
      • 因式分解与变量顺序
      • SAM流程
        • Batch √ SAM
        • Linear Incremental √ SAM
        • Non-linear Incremental √ SAM

论文《Square Root SAM Simultaneous Localization and Mapping via Square Root Information Smoothing》

Square Root SAM论文原理

核心原理

关于SLAM问题的求解:
1、将非线性SLAM问题通过一阶泰勒展开化成线性问题Ax=b,再通过对A或A^TA进行因式分解(factoration基于Variable Elimination),得到上三角矩阵R,构建新问题Rx=d。
通过回代(backward substitution)的方法进行求解。

2、本文说明,变量在雅可比矩阵的排列顺序能够影响因式分解的稀疏性,本文使用colamd的排序方法应用于SLAM块结构(将位姿或特征点坐标均看成一个变量),增加因式分解及回代求解效率。

3、提出的smoothing approach的框架(利用a good order & factoration),即高效优化截止到当前帧的整个机器人的位姿及特征点。

SLAM问题的3种表示

贝叶斯网络

在这里插入图片描述
在贝叶斯概率网络表示中,机器人的状态 x x x 受顶部的马尔可夫链限制,而机器人的环境在底部由一组地标 l l l 表示。中间层的测量值 z z z 受机器人的状态和测量的地标参数限制。
P ( X , L , Z ) = P ( x 0 ) ∏ i = 1 M P ( x i ∣ x i − 1 , u i ) ∏ k = 1 K P ( z k ∣ x i k , l j k ) P(X, L, Z)=P\left(x_{0}\right) \prod_{i=1}^{M} P\left(x_{i} \mid x_{i-1}, u_{i}\right) \prod_{k=1}^{K} P\left(z_{k} \mid x_{i_{k}}, l_{j_{k}}\right) P(X,L,Z)=P(x0)i=1MP(xixi1,ui)k=1KP(zkxik,ljk)
其中:
P ( x 0 ) P(x_{0}) P(x0)表示先验
P ( x i ∣ x i − 1 , u i ) P(x_{i}|x_{i-1},u_{i}) P(xixi1,ui)表示状态传递概率
P ( z k ∣ x i k , l j k ) P(z_{k}|x_{i_{k}},l_{j_{k}}) P(zkxik,ljk)表示观测概率

因子图(Factor graph)

在这里插入图片描述
上图中,位姿和地标分别对应于圆形节点,每个测量值对应于因子节点(黑色实心圆圈)
状态传递方程与后验概率方程:
x i = f i ( x i − 1 , u i ) + w i ⇔ P ( x i ∣ x i − 1 , u i ) ∝ exp ⁡ − 1 2 ∥ f i ( x i − 1 , u i ) − x i ∥ Λ i 2 ) x_{i}=f_{i}(x_{i-1},u_{i})+w_{i}\qquad\Leftrightarrow\qquad P(x_{i}|x_{i-1},u_{i})\propto\exp-{\frac{1}{2}}\|f_{i}(x_{i-1},u_{i})-x_{i}\|_{\Lambda_{i}}^{2}) xi=fi(xi1,ui)+wiP(xixi1,ui)exp21fi(xi1,ui)xiΛi2)

观测方程与后验概率方程:
z k = h k ( x i k , l j k ) + v k ⇔ P ( z k ∣ x i k , l j k ) ∝ exp ⁡ − 1 2 ∣ h k ( x i k , l j k ) − z k ∣ ∣ Σ k 2 z_{k}=h_{k}(x_{i k},l_{j k})+v_{k}\qquad\Leftrightarrow\qquad P(z_{k}|x_{i k},l_{j k})\propto\exp-{\frac{1}{2}}|h_{k}(x_{i k},l_{j k})-z_{k}||_{\Sigma_{k }}^{2} zk=hk(xik,ljk)+vkP(zkxik,ljk)exp21hk(xik,ljk)zkΣk2

马尔科夫随机场(Markov Random Field, MRF)

在这里插入图片描述
MRF 的图形是无向的,没有因子节点:其邻接结构指示哪些变量由公共因子(度量或约束)链接。
P ( Θ ) ∝ ∏ i ϕ i ( θ i ) ∏ { i , j } , i < j ψ i j ( θ i , θ j ) ϕ 0 ( x 0 ) ∝ P ( x 0 ) ψ ( i − 1 ) i ( x i − 1 , x i ) ∝ P ( x i ∣ x i − 1 , u i ) ψ i k j k ( x i k , l j k ) ∝ P ( z k ∣ x i k , l j k ) \begin{gathered} P(\Theta)\propto\prod_{i}\phi_{i}(\theta_{i})\prod_{\{i,j\},i<j}\psi_{ij}(\theta_{i},\theta_{j}) \\ \phi_0(x_0)\propto P(x_0) \\ \psi_{(i-1)i}(x_{i-1},x_i)\propto P(x_i|x_{i-1},u_i) \\ \psi_{i_kj_k}(x_{i_k},l_{j_k})\propto P(z_k|x_{i_k},l_{j_k}) \end{gathered} P(Θ)iϕi(θi){i,j},i<jψij(θi,θj)ϕ0(x0)P(x0)ψ(i1)i(xi1,xi)P(xixi1,ui)ψikjk(xik,ljk)P(zkxik,ljk)

SLAM最小二乘问题&线性化

结合状态传递方程与观测方程:
Θ ∗ = Δ argmin ⁡ Θ { ∑ i = 1 M ∥ f i ( x i − 1 , u i ) − x i ∥ Λ i 2 + ∑ k = 1 K ∥ h k ( x i k , l j k ) − z k ∥ Σ k 2 } \Theta^*\stackrel{\Delta}{=}\underset{\Theta}{\operatorname*{argmin}}\left\{\sum_{i=1}^M\|f_i(x_{i-1},u_i)-x_i\|_{\Lambda_i}^2+\sum_{k=1}^K\|h_k(x_{i_k},l_{j_k})-z_k\|_{\Sigma_k}^2\right\} Θ=ΔΘargmin{i=1Mfi(xi1,ui)xiΛi2+k=1Khk(xik,ljk)zkΣk2}

待解未知量:各个时刻的位姿(截至当前时刻): x i x_i xi, 观测路标点的位置 l j l_j lj
将状态传递方程与观测方程对变量进行线性化:
f i ( x i − 1 , u i ) − x i ≈ { f i ( x i − 1 0 , u i ) + F i i − 1 δ x i − 1 } − { x i 0 + δ x i } = { F i i − 1 δ x i − 1 − δ x i } − a i F i i − 1 = Δ ∂ f i ( x i − 1 , u i ) ∂ x i − 1 ∣ x i − 1 0 f_{i}(x_{i-1},u_{i})-x_{i}\approx\left\{f_{i}(x_{i-1}^{0},u_{i})+F_{i}^{i-1}\delta x_{i-1}\right\}-\left\{x_{i}^{0}+\delta x_{i}\right\}=\left\{F_{i}^{i-1}\delta x_{i-1}-\delta x_{i}\right\}-a_{i}\\F_{i}^{i-1}\stackrel{\Delta}{=}\frac{\partial f_{i}(x_{i-1},u_{i})}{\partial x_{i-1}}\Bigg|_{x_{i-1}^{0}} fi(xi1,ui)xi{fi(xi10,ui)+Fii1δxi1}{xi0+δxi}={Fii1δxi1δxi}aiFii1=Δxi1fi(xi1,ui) xi10

关于x_i的雅可比矩阵为单位阵。

h k ( x i k , l j k ) − z k ≈ { h k ( x i k 0 , l j k 0 ) + H k i k δ x i k + J k j k δ l j k } − z k = { H k i k δ x i k + J k j k δ l j k } − c k H k i k ≜ ∂ h k ( x i k , l j k ) ∂ x i k ∣ ( x i k 0 , l j k 0 ) J k j k ≜ ∂ h k ( x i k , l j k ) ∂ l j k ∣ ( x i k 0 , l j k 0 ) ← h_{k}(x_{i_{k}},l_{j_{k}})-z_{k}\approx\left\{h_{k}(x_{i_{k}}^{0},l_{j_{k}}^{0})+H_{k}^{i_{k}}\delta x_{i_{k}}+J_{k}^{j_{k}}\delta l_{j_{k}}\right\}-z_{k}=\left\{H_{k}^{i_{k}}\delta x_{i_{k}}+J_{k}^{j_{k}}\delta l_{j_{k}}\right\}-c_{k}\\H_{k}^{i_{k}}\triangleq\frac{\partial h_{k}(x_{i_{k}},l_{j_{k}})}{\partial x_{i_{k}}}\bigg|_{(x_{i_{k}}^{0},l_{j_{k}}^{0})}\quad J_{k}^{j_{k}}\triangleq\frac{\partial h_{k}(x_{i_{k}},l_{j_{k}})}{\partial l_{j_{k}}}\bigg|_{(x_{i_{k}}^{0},l_{j_{k}}^{0})}\quad\leftarrow hk(xik,ljk)zk{hk(xik0,ljk0)+Hkikδxik+Jkjkδljk}zk={Hkikδxik+Jkjkδljk}ckHkikxikhk(xik,ljk) (xik0,ljk0)Jkjkljkhk(xik,ljk) (xik0,ljk0)

得到线性化后的方程:

δ ∗ = argmin ⁡ δ { ∑ i = 1 M ∥ F i i − 1 δ x i − 1 + G i i δ x i − a i ∥ Λ i 2 + ∑ k = 1 K ∥ H k i k δ x i k + J k j k δ l j k − c k ∥ Σ k 2 } \delta^{*}=\underset{\delta}{\operatorname*{argmin}} \left\{\sum_{i=1}^{M}\|F_{i}^{i-1}\delta x_{i-1}+G_{i}^{i}\delta x_{i}-a_{i}\|_{\Lambda_{i}}^{2}+\sum_{k=1}^{K}\|H_{k}^{i_{k}}\delta x_{i_{k}}+J_{k}^{j_{k}}\delta l_{j_{k}}-c_{k}\|_{\Sigma_{k}}^{2}\right\} δ=δargmin{i=1MFii1δxi1+GiiδxiaiΛi2+k=1KHkikδxik+JkjkδljkckΣk2}

该方程使用马氏距离来定义: ∥ e ∥ Σ 2 = Δ e T Σ − 1 e = ( Σ − T / 2 e ) T ( Σ − T / 2 e ) = ∥ Σ − T / 2 e ∥ 2 2 \|e\|_{\Sigma}^{2}\stackrel{\Delta}{=}e^{T}\Sigma^{-1}e=(\Sigma^{-T/2}e)^{T}(\Sigma^{-T/2}e)=\left\|\Sigma^{-T/2}e\right\|_{2}^{2} eΣ2=ΔeTΣ1e=(ΣT/2e)T(ΣT/2e)= ΣT/2e 22

线性化方程的意义:

  1. 好的线性化点(该点与最优解与残差的变化成线性关系)直接将非线性问题转为线性问题。
  2. 使用迭代法完成求解时,使用该线性化方程求变量更新量,相当于一次牛顿法求\delta x

将整个整理成大矩阵:
在这里插入图片描述
在这里插入图片描述
A的维度为 ( N d x + K d z ) × ( N d x + M d l ) (N d_x + Kd_z) × (N d_x + M d_l) (Ndx+Kdz)×(Ndx+Mdl)

因式分解 factorization

求解上述线性方程,可以使用因式分解,构成Rx=d,其中R为上三角矩阵的形式,而后使用反向消元方法来完成求解。
1、 对 A T A A^TA ATA进行Cholesky分解:
Cholesky 分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。它要求矩阵的所有特征值必须大于零,故分解的下三角的对角元也是大于零的。Cholesky分解法又称平方根法,是当A为实对称正定矩阵时,LU三角分解法的变形。 A = L L T A=LL^T A=LLT
原问题可化为:
A T A δ ∗ = A T b A^TA\delta^*=A^Tb ATAδ=ATb I ≜ A T A = R T R \mathcal{I}\triangleq A^{T}A=R^{T}R IATA=RTR R T y = A T b R^Ty=A^Tb RTy=ATb R T y = A T , R δ ∗ = y R^{T}y=A^{T}, R\delta^*=y RTy=ATRδ=y
从而得:
R δ ∗ = y R\delta^*=y Rδ=y

2、三角化LDL 分解
将信息矩阵三角化:
I = R T R = L D L T \mathcal{I}=R^TR=LDL^T I=RTR=LDLT

3、 QR分解
QR 分解将一个矩阵分解一个正交矩阵 (酉矩阵) 和一个三角矩阵的乘积. QR 分解被广泛应用于线性最小二乘问题的求解和矩阵特征值的计算. A=QR。

A ∈ C m × n A ∈ C_{m×n} ACm×n(m ≥ n,约束方程远大于待求解变量个数). 则存在一个单位列正交矩阵 Q ∈ C m × n Q ∈ C_{m×n} QCm×n,(即 Q ∗ Q = I n × n Q∗Q =I_{n×n} QQ=In×n) 和 一个上三角矩阵 R ∈ C n × n R∈C_{n×n} RCn×n, 使得: A = Q R A = QR A=QR.

文中 Q ∈ C m × m , Q T ∗ A = R Q∈ C_{m×m},Q^T*A=R QCm×mQTA=R的补充。

Q T A = [ R 0 ] Q T b = [ d e ] Q^TA=\left[\begin{array}{c}R\\0\end{array}\right]\quad Q^Tb=\left[\begin{array}{c}d\\e\end{array}\right] QTA=[R0]QTb=[de]
对于密集矩阵A的分解,首选方法是从左到右逐列计算R。对于每一列j,通过将左边的A与Householder reflection矩阵Hj相乘,将对角线下方的所有非零元素归零。在n次迭代之后,A被完全因子化:
H n . . H 2 H 1 A = Q T A = [ R 0 ] \left.H_{n}..H_{2}H_{1}A=Q^{T}A=\left[\begin{array}{c}{R}\\{0}\end{array}\right.\right] Hn..H2H1A=QTA=[R0]

得到分解矩阵(上三角矩阵R)后,可以将原先的最小二乘问题(LS problem)转化为如下方程:
∥ A δ − b ∥ 2 2 = ∥ Q T A δ − Q T b ∥ 2 2 = ∥ R δ − d ∥ 2 2 + ∥ e ∥ 2 2 \|A \delta-b\|_{2}^{2}=\left\|Q^{T} A \delta-Q^{T} b\right\|_{2}^{2}=\|R \delta-d\|_{2}^{2}+\|e\|_{2}^{2} Aδb22= QTAδQTb 22=Rδd22+e22
即:
R δ = d R\delta =d Rδ=d

以上问题可以通过反向消元(back-substitution)的方法进行求解

因此QR分解的计算复杂度主要由Householder reflections 矩阵的计算决定:即2(m-n/3)n^2。
将 QR 与 Cholesky 因式分解进行比较,我们发现两种算法在 m>> n 时都需要 O(mn^2 ) 操作,但 QR 因式分解慢了 2 倍。虽然这些数字仅对密集矩阵有效,但我们已经看到,在实践中,LDL 和 Cholesky 因式分解在稀疏问题上也远远优于 QR 因式分解,而不仅仅是常数因子。

矩阵与图(Matrices ⇔ Graphs)

雅可比矩阵A对应SLAM因子图,表征观测与变量之间的关系;
信息矩阵ATA对应markov random field因子图,表征变量与变量之间的关系。
需要强调的是:SLAM问题的变量都是以参数块作为一个整体的,如特征点的3维位置作为一个参数块,6-DOF位姿作为一个参数块。矩阵与图的对应是块参数的对应关系。

从而:
A 的块结构与与 SAM 关联的因子图的邻接矩阵完全对应。
信息矩阵 I = ATA 是与 SLAM 问题相关的马尔可夫随机场矩阵。同样,在块级别,ATA 的稀疏性模式恰好是关联 MRF 的邻接矩阵。MRF 中的节点对应于机器人状态和地标。链接表示里程计或里程碑测量值。

在一些中,采用MRF图视图来揭示SLAM的滤波版本中固有的相关性结构。结果表明,当边缘化过去的轨迹 x 1 : M − 1 x_{1:M-1} x1:M1 时,信息矩阵不可避免地变得完全密集。因此,这些方法的重点是选择性地删除链接以降低滤波器的计算成本,并取得了巨大的成功。相比之下,这篇文章考虑了与平滑信息矩阵 I 相关的 MRF,该矩阵不会变得密集,因为过去的状态永远不会被边缘化,SAM信息矩阵存放的是是完整的变量关系(从初始到当前)。

因式分解&变量消元(Factorization ⇔ Variable Elimination )

QR分解和Cholesky(或LDL)分解都是基于变量消除算法的矩阵分解方法。在求解线性方程组、最小二乘问题等数值计算中,它们都涉及通过逐步消除变量来简化问题。让我们深入理解它们如何基于变量消除。

Cholesky分解(或LDL分解)

Cholesky分解将一个对称正定矩阵 (A) 分解为一个下三角矩阵 (L) 及其转置 (L^T) 的乘积,即 ( A = L L T A = LL^T A=LLT)。LDL分解是Cholesky分解的一种变体,将矩阵分解为一个下三角矩阵 (L),一个对角矩阵 (D),和 ( L T L^T LT) 的乘积,即 ( A = L D L T A = LDL^T A=LDLT)。

变量消除在Cholesky分解中的作用
  1. 逐步消除变量:通过逐步消去矩阵中的非对角线元素,将矩阵转换为一个易于处理的形式。
  2. 保持稀疏性:在处理稀疏矩阵时,通过选择合适的消除顺序,可以最大限度地保持矩阵的稀疏性,提高计算效率。
步骤
  1. 从矩阵 (A) 的第一行和第一列开始,计算第一个对角元素和下三角矩阵 (L) 的第一列。
  2. 对剩余的子矩阵进行类似的操作,消去非对角线元素,逐步构建下三角矩阵 (L) 和对角矩阵 (D)。
  3. 重复这一过程,直到处理完所有元素。
QR分解

QR分解将一个矩阵 (A) 分解为一个正交矩阵 (Q) 和一个上三角矩阵 (R) 的乘积,即 (A = QR)。QR分解可以通过一系列的Givens旋转或Householder反射来实现,这两者本质上都是基于变量消除的过程。

变量消除在QR分解中的作用
  1. Householder反射:通过引入Householder矩阵,将一个向量反射到某个子空间,使得消去矩阵中的某些元素。这个过程等价于逐步消除变量,简化矩阵结构。
  2. Givens旋转:通过Givens旋转,可以将矩阵的某些元素变为零,从而逐步将矩阵转换为上三角形。这也是一种变量消除的形式。
步骤
  1. 从矩阵 (A) 的第一列开始,通过Householder反射或Givens旋转将下面的元素变为零。
  2. 处理下一列,通过类似的操作消除非对角线元素。
  3. 继续这一过程,直到得到上三角矩阵 (R)。

QR分解和Cholesky分解(或LDL分解)都基于变量消除的思想,通过逐步消去矩阵中的非对角线元素,将问题简化为易于处理的上三角或下三角形式。这些分解方法不仅用于求解线性方程组和最小二乘问题,还广泛应用于各种数值计算和优化问题中。

选择合适的消除顺序对于保持矩阵稀疏性和提高计算效率至关重要。通过合理的变量消除,可以有效地简化复杂问题,降低计算复杂度。

变量在信息矩阵及A阵中的排列序列即决定了变量消元(elimination)的顺序(列驱动:从左到右)。
变量的排列序列影响因式分解的结果,影响R阵的稀疏程度。
使用反向消元求解的方法依赖R阵的稀疏性,当分解的结果R阵比较稠密时,此时使得因式分解的计算量变大,或者大于直接的A阵求逆。

变量计算(回代Back-substitution)

在得到R\delta=d的关系后,该等式示意图如下:
在这里插入图片描述
回代(Back-substitution)是一种用于求解上三角矩阵线性方程组的算法。它通常用于解决通过Gaussian消去法或QR分解等方法得到的方程组。

基本概念

假设我们有一个上三角矩阵 (R) 和一个向量 (b),并希望求解线性方程组 (Rx = b)。上三角矩阵的特点是它的下三角部分全是零,即所有的非零元素都位于对角线及其上方。

回代算法

回代的基本思路是从最后一个方程开始,逐步向上求解每个变量。具体步骤如下:

  1. 初始化:从最后一个方程(倒数第一行)开始求解。
  2. 逐步求解:根据上一个变量的值,向上求解前一个变量,直到所有变量都求出。
具体步骤

假设我们有一个 ( n × n n \times n n×n) 的上三角矩阵 ( R R R),和一个 ( n n n) 维向量 ( b b b),我们的目标是求解 ( R x = b Rx = b Rx=b) 中的 ( x x x)。

  1. 从第 ( n n n) 个方程开始:
    R n n x n = b n R_{nn} x_n = b_n Rnnxn=bn
    求解:
    x n = b n R n n x_n = \frac{b_n}{R_{nn}} xn=Rnnbn

  2. 对于第 ( k k k) 个方程(从 ( n − 1 n-1 n1) 到 1):
    [ R_{kk} x_k + R_{k, k+1} x_{k+1} + \cdots + R_{kn} x_n = b_k ]
    已知 (x_{k+1}, \ldots, x_n) 的值,求解:
    [ x_k = \frac{b_k - \sum_{j=k+1}^{n} R_{kj} x_j}{R_{kk}} ]

例子

假设我们有以下上三角矩阵 (R) 和向量 (b):

R = ( 2 3 1 0 1 4 0 0 3 ) , b = ( 8 7 9 ) R = \begin{pmatrix} 2 & 3 & 1 \\ 0 & 1 & 4 \\ 0 & 0 & 3 \end{pmatrix}, \quad b = \begin{pmatrix} 8 \\ 7 \\ 9 \end{pmatrix} R= 200310143 ,b= 879

我们从最后一个方程开始:
3 x 3 = 9 ⇒ x 3 = 3 3x_3 = 9 \Rightarrow x_3 = 3 3x3=9x3=3

然后是第二个方程:
x 2 + 4 x 3 = 7 ⇒ x 2 + 4 ( 3 ) = 7 ⇒ x 2 = 7 − 12 ⇒ x 2 = − 5 x_2 + 4x_3 = 7 \Rightarrow x_2 + 4(3) = 7 \Rightarrow x_2 = 7 - 12 \Rightarrow x_2 = -5 x2+4x3=7x2+4(3)=7x2=712x2=5

最后是第一个方程:
2 x 1 + 3 x 2 + x 3 = 8 ⇒ 2 x 1 + 3 ( − 5 ) + 3 = 8 ⇒ 2 x 1 − 15 + 3 = 8 ⇒ 2 x 1 − 12 = 8 ⇒ 2 x 1 = 20 ⇒ x 1 = 10 2x_1 + 3x_2 + x_3 = 8 \Rightarrow 2x_1 + 3(-5) + 3 = 8 \Rightarrow 2x_1 - 15 + 3 = 8 \Rightarrow 2x_1 - 12 = 8 \Rightarrow 2x_1 = 20 \Rightarrow x_1 = 10 2x1+3x2+x3=82x1+3(5)+3=82x115+3=82x112=82x1=20x1=10

所以,解 (x) 是:
x = ( 10 − 5 3 ) x = \begin{pmatrix} 10 \\ -5 \\ 3 \end{pmatrix} x= 1053

回代是一种简单而高效的方法,用于求解上三角矩阵的线性方程组。通过从最后一个变量开始逐步向上求解,可以快速找到解。这个过程在许多数值算法中,如Gaussian消去法和QR分解后,是不可或缺的步骤。

因式分解与变量顺序

1、A good ordering
变量的排列顺序影响因式分解的结果,下图维用于良好排序的三角化(对信息矩阵进行分解得到的上三角R阵)图(colamd)。这是一个有向图,其中每条边对应于 Cholesky 三角形 R 中的非零。

The triangulated graph for a good ordering (colamd):
在这里插入图片描述
Triangulated 用于说明经过因式分解三角化后的R对应的图。

QR和Cholesky(或LDL)两种分解方法都基于变量消元算法。这些方法的区别在于,QR 从因子图中消除变量节点并获得 A = Q R A = QR A=QR,而 Cholesky 或 LDL 从 MRF 开始,因此获得 A T A = R T R A^TA = R^TR ATA=RTR。两种方法一次消除一个变量,从 δ 1 δ1 δ1 开始,对应于 A A A I I I 的最左侧列。消元的结果是, δ 1 δ_1 δ1 现在表示为所有其他未知数 δ j > 1 δ_j>1 δj>1 的线性组合,系数位于 R R R 的相应行 R 1 R_1 R1 中。然而,在此过程中,连接到 δ 1 δ_1 δ1 的所有变量之间会引入新的依赖关系,这会导致边被添加到图形中。然后以类似的方式处理下一个变量,直到所有变量都被消除。消除所有变量的结果是一个有向的三角化(弦chordal)图。

2、LX ordering

一旦获得了关于 R R R的弦图,就可以得到 R R R的消元树,它被定义为消元后的弦图的深度优先生成树,它有助于说明回代阶段的计算流程。为了说明这一点,下图显示了通过使用众所周知的启发式方法获得的和弦图,该启发式方法首先消除地标,然后处理姿势(我们称之为 LX ordering)。

在这里插入图片描述
LX ordering triangulated graph

相应的消除树如下图所示。树的根对应于最后一个要消除的变量 δ n δ_n δn,这是在反向替换中要计算的第一个变量。然后,计算沿着树向下进行,虽然这通常是以相反的列顺序完成的,但不相交的子树中的变量可以按任何顺序计算,有关更详细的讨论。事实上,如果一个人只对某些变量感兴趣,就没有必要计算任何不包含它们的子树。
在这里插入图片描述
LX-ordering 排序的消元树,显示了如何通过反向替换来计算状态和地标估计值:首先计算根 - 这里是左侧的第一个姿势 - 然后沿着马尔可夫链向右继续。一旦计算出地标所连接的姿态,就可以计算出它们。

不同的参数排列顺序会影响因式分解后的结果,R的稀疏性。参数的排列会影响因式分解的效率(求R的复杂度)以及变量回代的效率。对于中等大小的问题,一种流行的方法是 colamd

在这里插入图片描述
左侧是相关的雅可比 A A A 测量值,其中有 3 × 95 + 2 × 24 = 333 3×95+2×24 = 333 3×95+2×24=333 个未知数。行数 1126 等于(标量)测量值的数量。右边:(顶部)信息矩阵 I = A T A \mathcal{I} = A^T A I=ATA;(中)其上三角形Cholesky三角形 R R R;(下)通过更好的变量排序(使用colamd)获得的替代因子 amdR。能够看出使用colamd方法进行变量排序得到的R阵更为稀疏。

这篇文章将标量变量的集合(3维特征点位置以及6-DOF位姿)视为单个变量,并创建一个较小的图形,该图形封装了这些块之间的约束,而不是单个变量。在这个较小的图形上调用 colamd 更高效。是关于SLAM的变量问题基于块结构,colamd或任何其他近似排序算法都无法对块矩阵进行处理。虽然对 colamd 性能的影响可以忽略不计,但我们发现,让它直接在 SLAM MRF (块结构上,位置和位姿视为单变量)上工作而不是在稀疏矩阵 I \mathcal{I} I 上工作可以产生 2 倍到有时 100 倍的改进。

在某些情况下,任何排序都会导致相同的大填充。最坏的情况是完全连接的 MRF:从每个位置都可以看到每个地标。在这种情况下,消除任何变量将完全连接另一侧的所有变量,然后完全知道集团树的结构:如果先选择位姿pose,则根将是整个地图,一旦知道地图,所有姿势都将被计算出来。反之亦然,如果选择了一个地标,则轨迹将是集团树根集团,计算将通过(昂贵的)轨迹优化进行,然后(非常便宜的)计算地标。这两种情况的解决方法依赖于块标准分割逆或“舒尔补码”。

SAM流程

Batch √ SAM
  1. 构建雅可比 A A A 和 RHS b b b 测量。
  2. 找到一个好的列排序 p p p,并重新排序 A p ← A A_p ← A ApA.
  3. 使用 Cholesky 或 QR 分解方法求解 δ p ∗ = a r g m i n δ ∣ ∣ A p δ p – b ∣ ∣ 2 2 δ^∗_p = argmin_\delta ||A_pδ_p – b||^2_2 δp=argminδ∣∣Apδpb22
  4. 通过 δ ← δ p δ ← δp δδp,最优解,并恢复原始排序: r = p − 1 r = p^{−1} r=p1
    在这里插入图片描述
Linear Incremental √ SAM

当有新的观测到来时,信息矩阵会得到新的填充或更新:
I ′ = I ′ + a a T \mathcal{I}'=\mathcal{I}'+aa^T I=I+aaT

其中 T 是测量值雅可比 A 的新行,对应于在任何给定时间步长传入的新测量值。但是,这些算法通常仅针对密集矩阵实现,因此我们必须使用稀疏存储方案以获得最佳性能。虽然存在稀疏的Cholesky更新,但它们的实现相对复杂。第二种可能性,易于实现并适用于稀疏矩阵,是使用一系列给定旋转逐个消除新测量行中的非零。第三种可能性,我们在下面的模拟中采用,是更新矩阵 I \mathcal{I} I,并简单地使用完整的 Cholesky(或 LDL)因式分解。虽然这似乎很昂贵,但我们发现,有了良好的排序,主要成本不再是因式分解,而是更新信息矩阵I。

例如,实验结果表明,在运行结束时,通过稀疏乘法获得 Hessian 的成本大约是因式分解的 5 倍。重要的是,信息矩阵包含了从开始到当前时刻所有的测量信息,因此无需在每个时间步长进行因式分解。原则上,我们可以等到最后,然后计算整个轨迹和地图。然而,在实验过程中的任何时候,地图和/或轨迹都可以通过简单的因式分解和反向替换来计算,例如,用于可视化和/或路径规划目的。

Non-linear Incremental √ SAM

如果在增量设置中,SLAM问题是非线性的,并且线性化点可用,则上述线性增量解决方案适用。是否是这种情况很大程度上取决于传感器和机器人可用的先验知识。例如,在室内移动机器人中,通常没有良好的绝对方向传感器,这意味着我们必须围绕当前猜测的方向进行线性化。这正是EKF在更大规模环境中出现问题的原因,因为当过去的机器人姿势从MRF中消除时,这些可能错误的线性化点被“烘焙到”信息矩阵中。平滑方法的一个显著优点是,我们从不致力于给定的线性化,因为不会从图形中消除任何变量。有两种不同的方法可以重新线性化:在某些情况下,例如沿完整轨迹闭合一个大循环,重新线性化所有变量的成本更低。从本质上讲,这意味着我们每次都必须调用上面的批处理版本。从好的方面来说,我们的实验结果将表明,即使是这种看似昂贵的算法在EKF方法难以解决的大型问题上也相当实用。当只有较少数量的变量受到线性化点变化的影响时,另一种方法是有利的。在这种情况下,可以应用降级和更新技术暂时从因子中删除这些变量,然后使用新的线性化点再次添加它们。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/web/41346.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Kafka系列之Kafka知识超强总结

一、Kafka简介 Kafka是什么 Kafka是一种高吞吐量的分布式发布订阅消息系统&#xff08;消息引擎系统&#xff09;&#xff0c;它可以处理消费者在网站中的所有动作流数据。 这种动作&#xff08;网页浏览&#xff0c; 搜索和其他用户的行动&#xff09;是在现代网络上的许多社…

14-22 剑和远方2 - 深度神经网络中的学习机制

概论 在第一部分中&#xff0c;我们深入探讨了人工智能的兴衰简史以及推动人工智能发展的努力。我们研究了一个简单的感知器&#xff0c;以了解其组件以及简单的 ANN 如何处理数据和权重层。在简单的 ANN 中&#xff0c;不会对数据执行特定操作。ANN 中的激活函数是一个线性函…

flask使用定时任务flask_apscheduler(APScheduler)

Flask-APScheduler描述: Flask-APScheduler 是一个 Flask 扩展&#xff0c;增加了对 APScheduler 的支持。 APScheduler 有三个内置的调度系统可供您使用&#xff1a; Cron 式调度&#xff08;可选开始/结束时间&#xff09; 基于间隔的执行&#xff08;以偶数间隔运行作业…

移动校园(7)ii:uniapp响应拦截器处理token,以及微信小程序报错当前页面正在处于跳转状态,请稍后再进行跳转....

依据昨天的写完&#xff0c;在token过期之后&#xff0c;再次调用接口&#xff0c;会触发后端拦截&#xff0c;扔进全局错误处理中间件 前端说明提示都没有&#xff0c;只有一个这个&#xff0c;现在优化一下&#xff0c;再写一个类似全局后置守卫&#xff0c;当状态码是401的时…

RAID 冗余磁盘阵列

RAID也是Linux操作系统中管理磁盘的一种方式。 只有Linux操作系统才支持LVM的磁盘管理方式。 而RAID是一种通用的管理磁盘的技术&#xff0c;使用于多种操作系统。 优势&#xff1a;提升数据的读写速度&#xff0c;提升数据的可靠性。具体实现哪什么功能&#xff0c;要看你所…

LVGL移植与VS模拟器使用

一、移植文件介绍 二、移植部分 第一步&#xff1a;创建LVGL文件夹 第二步&#xff1a; 构造LVGL文件夹&#xff1a;LVGL - GUI - lvgl - 第三步&#xff1a;添加文件 3.1 从examples中添加2个.c文件 3.2 从src中添加文件 draw文件 extra文件 第四步&#xff1a; 三、Ke…

Linux系统安装软件包的方法rpm和yum详解

起因&#xff1a; 本篇文章是记录学习Centos7的历程 关于rpm 常见命令 1&#xff09;查看已经安装的软件包 rpm -q 软件包名 2&#xff09;查看文件的相关信息 rpm -qi 软件包名 3&#xff09;查看软件包的依赖关系 就是说要想安装这个软件包&#xff0c;就必须把一些前…

三级_网络技术_04_中小型网络系统总体规划与设计

1.下列关于路由器技术特征的描述中&#xff0c;正确的是()。 吞吐量是指路由器的路由表容量 背板能力决定了路由器的吞吐量 语音、视频业务对延时抖动要求较低 突发处理能力是以最小帧间隔值来衡量的 2.下列关于路由器技术特征的描述中&#xff0c;正确的是()。 路由器的…

springboot公寓租赁系统-计算机毕业设计源码03822

摘要 1 绪论 1.1 研究背景与意义 1.2选题背景 1.3论文结构与章节安排 2 公寓租赁系统系统分析 2.1 可行性分析 2.1.1 技术可行性分析 2.1.2 经济可行性分析 2.1.3 法律可行性分析 2.2 系统功能分析 2.2.1 功能性分析 2.2.2 非功能性分析 2.3 系统用例分析 2.4 系…

韦东山嵌入式linux系列-第一个实验

1 前言 笔者使用的是韦东山STM32MP157 Pro的板子&#xff0c;环境搭建部分按照说明文档配置完成。配置桥接网卡实现板子、windows、ubuntu的通信&#xff0c;也在开发板挂载 Ubuntu 的NFS目录 &#xff0c;这里就不再赘述了。 板子: 192.168.5.9 windows: 192.168.5.10 ubunt…

机械键盘如何挑选

机械键盘的选择是一个关键的决策&#xff0c;因为它直接影响到我们每天的打字体验。在选择机械键盘时&#xff0c;有几个关键因素需要考虑。首先是键盘的键轴类型。常见的键轴类型包括蓝轴、红轴、茶轴和黑轴等。不同的键轴类型具有不同的触发力、触发点和声音。蓝轴通常具有明…

聚类分析方法(一)

目录 一、聚类分析原理&#xff08;一&#xff09;聚类分析概述&#xff08;二&#xff09;聚类的数学定义&#xff08;三&#xff09;簇的常见类型&#xff08;四&#xff09;聚类框架及性能要求&#xff08;五&#xff09;簇的距离 二、划分聚类算法&#xff08;一&#xff0…

Java 有什么必看的书?

Java必看经典书有这两本&#xff1a; 1、Java核心技术速学版&#xff08;第3版&#xff09; 经典Java开发基础书CoreJava速学版本&#xff01;Java入门优选书籍&#xff0c;更新至Java17&#xff0c;内容皆是精华&#xff0c;让Java学习更简单&#xff0c;让Java知识应用更快速…

【Linux】什么是进程间通信?方式有哪些?本质理解?

&#x1f490; &#x1f338; &#x1f337; &#x1f340; &#x1f339; &#x1f33b; &#x1f33a; &#x1f341; &#x1f343; &#x1f342; &#x1f33f; &#x1f344;&#x1f35d; &#x1f35b; &#x1f364; &#x1f4c3;个人主页 &#xff1a;阿然成长日记 …

NoSQL 之 Redis 集群部署

前言&#xff1a; &#xff08;1&#xff09;主从复制&#xff1a;主从复制是高可用Redis的基础&#xff0c;哨兵和集群都是在主从复制基础上实现高可用 的。主从复制主要实现了数据的多机备份&#xff0c;以及对于读操作的负载均衡和简单的故障恢复。缺陷&#xff1a; 故障…

vue3+antd 实现文件夹目录右键菜单功能

原本的目录结构&#xff1a; 右键菜单&#xff1a; 点击菜单以后会触发回调&#xff1a; 完整的前端代码&#xff1a; <template><a-directory-treev-model:expandedKeys"expandedKeys"v-model:selectedKeys"selectedKeys"multipleshow-li…

在 Docker 容器中运行 Vite 开发环境,有这两个问题要注意

容器化开发给我们带来了很多便捷&#xff0c;但是在开发环境下也有一些问题要注意&#xff0c;如果不解决这些问题&#xff0c;你的开发体验不会很好。 容器启动正常&#xff0c;却无法访问 我们用 Docker 启动一个 Vite Vue3 项目的开发环境后&#xff0c;发现端口日志一切…

计算机如何存储浮点数

浮点数组成 在计算机中浮点数通常由三部分组成&#xff1a;符号位、指数位、尾数位。IEEE-754中32位浮点数如下&#xff1a; 上图32bit浮点数包含1bit的符号位&#xff0c;8比特的指数位和23bit的尾数位。对于一个常规浮点数&#xff0c;我们来看看它是如何存储和计算的。这里…

conda env pip install error:No space left on device

conda 环境 pip install error&#xff1a;No space left on device 文章目录 conda 环境 pip install error&#xff1a;No space left on device现象1 实验2 分析和解决办法 现象 非root用户的服务器&#xff0c;需要安装环境&#xff0c;安装的环境超过2GB sudo pip insta…

医疗机器人中的具身智能进展——自主超声策略模型的任务编码和局部探索

医疗机器人一直是具身智能的研究热点。医学图像、医疗触诊、血压血氧、心率脉搏和生物电信号等多模态生物医学信息&#xff0c;不断丰富着医疗机器人的感知范畴。 自主超声 “自主超声”属于具身智能医疗机器人领域中话题度较高的研究方向。作为临床检查的重要手段之一&#…