SLAM后端：BA优化（Bundle Adjustment）

最新推荐文章于 2025-02-15 19:44:57 发布

LittleEmperor

最新推荐文章于 2025-02-15 19:44:57 发布

阅读量1.1w

点赞数 11

分类专栏： SLAM

本文链接：https://blog.csdn.net/LittleEmperor/article/details/105274299

版权

SLAM 专栏收录该内容

14 篇文章

订阅专栏

BA是一种优化方法，最小化重投影误差优化PnP得到的R、t

应用场景：利用共视点的3D坐标与相机内参矩阵，根据PnP计算位姿R、t（有误差）。利用R、t及相机的投影模型可以计算出这些特征点在第二帧图像上的投影（有误差），本身这些点在第二帧图像上有真实投影，利用这两个投影构建函数进行优化可以优化位姿。

1 BA解决的代价函数

观测模型如下：

（1）世界坐标系转相机坐标系

设P点的世界坐标 $(x,y,z)$ ，转到相机坐标系：

$P^{'} = RP+t=(x^{'},y^{'},z^{'})$

（2）归一化

$P^{'}_c = [u_c,v_c,1]^T=[x^{'}/z^{'},y^{'}/z^{'},1]^T$

（3）去畸变

$P^{'}_c(u_c,v_c)\rightarrow (u_c^{'},v_c^{'})$

（4）相机坐标系转像素坐标系

$\\ u_s = f_x\cdot u_c^{'}+c_x\\ v_s = f_y\cdot v_c^{'}+c_y\\$

（5）得到相机的观测模型：

$g(u_s,v_s) = h(R,t,x,y,z)$

（6）构建重投影误差模型：

$e = z - h(R,t,x,y,z)$

z是实际值，h是通过观测模型得到的值，知识针对空间中一点。

2 LM算法求解

BA优化一般通过LM算法求解，参考最优化八：高斯牛顿法、LM法。

1中（6）方程中表示了一个点一个位姿构成的重投影误差，利用多个位姿多个点构建优化方程（m个位姿，n个点）：

$minf(x) = min\sum_{1}^{m*n}e_{ij} = \sum_{i=1}^{m}\sum_{j=1}^{n}(z_{ij} -h(R_{ij},t_{ij},x_{ij},y_{ij},z_{ij}))$ （2-1）

f(x)中的x是R、t、xyz。位姿用李代数表示，并将变量分为两部分，位姿 $\xi$ 与 $p$ 点，变量如下：

$[\xi,p ]= [\xi_1,\xi_2,\xi_3...,\xi_m, p_1,p_2,p_3,...,p_n]$ （2-2）

则优化目标变为问题如下：

$minf(\xi ,p) =min \sum_{i=1}^{m}\sum_{j=1}^{n}(z_{ij} -h(\xi ,p))$ （2-3）

优化函数f(x)有m+n个变量待求取，一般用LM法求解，为什么不用最速下降法？我觉得是最速下降法迭代次数过多，效率低。LM法要用到梯度，梯度计算如下：

$[\frac{\partial f}{\partial \xi _1},...,\frac{\partial f}{\partial \xi _m},\frac{\partial f}{\partial p _1},...,\frac{\partial f}{\partial p _n}]$ （2-4）

参考最优化八：高斯牛顿法、LM法，针对函数f(x)求解更新变量 $\bigtriangleup x$ 需要求解如下方程：

$J^T(x_0)J(x_0)\bigtriangleup x = -J^T(x_0)f(x_0)$ （2-5）

J是一阶导数。为了计算方便，将一阶梯度替换如下：

$[F,E]=[\frac{\partial f}{\partial \xi _1},...,\frac{\partial f}{\partial \xi _m},\frac{\partial f}{\partial p _1},...,\frac{\partial f}{\partial p _n}]$ （2-6）

F代表 $\xi$ 的偏导数，E代表 $p$ 的偏导数，则参考式（2-5），用g代替等式右边，本问题方程如下：

$[F E]^T\cdot [F E] \bigtriangleup x=g$ （2-7）

等式左边展开并用H表示得到：

$H=\left [ \begin{matrix} F^TF & F^TE \\ E^TF & E^TE \\ \end{matrix} \right ]$ （2-8）

目的是求解（2-7）中的 $\bigtriangleup x$ ，需要求取H的逆，但是H矩阵很大，一般几百个点就会有几百个参数，求解需要用到H矩阵的稀疏特性。

3 H矩阵的稀疏性

这据说是21世纪最重要的发现。

最优化问题 $minf(\xi ,p)$ 中包含了m个位姿n个特征点，公式（2-4）可以写成如下方程：

$[\frac{\partial f}{\partial \xi _1},...,\frac{\partial f}{\partial \xi _m},\frac{\partial f}{\partial p _1},...,\frac{\partial f}{\partial p _n}]= [\sum_{m}^{i=1}\sum_{n}^{j=1}\frac{\partial e_{ij}}{\partial \xi _1},...,\sum_{m}^{i=1}\sum_{n}^{j=1}\frac{\partial e_{ij}}{\partial \xi _m},\sum_{m}^{i=1}\sum_{n}^{j=1}\frac{\partial e_{ij}}{\partial p _1},...,\sum_{m}^{i=1}\sum_{n}^{j=1}\frac{\partial e_{ij}}{\partial p _n}]$

即：

$[\frac{\partial f}{\partial \xi _1},...,\frac{\partial f}{\partial \xi _m},\frac{\partial f}{\partial p _1},...,\frac{\partial f}{\partial p _n}]= \sum_{i,j}[\frac{\partial e_{ij}}{\partial \xi _1},...,\frac{\partial e_{ij}}{\partial \xi _m},\frac{\partial e_{ij}}{\partial p _1},...,\frac{\partial e_{ij}}{\partial p _n}]$

对其中一项令：

$J_{ij}(\xi ,p) = (0_{2*6},...,\frac{\partial e_{ij}}{\partial \xi _i},...,0_{2*3},...\frac{\partial e_{ij}}{\partial p_j},..,0_{2*3} )$

则：

$H = \sum_{i,j}J_{ij}^TJ_{ij}=\left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ]$

H的稀疏性如下：

（1） $H_{11},H_{22}$ 是对角阵。

（2） $H_{12},H_{21}$ 有可能稀疏有可能稠密

利用H的稀疏性计算方程（2-7）需要用到苏耳消元法。

4 Schur消元

求解问题如下：

$\left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ]\bigtriangleup x=g$ $\left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ]$ （4-1）

将 $\bigtriangleup x$ 变量分开写为：

$\left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ] \left [ \begin{matrix} \bigtriangleup \xi \\ \bigtriangleup p \\ \end{matrix} \right ]= \left [ \begin{matrix} v \\ w \\ \end{matrix} \right ]$ （4-2）

消去矩阵H中右上角部分，等式左右同乘以矩阵：

$\left [ \begin{matrix} I & -H_{21}^{T}H_{22}^{-1} \\ 0 & I \\ \end{matrix} \right ] \left [ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \\ \end{matrix} \right ] \left [ \begin{matrix} \bigtriangleup \xi \\ \bigtriangleup p \\ \end{matrix} \right ]=\left [ \begin{matrix} I & -H_{21}^{T}H_{22}^{-1} \\ 0 & I \\ \end{matrix} \right ] \left [ \begin{matrix} v \\ w \\ \end{matrix} \right ]$ （4-3）

整理得到：

$\left [ \begin{matrix} H_{11}- H_{21}^TH_{22}^{-1}H_{21}& 0 \\ H_{21} & H_{22} \\ \end{matrix} \right ] \left [ \begin{matrix} \bigtriangleup \xi \\ \bigtriangleup p \\ \end{matrix} \right ]= \left [ \begin{matrix} v-H_{21}^TH_{22}^{-1}w \\ w \\ \end{matrix} \right ]$ （4-4）

方程第一项与为位置点p无关，取出：

$\left [ H_{11}- H_{21}^TH_{22}^{-1}H_{21} \right ] \cdot \left [ \begin{matrix}\bigtriangleup \xi \\ \end{matrix} \right ]= \left [ \begin{matrix} v-H_{21}^TH_{22}^{-1}w \\ \end{matrix} \right ]$ （4-5）