拆解KinectFusion算法

距离KinectFusion算法首次提出过去十多年了，网络上已经有非常多优秀的博客讲解，也有很多成熟的开源实现。所以，写这个博客的目的一是为了记录下曾经学习的过程，希望能给刚接触这个算法的同学一点点答疑解惑；另一方面也是练习下写博客，来记录更多新学到的一些知识。刚开始写博客，公式图表排版都不是很熟，所以计划先简要阐述原理，结合代码注释进行实操，具体地后面再慢慢补充。

KinectFusion的输入来自消费级RGB-D深度相机，比较有代表性产品有微软的Kinect系列(KinectV2, AzureKinect等)；Intel的RealSense系列(SR300, D400系列等)；最早的有以色列一家叫PrimeSense的公司后来被苹果收购，也就是后来苹果手机Face ID这个模块背后使用的技术，现在1.09系列的短距离摄像头还能从网上买到，长距离的很早就买不到了；还有华硕的Xtion Pro Live；国产的厂商有奥比中光、华捷艾米等等。深度相机可以分为主动式和被动式：主动式的就是通过主动发射激光然后根据接收器收到的反射回来的信号解算深度，主要有结构光技术和ToF技术；被动式主要就是双目立体相机，比如Zed相机，这类相机受室外光照影响较小，通过增大基线距离可以恢复很远距离的深度信息，因此在一些无人机和自动驾驶车辆上有使用，缺点就是解算深度很耗费计算资源。

假定我们已经拿到了一张深度图，例如640x480分辨率，其中每个像素的数值是以16bit的无符号整数来存储的，通常单位是mm，例如1300代表的深度是1.3m。假定相机无畸变或者已经去畸变，可以用针孔模型(Pinhole)来表述3D空间的点(x, y, z)和投影到像素平面的位置(u, v)以及该像素位置上深度值的关系。所谓针孔模型可以用四个参数来刻画，也叫相机内参(实际相机的畸变系数也属于内参)，两个焦距两个中心，(fx, fy, cx, cy)，例如(525.f, 525.f, 319.5f, 239.5f)，实际上标定之后fx和fy稍微有点差别但差距不大，两个中心也并不是严格的就在图像的中心，也是有些偏移，具体可以采用张氏棋盘格标定法进行标定，也可以直接读取相机的出厂参数。

下面给出从深度图计算VertexMap和NormalMap的代码，代码仅依赖OpenCV库：

	cv::Mat computeVertexMap(const Mat3f &K, const cv::Mat &depth)
	{
        //首先判断下depth图像是否为空；
        //类型是不是float类型的，如果是float的就假定是已经转换单位为米(这块刚开始容易出bug一定要注意乘1000还是除1000);
        //CV_32FC1, 是OpenCV内置的数据类型，32F表示4字节的float，C1表示单通道(Channel)，就是一个像素一个数值，C3就是三个数。
		if (depth.empty() || depth.type() != CV_32FC1)
			return cv::Mat();
        
        //K = [fx,  0, cx;
        //      0, fy, cy;
        //      0,  0, 1];
		float cx = K(0, 2);
		float cy = K(1, 2);
		float fx_inv = 1.0f / K(0, 0);
		float fy_inv = 1.0f / K(1, 1);
        
        
        //定义一个cv::Mat并初始化为0，也可以是cv::Mat::zeros(height, width, CV_32FC3);
        cv::Mat vertex_map = cv::Mat::zeros(depth.size(), CV_32FC3);
		for (int y = 0; y < depth.rows; ++y)
		{
			for (int x = 0; x < depth.cols; ++x)
			{
                //注意d的单位
				float d = depth.at<float>(y, x);
				float x0 = (float(x) - cx) * fx_inv;
				float y0 = (float(y) - cy) * fy_inv;
                //OpenCV的索引数据的惯例是先行后列，在图像坐标里，行的index对应y轴(或者v表示)，列的index对应x轴(或者u表示)
                vertex_map.at<cv::Vec3f>(y, x) = cv::Vec3f(x0 * d, y0 * d, d);
			}
		}
        return vertex_map;
	}

接下来给出NormalMap的计算代码，NormalMap存储的就是VertexMap上每个顶点的法向量，论文里用的计算思路很简单，就是当前像素的顶点和右边像素的顶点搞个向量，和下边像素的顶点搞个向量，两个向量叉乘得到垂直向量，具体看代码：

	cv::Mat computeNormals(const cv::Mat &vertex_map, float depth_threshold)
	{
        if (vertex_map.empty() || vertex_map.type() != CV_32FC3)
			return cv::Mat();

        int w = vertex_map.cols;
        int h = vertex_map.rows;
		cv::Mat normals = cv::Mat::zeros(h, w, CV_32FC3);
        const float* ptr_vert = reinterpret_cast<const float*>(vertex_map.data);

        for (int y = 1; y < h - 1; ++y)
		{
            for (int x = 1; x < w - 1; ++x)
			{
                size_t off = static_cast<size_t>((y*w + x) * 3);
                Vec3f vert(ptr_vert[off], ptr_vert[off + 1], ptr_vert[off + 2]);
                //深度是0的点就别算normal了，非要插值一个出来也没毛病
                if (vert[2] == 0.0f)
					continue;

                //这里用的是当前像素右边的像素的顶点减左边像素的顶点，问题不大;
                //实际上cv::Mat索引顶点有很多种办法，不记得当时为什么用了这么复杂冗长的写法，功能一样也懒得改了;
                //这是后来常用的写法，关于遍历像素应该有更好更快的写法，这里就不纠结了，实现功能即可：
                //cv::Vec3f vert = vertex_map.at<cv::Vec3f>(y, x)
                size_t off_x0 = static_cast<size_t>((y*w + x - 1) * 3);
                Vec3f vert_x0(ptr_vert[off_x0], ptr_vert[off_x0 + 1], ptr_vert[off_x0 + 2]);
                size_t off_x1 = static_cast<size_t>((y*w + x + 1) * 3);
                Vec3f vert_x1(ptr_vert[off_x1], ptr_vert[off_x1 + 1], ptr_vert[off_x1 + 2]);
                size_t off_y0 = static_cast<size_t>(((y - 1)*w + x) * 3);
                Vec3f vert_y0(ptr_vert[off_y0], ptr_vert[off_y0 + 1], ptr_vert[off_y0 + 2]);
                size_t off_y1 = static_cast<size_t>(((y + 1)*w + x) * 3);
                Vec3f vert_y1(ptr_vert[off_y1], ptr_vert[off_y1 + 1], ptr_vert[off_y1 + 2]);
                if (vert_x0[2] == 0.0f || vert_x1[2] == 0.0f || vert_y0[2] == 0.0f || vert_y1[2] == 0.0f)
					continue;

                Vec3f tangent_x = vert_x1 - vert_x0;
                Vec3f tangent_y = vert_y1 - vert_y0;
                //判断下，这个tangent的向量别太长了，太长说明相邻像素上的顶点离的挺远
                //既然像素挨着，距离很远说明深度差的多，有深度不连续或者断层的地方法向量没什么意义，算出来也是捣乱的
                if (tangent_x.norm() < depth_threshold && tangent_y.norm() < depth_threshold)
				{
                    //但凡算normal的地方，记得归一化
                    Vec3f n = (tangent_y.cross(tangent_x)).normalized();
					normals.at<cv::Vec3f>(y, x) = cv::Vec3f(n[0], n[1], n[2]);
				}
			}
		}

		return normals;
	}

这是CPU版本的简单实现，实际用的时候是基于GPU并行加速计算，一个线程算一个像素，没有for循环，GPU的话不能写太复杂的逻辑分支，就是简单无脑算，每个线程独立执行一块数据操作，然后再合并Reduce等等，有关GPU，CUDA编程的内容后面再细说。这两片小代码没记错应该是改自Intrinsic3D开源的代码，这里引用下出处。
第一次写博客，感觉想写的挺多，想到哪就写到哪了，显得杂乱无章了，后面慢慢改进吧hh，大家多包涵。后面陆续写ICP，TSDF，MarchingCubes，网格光栅化等模块。