diff --git a/include/laser/capture/capture_laser_3d.h b/include/laser/capture/capture_laser_3d.h
index 4e0e84f6f4deb54d8c8ae6cde43efd0b0dedd7dd..ecab3a7bba17d5540cede32e56bce09290e1369c 100644
--- a/include/laser/capture/capture_laser_3d.h
+++ b/include/laser/capture/capture_laser_3d.h
@@ -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_;
};
diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h
index 5c3acc5e03e23b576fd9133a184b1532c4a7fbe6..7992d951a32fb2ef8fa0b1753441e1694256b890 100644
--- a/include/laser/utils/laser3d_tools.h
+++ b/include/laser/utils/laser3d_tools.h
@@ -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
diff --git a/src/capture/capture_laser_3d.cpp b/src/capture/capture_laser_3d.cpp
index 7df9a1b1f8c1a95aedd6000668c819976923ffe5..c33518b728bb2dfc6bfbb268346ef365289eadcc 100644
--- a/src/capture/capture_laser_3d.cpp
+++ b/src/capture/capture_laser_3d.cpp
@@ -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
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index 979c8e5823bad6527087e69a8364dc141fc7e11b..eeede85b97e942a70e357671797188ac5fa010d9 100644
--- a/src/processor/processor_odom_icp_3d.cpp
+++ b/src/processor/processor_odom_icp_3d.cpp
@@ -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;
};