SLAM中相机位姿求解（李群李代数）

前言

slam中一个关键问题之一就是求解相机的位姿，人们找了很多以相机位姿为变量的误差函数，比如光度误差，重投影误差，3D几何误差等等，希望使得误差最小，进而求得比较准确的相机位姿。举一个重投影例子：

T * = m i n 1 2 \sum i = 0 n | | u i - 1 s i K T P i | | 2 (1)

$T^* = min \frac{1}{2} \sum_{i=0}^n || u_i - \frac{1}{s_i}KTP_i||^2 \tag{1}$

$P_i$
是3D点，

$u_i$
是

$P_i$
对应的像素位置，

$K$
为相机内参矩阵，

$s_i$
为

$u_i$
对应的深度值。

$T$
就是我们要求的变量。上式只是一个粗略的表达，其中没有显式说明齐次与非齐次，我们认为自动满足。有人可能会想对于这样的式子直接求导为零，不就可以解出T了么。但是不要忘记了T是满足如下约束的：

T = [R 0 T t 1], R T R = I, d e t (R) = 1, t \in R 3 (2)

$T = \left[ \begin{array}{cc} R&t \\ 0^T&1 \end{array} \right] , R^TR = I, det(R) = 1,t \in R^3 \tag{2}$

R为旋转矩阵，它是正交矩阵并且行列式为1，t是平移向量。在求解1式的时候，必须考虑到T满足的约束，那么这个问题就变成了有约束优化问题。但是我们不想去求有约束的问题，因为不好求。我们更喜欢的是去求无约束问题，而且无约束问题有很多现成的解法。

那怎么办呢？就有大神想出我们可以去另外一个空间求解，比如李代数。最后研究表明确实通过李代数可以将1式转化为无约束问题，然后很方便的通过高斯牛顿法，列文伯格-马夸尔特法（简称LM法）等优化算法求解了。当然不仅仅是上面这个优化问题了，我们以相机位姿为变量的建立的目标函数都可以在李代数上求解。下面就简单介绍一下李群和李代数。

李群和李代数

群

群是一种集合加上一种运算的代数结构，我们把集合记为A，运算记为

$^.$
，那么群可以记为

$G = (A, ^.)$
。群要求这个运算要满足封闭性，结合律，幺元和可逆。矩阵中常见的群有：

一般线性群GL（n）指

$n×n$
的可逆矩阵，它们对矩阵乘法成群
特殊正交群SO（n）就是旋转矩阵群，其中SO（2），SO（3）最为常见。
特殊欧式群SE（n）就是n维欧式变换，其中SE（2），SE（3）最为常见。

群结构保证了在群上的运算具有良好的性子，李群是指具有连续性质的群。SO（n），SE（n）在实数空间上是连续的，我们可以直观的感觉到一个刚体能够在空间中连续运动，所以SO（n）和SE（n）都是李群。那么相机的位姿就可以表示为：

S O (3) = {R \in R 3 \times 3 | R R T = I, d e t (R) = 1} S E (3) = {T = [R 0 T t 1] \in R 4 \times 4 | R \in S O (3), t \in R 3}

$SO(3) = \{ R \in \mathbb R^{3×3} | RR^T = I, det(R) = 1 \} \\ SE(3) = \{ T = \left[ \begin{array}{cc} R&t \\ 0^T&1 \end{array} \right] \in \mathbb R ^{4×4} | R \in SO(3), t \in \mathbb R^3 \}$

现在我们只是说明了相机位姿是特殊欧式群，旋转矩阵是特殊正交群。但是现在还不能直接去求解因为约束依然存在。正如前面所说的，我们希望将有约束的问题转化为无约束问题求解。数学上常用的方法就是去另外一个空间计算，就像分类问题一样，也许在这个空间没法分开，变换一个空间说不定就可以分开了。横看成岭侧成峰就是这个道理。数学家们研究表明每一个李群都已与之对应的李代数，因为李群是连续的。真如我们所期望的，在李代数求解1式时可以将其化为无约束问题，通过高斯牛顿法等方法迭代求解。这是因为旋转矩阵对加法不封闭，而李代数对加法封闭。

李群李代数相互转化

旋转矩阵对应的李代数是一个三维向量，相机位姿（刚性变换矩阵，没有尺度因子）是一个六位向量。那么我们想知道的是这些向量是怎么和对应的李群联系，或者说怎么变换。具体的推到我这就不在搬运了，大家可以参考高翔博士的《视觉slam十四讲从理论到实践》或者自行搜索。下面这张图也是来自这本书的：

这里写图片描述