RGB图和Depth图生成点云图原理与代码实现(realsense D435 )

1、首先,看看四个坐标系之间的转换关系,就明白了 :

dx dy 分别表示u轴和v轴方向上每个像素的物理尺寸(图中说法不正确)

RGB图和Depth图生成点云图原理与代码实现(realsense D435 )_第1张图片

 

RGB图和Depth图生成点云图原理与代码实现(realsense D435 )_第2张图片

RGB图和Depth图生成点云图原理与代码实现(realsense D435 )_第3张图片

 RGB图和Depth图生成点云图原理与代码实现(realsense D435 )_第4张图片

RGB图和Depth图生成点云图原理与代码实现(realsense D435 )_第5张图片

2、代码如下,可直接运行: 


#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;
// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud PointCloud;

// 相机内参
const double camera_factor = 1;
const double camera_cx = 649.402466;
const double camera_cy = 649.402466;
const double camera_fx = 640.685730;
const double camera_fy = 359.206085;
// 主函数

int main(int argc, char** argv)
{
    // 读取./data/rgb.png和./data/depth.png,并转化为点云

    // 图像矩阵
    cv::Mat rgb, depth;
    // 使用cv::imread()来读取图像
    // API: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread
    rgb = cv::imread("/home/xiang/picture/C1_Color.png");
    cout << "read rgb"<(m)[n];
        // d 可能没有值,若如此,跳过此点
        if (d == 0)
            continue;
        // d 存在值,则向点云增加一个点
        PointT p;

        // 计算这个点的空间坐标
        p.z = double(d) / camera_factor;
        p.x = (n - camera_cx) * p.z / camera_fx;
        p.y = (m - camera_cy) * p.z / camera_fy;

        // 从rgb图像中获取它的颜色
        // rgb是三通道的BGR格式图,所以按下面的顺序获取颜色
        p.b = rgb.ptr(m)[n * 3];
        p.g = rgb.ptr(m)[n * 3 + 1];
        p.r = rgb.ptr(m)[n * 3 + 2];

        // 把p加入到点云中
        cloud->points.push_back(p);
        //cout << cloud->points.size() << endl;
        }
    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cout << "point cloud size = " << cloud->points.size() << endl;
    cloud->is_dense = false;
    try{
        //保存点云图
        pcl::io::savePCDFile("/home/xiang/picture/pcd.pcd", *cloud);


    }
    catch (pcl::IOException &e){
        cout << e.what()<< endl;
    }
    //显示点云图
    pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");//直接创造一个显示窗口
    viewer.showCloud(cloud);//再这个窗口显示点云
    while (!viewer.wasStopped())
    {
    }

    //pcl::io::savePCDFileASCII("E:\\Visual Studio2013\\projectpointcloud.pcd", *cloud);
    // 清除数据并退出
    cloud->points.clear();
    cout << "Point cloud saved." << endl;
    return 0;
}

参考:

1、https://blog.csdn.net/qq_28448117/article/details/79526431

2、https://blog.csdn.net/blademan1234/article/details/79796901

你可能感兴趣的:(RGB图和Depth图生成点云图原理与代码实现(realsense D435 ))