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; } }