相机位姿

slam 求解相机的位姿求解核心思想：将有约束的李群问题转换成无约束的李代数问题，然后使用高斯牛顿算法或者LM(列文伯格-马夸尔特法)求解。

人们找了很多以相机位姿为变量的误差函数，比如光度误差，重投影误差，3D几何误差等等，希望使得误差最小，使用BA（又称位最小化重投影误差），进而求得比较准确的相机位姿。举一个重投影例子： 

T∗=min12∑i=0n||ui−1siKTPi||2(1)

Pi是3D点， ui是Pi对应的像素位置， K为相机内参矩阵，si为ui对应的深度值。T就是我们要求的变量。上式只是一个粗略的表达，其中没有显式说明齐次与非齐次，我们认为自动满足。有人可能会想对于这样的式子直接求导为零，不就可以解出T了么。但是不要忘记了T是满足如下约束的： 

T=[R0Tt1],RTR=I,det(R)=1,t∈R3(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）都是李群。那么相机的位姿就可以表示为：

SO(3)={R∈R3×3|RRT=I,det(R)=1}SE(3)={T=[R0Tt1]∈R4×4|R∈SO(3),t∈R3}

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

李群李代数相互转化

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

这张图给出了李群和李代数之间的转换公式。有了这个转换，离我们求解1式问题还有些距离。在使用类似高斯牛顿法这样的迭代算法时，目标函数关于变量的导数很重要，因为它提供了目标函数变小的方向。其实就是变量选择的方向，那么我们就想知道目标函数关于李代数的导数。

李代数求导

使用李代数解决问题的求导思路分为两种： 
1. 用李代数表示位姿，然后根据李代数加法来对李代数求导 
2. 对李群左乘或者右乘微小扰动量，然后对该扰动求导，成为左扰动和右扰动模型。

我们先说关于旋转矩阵对应的李代数SO(3)求导，第一种的求导过程，如下： 

这样我们就得到了旋转后点相对于李代数的导数： 

Jl的形式如下： 

由于旋转矩阵对应的李代数是一个三维向量，所以我们可以用模长与单位向量的乘积表示。ϕ=θa， θ为模长，a为单位向量，上式中θ 和 a 就是这个含义。

下来我们在来看看旋转矩阵李代数的左扰动模型，左乘一个微小扰动，对微小扰动求导： 

这种方式省去了计算雅克比Jl，所以使用更为常见。 
最后直接给出SE(3)上的李代数求导，使用左扰动模型： 

李代数上求解相机位姿

有了李群和李代数相互转换公式，李代数上的导数，那么我们就可以将1式用李代数重新表达。 

上式仍然只是一个粗略的表达，其中没有显式说明齐次与非齐次，我们认为自动满足。使用李代数我们可以成功的将位姿约束问题转化为无约束问题，但要用高斯牛顿法或者LM方法等迭代优化算法，我们还需要知道这个误差关于位姿李代数的导数。使用左扰动模型，利用链式法则展开： 

相机投影模型相对于P‘为： 

那么就可以得到： 

求导的第二项我们前面已经给出了： 

取出这部分的前三行，然后两个导数项相乘，就可以得到一个2*6的雅克比矩阵： 

如果变换矩se(3)的旋转在前，平移在后，只需要将上式的前面3列和后面3列对调即可。有了李代数表示，也有了李代数上的表示，下一步我们就可以利用一些优化库（ceres, g2o）去计算了。