从分割后的深度图像生成点云-Toy模板网

这篇具有很好参考价值的文章主要介绍了从分割后的深度图像生成点云。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

前言

前一段时间忙着秋招和修改论文意见反稿，就没有接着做关于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$ )坐标系中满足如下关系：
$\begin{bmatrix} u_m \\ v_m \\ 1 \end{bmatrix} = \begin{bmatrix} \frac{1}{d_x}&0&u_0 \\ 0&\frac{1}{d_y} &v_0 \\ 0&0&1 \end{bmatrix} \begin{bmatrix} x_m \\ y_m \\ 1 \end{bmatrix}$

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

1.2 关键部分

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

$z_w=z_c \qquad x_w = z_c \cdot (u_m -u_0) \cdot dx / f \qquad 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;
}