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