Table of Contents

本文为高博《视觉SLAM十四讲》李群李代数部分的学习笔记。

1.SLAM中优化目标函数的表示

SLAM的核心问题就是通过观测数据来得到其对应的位姿，通用的方法就是构建为一个优化问题，然后求解最优的旋转矩阵 $\large R$ 和平移向量 $\large t$ ，使得误差最小化。

构建优化问题来求解相机位姿是问题求解的总体思路，但是在构建优化问题时，发现 $\large R$ 作为要求解的优化变量，必须是一个正交矩阵并且行列式为1。如果直接将 $\large R$ 作为优化变量的话，会引入额外的约束，这样优化问题就变得困难了。

因为李群和李代数之间可以进行相互转换，如果将 $\large R$ （是特殊正交群）转换为其对应的李代数变量，用这个李代数变量作为优化变量就可以构造无约束的优化。求解出的将是旋转矩阵 $\large R$ 对应的李代数，再将李代数转换为对应的李群即是要求解的 $\large R$ 。

假设一种现实场景：在三维世界中，相机在某个时刻的位姿为 $\large T$ ，此时观察到远处的一个点 $\large p$ ，在相机中产生了观测数据 $\large z$ 。

此时，由相机的成像原理可得：

$\large z=Tp+w.$

$\large w$ 为误差项，所以误差项可表示为：

$\large e=z-Tp.$

假设相机从 $\large A$ 地点运动到了 $\large B$ 地点，在这个过程中有 $\large N$ 次观测，则整个过程中的每个误差取平方再进行累加，就得到了总体误差。对其取最小化可记为：

$\large \underset{T}{min}J(T)=\sum_{i=1}^{N}\left \| z_i-Tp_i \right \|_2^2.$

要求解该式对于 $\large T$ 的最小化，就需要计算关于变换矩阵 $\large T$ 的导数。SLAM中就是通过构建与位姿有关的函数（也就是最小误差的表达式），然后讨论该函数关于位姿的导数，以调整当前的估计值。

而 $\large SO(3),SE(3)$ 上并没有良好定义的加法，它们都是群。在上式中，如果要对 $\large T$ 进行求导，就需要考虑到其中的约束条件（其实就是变换矩阵 $\large T$ 中旋转矩阵 $\large R$ 的约束）。

此时，就引入的李群李代数来帮助求解。

2.李群李代数的引入

用于描述相机位姿的旋转矩阵构成了特殊正交群 $\large SO(3)$ ，而变换矩阵构成了特殊欧式群 $\large SE(3)$ ：

$\large SO(3)={R\in \mathbb{R}^{3\times 3}|RR^T=I,det(R)=1}.$

$\large SE(3)=\begin{Bmatrix} T= & \begin{bmatrix} R & t\\ 0^T& 1 \end{bmatrix}\in \mathbb{R}^{4\times 4} |R\in SO(3),t\in \mathbb{R}^3\end{Bmatrix}.$

由于群的定义中要求群对于一种运算要满足封闭性、结合律、幺元和逆四个条件。

但是，旋转矩阵和变换矩阵对加法是不封闭的。也就是说两个旋转矩阵相加，和不再是一个旋转矩阵：

$\large R_1+R_2\notin SO(3).$

这样的话，就会遇到一个问题：在SLAM中用旋转矩阵或者变换矩阵表示相机的位姿，但是在优化方程中由于它们不满足封闭性，所以都不便于求导。

这时候引入了李群的概念，具有连续光滑性质的群叫做李群，所以 $\large SO(3)$ 和 $\large SE(3)$ 都属于李群，每个李群都有其对应的李代数。

这里的思路就变化为：对李群不便于求导，发现其李代数比较方便求导，所以就先用李代数进行求导，然后将结果再转为李群的表示。

此时，高博书中那张很有名的旋转矩阵和变换矩阵的李群和李代数之间的转换图就出现了：

上边图中的李群李代数变换，进行下面两点说明：

1）图中的指数映射，其实就是罗德里格斯公式；

2）图中的对数映射，其实就是利用迹的性质分别求解转角和转轴。

三维旋转中，用三维的向量 $\large \phi$ 表示三维的旋转对应的李代数， $\large \phi^{\wedge }$ 表示将李代数向量写成对应的反对称矩阵。

三维变换中，用六维的向量 $\large \xi$ 表示变换矩阵的李代数， $\large \xi ^{\wedge }$ 表示将李代数向量拓展成对应的反对称矩阵。

旋转矩阵变换成李代数形式后，怎么进行求导呢？这是个重要话题，因为引入李代数就是为了方便优化函数对旋转矩阵的求导。

存在两种求解方法：

1） $\large SO(3)$ 李代数进行求导

假设空间点 $\large p$ 进行了旋转，得到了 $\large Rp$ 。现在要计算旋转之后点的坐标相对于旋转的导数，则要求的导数为： $\large \frac{\partial Rp}{\partial R}$ ，此时，由于 $\large R\in SO(3)$ 对加法不封闭，所以该导数无法按照导数的定义来进行求解。

设 $\large R$ 的李代数为 $\large \phi$ ，按照指数映射将导数转换为对李代数，再按照导数的定义展开：

$\large \frac{\partial(exp(\phi \wedge )p) }{\partial \phi }=\lim_{\delta \phi \rightarrow 0}\frac{exp((\phi +\delta \phi )^{\wedge })p-exp(\phi ^{\wedge })p}{\delta \phi}$

$\large =\lim_{\delta \phi \rightarrow 0}\frac{exp((J_{l}\delta \phi )^{\wedge })exp(\phi ^{\wedge })p-exp(\phi ^{\wedge })p}{\delta \phi }$ (由BCH线性近似所得)

$\large \approx \lim_{\delta \phi \rightarrow 0}\frac{(I+(J_{l}\delta \phi )^{\wedge })exp(\phi ^{\wedge })p-exp(\phi ^{\wedge })p}{\delta \phi }$ （泰勒展开舍去高阶项取近似）

$\large =\lim_{\delta \phi \rightarrow 0}\frac{(J_{l}\delta \phi )^{\wedge }exp(\phi ^{\wedge })p}{\delta \phi }$

$\large =\lim_{\delta \phi \rightarrow 0}\frac{-(exp(\phi ^{\wedge })p)^{\wedge }J_l\delta \phi }{\delta \phi }$ （反对称符号看做叉积，交叉变号）

$\large =-(Rp)^{\wedge }J_l.$

所以，可以得出旋转后的点相对于李代数的导数： $\large {\color{Red} \frac{\partial (Rp)}{\partial \phi }=(-Rp)^{^}J_l.}$

该方式求导的缺点：含有形式比较复杂的 $\large J_l$ ，一般不太希望有这么复杂的计算。所以就有了另一种更简单的导数计算方式：扰动模型。

这里的扰动模型还是需要花心思来理解的，高博《视觉SLAM十四讲》第四讲中有主要步骤的介绍。

两个李代数指数映射乘积完整形式由BCH公式给出，当其中的存在小量的时候，小量的二次以上的项忽略掉后可得到如下线性近似表达：

$\large ln(exp(\phi _1^{\wedge })exp(\phi _2^{\wedge }))^{\vee }\approx \left\{\begin{matrix} J_l(\phi _2)^{-1} \phi _1& + &\phi _2 \\ J_r(\phi _1)^{-1}\phi _2& + & \phi _1 \end{matrix}\right.$

1）第一个近似公式中 $\large \phi _1$ 为小量。当对旋转矩阵 $\large R_2$ (李代数为 $\large \phi _2$ )左乘一个微小旋转矩阵 $\large R_1$ (李代数为 $\large \phi_1$ )时，可以近似地看做，在原有李代数 $\large \phi_2$ 上加上一项 $\large J_l(\phi _2)^{-1}\phi _1$ 。该式为左乘近似公式。

2）同理第二个近似公式中中 $\large \phi _2$ 为小量。该式为右乘近似公式。

2） $\large SO(3)$ 扰动模型（左乘）

对 $\large R$ 进行一次扰动 $\large \Delta R$ 来求解导数，这个扰动可以乘在左边也可以乘在右边。这里看一下左乘的求导。

设左扰动 $\large \Delta R$ 对应的李代数为 $\large \varphi$ ，然后对 $\large \varphi$ 求导：

$\large \frac{\partial (Rp) }{\partial \varphi }=\lim_{\varphi \rightarrow 0}\frac{exp(\varphi ^{\wedge })exp(\phi ^{\wedge })p-exp(\phi ^{\wedge })p}{\varphi }$

$\large \approx \lim_{\varphi \rightarrow 0}\frac{(I+\varphi ^{\wedge })exp(\phi ^{\wedge })p-exp(\phi ^{\wedge })p}{\varphi }$

$\large =\lim_{\varphi \rightarrow 0}\frac{\varphi ^{\wedge }Rp}{\varphi }$

$\large =\lim_{\varphi \rightarrow 0}\frac{-(Rp)^{\wedge }\varphi}{\varphi }$

$\large =-(Rp)^{\wedge }.$

从求解结果可以看出，相比于直接对李代数求导，扰动模型省去了一个雅克比 $\large {\color{Red} J_l}$ 的计算，因此，扰动模型更为实用。

$\large SE(3)$ 上用扰动模型对李代数进行求导：

假设某空间点 $\large p$ 经过一次变换 $\large T$ （对应李代数为 $\large \xi$ ），得到 $\large Tp$ 。

给 $\large T$ 左乘一个扰动 $\large \Delta T=exp(\delta \xi ^{\wedge })$ ，设扰动项的李代数为 $\large \delta \xi =\left [ \delta \rho ,\delta \phi \right ]^{T}$ 。那么：

$\large \frac{\partial(Tp) }{\partial \delta \xi }=\lim_{\delta \xi \rightarrow 0}\frac{{\color{Red} exp(\delta \xi ^{\wedge })}exp(\xi ^{\wedge })p-exp(\xi ^{\wedge })p}{\delta \xi }$

$\large \approx \lim_{\delta \xi \rightarrow 0}\frac{{\color{Red} (I+\delta \xi ^{\wedge })}exp(\xi ^{\wedge })p-exp(\xi ^{\wedge })p}{\delta \xi }$

$\large =\lim_{\delta \xi \rightarrow 0}\frac{{\color{Red} \delta \xi ^{\wedge }}exp(\xi ^{\wedge })p}{\delta \xi }$

$\large =\lim_{\delta \xi \rightarrow 0}\frac{\begin{bmatrix} \delta \phi ^{\wedge } & \delta \rho \\ 0^T & 0 \end{bmatrix}\begin{bmatrix} Rp+t& \\ 1& \end{bmatrix}}{\delta \xi }$

$\large = \lim_{\delta \xi \rightarrow 0}\frac{\begin{bmatrix} \delta \phi ^{\wedge }(Rp+t)+\delta \rho \\0 \end{bmatrix}}{\delta \xi }$

$\large {\color{Red} =\begin{bmatrix} I & -(Rp+t)^{\wedge }\\ 0^{T}& 0^{T} \end{bmatrix}}$

$\large \overset{\Delta }{=}\left ( Tp \right )^{\odot} .$

下面将利用这里的求解结果来计算重投影误差。

3.BA

求解3D-2D点对运动的方法叫做PnP(Perspective-n-Point)。它描述了当知道n个3D空间点及其在像素平面中投影位置的时候，如何估计相机的位姿。也就是求解世界坐标系中的3D点到像素坐标中点的旋转和平移。BA(Bundle Adjustment)就是PnP的一种求解方法，通过构造最小二乘问题来进行迭代优化求解。是一个最小化重投影误差问题。

n个空间点 $\large P$ 及其投影 $\large p$ ，要计算的相机位姿 $\large R,t$ ，它的李代数表示为 $\large \xi$ 。

对于空间点 $\large P_i=\left [ X_i,Y_i,Z_i \right ]^T$ ，其在相机中的像素平面投影坐标为 $\large u_i=\left [ u_i,v_i \right ]^T$ 。

则根据相机模型可得到下面公式：

$\large s_i\begin{bmatrix} u_i\\ v_i\\ 1 \end{bmatrix} = K exp(\xi ^{\wedge })\begin{bmatrix} X_i\\ Y_i\\ Z_i\\ 1 \end{bmatrix}.$

写成矩阵形式：

$\large \large s_i\mathbf{u_i}=Kexp(\xi ^{\wedge })P_i.$ $\large \Rightarrow \mathbf{u_i}=\frac{1}{s_i}Kexp(\xi ^{\wedge })P_i.$ （这里隐含存在齐次到非齐次的转换。）

由于相机位姿未知和观测点的噪声，该等式也存在误差。

而Slam中的优化就是把所有观测点的误差求和，构造最小二乘问题，然后寻找最好的相机位姿使最小二乘函数最小化。其实也就是求误差最小的情况下的相机位姿。

$\large \xi ^{\ast }=arg \underset{\xi }{min}\frac{1}{2}\sum_{i=1}^{n}\left \| \mathbf{u_i}-\frac{1}{s_i}Kexp(\xi ^{\wedge })P_i \right \|_2^2.$

可以看出，该式要表达的其实就是将像素坐标与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称为重投影误差。

如上图所示，通过特征点匹配知道了 $\large p_1$ 和 $\large p_2$ 是同一个空间点 $\large P$ 的投影，但此时相机的位姿是未知的。在初始值中， $\large P$ 的投影 $\large \hat{p_2}$ 与实际的 $\large p_2$ 之间有一定的距离。于是调整相机的位姿，使得这个距离变小(迭代优化其实就是做了这么一件事)。

误差函数记为 $\large e(x)$ ：

$\large e(x+\Delta x)\approx e(x)+J\Delta x.$

因为 $\large e$ 为像素坐标误差，所以是2维的。 $\large x$ 为相机位姿，是6维的。 $\large J$ 将是一个2*6的矩阵。

空间点 $\large P$ 投影到相机空间坐标系下为 $\large {P}'$ ：

$\large {P}'=(exp(\xi ^{\wedge })P)_{1:3=}\left [ {X}',{Y}' ,{Z}'\right ]^{T}.$

根据相机坐标到像素归一化平面坐标变换可得：

$\large \begin{bmatrix} su\\ sv\\ s\end{bmatrix}=\begin{bmatrix} f_x & 0 &c_x \\ 0& f_y&c_y \\ 0& 0& 1 \end{bmatrix}\begin{bmatrix} {X}'\\ {Y}'\\ {Z}'\end{bmatrix}.$

消去其中的 $\large s$ （实际上就是 $\large {P}'$ 的距离），得：

$\large u=f_x\frac{{X}'}{{Z}'}+c_x,$ $\large v=f_y\frac{{Y}'}{{Z}'}+c_y.$

此时对误差 $\large e$ 利用链式法则求导：

$\large {\color{Red} \large \frac{\partial e}{\partial \delta \xi }=\lim_{\delta \xi \rightarrow 0}\frac{e(\delta \xi \oplus \xi )}{\delta \xi }=\frac{\partial e}{\partial {P}'}\frac{\partial {P}'}{\partial \delta \xi }}$

这里的 $\large \oplus$ 指李代数上的左乘扰动。

第一项是误差关于投影点的导数，则可得：

$\large \frac{\partial e}{\partial {P}'}=-\begin{bmatrix} \frac{\partial u}{\partial {X}'} & \frac{\partial u}{\partial {Y}'} & \frac{\partial u}{\partial {Z}'} \\ \frac{\partial v}{\partial {X}'} & \frac{\partial v}{\partial {Y}'} & \frac{\partial v}{\partial {Z}'} \end{bmatrix}=-\begin{bmatrix} \frac{f_x}{{Z}'} & 0 & -\frac{f_x{X}'}{^{{Z}'^2}}\\ 0& \frac{f_y}{{Z}'} & -\frac{f_y{Y}'}{^{{Z}'^2}} \end{bmatrix}$

第二项为变换后的点关于李代数的导数：

$\large \frac{\partial (TP)}{\partial \delta \xi }=(TP)^{\bigodot }=\begin{bmatrix} I &-{P}'^{\wedge } \\ 0^{T}&0^{T} \end{bmatrix}.$

$\large {P}'$ 定义中取前三维，得到：

$\large \frac{\partial {P}'}{\partial \delta \xi }=\left [ I,-{P}'^{\wedge } \right ]=\begin{bmatrix} 1 & 0 &0 & 0 & {Z}' & {-Y}' \\ 0& 1& 0 & -{Z}' & 0 &{X}' \\ 0&0 & 1& {Y}'& {-X}' & 0 \end{bmatrix}.$

此时，可得到2*6的雅克比矩阵为：

$\large {\color{Red} \frac{\partial e}{\partial \delta \xi }=-\begin{bmatrix} \frac{f_x}{{Z}'} & 0 & -\frac{f_x{X}'}{{Z}'^{2}} & -\frac{f_x{X}'{Y}'}{{Z}'^{2}} & f_x+ \frac{f_x{X}'^{2}}{{Z}'^{2}}&-\frac{f_x{Y}'}{{Z}'} \\ 0& \frac{f_y}{{Z}'} & -\frac{f_y{Y}'}{{Z}'^{2}} & -f_y -\frac{f_y{Y}'^{2}}{{Z}'^{2}} &\frac{f_y{X}'{Y}'}{{Z}'^{2}} & \frac{f_y{X}'}{{Z}'} \end{bmatrix}.}$

这个雅克比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系，指导着位姿的优化。

除了优化位姿，还有特征点的空间位置。此时需要求解误差 $\large e$ 关于空间点 $\large P$ 的导数，这里仍然需要用到链式法则：

$\large \frac{\partial e}{\partial P}=\frac{\partial e}{\partial {P}'}\frac{\partial {P}'}{\partial P}.$

由于 $\large {P}'=exp(\xi ^{\wedge })P=RP+t,$ 可得 $\large {P}'$ 对 $\large P$ 求导后将只剩下 $\large R$ 。此时可得：

$\large {\color{Red} \frac{\partial e}{\partial P}=-\begin{bmatrix} \frac{f_x}{{Z}'}& 0&-\frac{f_x{X}'}{{Z}'^2} \\ 0 & \frac{f_y}{{Z}'} & -\frac{f_y{Y}'}{{Z}'^2} \end{bmatrix}R.}$

该矩阵为优化特征点空间位置时的雅克比矩阵，指导着迭代的方向。

好在优化部分已经有第三方库提供了相应计算，所以写代码的时候调用就可以了，简化了实际的代码编写。

文科升

发布了72 篇原创文章 · 获赞 190 · 访问量 15万+

私信关注

SLAM中的优化、李群李代数和BA

1.SLAM中优化目标函数的表示

2.李群李代数的引入

3.BA

猜你喜欢