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