static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
pcl::PointCloud<pcl::PointXYZI> scan;
pcl::fromROSMsg(*input, scan);
if(measurement_range != MAX_MEASUREMENT_RANGE){
scan = removePointsByRange(scan, 0, measurement_range);
}
pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan));
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
sensor_msgs::PointCloud2 filtered_msg;
filter_start = std::chrono::system_clock::now();
// if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)
if (voxel_leaf_size >= 0.1)
{
// Downsampling the velodyne scan using VoxelGrid filter
pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;
voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
voxel_grid_filter.setInputCloud(scan_ptr);
voxel_grid_filter.filter(*filtered_scan_ptr);
pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);
}
else
{
pcl::toROSMsg(*scan_ptr, filtered_msg);
}
filter_end = std::chrono::system_clock::now();
filtered_msg.header = input->header;
filtered_points_pub.publish(filtered_msg);
points_downsampler_info_msg.header = input->header;
points_downsampler_info_msg.filter_name = "voxel_grid_filter";
points_downsampler_info_msg.measurement_range = measurement_range;
points_downsampler_info_msg.original_points_size = scan.size();
if (voxel_leaf_size >= 0.1)
{
points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size();
}
else
{
points_downsampler_info_msg.filtered_points_size = scan_ptr->size();
}
points_downsampler_info_msg.original_ring_size = 0;
points_downsampler_info_msg.filtered_ring_size = 0;
points_downsampler_info_msg.exe_time = std::chrono::duration_cast<std::chrono::microseconds>(filter_end - filter_start).count() / 1000.0;
points_downsampler_info_pub.publish(points_downsampler_info_msg);
if(_output_log == true){
if(!ofs){
std::cerr << "Could not open " << filename << "." << std::endl;
exit(1);
}
ofs << points_downsampler_info_msg.header.seq << ","
<< points_downsampler_info_msg.header.stamp << ","
<< points_downsampler_info_msg.header.frame_id << ","
<< points_downsampler_info_msg.filter_name << ","
<< points_downsampler_info_msg.original_points_size << ","
<< points_downsampler_info_msg.filtered_points_size << ","
<< points_downsampler_info_msg.original_ring_size << ","
<< points_downsampler_info_msg.filtered_ring_size << ","
<< points_downsampler_info_msg.exe_time << ","
<< std::endl;
}
}
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
pcl::PointXYZI sampled_p;
pcl::PointCloud<pcl::PointXYZI> scan;
pcl::fromROSMsg(*input, scan);
if(measurement_range != MAX_MEASUREMENT_RANGE){
scan = removePointsByRange(scan, 0, measurement_range);
}
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
filtered_scan_ptr->header = scan.header;
int points_num = scan.size();
double w_total = 0.0;
double w_step = 0.0;
int m = 0;
double c = 0.0;
filter_start = std::chrono::system_clock::now();
for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin(); item != scan.end(); item++)
{
w_total += item->x * item->x + item->y * item->y + item->z * item->z;
}
w_step = w_total / sample_num;
pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin();
for (m = 0; m < sample_num; m++)
{
while (m * w_step > c)
{
item++;
c += item->x * item->x + item->y * item->y + item->z * item->z;
}
sampled_p.x = item->x;
sampled_p.y = item->y;
sampled_p.z = item->z;
sampled_p.intensity = item->intensity;
filtered_scan_ptr->points.push_back(sampled_p);
}
sensor_msgs::PointCloud2 filtered_msg;
pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);
filter_end = std::chrono::system_clock::now();
filtered_msg.header = input->header;
filtered_points_pub.publish(filtered_msg);
points_downsampler_info_msg.header = input->header;
points_downsampler_info_msg.filter_name = "distance_filter";
points_downsampler_info_msg.measurement_range = measurement_range;
points_downsampler_info_msg.original_points_size = points_num;
points_downsampler_info_msg.filtered_points_size = std::min((int)filtered_scan_ptr->size(), points_num);
points_downsampler_info_msg.original_ring_size = 0;
points_downsampler_info_msg.original_ring_size = 0;
points_downsampler_info_msg.exe_time = std::chrono::duration_cast<std::chrono::microseconds>(filter_end - filter_start).count() / 1000.0;
points_downsampler_info_pub.publish(points_downsampler_info_msg);
if(_output_log == true){
if(!ofs){
std::cerr << "Could not open " << filename << "." << std::endl;
exit(1);
}
ofs << points_downsampler_info_msg.header.seq << ","
<< points_downsampler_info_msg.header.stamp << ","
<< points_downsampler_info_msg.header.frame_id << ","
<< points_downsampler_info_msg.filter_name << ","
<< points_downsampler_info_msg.original_points_size << ","
<< points_downsampler_info_msg.filtered_points_size << ","
<< points_downsampler_info_msg.original_ring_size << ","
<< points_downsampler_info_msg.filtered_ring_size << ","
<< points_downsampler_info_msg.exe_time << ","
<< std::endl;
}
}
