From 0f68e59b2c4ec63631555dec7f758f9c3c512ec1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Wed, 10 Aug 2022 11:26:10 +0200 Subject: [PATCH] Add timing debug info to pairAlign() --- include/laser/utils/laser3d_tools.h | 33 +++++++++++++++++++++++------ 1 file changed, 27 insertions(+), 6 deletions(-) diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h index 74a622219..5c3acc5e0 100644 --- a/include/laser/utils/laser3d_tools.h +++ b/include/laser/utils/laser3d_tools.h @@ -23,6 +23,8 @@ #ifndef LASER3D_TOOLS_H #define LASER3D_TOOLS_H +#include <core/utils/logging.h> + #include <pcl/point_types.h> #include <pcl/point_cloud.h> #include <pcl/common/transforms.h> @@ -34,11 +36,6 @@ #include <pcl/registration/icp_nl.h> #include <pcl/registration/transforms.h> -// using PointCloud = pcl::PointCloud<pcl::PointXYZ>; -// typedef boost::shared_ptr<PointCloud> PointCloudBoostPtr; -// typedef boost::shared_ptr<const PointCloud> PointCloudBoostConstPtr; - -// typedef std::shared_ptr<pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>> RegistrationPtr; typedef pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr RegistrationPtr; @@ -55,6 +52,13 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref, bool _downsample = false, double _voxel_size = 0.05) { + + // DURATION -------------------------------------------------------- + auto start = std::chrono::high_resolution_clock::now(); + // DURATION -------------------------------------------------------- + + + // Internal PointCloud pointers (boost::shared_ptr) pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_other = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); @@ -82,6 +86,12 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref, _registration_solver->setInputSource(cloud_ref); _registration_solver->setInputTarget(cloud_other); + // DURATION -------------------------------------------------------- + auto end_downsampling = std::chrono::high_resolution_clock::now(); + auto start_aligning = std::chrono::high_resolution_clock::now(); + // DURATION -------------------------------------------------------- + + // Declare variables Eigen::Matrix4f transform_pcl; @@ -95,7 +105,18 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref, // Output the transformation as Isometry of double(s) _transform_final = Eigen::Isometry3d(transform_pcl.cast<double>()); - // _transform_final = _transform_final.inverse(); // Maybe we messed up + + // DURATION -------------------------------------------------------- + auto end = std::chrono::high_resolution_clock::now(); + + auto duration_downsampling = std::chrono::duration_cast<std::chrono::microseconds>(end_downsampling - start); + auto duration_align = std::chrono::duration_cast<std::chrono::microseconds>(end - start_aligning); + auto duration_total = std::chrono::duration_cast<std::chrono::microseconds>(end - start); + + WOLF_INFO("Laser3d_tools.h: Duration downsampling: ", 1e-3*(duration_downsampling).count(), " ms"); + WOLF_INFO("Laser3d_tools.h: Duration aligning : ", 1e-3*(duration_align).count(), " ms"); + WOLF_INFO("Laser3d_tools.h: Duration total : ", 1e-3*(duration_total).count(), " ms"); + // DURATION -------------------------------------------------------- } } // namespace wolf -- GitLab