diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h
index bf4ac864099113e28d72e088437b0581a8c195e5..b9b77f457de2aa0188943218b66e5b35aeb7606e 100644
--- a/include/laser/processor/processor_odom_icp_3d.h
+++ b/include/laser/processor/processor_odom_icp_3d.h
@@ -29,6 +29,7 @@
#include "laser/capture/capture_laser_3d.h"
#include "laser/sensor/sensor_laser_3d.h"
+#include "laser/utils/laser3d_tools.h"
#include "core/common/wolf.h"
#include "core/processor/processor_tracker.h"
@@ -40,6 +41,7 @@
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp_nl.h>
+#include <pcl/registration/registration.h>
namespace wolf
{
@@ -101,7 +103,9 @@ class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
Eigen::Isometry3d T_origin_last_, T_last_incoming_, T_origin_incoming_, T_robot_sensor_, T_sensor_robot_;
- pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver_;
+ RegistrationPtr registration_solver_;
+
+ // std::shared_ptr<pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>> registration_solver_;
Eigen::Matrix6d transform_cov_;
diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h
index 9040dd50f124b207e049506803cf8a0a11ceb3e8..47f2cec9be81ed440abff579904bb677d2040bd0 100644
--- a/include/laser/utils/laser3d_tools.h
+++ b/include/laser/utils/laser3d_tools.h
@@ -38,16 +38,22 @@
// typedef boost::shared_ptr<PointCloud> PointCloudBoostPtr;
// typedef boost::shared_ptr<const PointCloud> PointCloudBoostConstPtr;
+// typedef std::shared_ptr<pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>> RegistrationPtr;
+
+typedef pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr RegistrationPtr;
+
+
namespace wolf
{
// _cloud_ref: first PointCloud
// _cloud_other: second PointCloud
-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,
- pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> &_registration_solver,
- bool _downsample = false)
+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)
{
// Internal PointCloud pointers (boost::shared_ptr)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ref = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
@@ -73,8 +79,8 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr
}
// Set input clouds
- _registration_solver.setInputSource(cloud_ref);
- _registration_solver.setInputTarget(cloud_other);
+ _registration_solver->setInputSource(cloud_ref);
+ _registration_solver->setInputTarget(cloud_other);
// Declare variables
Eigen::Matrix4f transform_pcl;
@@ -83,8 +89,8 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr
Eigen::Matrix4f transform_guess = _transform_guess.matrix().cast<float>();
// Alignment
- _registration_solver.align(*cloud_ref, transform_guess);
- transform_pcl = _registration_solver.getFinalTransformation();
+ _registration_solver->align(*cloud_ref, transform_guess);
+ transform_pcl = _registration_solver->getFinalTransformation();
// Output the transformation as Isometry of double(s)
_transform_final = Eigen::Isometry3d(transform_pcl.cast<double>());
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index 7624be9d7463e7fe6a4e03a295cf5e1c49731823..b9b7982597622547c255a15a88f93a10376b2dba 100644
--- a/src/processor/processor_odom_icp_3d.cpp
+++ b/src/processor/processor_odom_icp_3d.cpp
@@ -19,9 +19,19 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+
+// wolf laser
#include "laser/processor/processor_odom_icp_3d.h"
#include "laser/utils/laser3d_tools.h"
+
+// wolf core
#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+#include "core/utils/logging.h"
+
+// pcl
+#include <pcl/registration/icp.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/registration/icp_nl.h>
namespace wolf
{
@@ -35,10 +45,30 @@ ProcessorOdomIcp3d::ProcessorOdomIcp3d(ParamsProcessorOdomIcp3dPtr _params)
T_robot_sensor_.setIdentity();
T_sensor_robot_.setIdentity();
- registration_solver_.setMaximumIterations(_params->icp_max_iterations);
- registration_solver_.setTransformationRotationEpsilon(_params->icp_transformation_rotation_epsilon);
- registration_solver_.setTransformationEpsilon(_params->icp_transformation_translation_epsilon);
- registration_solver_.setMaxCorrespondenceDistance(_params->icp_max_correspondence_distance);
+ if (std::strcmp(_params->icp_algorithm.c_str(), "icp_nl") == 0)
+ {
+ WOLF_INFO("ProcessorOdomIcp3d: Using icp_nl");
+ registration_solver_ = std::make_shared<pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>>();
+ }
+ else if (std::strcmp(_params->icp_algorithm.c_str(), "icp") == 0)
+ {
+ WOLF_INFO("ProcessorOdomIcp3d: Using icp");
+ registration_solver_ = std::make_shared<pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>>();
+ }
+ else if (std::strcmp(_params->icp_algorithm.c_str(), "gicp") == 0)
+ {
+ WOLF_INFO("ProcessorOdomIcp3d: Using gicp");
+ registration_solver_ = std::make_shared<pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>>();
+ }
+ else
+ {
+ WOLF_ERROR("ProcessorOdomIcp3d: Unrecognized ICP algorithm \"", _params->icp_algorithm, "\". Valid options are \"icp\", \"icp_nl\" and \"gicp\" only.");
+ throw("ICP algorithm not recognized");
+ }
+ registration_solver_->setMaximumIterations(_params->icp_max_iterations);
+ registration_solver_->setTransformationRotationEpsilon(_params->icp_transformation_rotation_epsilon);
+ registration_solver_->setTransformationEpsilon(_params->icp_transformation_translation_epsilon);
+ registration_solver_->setMaxCorrespondenceDistance(_params->icp_max_correspondence_distance);
params_odom_icp_ = _params;
transform_cov_.setIdentity();
diff --git a/test/gtest_laser3d_tools.cpp b/test/gtest_laser3d_tools.cpp
index 8c5541d5f51b220ebb2c56f1a702932fe629bd8b..c8be82381b387e40be45446b503da317d9f6c77b 100644
--- a/test/gtest_laser3d_tools.cpp
+++ b/test/gtest_laser3d_tools.cpp
@@ -76,22 +76,23 @@ TEST(pairAlign, identity)
Isometry3d transformation_final;
// Solver configuration
- pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver;
- registration_solver.setTransformationEpsilon(1e-6);
+ RegistrationPtr registration_solver =
+ std::make_shared<pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>>();
+ registration_solver->setTransformationEpsilon(1e-6);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
- registration_solver.setMaxCorrespondenceDistance(0.5); // visaub: changed to 100 cm
+ registration_solver->setMaxCorrespondenceDistance(0.5); // visaub: changed to 100 cm
// Set the point representation
// reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation));
- registration_solver.setMaximumIterations(200);
+ registration_solver->setMaximumIterations(200);
// Alignment
wolf::pairAlign(pcl_ref,
- pcl_other,
- transformation_guess,
- transformation_final,
- registration_solver,
- true); // false);
+ pcl_other,
+ transformation_guess,
+ transformation_final,
+ registration_solver,
+ true); // false);
WOLF_INFO("Final transform: \n", transformation_final.matrix());
@@ -121,33 +122,34 @@ TEST(pairAlign, known_transformation)
WOLF_INFO("Applied transformation: \n", transformation_known.matrix());
// Solver configuration
- pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> registration_solver;
+ RegistrationPtr registration_solver =
+ std::make_shared<pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>>();
// pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> registration_solver;
double max_trans_epsilon_meters = 0.001;
double max_rot_epsilon_degrees = 0.1;
double trf_rotation_epsilon = cos(toRad(max_rot_epsilon_degrees));
- registration_solver.setTransformationEpsilon(max_trans_epsilon_meters * max_trans_epsilon_meters);
- registration_solver.setTransformationRotationEpsilon(trf_rotation_epsilon);
+ registration_solver->setTransformationEpsilon(max_trans_epsilon_meters * max_trans_epsilon_meters);
+ registration_solver->setTransformationRotationEpsilon(trf_rotation_epsilon);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
- registration_solver.setMaxCorrespondenceDistance(0.5); // visaub: changed to 100 cm
- registration_solver.setMaximumIterations(100);
+ registration_solver->setMaxCorrespondenceDistance(0.5); // visaub: changed to 100 cm
+ registration_solver->setMaximumIterations(100);
// Alignment
wolf::pairAlign(pcl_ref,
- pcl_other,
- transformation_guess,
- transformation_final,
- registration_solver,
- true); // false);
+ pcl_other,
+ transformation_guess,
+ transformation_final,
+ registration_solver,
+ true); // false);
WOLF_INFO("Final transform: \n", transformation_final.matrix());
- auto convcrit = registration_solver.getConvergeCriteria();
+ // auto convcrit = registration_solver->getConvergeCriteria();
- WOLF_INFO("convergence criteria: ", registration_solver.getConvergeCriteria()->getConvergenceState());
+ // WOLF_INFO("convergence criteria: ", registration_solver->getConvergeCriteria()->getConvergenceState());
ASSERT_MATRIX_APPROX(transformation_final.matrix(), transformation_known.matrix(), 1e-2);
}