Use pointer to registration_solver, enable 3 algorithms

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_;
 

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>());

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();

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);
 }