Skip to content
Snippets Groups Projects
Commit 9de14670 authored by Víctor Sainz Ubide's avatar Víctor Sainz Ubide
Browse files

Downsampling moved from constructor of capture to preProcess()

parent 6afb6266
No related branches found
No related tags found
1 merge request!41Draft: Resolve "New branch laser 3d"
......@@ -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
......
......@@ -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
{
......
......@@ -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() {}
......
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment