[Point cloud processing] Generate point cloud using 16-bit depth map and RGB image frame - C language/C++

After previously learning and practicing using Intel Realsense d435 to shoot RGB videos and depth map videos, I obtained continuous depth map and RGB color map image frames through image processing. Now I generate a local point cloud of the practical scene based on the depth map and color map. The following is the overall code:

// 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;
}

Code analysis:

1.Library reference:

// 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;

In the point cloud processing program summary, basic image processing is required, and the Opencv library is used;

The definition, processing and generation of point clouds call the PCL library;

There are also basic libraries of C++; among them, the PCL library and Opencv library need to be installed by yourself. The following are reference links:

VisualStudio2019 configures point cloud library PCL1.11.1 under Win10 system

OpenCV installation tutorial: Windows installation Visual Studio + OpenCV + OpenCV contrib

2. Image reading:

        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);

The folder where the depth map and RGB image are located, the image frame name needs to be modified according to local conditions;

The IMREAD_UNCHANGED parameter needs to be used during the depth map reading process, otherwise the 16-bit depth data will be forced to convert; reading the wrong depth map value will cause subsequent point cloud generation errors.

3. Point cloud generation:

// 遍历深度图图像，获取每个像素点的深度图
    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);

First, the depth value of the corresponding pixel is obtained based on the depth map, and then the 2D coordinates of the pixel are obtained based on the pixel position in the RBG map. According to the projection relationship, the 2D pixel is three-dimensionalized based on the camera internal parameters to obtain the three-dimensional coordinates of the point cloud point. The three-dimensional point cloud points are then saved.

This traverses the image pixels, converts all 2D pixels into three-dimensional point cloud points, and completes the construction of local point clouds.

Considered practical results:

RGB picture:

Depth map:

Local point cloud:

 The above completes the local point cloud generation based on the depth map and RGB map!