Downsampling moved from constructor of capture to preProcess()

include/laser/capture/capture_laser_3d.h

@@ -28,7 +28,6 @@
 // PCL includes
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
-#include <pcl/filters/voxel_grid.h>
 
 namespace wolf
 {
@@ -38,17 +37,17 @@ WOLF_PTR_TYPEDEFS(CaptureLaser3d);
 class CaptureLaser3d : public CaptureBase
 {
   public:
-    CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud_raw, bool _downsample, double _voxel_size);
+    CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud_raw);//, bool _downsample, double _voxel_size);
     ~CaptureLaser3d();
     pcl::PointCloud<pcl::PointXYZ>::Ptr getPointCloud();
     pcl::PointCloud<pcl::PointXYZ>::ConstPtr getPointCloud() const;
     pcl::PointCloud<pcl::PointXYZ>::Ptr getPointCloudRaw();
     pcl::PointCloud<pcl::PointXYZ>::ConstPtr getPointCloudRaw() const;
-
+  
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_;
     
   private:
     pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_raw_;
-    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_;
 };
 
 }  // namespace wolf

include/laser/processor/processor_odom_icp_3d.h

@@ -41,6 +41,7 @@
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/registration/registration.h>
+#include <pcl/filters/voxel_grid.h>
 
 namespace wolf
 {

src/capture/capture_laser_3d.cpp

@@ -26,22 +26,10 @@ namespace wolf
 
 CaptureLaser3d::CaptureLaser3d(const TimeStamp&                    _timestamp,
                                SensorBasePtr                       _sensor,
-                               pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud_raw,
-                               bool                                _downsample = false,
-                               double                              _voxel_size = 0.05)
+                               pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud_raw)
     : CaptureBase("CaptureLaser3d", _timestamp, _sensor), point_cloud_raw_(_point_cloud_raw)
 {
-    if (_downsample)
-    {
-        pcl::VoxelGrid<pcl::PointXYZ> grid; 
-        grid.setLeafSize(_voxel_size, _voxel_size, _voxel_size);
-        grid.setInputCloud(_point_cloud_raw);
-        grid.filter(*point_cloud_);
-    }
-    else
-    {
-        *point_cloud_ = *_point_cloud_raw;
-    }
+
 }
 
 CaptureLaser3d::~CaptureLaser3d() {}

src/processor/processor_odom_icp_3d.cpp

@@ -89,6 +89,18 @@ void ProcessorOdomIcp3d::configure(SensorBasePtr _sensor)
 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; 
+        grid.setLeafSize(pcl_voxel_size, pcl_voxel_size, pcl_voxel_size);
+        grid.setInputCloud(incoming_laser_->getPointCloudRaw());
+        grid.filter(* incoming_laser_->point_cloud_);    // public member of class, better with setter
+    }
+    else
+    {
+        incoming_laser_->point_cloud_ = incoming_laser_->getPointCloudRaw();
+    }
 }
 
 /** \brief Tracker function