KinectFusion

前言

本文是针对KinectFusion的一些学习总结，参考资料中的KinectFusion 解析一文已经有了详细解析，但是对与零基础的笔者来说，还是有些地方不够详尽。

参考资料

论文：KinectFusion: Real-time 3D Reconstruction and Interaction Using a Moving Depth Camera
PPT：KinectFusion和ElasticFusion三维重建方法
博客：KinectFusion 解析
课程：PCL-3D点云

流程

图片来源：论文：KinectFusion: Real-time 3D Reconstruction and Interaction Using a Moving Depth Camera
a) 2D 深度图像转换成 3D 点云并求得每一点的法向量
b) ICP 迭代求当前帧相机位姿
c) 根据相机的位姿将点云数据融合到场景的三维模型中
d) 光线投影算法求当前视角下能够看到的场景表面，得到点云，并且计算其法向量，用来对下一帧的输入图像进行配准，即重复b)

步骤详解

a) Depth Map Conversion

三维点云转换

相机参数：相机焦距（$f_x/f_y$），光学中心（$c_x/c_y$）、畸变参数（$d_0-d_4$）和深度因子。
假定像素坐标为$(u,v)$，v高，u宽，深度值为$Z$，三维点云信息：
$$p(u,v)=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=Z\left[\begin{array}{ccc}1/f_x& 0& 0\\ 0& 1/f_y& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u-c_x\\ v-c_y\\ 1\end{array}\right]$$
上述变换，得到的坐标系是以相机为原点的坐标。在进行上述转换之前必须对$u$,$v$进行undistort运算，即便如此最终得到的点云数据还是存在误差的。棋盘标定出的相机内参本身是带有误差的，它是一种近似逼近值，也就是说它不能真实的映射相机内部结构。

假设：深度图的缩放是5000：也就是说像素值为5000表示距离相机是1米，10000是两米；图像像素为640*480；深度图为16-bit的数据，转换代码如下：

fx = 525.0  # focal length x
fy = 525.0  # focal length y
cx = 319.5  # optical center x
cy = 239.5  # optical center y

factor = 5000 # for the 16-bit PNG files
# OR: factor = 1 # for the 32-bit float images in the ROS bag files

for v in range(depth_image.height):
  for u in range(depth_image.width):
    Z = depth_image[v,u] / factor;
    X = (u - cx) * Z / fx;
    Y = (v - cy) * Z / fy;

对于一张$x\ast y$像素的图片是如何估计相机内参的呢？下面代码给出了另外一个1280720（宽高）：

cx = 1280 / 2
cy = 720 / 2
fx = 1280 / (2 * np.tan(0.5 * np.pi * 90 / 180))
fy = 720 / (2 * np.tan(0.5 * np.pi * 59 / 180))

参考资料

1. 深度图如何转为点云？
2. TUM: Dataset File Formats
3. ttps://blog.csdn.net/CH_monsy/article/details/113312644 原理详解

每个点的法向量估计

表面法线是几何表面的重要属性，在许多领域（例如计算机图形应用程序）中大量使用，以应用正确的光源以产生阴影和其他视觉效果。

给定一个几何表面，通常很难将表面某个点的法线方向推断为垂直于该点表面的向量。但是，由于我们获取的点云数据集是真实表面上的一组点样本，因此有两种可能性：

1. 使用表面网格化技术从获取的点云数据集中获取基础表面，然后从网格中计算表面法线；
2. 使用近似值直接从点云数据集中推断表面法线。

KinectFusion中使用的是第二种：临近点重映射点
$$n(u,v)=[p(u+1,v)-p(u,v)]\times [p(u,v+1)-p(u,v)]$$
且将法线重新归一化到长度为1。公式思想很简单，就是法线必须与切平面垂直，通过高度算法线，通过法线算高度都可以基于这个原理。但是，这种转换并不是非常准确。

假定一个6DOF相机的位姿（pose）在$i$时刻相对于世界坐标系（global coordinates）的转换矩阵$$T_i=[R_i|t_i],$$其中，$R_i$是$3\times 3$的旋转矩阵，$t_i$是三维平移矩阵。所计算出来的坐标和法线，在世界坐标系中：$$p_i^g(u,v)=T_i{p}_i(u,v),$$$$n_i^g(u,v)=R_i{n}_i(u,v).$$

b) Camera Tracking

使用ICP，通过最小化匹配点到其切平面得距离，来求得每一帧之间得6DOF变换矩阵。ICP 算法主要用于点云配准，在两帧点云位姿差别较大时，通常先用 3D 特征匹配计算粗略位姿，然后再用 ICP 算法对位姿做 refine。在实时的三维重建中，由于相邻两帧之间位姿变化比较小，在变量求解时，直接用上一帧的位姿初始化。${}^1$
上一帧位姿变换矩阵$T_{i-1}$，当前深度图$Z_i$

1. 根据上一次迭代求得的 pose 变换点坐标。 初始值为$T_{i-1}$
2. 投影匹配算法：匹配两幅点云中的点
3. 位姿估计算法：极小化匹配点到切平面的距离
4. 目标函数误差小于一定值时，或者到达设置的迭代次数停止迭代，否则进入第 1 步。

投影匹配算法

位姿估计

图片来源：论文：Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration

如上图，其实就是最小化$l_1+l_2+l_3$。目标函数：
$$argmin\sum _{(u,v),Z(u,v)>0}||(T_i{p}_i(u,v)-p_{i-1}^g(u^{\prime },v^{\prime }))\cdot n_{i-1}^g(u^{\prime },v^{\prime })|{|}^2$$
其中，$T_i$是待求位姿。
利用某些近似，将$T_i=[R_i|t_i]$线性化。上式可转化为如下形式：$$x_{opt}=argmi{n}_x|Cx-b{|}^2。$$

旋转矩阵：$$R(\alpha ,\beta ,\gamma )=R_z(\gamma )\cdot R_y(\beta )\cdot R_x(\alpha )=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$
$$\begin{array}{rl}{r}_{11}& =cos\gamma cos\beta \\ r_{12}& =-sin\gamma cos\alpha +cos\gamma sin\beta sin\alpha \\ r_{13}& =sin\gamma sin\alpha +cos\gamma sin\beta cos\alpha \\ r_{21}& =sin\gamma cos\beta \\ r_{22}& =cos\gamma cos\alpha +sin\gamma sin\beta sin\alpha \\ r_{23}& =-cos\gamma sin\alpha +cos\gamma sin\beta cos\alpha \\ r_{31}& =-sin\beta \\ r_{32}& =cos\beta sin\alpha \\ r_{33}& =cos\beta cos\alpha \end{array}$$
其中，$R_x(\alpha), R_y(\beta) 和 R_z(\gamma) $ 是绕$x,y,z$轴分别旋转$\alpha ,\beta 和\gamma$。坐标轴旋转见绕任意轴旋转。
当$\theta \approx 0$时，有$sin(\theta )\approx \theta ,cos(\theta )\approx 1$。
假设：相邻两帧之间位姿变化较小时，即$\alpha ,\beta ,\gamma \approx 0$，我们有：
$$R(\alpha ,\beta ,\gamma )\approx \left[\begin{array}{ccc}1& -\gamma & \beta \\ \gamma & 1& -\alpha \\ -\beta & \alpha & 1\end{array}\right]$$
令$t=(t_x,t_y,t_z)$，位姿矩阵：
$$T_i=\left[\begin{array}{cccc}1& -\gamma & \beta & t_x\\ \gamma & 1& -\alpha & t_y\\ -\beta & \alpha & 1& t_z\\ 0& 0& 0& 1\end{array}\right]$$

其怎么转换为$Ax-b$形式在论文：Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration中有详细介绍。理解代码的话，需要加上RGB-D 实时三维重建/SLAM 中的 ICP 算法解析中的介绍。

参考资料：

1. RGB-D 实时三维重建/SLAM 中的 ICP 算法解析
2. GPU 极小化 ICP 算法中的目标函数
3. 论文：Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration

c) Volumetric Representation和点云融合

Volumetric Representation

将整个场景看作一个空间(Volume)，空间由许多小体素(voxel)组成，如下图所示：
图片来源：PPT KinectFusion和ElasticFusion三维重建方法

TSDF 算法 truncated signed distance function

算法思想：对与当前体素，计算其到相机观测表面的距离（带符号）；对不同的视图，用权重去平均的方法融合共有体素的距离值。Truncated截断指的是，离相机过远、过近或者在视域范围之外的这些体素，可以不用更新。

图片来源：论文 A Volumetric Method for Building Complex Models from Range Images
Step 1： 准备工作

1. 如上图，对场景建立长方体包围盒，能够完全包围要重建的物体。
2. 划分网格体素，对包围盒尽心划分n等分，体素的大小取决于包围盒和划分体素的数目决定。GPU并行时，可以一个线程处理一条(x,y)晶格柱，上图蓝色部分。

Step 2: 当前帧的TSDF值和权重计算，并进行融合

假定当前帧的位姿为$T_i$

1. 对于构造的立体中的每个体素$g$，转化为世界坐标系下的三维位置点$p^g$（根据体素的大小，以及体素的数目）;
2. 从世界坐标系转换到相机坐标系$p\leftarrow T_i^{-1}{p}^g$
3. 由相机内参矩阵，计算$p$点对应的像素$(u,v)$，也可以所是投影到相平面
4. 如果像素$(u,v)$在相机视场内，深度值为$D_s(g)=D_i(u,v)$，点$p^g$到相机坐标原点的距离为$D_c(g)=||t_i-p^g||$，其中$t_i$为位姿的平移向量。则其TSDF值：$V_{sdf}(g)=D_c(v)-D_s(u,v)$
5. 进行截断：
   $$V_{tfds}(g)=\{\begin{array}{ll}1,& \text{if }{V}_{sdf}(g)\ge T_{tfds\_max}\\ V_{sdf}(g)/T_{tfds\_max},& \text{if }0<V_{sdf}(g)<T_{tfds\_max}\\ V_{sdf}(g)/T_{tfds\_min},& \text{if }-T_{tfds\_min}<V_{sdf}(g)<0\\ & \\ -1,& \text{if }{V}_{sdf}(g)<-T_{tfds\_min}\end{array}$$
   也就是说，小于某个值，为$-1$，且非正数部分归一到$[-1,0]$；大于某个值，为$1$，且正数部分归一到$(0,1]$。
6. 计算权重：$w_i(g)=min(max\_weight,w_{i-1}(g))$
7. 平均：$V_{tsfd\_i}(g)=\frac{w_{i-1}(g)\ast V_{tsfd\_i-1}(g)+w_i(g)\ast V_{tsfd\_i}(g)}{w_{i-1}(g)+w_i(g)}$，如下图：
   
   图片来源：论文 A Volumetric Method for Building Complex Models from Range Images

在其他TSFD算法中，权重计算公式$w(g)=cos(\theta )/D_c(g)$，$\theta$为点到相机的射线与表面法线的夹角。如下图，可以看到，入射角更小，则是比较好的情况，入射角越大，带来的结果也相对更不可靠，因为很明显它不应该是距离表面最近的距离。因此实际中的权重，应该给入射角小的相机位置采集到的帧率更大的权重。

图片来源：3D Reconstruction——TSDF volume reconstruction

TSDF 生成的网格的细节保持比较好，而且精确度也比较好，但是在边缘处以及前后景交界处，会出现较大的拖尾现象。因为在体素p向像素坐标系投影时会有一定的误差。TSDF学习_weixin_44299305的博客-程序员宅基地中有介绍hasing voxel。

d) Adaptive Raycasting

Raycasting是从TSDF场中恢复出表面的过程。从一个地方射出一条线过去，如果它经过了表面，那么这个光线经过的体素的TSDF值一定有一个从正变负，或者从负变正（如果是实时重建，那么只有从正到负）的过程。这个射出的方向，就是相机坐标系z轴的。而正负交接点0点，我们称为zero-crossing，就是表面所在的voxel。我们要做的就是找到这个地方，当然有时候可能不会有哪个的体素大小直接是0,这时候就需要进行三线性插值（trilineal interpolation），找到逼近0的哪个地方。
具体步骤：

1. 对输出图片中的每一个像素：添加第三维度0/1，并转换为体素网格中的位置，下图1-3.
2. 求对应的射线$ra{y}^{dir}$，下图4
3. 出射射线所撞击的第一个体素单元$g$，下图6
4. 求出对应网格顶点到起点的距离，作为另外一个终止条件，下图7-8
5. 若$g$在我们的包围盒中：下图9
6. 更新$ra{y}^{len}$，保存$g$到$g^{pre}$，并沿着射线$ra{y}^{dir}$求得下一个体素网格点$g$，下图10-12
7. 判断$g$到$g^{pre}$是否是正负交接点0点(zero-crossing，从正变负，或者从负变正)，如是，转8；否则转5。下图13
8. 线性插值得到零点$p$，将点$p$转换到世界坐标，下图14-15
9. 通过表面tsdf的梯度计算点$p$的法线，下图16
10. 渲染该点，得到颜色值；或进行光线跟踪。下图17-18
    
    图片来源：论文：KinectFusion: Real-time 3D Reconstruction and Interaction Using a Moving Depth Camera

具体操作中，为了加速，可能会开始已一个比较大的步长，找到了zero-crossing，换成更小的步长，直到得到要求的精度，使得选取点的TSDF值尽量接近于0。得到这个点之后，再同样根据线性插值，写入color（实际中color可能是包含在TSDF场中的，以及权重）。

图片来源：3D Reconstruction——TSDF volume reconstruction

图片来源：PPT KinectFusion和ElasticFusion三维重建方法

参考资料：

1. TSDF学习_weixin_44299305的博客-程序员宅基地
2. 网格生成之TSDF算法学习笔记
3. 3D Reconstruction——TSDF volume reconstruction
4. 论文 A Volumetric Method for Building Complex Models from Range Images