转自个人博客：三维点云投影二维图像的原理及实现
1. 概述
1.1 原理概述
三维点云模型是由深度相机采集深度信息和RGB信息进行生成的，深度相机能直接获取到深度图和二维RGB图像，也就是说利用相机原本的关系就可以把深度信息投影回二维图像，即把点云中每个点投影到二维的正确位置，再把原本每个点的RGB颜色信息保留，涂抹到二维的对应位置，就实现了三维点云模型向二维图像的投影。
首先获取相机的相关参数，再利用参数的变换关系把点云一一映射为图像的像素。
2.2 效果示例
原相机拍摄得到的二维图像：
转换为三维点云模型：
对三维点云模型上手动叠加一个点云模型：
同样可以投影回二维图像如下：
2. 相机参数及其获取
2.1 相机原理
1. 先介绍一下需要涉及四种坐标系：
- 世界坐标系：是一个固定的全局参考坐标系，通常用于描述物体在真实世界中的位置和方向。
- 相机坐标系：是相机拍摄时直接使用的坐标系，以相机光心为原点，沿着相机光轴（通常是z轴）为前方，x轴和y轴分别与相机图像平面平行的坐标系。
- 图像坐标系：是相机坐标系经过投影后在图像平面上的坐标系，坐标原点位于图像平面中心，单位是mm。
- 像素坐标系：是图像上的实际像素位置，原点位于图像的左上角，x轴向右，y轴向下，单位是像素。
2. 而这四种坐标系与相机内外参的关系如下：
从世界坐标系到相机坐标系需要通过相机的外参（旋转和平移矩阵）作用；
从相机坐标系到图像坐标系再到像素坐标系需要通过相机的内参（焦距、主点）作用。
要注意的是，相机内参是由相机坐标系到图像坐标系的变换矩阵和图像坐标系到像素坐标系的变换矩阵相乘得到，所以只要知道：相机使用内参将相机坐标系转换到像素坐标系
所以通俗来讲，整个流程：相机拍摄 --> 世界坐标系 --外参–> 相机坐标系 --内参–> 像素坐标系
3. 对于三维点云模型：
三维点云模型不是由相机直接得到的，是人为使用深度图像信息和二维图像信息变换出来的，一般在世界坐标系中去描述三维点云模型。
那么要把三维点云模型投影到二维图像就要利用相机的内外参原理，把世界坐标系转化到像素坐标系也同样利用上述的整个流程。
先把世界坐标点乘以外参得到相机坐标点，再乘以内参得到像素坐标点
4. 总的坐标变换公式如下：
相机坐标点 = 世界坐标系中点经过旋转变化R + 平移向量t = 世界坐标点 * 相机外参：
$$ \begin{bmatrix} x_c \\ y_c \\ z_c \\ 1\end{bmatrix} = R \cdot P_W + t = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \cdot P_W = \text{extrinsics} \cdot \begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \end{bmatrix} $$
深度 * 像素坐标 = 相机内参 * 相机坐标：
$$ z_c \cdot \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \text{intrinsics} \cdot \begin{bmatrix} x_c \\ y_c \\ z_c \end{bmatrix} $$
要注意的是，外参（extrinsics）是 4 x 4 矩阵，内参（intrinsics）是 3 x 3 矩阵。
2.2 相机内外参
1. 相机外参：
相机外参描述相机在世界坐标系中的位姿，即位置和朝向。
相机的外参（extrinsics）包括旋转矩阵 R 和平移向量 t，它们将世界坐标系中的点转换到相机坐标系中。旋转矩阵 R是一个 3 x 3 的矩阵，而外参矩阵通常表示为一个 4 x 4 的齐次变换矩阵：
$$\text{extrinsics} = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}$$
其中 R 是 3 x 3 的旋转矩阵， t 是 3 x 1 的平移向量。
如果已知世界坐标系中的点 $$\mathbf{P}_w = (x_w, y_w, z_w, 1)^T$$，要得到相机坐标系中的点 $$ \mathbf{P}_c = (x_c, y_c, z_c, 1)^T $$，就可以如下计算：
$$\mathbf{P}_c = \text{extrinsics} \cdot \mathbf{P}_w$$
2. 相机内参：
相机内参描述相机内部属性，取决于相机内部参数。
相机的内参（intrinsics）将相机坐标系中的三维点投影到图像坐标系中的二维图像平面。内参矩阵通常表示为：
$$ \text{intrinsics} = \left[ \begin{array}{c} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1\end{array} \right] $$
$$ f_x $$ 和 $$f_y$$ 是沿 x 和 y 轴的焦距（通常与像素有关）， $$c_x$$和 $$c_y$$是图像平面的光心坐标。
如果已知相机坐标系中的三维点 $$ \mathbf{P}_c = (x_c, y_c, z_c)^T$$，要得到投影到图像平面上的点 $$ \mathbf{P}_i = (u', v')^T $$ ，就可以如下计算：
先$$\begin{bmatrix} u \\ v \\ w \end{bmatrix} = \text{intrinsics} \cdot \begin{bmatrix} x_c \\ y_c \\ z_c \end{bmatrix} $$，得到齐次坐标 $$(u, v, w)^T$$ 后，再通过归一化得到：$$u' = \frac{u}{w}, \quad v' = \frac{v}{w}$$
2.3 参数获取
相机内参是相机出厂就固定的。
相机外参与相机位姿有关，一般使用单位矩阵。
不同厂商的相机参数获取的方式不同，这里以Intel Realsense相机为例，有两种获取方法：
-
使用官方程序获取
下载官方摄像头工具，在
C:\Program Files (x86)\Intel RealSense SDK 2.0\tools目录下打开终端，输入命令~$ rs-sensor-control，依次选择后得到：Please select the desired streaming profile: 0Principal Point : 966.325, 532.394 Focal Length : 1395.13, 1395.56 Distortion Model : Brown Conrady Distortion Coefficients : [0,0,0,0,0]从中得知主坐标（Principal Point ）
$$ X_0 $$和$$ Y_0 $$，以及焦距（Focal Length）$$F_x$$和$$F_y$$，因为内参矩阵是：$$ \begin{bmatrix} \begin{array}{c} F_x & 0 & X_0 \\ 0 & F_y & Y_0 \\ 0 & 0 & 1\end{array} \end{bmatrix} $$，就可以得知最终内参矩阵：$$ \begin{bmatrix} \begin{array}{c} 1395.13 & 0 & 966.325 \\ 0 & 1395.56 & 532.394 \\ 0 & 0 & 1\end{array} \end{bmatrix} $$ -
根据官方库，在代码中获取
不同厂商提供的库不同，比如Intel RealSense在C++下有librealsense2库、在Python下有pyrealsense2包等，具体API就不细说了，自行查找官方文档
3. 投影主要思路和代码
3.1 思路
具体思路的原理见第二节，总的来说就是：先把世界坐标点乘以外参得到相机坐标点，再乘以内参得到像素坐标点。
$$ z_c \cdot \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \text{intrinsics} \cdot \left( \text{extrinsics} \cdot \begin{bmatrix} x_w \\ y_w \\ z_w \\ 1 \end{bmatrix} \cdot \begin{bmatrix} \begin{array}{c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0\end{array} \end{bmatrix} \right)$$
点乘这个矩阵
$$ \begin{bmatrix} \begin{array}{c} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0\end{array} \end{bmatrix}$$，是为了取前三位，去齐次，外参（extrinsics）是 4 x 4 矩阵，内参（intrinsics）是 3 x 3 矩阵。
3.2 代码实现
这里代码使用C++编写，点云库使用Open3d，可参考
// pcd：需要转换的点云；img：原图像，只用到其长宽
QPixmap pcdToOriginalImg(open3d::geometry::PointCloud *pcd, QImage *img)
{// 输入获取到的相机的内参和外参Eigen::Matrix3d intrinsics;intrinsics << 1395.13, 0, 966.325,0, 1395.56, 532.394,0, 0, 1;Eigen::Matrix4d extrinsics = Eigen::Matrix4d::Identity();int width = img->width();int height = img->height();// 遍历需要投影的所有点for(int i = 0; i < pcd->points_.size(); i++){Eigen::Vector3d point = pcd->points_[i];Eigen::Vector3d color = pcd->colors_[i];// 将点三维坐标齐次化，便于与外参相乘Eigen::Vector4d pointH(point[0], point[1], point[2], 1.0);// 依次利用外参和内参得到投影的二维图像上对应点// .head<3>是取四维坐标的前三位，即去除齐次化Eigen::Vector3d imgH = (intrinsics * (extrinsics * pointH).head<3>());// 进行归一化// 注意这里我自己用的时候会有坐标上的偏差，需要自己对应调整Eigen::Vector2d imgD = Eigen::Vector2d(imgH[0] / imgH[2], imgH[1] / imgH[2]);// 镜像翻转imgD[0] = width - imgD[0];// 根据算得的点坐标进行颜色的映射，即得到对应二维图像// 这里直接在原图像上映射，也可以另设图像if(imgD[0] >= 0 && imgD[0] < width && imgD[1] >= 0 && imgD[1] < height){QColor qcolor(int(color[0] * 255), int(color[1] * 255), int(color[2] * 255));img->setPixel(imgD[0], imgD[1], qcolor.rgb());}}return QPixmap::fromImage(img);
}
