포인트 클라우드 voxelization

rosbag으로 라이다 데이터를 받아서, voxelization을 해보았다.

rosbag에서 sensor_msgs/PointCloud2 데이터를 받아서 처리해 PCL 홈페이지의 예제와는 데이터 형 처리하는데서 달라서 참고자료를 더 찾아서 코드를 짜 보았다.

윗 부분에는 데이터의 xyz 방향이 반대가 되어있어서 돌리는 부분도 추가하였다.

#include <iostream>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <string>
#include <pcl/visualization/pcl_visualizer.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>

ros::Publisher vox_pub;

void voxel_callback(const sensor_msgs::PointCloud2ConstPtr& scan_msgs){
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::fromROSMsg(*scan_msgs, *cloud_in);

    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    transform_2.rotate(Eigen::AngleAxisf (M_PI, Eigen::Vector3f::UnitZ()));
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_turn(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::transformPointCloud(*cloud_in, *cloud_turn, transform_2);

    pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
    pcl::PointCloud<pcl::PointXYZI>::Ptr voxel_pc(new pcl::PointCloud<pcl::PointXYZI>());

    double voxelsize = 0.5;

    voxel_filter.setInputCloud(cloud_turn);
    voxel_filter.setLeafSize(voxelsize,voxelsize,voxelsize);
    voxel_filter.filter(*voxel_pc);

    sensor_msgs::PointCloud2 cloud_out;
    pcl::toROSMsg(*voxel_pc, cloud_out);

    cloud_out.header.frame_id = "os1_lidar";
    vox_pub.publish(cloud_out);    
}

int main(int argc, char** argv){
    ros::init(argc, argv, "main");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/lidar_data",1, voxel_callback);
    vox_pub = nh.advertise<sensor_msgs::PointCloud2>("/voxel_lidar_data", 1);
    ros::spin();
    return 0;
}

끝