发表文章

[最新] 根据视差图发布点云

misandao2014 4月前 1

Mat xyz;
reprojectImageTo3D(disp, xyz, Q, true);
xyz = xyz*16;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
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<Vec3f>(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<pcl::PCLPointCloud2> 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);
相关推荐
最新评论 (0)
返回
发表文章
misandao2014
文章数
17
评论数
0
注册排名
1309170