diff --git a/include/laser/capture/capture_laser_3d.h b/include/laser/capture/capture_laser_3d.h
index ecab3a7bba17d5540cede32e56bce09290e1369c..dc3b0457bb660b06214fa7e7238f27b6f56c587e 100644
--- a/include/laser/capture/capture_laser_3d.h
+++ b/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
diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h
index 9cd130a8dd2151fcd0fc6dbb0f942c5482b7f0bb..ecb8d369014ed43ffa42550ae88502d32657269a 100644
--- a/include/laser/processor/processor_odom_icp_3d.h
+++ b/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
 {
diff --git a/src/capture/capture_laser_3d.cpp b/src/capture/capture_laser_3d.cpp
index c33518b728bb2dfc6bfbb268346ef365289eadcc..42bcf015e60785fe980a30db8138140493991296 100644
--- a/src/capture/capture_laser_3d.cpp
+++ b/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() {}
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index eeede85b97e942a70e357671797188ac5fa010d9..6f6c4e7a025e8315f821e18d8952164c2aba8580 100644
--- a/src/processor/processor_odom_icp_3d.cpp
+++ b/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