diff --git a/include/laser/capture/capture_laser_3d.h b/include/laser/capture/capture_laser_3d.h
index b7a0b1cef35a8cc5900cc3cdbc241f16136ac9b2..e6fae887f95e10c1bfc21d6fa6ddb394bf7496a2 100644
--- a/include/laser/capture/capture_laser_3d.h
+++ b/include/laser/capture/capture_laser_3d.h
@@ -43,11 +43,11 @@ class CaptureLaser3d : public CaptureBase
     pcl::PointCloud<pcl::PointXYZ>::ConstPtr getPCLDownsampled() const;
     pcl::PointCloud<pcl::PointXYZ>::Ptr getPCLRaw();
     pcl::PointCloud<pcl::PointXYZ>::ConstPtr getPCLRaw() const;
-  
-    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_;
+    void setPCLDownsampled(pcl::PointCloud<pcl::PointXYZ>::Ptr _pcl_ptr);
     
   private:
     pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_raw_;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_;
 };
 
 }  // namespace wolf
diff --git a/src/capture/capture_laser_3d.cpp b/src/capture/capture_laser_3d.cpp
index 5c9ed0cc205b7d2aed801f364cae150556193a24..234713ba6f650b5ea374618287dc5e132c5292e9 100644
--- a/src/capture/capture_laser_3d.cpp
+++ b/src/capture/capture_laser_3d.cpp
@@ -29,11 +29,16 @@ CaptureLaser3d::CaptureLaser3d(const TimeStamp&                    _timestamp,
                                pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud_raw)
     : CaptureBase("CaptureLaser3d", _timestamp, _sensor), point_cloud_raw_(_point_cloud_raw)
 {
-
+    point_cloud_ = nullptr;
 }
 
 CaptureLaser3d::~CaptureLaser3d() {}
 
+void CaptureLaser3d::setPCLDownsampled(pcl::PointCloud<pcl::PointXYZ>::Ptr _pcl_ptr)
+{
+    point_cloud_ = _pcl_ptr;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr CaptureLaser3d::getPCLDownsampled()
 {
     return point_cloud_;
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index e3d606c9f039b38c54daeca9d0a6806c8a44f5f6..bae57adfa493d68393e09a8a2bc9a46787b4dc02 100644
--- a/src/processor/processor_odom_icp_3d.cpp
+++ b/src/processor/processor_odom_icp_3d.cpp
@@ -95,11 +95,14 @@ void ProcessorOdomIcp3d::preProcess()
         pcl::VoxelGrid<pcl::PointXYZ> grid; 
         grid.setLeafSize(pcl_voxel_size, pcl_voxel_size, pcl_voxel_size);
         grid.setInputCloud(incoming_laser_->getPCLRaw());
-        grid.filter(* incoming_laser_->point_cloud_);    // public member of class, better with setter
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr   = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+        grid.filter(*pcl_ptr); 
+        incoming_laser_->setPCLDownsampled(pcl_ptr);
     }
     else
     {
-        incoming_laser_->point_cloud_ = incoming_laser_->getPCLRaw();
+        incoming_laser_->setPCLDownsampled(incoming_laser_->getPCLRaw());
     }
 }