KinFu及其ICP、TSDF、Raycast理解

作者：weixin_40725706 | 2024-04-07 05:59:18

踩

kinfu

参考链接：

kinect fusion 3D重建基本算法

Kinect Fusion 算法浅析：精巧中带坑

The Basics of GPU Voxelization

文章目录

Pre-work

在进行下面的处理处理之前，kinectFusion实际还对原始的深度信息进行了一定的降噪平滑，采用双边滤波（Bilateral filtering），在保留边缘的基础上进行平滑，是个可以接受的选择。

Depth Map Conversion

主要由原先的图像点u=(x,y)，以及深度值D(u)，求得每个点的法向量n(u)。根据相机的内部矩阵，将图像2D坐标转化为相机原点坐标系的3D点。其重点是，如何从raw depth 计算出 vertex和normal？

已知的raw depth可以认为是一个2.5D 的信息，即由像素u的坐标x、y和对应深度 $\mathbf{D}_{i}(\mathbf{u})$ 来组成。在相机校准后已经可以获得相机的焦距，光心，扭曲参数以及相机变化矩阵（world->camera），所以我们可以通过back project的方法 $\mathbf{v}_{i}(\mathbf{u})=\mathbf{D}_{i}(\mathbf{u}) \mathbf{K}^{-1}[\mathbf{u}, \mathbf{1}]$ 来获得在camera space下的三维点 $\mathbf{v}_{i}(\mathbf{u})$ ，并且根据深度图上的相邻像素来计算出每个点的normal $\mathbf{n}_{i}(\mathbf{u})=\left(\mathbf{v}_{i}(x+1, y)-\mathbf{v}_{i}(x, y)\right) \times\left(\mathbf{v}_{i}(x, y+1)-\mathbf{v}_{i}(x, y)\right)$ 。在这个计算过程中，Kinect fusion算法使用了multi-scale方法，对每个深度图进行了三层缩放，每层的分辨率是位于下一层的一半。

Camera Tracking

ICP: Iterative Closest Point

《S. Rusinkiewicz and M. Levoy. Efficient variants of the ICP algorithm. 3D Digital Imaging and Modeling, Int. Conf. on, 0:145, 2001.》

此步骤用registration中最常用的ICP（iterative closest point）算法，即通过energy function（最小二乘结构）设计迭代算法来达到最优的拟合，求解出相机每次的相对位移与转动。相机位置可以用来将相机原点坐标系的结果转化到世界坐标系。

CUDA实现详细的介绍可以参考应用Fast ICP进行点集或曲面配准算法解析中关于Fast ICP的介绍。

每次迭代的基本思路就是：

筛选：点集或曲面的筛选（滤波）
1. 有全选，随意筛选，均匀分布，特征筛选等多种方法，kinectFusion中使用的应该是最朴素的全选（由于使用了GPU，并行化增加的前提下简单的算法有无可比拟的优势）
匹配：两个点集之间的点进行配对
1. 注意这并不是要保证匹配的点对是真实的匹配，因为本身ICP是迭代计算出匹配点并将其拟合的收敛过程。我们需要做的是每次选取合适的初始值。
2. KinectFusion采用的是投影关联(projective data association)。简单来说就是用上一个世界坐标系中的点，先转化成上个相机坐标系点 $v_{i-1}$ （3D），再转化成上一个图像坐标系点 $P$ （2D），然后找到本次图片中同样的2D坐标点，用当前的Ti值和Ri值去计算出点v及其法向量n，最后判断v和n与其对应点的相容性（其实是第4步去除的操作）。这样就完成了一次找匹配点的计算。
权重：给每个匹配的点对分配权重
去除：去除不符合条件的点对
误差度量：基于以上点对，给出每个点对的误差计算方法
最小化：最小化误差度量

经过上面的一次迭代，我们找到一堆匹配点，并求出其中使得匹配度最优（可以取类似最小二乘的值）的T，然后将本次测量的深度图进行相应的变换来进行下次迭代。

由于采用的是类似连续图像处理的方式，每一帧的深度测量值相差都很接近，这样才能使得算法能够迭代收敛。对每个点用一个GPU线程去处理的方法有效的简化了ICP的过程。

上面算法的大致思想是通过预测表面点乘以上一帧的变化的逆（这里说的是逆的变化是从global->camera）得到camera space下的上一帧顶点坐标，然后利用相机参数投影到vertex map上在uv space下找到对应点。之后通过当前帧的变化矩阵来得到在global空间下的顶点和法向量，这样就和上一帧计算出的表面点在同一空间，即global空间内了。然后根据设定的距离阈值和法向量阈值来判断是否是相关点。

使用迭代法来求解最小二乘问题。通过每次迭代求出一个incremental变化矩阵来逐渐逼近最优解，在此问题中，我们需要求解六个参数，即三个旋转和三个平移。在这里有一个近似就是当旋转角度足够小时可以近似把sinA = A， cosA = 1。因为我们需要最小值，而在极值点的一个显著特性就是导数为0，所以我们可以对object function求导即得到所谓的residual（残差），通过使残差逼近为0来实现求解。

对于此类rigid模型我们最后求解的是一个6x6的矩阵，可以轻松的用Cholesky分解来求解，加速可以考虑使用GPU来求解此线性系统。对于full resolution深度图的求解需要大概10次循环。如果相机在tracking的时候位置计算出错，则使用最后已知的相机姿态来计算surface prediction。而如果通过这样计算出的结果可靠而有效，则开始继续tracking和mapping

Volumetric Integration

Truncated Signed Distance Function

《B. Curless and M. Levoy. A volumetric method for building complex models from range images. In ACM Transactions on Graphics (SIGGRAPH), 1996.》

Voxel空间：

KinFu将重建空间进行体素划分，比如划分成512³（即长宽高都是512的立方体晶格）。我们将整个空间的体素全部存入GPU的显存，所以KinectFusion的这种算法对显存的消耗极大，如果不加改造直接用在大场景的重建会有问题。但是显存随着硬件发展在不断增加，同时一些减少显存消耗的算法改进（比如八叉树）能有效减少KinectFusion的显存消耗。
对于(x,y,z)的晶格坐标，每个GPU进程扫描处理一个(x,y)坐标下的晶格柱。

Voxelization_blog_fig_1

正交摄像机下的体素网格表示

TSDF

SDF为相机坐标系下的一个三维点p到相机光心的预测距离与由p点在像素坐标下的投影点u的深度值，即测量距离的差值。

TSDF是从当前体素空间位置到最近的表面的带符号的距离,这里的 $F$ 是一个truncated function，也就是说，给予一个空间位置（x,y,z）可以通过 $F$ 来计算出对应的TSDF值。在物体表面TSDF为0，大于0表示在物体表面前，小于0表示在表面后。除了每个空间位置保存有TSDF值，同时也保存一个权重值w，目的是为了衡量此位置TSDF值的可靠度。

\begin{aligned} F_{R_{k}} (p) & = Ψ (λ^{- 1} ‖ (t_{g, k} - p ‖_{2} - R_{k} (x)) \\ λ & = {‖ K^{- 1} \dot{x} ‖}_{2} \\ x & = [π ({K T}_{g, k}^{- 1} p)] \\ Ψ (η) & = {\begin{array}{cc} min (1, \frac{η}{μ}) sgn (η) & iff η \geq - μ \\ null & otherwise \end{array} \end{aligned}

$\begin{aligned}\mathrm{F}_{\mathrm{R}_{k}}(\mathbf{p}) &=\Psi\left(\lambda^{-1} \|\left(\mathbf{t}_{g, k}-\mathbf{p} \|_{2}-\mathrm{R}_{k}(\mathbf{x})\right)\right.\\\lambda &=\left\|\mathrm{K}^{-1} \dot{\mathbf{x}}\right\|_{2} \\\mathbf{x} &=\left[\pi\left(\mathrm{KT}_{g, k}^{-1} \mathbf{p}\right)\right] \\\Psi(\eta) &=\left\{\begin{array}{cc}\min \left(1, \frac{\eta}{\mu}\right) \operatorname{sgn}(\eta) & \text { iff } \eta \geq-\mu \\\text { null } & \text { otherwise }\end{array}\right.\end{aligned}$

F_{R_{k}} (p) λ x Ψ (η) = Ψ (λ^{- 1} ∥ (t_{g, k} - p ∥_{2} - R_{k} (x)) = ∥ ∥ K^{- 1} \dot{x} ∥ ∥_{2} = [π (K T_{g, k}^{- 1} p)] = {min (1, \frac{η}{μ}) s g n (η) null iff η \geq - μ otherwise

Frk指的是计算位于在global space下的p位置关于当前深度图Rk的TSDF 值，最外面的函数用于实现truncation，然后里面是SDF项。SDF值是由当前空间位置p的L2结合pixel ray的scale（λ项）与投影p到深度图的深度之差得到的。其中需要注意的是计算x是需要考虑floor操作来避免不连续性情况出现。

1,2：对于每个x,y坐标下的体元g，并行的从前往后扫描
3：将晶格坐标g转换到对应的世界坐标系点 $v^{g}$
4：对于每次TSDF操作时的拍摄变换 $T_{i}$ 反变换到对应的相机坐标系坐标v
5：相机坐标系点v投影到图像坐标点p，从3D到2D
6：如果v在此摄像机的投影范围内，用它修正现有tsdf表示
7： $s d f_{i}$ 是该相机坐标系点 $v^{g}$ 到本次相机原点 $t_{i}$ 的距离与本次观测深度 $D_{i}(p)$ 的差值
8-11为截断的过程，Truncated的意义所在，用max truncation表示选取的截断范围，此值将会关系到最后重建结果的精细程度

8：如果差值为正，表示该晶格在本次测量的面的后面
9： $ts d f_{i}$ 赋值【0,1】之间，越靠近观测面的地方值越接近0
10：如果差值为负，表示该晶格在本次测量的面的前面
11： $ts d f_{i}$ 赋值【-1，0】之间，越靠近观测面的地方值越接近0

12：选取本次计算值的tsdf的权值wiwi，这个权值的选取直接关系到图片的适应性，以及抗噪声的能力，其实这里有点类似卡尔曼滤波。注意这里每次权值+1的操作基于这样的原因，由于只有在相机拍摄范围内的点才会进入求tsdf的操作，每次的权值在原先的基础上增加1能照顾到迅速变化的或很少扫描到的面的变化。
13：加权平均求出 $ts d f^{avg}$
14：将 $w_{i}$ 和 $ts d f^{avg}$ 存储在对应的晶格，进行下个晶格的扫描操作
经过上面的扫描，最终立方体晶格中存储的tsdf值形成了重建物体外是负值，物体内部是正值，物体表面是0值得形式（可能没有准确的零值，但是可以根据正负值插值求出零值点，所以最后物体表面的分辨率将会超过晶格的分辨率）

Voxelization_blog_fig_2

渲染目标上的像素位置及其深度值对应于体素网格的X，Y和Z

Raycasting

一个基于GPU的raycaster生成体积内隐式表面的视图，用于渲染和跟踪(见伪代码)。在并行的情况下，每个GPU线程沿着单个光线行走，并在输出图像中渲染单个像素。

给定光线的起始位置和方向，每个GPU线程沿着光线遍历体素，
通过观察零交叉(沿着光线存储的TSDF值的符号跳变)来提取隐式表面的位置。最后的表面交点计算使用简单的线性插值给定三线性采样点的任何一侧的零交点。
假设梯度与表面界面正交，则表面法线直接计算为TSDF在交点处的导数。

因此，每个找到光线/表面交点的GPU线程都可以计算单个插值顶点和法线，它们可以作为输出像素上照明计算的参数，以渲染表面。

本文内容由网友自发贡献，转载请注明出处：https://www.wpsshop.cn/w/weixin_40725706/article/detail/376817?site