根据视差图发布点云

Mat xyz;

reprojectImageTo3D(disp, xyz, Q, true);

xyz = xyz*16;

pcl::PointCloud::Ptr cloud(new pcl::PointCloud);

cloud->width  = 752;

cloud->height = 480;

cloud->points.resize(cloud->width * cloud->height);

for(int y = 0; y < xyz.rows; y++)

{

    for(int x = 0; x < xyz.cols; x++)

    {

        Vec3f point = xyz.at(y, x);

      if(fabs(point[2]) > 8 || fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z)

            continue;

        double c = point[2];

        double a = point[0];

        double b = point[1];

        cloud->points[x+y*xyz.cols].x = a;

        cloud->points[x+y*xyz.cols].y = b;

        cloud->points[x+y*xyz.cols].z = c;

     }

}

pcl::PCLPointCloud2::Ptr cloud2 (new pcl::PCLPointCloud2 ());

pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());

toPCLPointCloud2(*cloud, *cloud2);

std::cerr << "PointCloud before filtering: " << cloud2->width * cloud2->height

       << " data points (" << pcl::getFieldsList (*cloud2) << ")." << endl;;

pcl::VoxelGrid sor;  //创建滤波对象

sor.setInputCloud (cloud2);            //设置需要过滤的点云给滤波对象

sor.setLeafSize (0.05f, 0.05f, 0.05f);  //设置滤波时创建的体素体积为5cm的立方体sor.filter (*cloud_filtered);           //执行滤波处理，存储输出

std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height

       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << endl;

pcl_conversions::fromPCL(*cloud_filtered, point_cloud);

pcl::toROSMsg(*cloud, point_cloud);

point_cloud.header.frame_id = "depth_frame";

point_cloud.header.stamp = ros::Time::now();

pointcloud_pub.publish(point_cloud);