【点云处理】使用16位深度图和RGB图像帧生成点云-C语言/C++

在之前学习并实践了使用Intel Realsense d435 拍摄RGB视频及深度图视频后，经过图像处理获得了连续的深度图与RGB颜色图图像帧，现根据深度图与颜色图生成实践场景的局部点云，以下为总体代码：

// C++ 标准库
#include <iostream>
#include <string>
#include <vector>
#include <algorithm>

// OpenCV 库
#include "opencv2/opencv.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/core/utility.hpp"
#include <opencv2/ximgproc/disparity_filter.hpp>

// PCL 库
//#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>

// 定义点云类型
// 定义点云使用的格式：这里用的是XYZRGBA
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;


using namespace std;
using namespace cv;


int main()
{
    // 数据读取
    cv::Mat rgb, depth_origin, depth, rdepth;
    
    // 相机内参
    double K_cx = 640;
    double K_cy = 360;
    double K_fx = 915;
    double K_fy = 915;
    double depthScale = 1000;


    // 定义点云点的三维坐标值：计算每个点对应的XYZRGB值
    PointCloud::Ptr __pointCloud(new PointCloud);

        string path_rgb = "./rgb";
        int    imagenum ;
        imagenum = j;
        string path_rgb_imgtest = path_rgb + "/" + to_string(imagenum) + "_rgb.png";
        string path_depth = "./depth";
        string path_depth_imgtest = path_depth + "/" + to_string(imagenum) + "_depth.png";

        rgb = cv::imread(path_rgb_imgtest);
        depth = cv::imread(path_depth_imgtest, IMREAD_UNCHANGED);
      
        
    if (!__pointCloud->empty())
        __pointCloud->clear();
    
    // 遍历深度图图像，获取每个像素点的深度图
    for (int v = 0; v < rgb.rows; v++)
    {
        for (int u = 0; u < rgb.cols; u++)
        {

            unsigned int d = depth.ptr<unsigned short>(v)[u];    

            if (d == 0)            //坏点滤除：深度为0的点
                continue;

            //点云的三维坐标计算
            PointT p;
            p.z = double(d) / depthScale;   
            p.x = (u - K_cx) * p.z / K_fx;
            p.y = (v - K_cy) * p.z / K_fy;
            //点云点BGR颜色通道
            p.b = rgb.data[v * rgb.step + u * rgb.channels()];
            p.g = rgb.data[v * rgb.step + u * rgb.channels() + 1];
            p.r = rgb.data[v * rgb.step + u * rgb.channels() + 2];
            //点云保存
            __pointCloud->points.push_back(p);

        }

 

    //点云保存
    cout << imagenum << "号点云共有" << __pointCloud->size() << "个点." << endl;
    string path_depth_save = "./"+to_string(imagenum)+ ".ply";
    pcl::io::savePLYFile(path_depth_save, *__pointCloud);

    }
    return 0;
}

代码分析：

1.库引用：

// C++ 标准库
#include <iostream>
#include <string>
#include <vector>
#include <algorithm>

// OpenCV 库
#include "opencv2/opencv.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/core/utility.hpp"
#include <opencv2/ximgproc/disparity_filter.hpp>



// PCL 库
//#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>


// 定义点云类型
// 定义点云使用的格式：这里用的是XYZRGB
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;


using namespace std;
using namespace cv;

在点云处理程序汇总，需要进行基本的图像处理，使用了Opencv库；

点云的定义、处理和生成，调用了PCL库；

还有C++的基本库；其中PCL库和Opencv库需要自行安装，以下为参考链接：

Win10 系统下 VisualStudio2019 配置点云库 PCL1.11.1

OpenCV安装教程：Windows 安装 Visual Studio + OpenCV + OpenCV contrib

2.图像读取：

        string path_rgb = "./rgb";
        int    imagenum ;
        imagenum = j;
        string path_rgb_imgtest = path_rgb + "/" + to_string(imagenum) + "_rgb.png";
        string path_depth = "./depth";
        string path_depth_imgtest = path_depth + "/" + to_string(imagenum) + "_depth.png";

        rgb = cv::imread(path_rgb_imgtest);
        depth = cv::imread(path_depth_imgtest, IMREAD_UNCHANGED);

其中深度图与RGB图片所在文件夹，图像帧名称需要根据本地情况进行修改；

深度图读取过程中需要使用IMREAD_UNCHANGED参数，否则会将16位深度数据强制转换；进行读取错误的深度图值，造成后续点云生成错误

3.点云生成：

// 遍历深度图图像，获取每个像素点的深度图
    for (int v = 0; v < rgb.rows; v++)
    {
        for (int u = 0; u < rgb.cols; u++)
        {

            unsigned int d = depth.ptr<unsigned short>(v)[u];    

            if (d == 0)            //坏点滤除：深度为0的点
                continue;

            //点云的三维坐标计算
            PointT p;
            p.z = double(d) / depthScale;   
            p.x = (u - K_cx) * p.z / K_fx;
            p.y = (v - K_cy) * p.z / K_fy;
            //点云点BGR颜色通道
            p.b = rgb.data[v * rgb.step + u * rgb.channels()];
            p.g = rgb.data[v * rgb.step + u * rgb.channels() + 1];
            p.r = rgb.data[v * rgb.step + u * rgb.channels() + 2];
            //点云保存
            __pointCloud->points.push_back(p);

首先根据深度图获取对应像素点的深度值，后根据RBG图中像素点位置获取像素点的2D坐标，根据投影关系，结合相机内参参数将2D像素点三维化，获取点云点的三维坐标，随后保存三维的点云点。

由此遍历图像像素点，将所有2D像素点转换为三维点云点，完成局部点云构建。

以为为实践效果：

RGB图：

深度图：

局部点云：

 以上便完成了基于深度图与RGB图的局部点云生成！