PointCloudをVoxel Gridにダウンサンプリング

7786 ワード

point cloudをいくつかのvoxelgridに下にサンプリング
では、3 d voxel gridとは何でしょうか.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. The VoxelGrid class creates a 3D voxel grid (think about a voxel grid as a set of tiny 3D boxes in space) over the input point cloud data. Then, in each voxel (i.e., 3D box), all the points present will be approximated (i.e., downsampled) with their centroid. This approach is a bit slower than approximating them with the center of the voxel, but it represents the underlying surface more accurately.
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/filters/voxel_grid.h>

ros::Publisher pub;

void cloud_callBack(const sensor_msgs::PointCloud2ConstPtr& input)
{

    // container for original and filtered data
    pcl::PCLPointCloud2 * cloud = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); // pointer for dynamic cloud
    pcl::PCLPointCloud2 cloud_filtered;

    //conversion ROS sensor_msg/PointCloud2 -> pcl lib의 PointCloud2 type 
    pcl_conversions::toPCL(*input, *cloud);
    
    // pcl 의 VoxelGrid 타입
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloudPtr);
    sor.setLeafSize (0.1, 0.1, 0.1); // 샘플링 하는 방법 이거 너무 작게 하면 샘플링 에러 메세지 뜸 고것을 주의 하자
    sor.filter(cloud_filtered);

    sensor_msgs::PointCloud2 output;
    //pcl lib PointCLoud2 type -> ROS sensor_msgs data
    pcl_conversions::moveFromPCL(cloud_filtered, output);
    pub.publish(output);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_pcl_tutorial");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("input", 1, cloud_callBack);
    pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);

    ros::spin();
}
pclチュートリアルで使用されるロビーlidar.bagファイルの使用
rosrun pcl_cpp_tutorial example input:=/velodyne_points
// /velodyne_points 라는 토픽으로 publish 되는 것을 input 이라는 이름의 토픽으로 받겠다는 뜻
ダウンサンプリング前

ダウンサンプリング後

以下に示すように、点群をサンプリングしました.