根据相机坐标系下的点云数据rgb在图像提供的像素坐标系下$x,y$坐标(即公式中的$u，v$）和相机内参可以在相机坐标系下找到$X,Y$坐标值。同时，深度图直接在相机坐标系下提供$Z$坐标值。然后得到相机坐标系下的坐标$$P=?\begin{array}{c}X\\ Y\\ Z\end{array}]$$，相机坐标系下的障碍物点的坐标，就是点云传感器数据，也就是相机坐标系下的点云数据。相机坐标系$p$与像素坐标系$p_{UV}$下的点的坐标的关系公式:

$$Z{P}_{uv}=Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=KP$$

将上式整理后的具体求解公式如下：

$$\begin{gathered} X=Z(u-c_x)/f_x \\ Y=Z(v-c_y)/f_y \\ Z=d \end{gathered}$$

一般来说，公式中点在相机坐标系下的𝑍值就是相机测出的深度值𝑑，即就是真实点到相机平面的距离。如果不是可以加倍数补偿。

如果在对应点的位置再加入彩色信息就能够构成彩色点云。

根据世界坐标系到像素坐标系下点的坐转换公式：
$$\begin{gathered} Z{P}_{uv}=Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=KP=K\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=K(R\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\end{array}\right]+t) \\ 或者 \\ Z{P}_{uv}=Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=KP=K\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=KT\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]（隐含齐次坐标转非齐次坐标） \\ K=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right] \end{gathered}$$

上述描述了世界坐标系𝑃𝑤到像素坐标系𝑃𝑢𝑣下的点的坐标关系。因此这里利用上式，输入像素坐标[𝑢,𝑣]和深度𝑍之后，就可以求得世界坐标系下点的坐标𝑝𝑤，所有世界坐标系下的障碍物点的坐标，就是点云地图数据。深度图能够直接提供相机坐标系下某个点的𝑍坐标值，作为式子中的𝑍值。rgb图提供了像素坐标系下点的坐标𝑃𝑢𝑣=[𝑢,𝑣]。根据内参公式就可以计算出该点在相机坐标系下的三维坐标𝑃=[𝑋,𝑌,𝑍]。再根据有相机的齐次变换矩阵𝑇或者旋转矩阵和平移向量𝑅,𝑡，就可以求出该点在世界坐标系下的三维坐标𝑃𝑤=[𝑋𝑤,𝑌𝑤,𝑍𝑤]。𝑃𝑤=[𝑋𝑤,𝑌𝑤,𝑍𝑤]就是在世界坐标系下标定的点云。也就是点云地图。

总结： 根据RGBD图求解点云地图数据，就是将像素坐标系下点的坐标𝑃𝑢𝑣=[𝑢,𝑣]和相机坐标系下的点的𝑍坐标值代入公式求出在世界坐标系下的三维坐标𝑃𝑤=[𝑋𝑤,𝑌𝑤,𝑍𝑤]，𝑃𝑤就是最终要求的世界坐标系的点云。根据上式一旦确定了相机内参矩阵K和外参旋转矩阵R和平移矩阵t，就可以根据RGBD图像完成世界坐标系下的点云生成。

如果设定世界坐标系和相机坐标系是重合的，即没有旋转和平移，这时实质就是以相机坐标系建立的点云。这时相机坐标系和世界坐标系的相对位姿的旋转矩阵R就为单位矩阵，位移向量t就是零向量。

说明：
这里提到了要根据相机的位姿将所有像素的坐标在相机坐标系下转换到世界坐标系下。这个过程只有在想要建立周围环境的点云地图时才是必不可少的，因为要先建立整个环境的点云就需要点云的坐标系是静止的。如果不使用相机位姿也可以建立以相机坐标系为参考的点云数据，但是此时的点云信息就只能显示相机所视范围内图片的点云数据，所有点都是相对于相机而言的，一旦变换相机位置，将重新建立相机所视范围内图片的点云数据，因为此时点云坐标系都变了，之前的点云数据不再有意义。如果要使用多张图片生成点云那么就要把像素转化到世界坐标系中。

转载：https://www.cnblogs.com/sxy370921/p/11633568.html