#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/centroid.h>
#include <pcl/common/distances.h>
#include <pcl/common/eigen.h>
#include <pcl/common/gaussian.h>
#include <pcl/common/transforms.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
for (int j = 0; j < depth.rows; ++j)
{
for (int i = 0; i < depth.cols; ++i)
{
auto d = depth.at<cv::Vec3f>(j, i)[2];
if (d == INFINITY || d == 0 || d > 5)
continue;
PointXYZRGB p;
p.z = -d*1000; // Zc = baseline * f / (d + doffs)
p.x = depth.at<Vec3f>(j, i)[0] * 1000; //XC to the right, Yc down as positive
p.y = -depth.at<Vec3f>(j, i)[1] * 1000;
//RGB
p.b = src_RGB.ptr<uchar>(j)[i * 3];
p.g = src_RGB.ptr<uchar>(j)[i * 3 + 1];
p.r = src_RGB.ptr<uchar>(j)[i * 3 + 2];
cloud->points.push_back(p);
}
}
cloud->height = depth.rows;
cloud->width = depth.cols;
cloud->points.resize(cloud->height * cloud->width);
pcl::io::savePLYFile("test1.ply", *cloud);
pcl::io::savePCDFile("../image/cloud.pcd", *cloud);