moved PCLdownsample and renamed variables

include/laser/utils/laser3d_tools.h

@@ -23,19 +23,29 @@
 #ifndef LASER3D_TOOLS_H
 #define LASER3D_TOOLS_H
 
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "laser/capture/capture_laser_3d.h"
+#include "laser/sensor/sensor_laser_3d.h"
+
 #include <core/utils/logging.h>
 
+/**************************
+ *      PCL includes      *
+ **************************/
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/common/transforms.h>
 #include <pcl/io/pcd_io.h>
-
 #include <pcl/filters/voxel_grid.h>
-
 #include <pcl/registration/icp.h>
 #include <pcl/registration/icp_nl.h>
 #include <pcl/registration/transforms.h>
 
+/**************************
+ *    Standard includes   *
+ **************************/
 #include <iostream>
 #include <fstream>
 
@@ -43,21 +53,19 @@ typedef pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr RegistrationPtr;
 
 namespace wolf
 {
-void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ> &cloud)
+void loadData(std::string _fname, pcl::PointCloud<pcl::PointXYZ> &_cloud)
 {
-    //std::string s_pcd = ".pcd", s_bin = ".bin";
-    if (fname.compare(fname.size() - 4, 4, ".pcd") == 0)
+    if (_fname.compare(_fname.size() - 4, 4, ".pcd") == 0)
     {
-        pcl::io::loadPCDFile(fname, cloud);
-
+        pcl::io::loadPCDFile(_fname, _cloud);
     }
-    else if (fname.compare(fname.size() - 4, 4, ".bin") == 0)
+    else if (_fname.compare(_fname.size() - 4, 4, ".bin") == 0)
     {  // file is binary
-        //pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+        // pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 
         std::ifstream stream_input;
-    
-        stream_input.open(fname, std::ios::in);
+
+        stream_input.open(_fname, std::ios::in);
         for (int i = 0; stream_input.good() && !stream_input.eof(); i++)
         {
             pcl::PointXYZI point_raw;
@@ -70,16 +78,32 @@ void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ> &cloud)
             tPoint.y = point_raw.y;
             tPoint.z = point_raw.z;
 
-            cloud.push_back(tPoint);
+            _cloud.push_back(tPoint);
         }
         stream_input.close();
-        
+
         // remove NAN points from the cloud
         pcl::Indices indices;
-        pcl::removeNaNFromPointCloud(cloud, cloud, indices);
+        pcl::removeNaNFromPointCloud(_cloud, _cloud, indices);
     }
 };
+void PCLdownsample(CaptureLaser3dPtr _capture_laser, bool _pcl_downsample = true, double _pcl_voxel_size = 0.05)
+{
+    if (_pcl_downsample)
+    {
+        pcl::VoxelGrid<pcl::PointXYZ> grid;
+        grid.setLeafSize(_pcl_voxel_size, _pcl_voxel_size, _pcl_voxel_size);
+        grid.setInputCloud(_capture_laser->getPCLRaw());
 
+        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+        grid.filter(*pcl_ptr);
+        _capture_laser->setPCLDownsampled(pcl_ptr);
+    }
+    else
+    {
+        _capture_laser->setPCLDownsampled(_capture_laser->getPCLRaw());
+    }
+}
 // _cloud_ref: first PointCloud
 // _cloud_other: second PointCloud
 inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,

src/processor/processor_odom_icp_3d.cpp

@@ -89,31 +89,9 @@ void ProcessorOdomIcp3d::configure(SensorBasePtr _sensor)
 void ProcessorOdomIcp3d::preProcess()
 {
     incoming_laser_ = std::static_pointer_cast<CaptureLaser3d>(incoming_ptr_);
-    if (params_odom_icp_->pcl_downsample)
-    {
-        // 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);
-        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
-    {
-        incoming_laser_->setPCLDownsampled(incoming_laser_->getPCLRaw());
-    }
+    double   pcl_voxel_size = params_odom_icp_->pcl_downsample_voxel_size;
+    bool     pcl_downsample = params_odom_icp_->pcl_downsample;
+    PCLdownsample(incoming_laser_, pcl_downsample, pcl_voxel_size);
 }
 
 /** \brief Tracker function