2019年8月4日 16:26:40
RGB-D 图像中的rgb图片提供了像素坐标系下的x，y坐标，而深度图直接提供了相机坐标系下的$Z$坐标，也就是相机与点的距离。
根据 RGB-D 图像的信息和相机的内参，可以计算出任何一个像素点在相机坐标系下的坐标。
根据 RGB-D 图像的信息和相机的内参与外参，可以计算出任何一个像素点在世界坐标系下的坐标。
相机视野范围内，相机坐标系下的障碍物点的坐标，就是点云传感器数据，也就是相机坐标系下的点云数据。点云传感器数据可以根据 RGB-D 图像提供的坐标与相机内参算出来。
所有世界坐标系下的障碍物点的坐标，就是点云地图数据，也就是世界坐标系下的点云数据。点云地图数据可以根据RGB-D 图像提供的坐标与相机内参和外参算出来。
相机坐标系下的点云数据是根据rgb图像提供的像素坐标系下的x，y坐标（即公式中的u，v）和相机内参就能求出相机坐标系下的$X$,$Y$坐标值。同时深度图直接提供相机坐标系下的$Z$坐标值。进而得到相机坐标系下的坐标$P=\begin{bmatrix}
X\
Y\
Z
\end{bmatrix}$，相机坐标系下的障碍物点的坐标，就是点云传感器数据，也就是相机坐标系下的点云数据。
相机坐标系$P$与像素坐标系$P_{uv}$下的点的坐标的关系公式:
$$
ZP_{uv}=Z\begin{bmatrix}
u\
v\
1
\end{bmatrix}=\begin{bmatrix}
f_x&0&c_x\
0&f_y&c_y\
0&0&1
\end{bmatrix}\begin{bmatrix}
X\
Y\
Z
\end{bmatrix}=KP
$$
将上式整理后的具体求解公式如下：
$$
X=Z(u-c_x)/f_x\
Y= Z(v-c_y)/f_y\
Z=d
$$
一般来说，公式中点在相机坐标系下的$Z$值就是相机测出的深度值$d$，即就是真实点到相机平面的距离。如果不是可以加倍数补偿。
如果在对应点的位置再加入彩色信息就能够构成彩色点云。
根据世界坐标系到像素坐标系下点的坐转换公式：
$$
ZP_{uv}=Z\begin{bmatrix}
u\
v\
1
\end{bmatrix}=KP=K\begin{bmatrix}
X\
Y\
Z
\end{bmatrix}=K(R\begin{bmatrix}
X_w\
Y_w\
Z_w
\end{bmatrix}+t)\或者\ZP_{uv}=Z\begin{bmatrix}
u\
v\
1
\end{bmatrix}=KP=K\begin{bmatrix}
X\
Y\
Z
\end{bmatrix}=KT\begin{bmatrix}
X_w\
Y_w\
Z_w\
1
\end{bmatrix}（隐含齐次坐标转非齐次坐标）
$$
$$
K=\begin{bmatrix}
f_x&0&c_x\
0&f_y&c_y\
0&0&1
\end{bmatrix}
$$
上述描述了世界坐标系$P_w$到像素坐标系$P_{uv}$下的点的坐标关系。因此这里利用上式，输入像素坐标$[u,v]$和深度$Z$之后，就可以求得世界坐标系下点的坐标$p_w$，所有世界坐标系下的障碍物点的坐标，就是点云地图数据。
深度图能够直接提供相机坐标系下某个点的$Z$坐标值，作为式子中的$Z$值。rgb图提供了像素坐标系下点的坐标$P_{uv}=[u,v]$。
根据内参公式就可以计算出该点在相机坐标系下的三维坐标$P=[X,Y,Z]$。再根据有相机的齐次变换矩阵$T$或者旋转矩阵和平移向量$R,t$，就可以求出该点在世界坐标系下的三维坐标$P_w=[X_w,Y_w,Z_w]$。$P_w=[X_w,Y_w,Z_w]$就是在世界坐标系下标定的点云。也就是点云地图。
总结：根据RGBD图求解点云地图数据，就是将像素坐标系下点的坐标$P_{uv}=[u,v]$和相机坐标系下的点的$Z$坐标值代入公式求出在世界坐标系下的三维坐标$P_w=[X_w,Y_w,Z_w]$，$P_w$就是最终要求的世界坐标系的点云。根据上式一旦确定了相机内参矩阵K和外参旋转矩阵R和平移矩阵t，就可以根据RGBD图像完成世界坐标系下的点云生成。
如果设定世界坐标系和相机坐标系是重合的，即没有旋转和平移，这时实质就是以相机坐标系建立的点云。这时相机坐标系和世界坐标系的相对位姿的旋转矩阵R就为单位矩阵，位移向量t就是零向量。
说明：
这里提到了要根据相机的位姿将所有像素的坐标在相机坐标系下转换到世界坐标系下。这个过程只有在想要建立周围环境的点云地图时才是必不可少的，因为要先建立整个环境的点云就需要点云的坐标系是静止的。如果不使用相机位姿也可以建立以相机坐标系为参考的点云数据，但是此时的点云信息就只能显示相机所视范围内图片的点云数据，所有点都是相对于相机而言的，一旦变换相机位置，将重新建立相机所视范围内图片的点云数据，因为此时点云坐标系都变了，之前的点云数据不再有意义。如果要使用多张图片生成点云那么就要把像素转化到世界坐标系中。
《机器人学、机器视觉与控制》
https://www.cnblogs.com/gaoxiang12/p/4652478.html
http://www.cnblogs.com/cv-pr/p/5719350.html
来源：博客园
作者：前路有光，初心莫忘
链接：https://www.cnblogs.com/sxy370921/p/11633568.html