直接法(Direct Method)计算相机位置和姿态

1. 直接法的思想

我们在<LK光流法跟踪特征点>这篇文章介绍了如何跟踪某个像素并形成轨迹。事实上跟踪这些特征点的最终目的还是为了计算相机的位置和姿态变化。今天我们要介绍的直接法就是不先计算像素的轨迹而是直接计算出相机的位置和姿态。直接法的思想是：“同一个块它的灰度值是不会发生变化的（这和光流法非常像）。比如说我们相机绕着一个球去拍摄。那么时刻t的这帧里面球所在的那块像素的灰度值，与它在时刻t+dt这帧图片中球所在的那块像素灰度值相同。唯一变化的是球所在的那个像素块在图片中的相对位置发生了变化。变化了的原因是因为相机的位置和姿态发生了变化。我们要做的是求出相机怎么位置和姿态怎么发生了怎样的变化。那么怎么求呢？我们先假设相机发生了某种变化$R$，假设球在时刻t的那帧图片中的位置是$p_1$（看看后面那张图）。那么在t+dt时刻那帧图片中球的位置就是$p_2=R{p}_1$。现在已知$p_1$和$p_2$处的像素灰度值一样，要求得$R$。我们可以遍历$R$的各种取值，如何评价这个取值好不好呢？我只需要比对用选定的那个R计算出的$p_2$那个地方的灰度值与$p_1$处的灰度值是否接近。直接法的思想就是这么简单。”。我们知道LK光流法它是需要先提取特征点，但是直接法是不需要提取特征点的所以计算复杂度更低适合嵌入式这种计算资源有限的场合。

2. 直接法的推导

2.1误差模型

我们假设相机发生了某种旋转和平移变换，旋转矩阵是R平移量是t。我们可以用一个变换矩阵T来表示旋转R+平移t。我们要做到的就是求出T。根据什么求出T？根据第1帧上$p_1$所在的那块像素的灰度值，应该与第2帧$p_2$所在的那个块的像素的灰度值一样来计算矩阵T。那$p_1$与$p_2$又是什么关系？答：是同一个物体在不同视角下拍摄到的像素块。举个例子解释下：假设$p_1$那个像素块是个球，我希望相机进行了变换T之后（$p_1$那个点就变成了$p_2=T{p}_1$），我希望$p_2$这个像素块也是个球。当然我们只会比对$p_1$和$p_2$这两块像素灰度值是否一样。很明显这是个优化问题，因为$T$设置一个值，那么就会计算出一个$p_2$。计算出一个$p_2$那么我就得看看它是否是与$p_1$那个块足够相似。我们假设$p^{}$