基于深度图和RGB图的三维映射（从2D到3D空间的重映射）

本文图片来自下面两处链接，感谢原文作者！
参考链接1
参考链接2

一、概述
常听到的RGB-D相机是指带有彩色信息的3D相机，其输出的每个点包含了XYZ三轴坐标和彩色信息（RGB三个通道的值）。从硬件上来看，可以由一个能测深度的传感器结合一个2D的彩色相机构成。那么如何由一张彩色图片结合其对应的“深度图”来重建一张3D图片呢？首先解释一下什么是深度图，从图片维度上来看，它是一张2D图片，与“RGB彩色图片”尺寸一致，只不过每个像素p(x,y)对应的灰度值代表了物体上某个点到相机中心点的垂直距离。下面我们要讨论的就是由一张2D的彩色图片结合其对应的深度图来求一张3D点云图。或许你会疑惑这有什么用呢？RGB-D相机本身输出的不就是点云图吗？为何自己要大费周章地重新做这件事情呢？因为在实际使用3D相机的过程中，你可能要利用其输出的2D图片做一些2D算法的检测（显然是因为有些检测用2D算法更加有用高效,比如使用opencv里的算子），然后将2D的点重新映射到3D的点云上进行表征，这就需要使用下面的方法帮助你了。

二、原理

如上图所示,已知实际场景中的点M在相机平面上的成像为点Xm,从RGB图像中可以得到Xm的二维坐标p(x,y)以及颜色信息，从深度图上可以得到p(x,y)对应的深度信息（RGB图与深度图尺寸一致，一一对应），并且已知彩色图像中心点和相机中心点的距离：f (即焦距），现在求的就是M点在相机坐标系（图中点C为原点的坐标系）下的三维坐标。从上图中直观的就可以看出M点在相机坐标系下的Z轴坐标就是深度值，而XY轴坐标则可以依据p(x,y)坐标值以及“三角关系”来求（注：p(x,y)坐标以图像中心点为原点，如果使用opencv来读取图片，其像素坐标是以图片左上角点作为原点，这里需要做一个平移转换，平移量需要根据相机内参来获取）。XY轴坐标计算方法如下：

注：w指M点在相机坐标系下的X值，h指M点在相机坐标系下的Y值

注：实际使用时，焦距有fx,fy两个来表示。文章开头处的参考链接2有详细说明。

三、代码实现

                Eigen::Vector3d point; 
                point[2] = double(d); 
                point[0] = (u - cx) * point[2] / fx;
                point[1] = (v - cy) * point[2] / fy; 

注：u,v是像素坐标系下的坐标，以左上角为原点。cx、cy、fx、fy均可以由内参矩阵获得。内参矩阵如上图，可以通过张正友标定法来计算。