详解kinectfusion之三位姿估计

本文主要是介绍详解kinectfusion之三位姿估计，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

kinectfusion中使用的位姿估计方式是点到面的ICP。这个方法的选择主要是考虑到相机快速运动，相邻两帧之间位姿变化微小。该ICP方法的优势是可以快速收敛至最优解。

我们先来回顾一下ICP的通用流程。

1. 输入源点云与目标点云，源点云为需要进行变换的点云，目标点云为固定点云，也是源点云经变换后能与之重合的点云。

2. 确定源点云与目标点云之间的点云匹配关系。

3. 根据上一步骤确定的匹配点云，计算旋转平移变换矩阵。

4. 将上一步骤计算的变换矩阵，作用于源点云，得到新的源点云。

5. 计算新的源点云与目标点云之间的重合误差，若小于设定的阈值，那么就结束；否则重复2-4步骤。

不同的ICP方法，主要是在计算变换矩阵这一步骤有所不同。

点到面的ICP方法介绍

接下来，针对本文中使用的icp方法，涉及到的具体步骤详细展开介绍一下。

1.源点云为当前相机拍摄到的数据帧，记作 $V_{k}$ ，目标点云为上一帧数据，记作 $V_{k-1}$ 。方法中为了提高准确率，目标点云不是直接用的上一帧数据，而是根据上一帧的位姿，生成的新的数据帧，生成方法，我们会在后面展开介绍，此处以上一帧数据代替，进行讲解。

2. 点云匹配关系的确定，本文采用的是投影法

假设 $v_{k-1}$ 是 $V_{k-1}$ 中的任一点， $V_{k-1}$ 对应的位姿为 $T_{k-1}$ ； $v_{k}$ 是 $V_{k}$ 中的任一点， $V_{k}$ 对应的位姿为 $T_{k}$ ，其中， $T_{k}$ 的初始值与 $T_{k-1}$ 相同，后面会随着ICP迭代更新。

——————————————————————————————————————————

对于 $V_{k-1}$ 中的每一个点 $v_{k-1}$ 作以下操作

如果 $v_{k-1}$ 是个有效的三维坐标

$v_{k}^{g}=T_{k}*v_{k}$ 注：将源点云经变换矩阵，变换到世界坐标系

$\dot{v_{k-1}}=inv(T_{k-1})*v_{k}^{g}$ 注：世界坐标下的源点云经逆变换矩阵，变换到 $V_{k-1}$ 所在坐标系

$u_{k-1}=P*v_{k-1}$ 注：k-1坐标系中的源点云坐标，经投影矩阵，到k-1像素坐标

如果 $u_{k-1}$ 是有效的

$\left \| \dot{v_{k-1}}- v_{k-1} \right \|<$ 距离阈值

$\left \| \dot{n_{k-1}} .dot(n_{k-1}) \right \|<$ 法向量夹角阈值

那么该匹配关系成立

——————————————————————————————————————————

3. 根据上一步骤得到的点匹配关系，最小化误差函数，计算变换矩阵。

基于点到平面的ICP的误差函数为

$J=arg min_{r}\sum \left ( \left ( R*v_{k}+t-v_{k-1} \right ) * n_{k-1}\right )^{2}$

由于本文最初假设了相机运行速度快，并且旋转角度 $\theta$ 非常小，趋近于0，那么

$cos \theta \approx 1$ , $sin \theta \approx \theta$ , $\theta ^{2}\approx 0$ ,

假设绕x,y,z轴旋转的角度分别是 $\alpha$ ， $\beta$ ， $\gamma$ ，并且取值非常小，趋近于0，那么旋转矩阵可以近似为

$R\approx \begin{bmatrix} 1 & -\gamma & \beta \\ \gamma &1 &-\alpha \\ -\beta &\alpha &1 \end{bmatrix}$

可以重写误差函数

$J=arg min_{r}\sum \left ( \left ( R*v_{k}+t-v_{k-1} \right ) * n_{k-1}\right )^{2}\\ =\left \| Ax-b \right \|^{2}$

这样非线性的误差函数，近似为线性，每一步的最优解x为：

$\hat{x}=(A^{T}A)^{-1}A^{T}b$

那么 $\alpha =\hat{x}(0)$ , $\beta=\hat{x}(1)$ , $\gamma =\hat{x}(2)$

转为旋转矩阵与平移向量为

$R=\begin{bmatrix} cos\gamma &-sin\gamma&0 \\ sin\gamma &cos\gamma&0\\ 0&0&1\end{bmatrix}* \begin{bmatrix} cos\beta&0 &sin\beta\\ 0&1 &0 \\ -sin\beta&0 &cos\beta\end{bmatrix}* \begin{bmatrix} 1&0 &0\\ 0&cos\alpha &-sin\alpha \\ 0&sin\alpha &cos\alpha\end{bmatrix}$