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

Downsampling now at capture

parent 61b37ba2
No related branches found
No related tags found
1 merge request!41Draft: Resolve "New branch laser 3d"
......@@ -28,6 +28,7 @@
// PCL includes
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
namespace wolf
{
......@@ -37,13 +38,16 @@ WOLF_PTR_TYPEDEFS(CaptureLaser3d);
class CaptureLaser3d : public CaptureBase
{
public:
CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud);
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;
private:
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_raw_;
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_;
};
......
......@@ -36,10 +36,8 @@
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
typedef pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr RegistrationPtr;
namespace wolf
{
// _cloud_ref: first PointCloud
......@@ -48,39 +46,19 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_other,
const Eigen::Isometry3d &_transform_guess,
Eigen::Isometry3d &_transform_final,
RegistrationPtr &_registration_solver,
bool _downsample = false,
double _voxel_size = 0.05)
RegistrationPtr &_registration_solver)
{
// DURATION --------------------------------------------------------
auto start = std::chrono::high_resolution_clock::now();
// DURATION --------------------------------------------------------
// Internal PointCloud pointers (boost::shared_ptr)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_other = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
// Downsample for consistency and speed
// \note enable this for large datasets
pcl::VoxelGrid<pcl::PointXYZ> grid; // downsample for performance
if (_downsample)
{
grid.setLeafSize(_voxel_size, _voxel_size, _voxel_size);
// grid.setInputCloud(boost::static_pointer_cast<PointCloud>(_cloud_ref));
grid.setInputCloud(_cloud_ref);
grid.filter(*cloud_ref);
grid.setInputCloud(_cloud_other);
grid.filter(*cloud_other);
}
else
{
*cloud_ref = *_cloud_ref;
*cloud_other = *_cloud_other;
}
// Point clouds are considered already downsampled
*cloud_ref = *_cloud_ref;
*cloud_other = *_cloud_other;
// Set input clouds
_registration_solver->setInputSource(cloud_ref);
......@@ -91,7 +69,6 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
auto start_aligning = std::chrono::high_resolution_clock::now();
// DURATION --------------------------------------------------------
// Declare variables
Eigen::Matrix4f transform_pcl;
......@@ -105,7 +82,6 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
// Output the transformation as Isometry of double(s)
_transform_final = Eigen::Isometry3d(transform_pcl.cast<double>());
// DURATION --------------------------------------------------------
auto end = std::chrono::high_resolution_clock::now();
......@@ -113,12 +89,12 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
auto duration_align = std::chrono::duration_cast<std::chrono::microseconds>(end - start_aligning);
auto duration_total = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
WOLF_INFO("Laser3d_tools.h: Duration downsampling: ", 1e-3*(duration_downsampling).count(), " ms");
WOLF_INFO("Laser3d_tools.h: Duration aligning : ", 1e-3*(duration_align).count(), " ms");
WOLF_INFO("Laser3d_tools.h: Duration total : ", 1e-3*(duration_total).count(), " ms");
WOLF_INFO("Laser3d_tools.h: Duration downsampling: ", 1e-3 * (duration_downsampling).count(), " ms");
WOLF_INFO("Laser3d_tools.h: Duration aligning : ", 1e-3 * (duration_align).count(), " ms");
WOLF_INFO("Laser3d_tools.h: Duration total : ", 1e-3 * (duration_total).count(), " ms");
// DURATION --------------------------------------------------------
}
} // namespace wolf
#endif // LASER3D_TOOLS_H
\ No newline at end of file
#endif // LASER3D_TOOLS_H
\ No newline at end of file
......@@ -24,9 +24,24 @@
namespace wolf
{
CaptureLaser3d::CaptureLaser3d(const TimeStamp& _timestamp, SensorBasePtr _sensor, pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud)
: CaptureBase("CaptureLaser3d", _timestamp, _sensor), point_cloud_(_point_cloud)
CaptureLaser3d::CaptureLaser3d(const TimeStamp& _timestamp,
SensorBasePtr _sensor,
pcl::PointCloud<pcl::PointXYZ>::Ptr _point_cloud_raw,
bool _downsample = false,
double _voxel_size = 0.05)
: 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() {}
......@@ -41,4 +56,14 @@ pcl::PointCloud<pcl::PointXYZ>::ConstPtr CaptureLaser3d::getPointCloud() const
return point_cloud_;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr CaptureLaser3d::getPointCloudRaw()
{
return point_cloud_raw_;
}
pcl::PointCloud<pcl::PointXYZ>::ConstPtr CaptureLaser3d::getPointCloudRaw() const
{
return point_cloud_raw_;
}
} // namespace wolf
\ No newline at end of file
......@@ -123,9 +123,7 @@ unsigned int ProcessorOdomIcp3d::processKnown()
incoming_laser_->getPointCloud(),
T_origin_last_,
T_origin_incoming_,
registration_solver_,
params_odom_icp_->pcl_downsample,
params_odom_icp_->pcl_downsample_voxel_size);
registration_solver_);
}
return 0;
};
......@@ -142,9 +140,7 @@ unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
incoming_laser_->getPointCloud(),
Eigen::Isometry3d::Identity(),
T_last_incoming_,
registration_solver_,
params_odom_icp_->pcl_downsample,
params_odom_icp_->pcl_downsample_voxel_size);
registration_solver_);
}
return 0;
};
......
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