KinectFusion: Real-Time Dense Surface Mapping and Tracking ICP迭代更新部分推导

其实就是kinectfusion这篇文章里面的§3.5那一部分拿出来仔细读一下,代码里面要参考着写ICP部分

实时相机定位需要对于每个新进来的深度图估计当前相机位姿$T_{w,k}$，很多方法为了加速运算选择从深度图里面挑选出特征点，抛弃剩下的深度信息。而在我们的工作里面，利用两个trick来用上所有的数据，实现稠密的ICP：

• 首先保证采集的数据帧率够高，这样可以用小角度假设，优化我们的运算
• 现代GPU硬件允许满并行的流水线，可以利用得到的信息

点平面误差度量与通过投影数据关联获得的对应关系首次在一个实时建模系统中演示，该系统使用帧到帧跟踪（使用固定相机）进行深度地图对齐。在我们的系统中通过把一个当前表面测量（$V_k$,$N_k$）和从之前的帧得到的模型预测（${\hat{V}}_{k-1}$,${\hat{N}}_{k-1}$）相对齐（对齐的意思是指用IMU算出的变换矩阵变换之后做最小二乘）。注意，帧到帧的tracking是把（${\hat{V}}_{k-1}$,${\hat{N}}_{k-1}$）用（$V_{k-1}$,$N_{k-1}$）代替了，这个比较在后面的实验部分也有。

后面的推导中会经常用到一个对于任意一个体素从当前帧坐标系到世界坐标系的转换，大致形式如下$V_k^g(u)=T_{g,k}{\dot{V}}_k(u)$，其中$\dot{V}(u)$的含义是把$V(u)$加上一维进行齐次化，便于运算，而${\tilde{T}}_{g,k}$则是对$T_{g,k}$的估计,${\hat{V}}_k$是从TSDF中以$T_{g,k}$位姿观测得到的体素图，而非直接观测得到的$V_k$

优化表面预测，以$T_{g,k}$为可优化量可以写出来损失函数如下

在这个函数里，每一个全局帧的表面预测（${\hat{V}}_{k-1}^g(\hat{u})$）是通过预先固定好的姿势估计$T_{g,k-1}$获得的。我们的投影数据关联算法通过计算透视投影点来得到体素之间的对应关系（也就是说$u$这个体素是和$\hat{u}=\pi (K{\tilde{T}}_{k-1,k}{\dot{V}}_k(u))$这一在前一帧的体素相对应的），其中${\tilde{T}}_{k-1,k}^z=T_{g,k-1}^{-1}{\tilde{T}}_{g,k}^z$，而且

通过线性化上面的损失函数来进行迭代求解${\tilde{T}}_{g,k}^z$，其中的变量应当在最初被设置为${\tilde{T}}_{g,k}^{z-1}$，也就是在第z-1次迭代时的T矩阵（注意，在算法中我们用的T矩阵表示的都是从当前帧相机坐标系到世界坐标系下的转换），而两次迭代之间的转换方程为${\tilde{T}}_{g,k}^z={\tilde{T}}_{inc}^z{\tilde{T}}_{g,k}^{z-1}$利用小角度假设可以把${\hat{T}}_{inc}^z$写成如下形式$$\begin{array}{cccc}1& \alpha & -\gamma & t_x\\ -\alpha & 1& \beta & t_y\\ \gamma & -\beta & 1& t_z\\ 0& 0& 0& 1\end{array}$$（怀疑原本论文里面写的不够标准，原论文里面没有最后一行，但这样矩阵乘法就不成立了），将${\tilde{T}}_{inc}^z$里面的自由变量提出（一共六个），写成x=($\beta ,\gamma ,\alpha ,t_x,t_y,t_z{)}^T$这一向量形式，由当前帧坐标系到世界坐标系的转化方程来看，有${\tilde{V}}_k^g(u)={\tilde{T}}_{g,k}^{z-1}{\dot{V}}_k(u)$，这就是在第z-1次迭代下，第k帧中某一个体素在世界坐标系下的坐标表示方式，所以第z次迭代后k帧中的某一体素在世界坐标系下的坐标计算方式应该是${\tilde{T}}_{g,k}^z{\dot{V}}_k(u)={\tilde{T}}_{inc}^z{\tilde{T}}_{g,k}^{z-1}{\dot{V}}_k(u)={\tilde{T}}_{inc}^z{\tilde{V}}_k^g(u)={\tilde{R}}^z{\tilde{V}}_k^g(u)+{\tilde{t}}^z$，接下来我们规定$G(u)=[[{\tilde{V}}_k^g(u){]}_{\times }|I_{3\times 3}]$，其中$[V{]}_{\times }$代表着向量V对应的反对称矩阵，比如向量(a,b,c)的反对称矩阵就是$$\begin{array}{ccc}0& -c& b\\ c& 0& -a\\ -b& a& 0\end{array}$$，所以可以看出，上面的表达式用含G(u)的方程来表示的话就是$G(u)x+{\tilde{V}}_k^g(u)$，再回顾一下我们的loss function

从这个图里面把括号中的第一项替换掉，就可以变成

其中

接下来，如果我们做如下的替换：

就会发现$E=Ax-b$，这样目标函数又可以写成$min\Sigma (E^{T}E)$
对这个目标函数进行对x的导数，也就是对$x^{T}A{A}^{T}x-x^T{A}^{T}b-b^{T}Ax+b^{T}b$进行对x的求导，另外可以注意到$A^{T}b=b^{T}A$，因为他们乘出来都是$1\times 1$的，所以转置和自身相等，令求导出来的结果为0，移项得方程

求解这个方程即可得到x，然后把x对应着返回到矩阵里面就可以知道每次迭代的时候对位姿矩阵做的扰动矩阵应该是什么样子的