From 9de1467039f84787de397bdfdfa6291f8e285eca Mon Sep 17 00:00:00 2001
From: vsainz <vsainz@iri.upc.edu>
Date: Wed, 10 Aug 2022 17:12:46 +0200
Subject: [PATCH] Downsampling moved from constructor of capture to
preProcess()
---
include/laser/capture/capture_laser_3d.h | 7 +++----
include/laser/processor/processor_odom_icp_3d.h | 1 +
src/capture/capture_laser_3d.cpp | 16 ++--------------
src/processor/processor_odom_icp_3d.cpp | 12 ++++++++++++
4 files changed, 18 insertions(+), 18 deletions(-)
diff --git a/include/laser/capture/capture_laser_3d.h b/include/laser/capture/capture_laser_3d.h
index ecab3a7bb..dc3b0457b 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 9cd130a8d..ecb8d3690 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 c33518b72..42bcf015e 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 eeede85b9..6f6c4e7a0 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
--
GitLab