Add duration of downsampling

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
     {