单幅RGB图像+Depth深度图得到点云模型示例

墨蓝 2022-12-11 15:07 237阅读 0赞

# 目标： #

*  **输入RGB图像 + Depth深度图像，输出单幅图像的点云模型pointcloud.pcd**

## **Main.cpp** ##

// C++ 标准库
    #include <iostream>
    #include <string>
    using namespace std;
    
    // OpenCV 库
    #include <opencv2/core/core.hpp>
    #include <opencv2/highgui/highgui.hpp>
    
    // PCL 库
    #include <pcl/io/pcd_io.h>
    #include <pcl/point_types.h>
    
    // 定义点云类型
    typedef pcl::PointXYZRGBA PointT;
    typedef pcl::PointCloud<PointT> PointCloud; 
    
    // 相机内参
    const double camera_factor = 1000;
    const double camera_cx = 325.5;
    const double camera_cy = 253.5;
    const double camera_fx = 518.0;
    const double camera_fy = 519.0;
    
    // 主函数 
    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( "./data/rgb.png" );
        // rgb 图像是8UC3的彩色图像
        // depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
        depth = cv::imread( "./data/depth.png", -1 );
    
        // 点云变量
        // 使用智能指针，创建一个空点云。这种指针用完会自动释放。
        PointCloud::Ptr cloud ( new PointCloud );
        // 遍历深度图
        for (int m = 0; m < depth.rows; m++)
            for (int n=0; n < depth.cols; n++)
            {
                // 获取深度图中(m,n)处的值
                ushort d = depth.ptr<ushort>(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<uchar>(m)[n*3];
                p.g = rgb.ptr<uchar>(m)[n*3+1];
                p.r = rgb.ptr<uchar>(m)[n*3+2];
    
                // 把p加入到点云中
                cloud->points.push_back( p );
            }
        // 设置并保存点云
        cloud->height = 1;
        cloud->width = cloud->points.size();
        cout<<"point cloud size = "<<cloud->points.size()<<endl;
        cloud->is_dense = false;
        pcl::io::savePCDFile( "./pointcloud.pcd", *cloud );
        // 清除数据并退出
        cloud->points.clear();
        cout<<"Point cloud saved."<<endl;
        return 0;
    }

## CMakelist.txt ##

CMAKE_MINIMUM_REQUIRED( VERSION 2.8 )
    PROJECT( slam )
    
    SET(CMAKE_CXX_COMPILER "g++")
    SET( CMAKE_BUILD_TYPE Debug  )
    SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
    SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
    
    INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include )
    LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib)
    
    ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src )

参考：[一起做RGB-D SLAM (2)][RGB-D SLAM _2]

[RGB-D SLAM _2]: https://www.cnblogs.com/gaoxiang12/p/4652478.html