彩色图片和深度图片生成点云文件

// C++ 标准库
#include 
#include 
using namespace std;

// OpenCV 库
#include 
#include 

// PCL 库
#include 
#include 
#include 

// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud PointCloud; 

// 相机内参
const double camera_factor = 1000;
const double camera_cx = 979.674;
const double camera_cy = 535.383;
const double camera_fx = 1043.02;
const double camera_fy = 1047.78;

// 主函数 
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（ “0_Color.png“ ）;
    // rgb 图像是8UC3的彩色图像
    // depth 是16UC1的单通道图像，注意flags设置-1表示读取原始数据不做任何修改
    depth = cv::imread（ “0_Depth.png“ -1 ）;

    // 点云变量
    // 使用智能指针，创建一个空点云。这种指针用完会自动释放。
    PointCloud::Ptr cloud （ new PointCloud ）;
    // 遍历深度图
    for （int m = 0; m < depth.rows; m++）
        for （int n=0; n < depth.cols; n++）
        {
            // 获取深度图中（mn）处的值
            ushort d = depth.ptr（m）[n];
            // d 可能没有值，若如此，跳过此点
            // if （d == 0）
            //     continue;
            // d 存在值，则向点云增加一个点
            PointT p;

            // 计算这个点的空间坐标
            p.z = double（d） / camera_factor;
             // if（p.z>1）
             //    continue;
            p.x = （n - camera_cx） * p.z / camera_fx;
             // if（p.x<-0.15||p.x>0.15）
             //    continue;
            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 ）;
        }
    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size（）;
    cout<<“point cloud size = “<points.size（）<    cloud->is_dense = false;
    pcl::io::savePCDFile（ “./0_image.pcd“ *cloud ）;
    pcl::io::savePLYFile（ “./0_image.ply“ *cloud ）;
    // 清除数据并退出
    cloud->points.clear（）;
    cout<<“Point cloud saved.“<    return 0;
}