Add timing debug info to pairAlign()

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