PCL+OpenCV+WebCam实现模拟深度图像实时点云数据采集

#include <iostream>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace std;

pcl::PointCloud<pcl::PointXYZ>::Ptr MatToPoinXYZ(cv::Mat depthMat)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr ptCloud (new pcl::PointCloud<pcl::PointXYZ>);

// calibration parameters
float const fx_d = 5.9421434211923247e+02;
float const fy_d = 5.9104053696870778e+02;
float const cx_d = 3.3930780975300314e+02;
float const cy_d = 2.4273913761751615e+02;

unsigned char* p = depthMat.data;
for (int i = 0; i<depthMat.rows; i++)
{
for (int j = 0; j < depthMat.cols; j++)
{
float z = static_cast<float>(*p);
pcl::PointXYZ point;
point.z = 0.001 * z;
point.x = point.z*(j - cx_d)  / fx_d;
point.y = point.z *(cy_d - i) / fy_d;
ptCloud->points.push_back(point);
++p;
}
}
ptCloud->width = (int)depthMat.cols;
ptCloud->height = (int)depthMat.rows;

return ptCloud;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr MatToPoinXYZ2(cv::Mat OpencVPointCloud)
{

//char pr=100, pg=100, pb=100;
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);//(new pcl::pointcloud<pcl::pointXYZ>);

for(int i=0;i<OpencVPointCloud.cols;i++)
{
//std::cout<<i<<endl;

pcl::PointXYZ point;
point.x = OpencVPointCloud.at<float>(0,i);
point.y = OpencVPointCloud.at<float>(1,i);
point.z = OpencVPointCloud.at<float>(2,i);
point_cloud_ptr -> points.push_back(point);

}
point_cloud_ptr->width = (int)point_cloud_ptr->points.size();
point_cloud_ptr->height = 1;

return point_cloud_ptr;

}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr MatToPoinXYZRGB(cv::Mat depthMat)
{

pcl::PointCloud<pcl::PointXYZRGB>::Ptr ptCloud (new pcl::PointCloud<pcl::PointXYZRGB>);

// calibration parameters
float const fx_d = 5.9421434211923247e+02;
float const fy_d = 5.9104053696870778e+02;
float const cx_d = 3.3930780975300314e+02;
float const cy_d = 2.4273913761751615e+02;

unsigned char* p = depthMat.data;
for (int i = 0; i<depthMat.rows; i++)
{

for (int j = 0; j < depthMat.cols; j++)
{
float z = static_cast<float>(*p);
pcl::PointXYZRGB point;
point.z = 0.001 * z;
point.x = point.z*(j - cx_d)  / fx_d;
point.y = point.z *(cy_d - i) / fy_d;
point.r = depthMat.at<cv::Vec3b>(i,j)[2];
point.g = depthMat.at<cv::Vec3b>(i,j)[1];
point.b = depthMat.at<cv::Vec3b>(i,j)[0];
ptCloud->points.push_back(point);
++p;
}
}
ptCloud->width = (int)depthMat.cols;
ptCloud->height = (int)depthMat.rows;

return ptCloud;
}

int main()
{
pcl::visualization::CloudViewer viewer("Cloud Viewer");//创建viewer对象
cv::VideoCapture cap(0);
if(!cap.isOpened()){
std::cerr << "cannot open camera" << std::endl;
exit(0);
}
cv::Mat frame;

while (!viewer.wasStopped ())
{
cap >> frame;
if(frame.empty()){
continue;
}
viewer.showCloud(MatToPoinXYZRGB(frame));
}
return 0;
}