Improve downsample options

include/laser/processor/processor_odom_icp_3d.h

@@ -40,7 +40,6 @@
  **************************/
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
-#include <pcl/registration/icp_nl.h>
 #include <pcl/registration/registration.h>
 
 namespace wolf
@@ -50,10 +49,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
 
 struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
 {
-    bool pcl_downsample;
-
     double max_time_span;
 
+    bool   pcl_downsample;
+    double pcl_downsample_voxel_size;
+
+    string icp_algorithm;                           // "icp", "icpnl", "gicp"
     int    icp_max_iterations;
     double icp_transformation_translation_epsilon;  // squared value of translation epsilon
     double icp_transformation_rotation_epsilon;     // cosinus of rotation angle epsilon
@@ -65,10 +66,12 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo
     ParamsProcessorOdomIcp3d(std::string _unique_name, const ParamsServer& _server)
         : ParamsProcessorTracker(_unique_name, _server), ParamsMotionProvider(_unique_name, _server)
     {
-        pcl_downsample = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample");
-
         max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span");
 
+        pcl_downsample            = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample");
+        pcl_downsample_voxel_size = _server.getParam<double>(prefix + _unique_name + "/pcl_downsample_voxel_size");
+
+        icp_algorithm      = _server.getParam<string>(prefix + _unique_name + "/icp_algorithm");
         icp_max_iterations = _server.getParam<int>(prefix + _unique_name + "/icp_max_iterations");
         icp_transformation_translation_epsilon =
             _server.getParam<double>(prefix + _unique_name + "/icp_transformation_translation_epsilon");
@@ -79,8 +82,8 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo
     }
 
     std::string print() const override
-    {   
-      // TODO
+    {
+        // TODO
         return "\n" + ParamsProcessorTracker::print() + "\n" + ParamsMotionProvider::print() + "\n";
         // + "initial_guess: "             + initial_guess                                         + "\n"
         // + "keyframe_vote/min_dist: "    + std::to_string(vfk_min_dist)                          + "\n"
@@ -93,21 +96,16 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo
 
 WOLF_PTR_TYPEDEFS(ProcessorOdomIcp3d);
 
+
 class ProcessorOdomIcp3d : public ProcessorTracker, public MotionProvider
 {
   protected:
     // Useful sensor stuff
-    SensorLaser3dPtr sensor_laser_;  // casted pointer to parent
-
+    SensorLaser3dPtr  sensor_laser_;  // casted pointer to parent
     CaptureLaser3dPtr origin_laser_, last_laser_, incoming_laser_;
-
     Eigen::Isometry3d T_origin_last_, T_last_incoming_, T_origin_incoming_, T_robot_sensor_, T_sensor_robot_;
-
-    RegistrationPtr registration_solver_;
-
-    // std::shared_ptr<pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ>> registration_solver_;
-
-    Eigen::Matrix6d transform_cov_;
+    RegistrationPtr   registration_solver_;
+    Eigen::Matrix6d   transform_cov_;
 
   public:
     ParamsProcessorOdomIcp3dPtr params_odom_icp_;

include/laser/utils/laser3d_tools.h

@@ -64,7 +64,7 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
     pcl::VoxelGrid<pcl::PointXYZ> grid;  // downsample for performance
     if (_downsample)
     {
-        grid.setLeafSize(0.05, 0.05, 0.05);
+        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);

src/processor/processor_odom_icp_3d.cpp

@@ -123,7 +123,9 @@ unsigned int ProcessorOdomIcp3d::processKnown()
                   incoming_laser_->getPointCloud(),
                   T_origin_last_,
                   T_origin_incoming_,
-                  registration_solver_);
+                  registration_solver_,
+                  params_odom_icp_->pcl_downsample,
+                  params_odom_icp_->pcl_downsample_voxel_size);
     }
     return 0;
 };

test/yaml/problem_odom_icp_3d.yaml

@@ -31,19 +31,21 @@ config:
     sensor_name: "Lidar"
     plugin: "laser"
     time_tolerance: 0.05
+    max_new_features: 0
+    apply_loss_function: false
     keyframe_vote:
       voting_active: true
       min_features_for_keyframe : 0
       max_time_span: 0.99
       angle_turned: 1.0
-    max_new_features: 0
-    apply_loss_function: false
     state_getter: true
     state_priority: 1
+    pcl_downsample: true
+    pcl_downsample_voxel_size: 0.2
+    icp_algorithm: "icp_nl"                        # "icp", "icp_nl", "gicp"
     icp_max_iterations:  20
-    icp_transformation_rotation_epsilon:  0.99
-    icp_transformation_translation_epsilon: 1e-6
+    icp_transformation_rotation_epsilon:  0.99    # this is cos(alpha)
+    icp_transformation_translation_epsilon: 1e-6  # this is translation squared
     icp_max_correspondence_distance:  0.5
-    pcl_downsample: true