diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp index bae57adfa493d68393e09a8a2bc9a46787b4dc02..15a388fb2674b3cb82683ab0cda336c252d1afe1 100644 --- a/src/processor/processor_odom_icp_3d.cpp +++ b/src/processor/processor_odom_icp_3d.cpp @@ -91,14 +91,24 @@ void ProcessorOdomIcp3d::preProcess() incoming_laser_ = std::static_pointer_cast<CaptureLaser3d>(incoming_ptr_); if (params_odom_icp_->pcl_downsample) { - double pcl_voxel_size = params_odom_icp_->pcl_downsample_voxel_size; - pcl::VoxelGrid<pcl::PointXYZ> grid; + // DURATION -------------------------------------------------------- + auto start = std::chrono::high_resolution_clock::now(); + // DURATION -------------------------------------------------------- + + double pcl_voxel_size = params_odom_icp_->pcl_downsample_voxel_size; + pcl::VoxelGrid<pcl::PointXYZ> grid; grid.setLeafSize(pcl_voxel_size, pcl_voxel_size, pcl_voxel_size); grid.setInputCloud(incoming_laser_->getPCLRaw()); - pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); - grid.filter(*pcl_ptr); + pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); + grid.filter(*pcl_ptr); incoming_laser_->setPCLDownsampled(pcl_ptr); + + // DURATION -------------------------------------------------------- + auto duration = + std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start); + WOLF_INFO("ProcessorOdomIcp3d.h: downsample: duration: ", 1e-3 * (duration).count(), " ms"); + // DURATION -------------------------------------------------------- } else {