从分割后的深度图像生成点云

前言

前一段时间忙着秋招和修改论文意见反稿，就没有接着做关于Azure Kinect DK相关的探索总结，现在有时间就慢慢补起来。上一篇是利用PP-Humanseg模型分割出color图和深度图中的人像，这一篇紧接着上一篇的工作，从人像分割后的图像结果获得人体的点云数据（也可以直接先生成点云再作点云的分割，等后续探索）。

一、深度图转点云

1.1 原理

关于这部分的原理很多博客和文章都已经有详细的叙述，这里就作一个简单的记录。

首先，我们需知道相机成像原理中的一些映射过程：

上图中有四个坐标系分别为世界坐标系($X_w$, $Y_w$, $Z_w$)，相机坐标系($X_c$, $X_c$, $X_c$)，像素坐标系($u$, $v$)和图像物理坐标系($x$, $y$)。

图像中任意一个像素点m在世界坐标系坐标为($x_w$, $y_w$, $z_w$)，在摄像机坐标系坐标为($x_c$, $y_c$, $z_c$)，在像素坐标系坐标为($u_m$, $v_m$)，在图像物理坐标系坐标为($x_m$, $y_m$)。

图像物理坐标系的原点在图像坐标系中的原点为($u_0$, $v_0$)，图像上每个点在$x$，$y$轴方向上的物理尺寸是$d_x$，$d_y$。则图像中任意一个像素点m在($u$, $v$)坐标系中满足如下关系：
$$\left[\begin{array}{c}{u}_m\\ v_m\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& u_0\\ 0& \frac{1}{d_y}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_m\\ y_m\\ 1\end{array}\right]$$

根据刚体变换的过程，世界坐标系中的一点到相机坐标系中的点，可以由一个旋转矩阵R和平移矩阵T来描述：
$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0_3^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$
又因为：
$$x_m=f\frac{x_c}{z_c},y_m=f\frac{y_c}{z_c},--->z_c\left[\begin{array}{c}{x}_m\\ y_m\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]$$
由上述描述的三个矩阵等式变换可得：
$$z_c\left[\begin{array}{c}{u}_m\\ v_m\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& u_0\\ 0& \frac{1}{d_y}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0_3^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$
$$=\left[\begin{array}{cccc}\frac{f}{d_x}& 0& u_0& 0\\ 0& \frac{f}{d_y}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0_3^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$
其中等式右边的第一个矩阵是相机标定的内参矩阵，第二个矩阵是相机的外参矩阵。

1.2 关键部分

对于单个相机来说，由于世界坐标原点和相机原点重合，也就无旋转和平移，所以有：
$$z_c\left[\begin{array}{c}{u}_m\\ v_m\\ 1\end{array}\right]=\left[\begin{array}{cccc}\frac{f}{d_x}& 0& u_0& 0\\ 0& \frac{f}{d_y}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$
从以上的矩阵变换可以得到像素点到世界坐标点的变换，即：

$$z_w=z_c{x}_w=z_c\cdot (u_m-u_0)\cdot dx/f{y}_w=z_c\cdot (v_m-v_0)\cdot dy/f$$

1.3 关键代码

pcl::PointCloud<pcl::PointXYZRGB>::Ptr ImageToPointcloud(cv::Mat& color, cv::Mat& depth){
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud( new pcl::PointCloud<pcl::PointXYZRGB>() );
    for (int v = 0; v < depth.rows; v++){
        for (int u = 0; u < depth.cols; u++){
            unsigned int d = depth.ptr<unsigned short>(v)[u];
            pcl::PointXYZRGB point;

            point.z = double(d) / _depthScale;
            point.x = (u - _cx) * point.z / _fx;  // _cx, _cy是摄像头光学中心
            point.y = (v - _cy) * point.z / _fy;  // _fx, _fy是摄像头焦距
            
            point.b = color.data[v*color.step+u*color.channels()];
            point.g = color.data[v*color.step+u*color.channels() + 1];
            point.r = color.data[v*color.step+u*color.channels() + 2];

            pointcloud->points.push_back(point);
        }
    }
    pointcloud->height = 1;
    pointcloud->width = pointcloud->points.size();
    pointcloud->is_dense = false;

    return pointcloud;
}

二、结果展示

这里放出分割后的color图和对应生成的点云：

总结

关于从深度图像生成点云，就是坐标系点的转换，只要明白其中的原理，代码还是很好写的，后续还会进行相机标定和点云拼接等操作。