From c4949f3c2f7c0dbf1ece040bed4a0b2ccc659f11 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 10 Aug 2022 21:19:57 +0200
Subject: [PATCH] Add duration of downsampling
---
src/processor/processor_odom_icp_3d.cpp | 18 ++++++++++++++----
1 file changed, 14 insertions(+), 4 deletions(-)
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index bae57adfa..15a388fb2 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
{
--
GitLab