Merge branch '27-new-branch-laser-3d-2' of...

Merge branch '27-new-branch-laser-3d-2' of https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/laser into 27-new-branch-laser-3d-2

include/laser/processor/processor_odom_icp_3d.h

@@ -51,6 +51,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
 struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
 {
     double max_time_span;
+    double max_fitness_score; // maximum Euclidean fitness score (e.g., mean of squared distances from the source to the target)
 
     bool   pcl_downsample;
     double pcl_downsample_voxel_size;
@@ -68,6 +69,7 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo
         : ParamsProcessorTracker(_unique_name, _server), ParamsMotionProvider(_unique_name, _server)
     {
         max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span");
+        max_fitness_score = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_fitness_score");
 
         pcl_downsample            = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample");
         pcl_downsample_voxel_size = _server.getParam<double>(prefix + _unique_name + "/pcl_downsample_voxel_size");

include/laser/utils/laser3d_tools.h

@@ -101,17 +101,9 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
     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>>();
-
-    // Point clouds are considered already downsampled
-    *cloud_ref   = *_cloud_ref;
-    *cloud_other = *_cloud_other;
-
     // 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;
@@ -120,7 +112,8 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref,
     Eigen::Matrix4f transform_guess = _transform_guess.matrix().cast<float>();
 
     // Alignment
-    _registration_solver->align(*cloud_ref, transform_guess);
+    pcl::PointCloud<pcl::PointXYZ>cloud_out;
+    _registration_solver->align(cloud_out, transform_guess);
     transform_pcl = _registration_solver->getFinalTransformation();
 
     // Output the transformation as Isometry of double(s)

src/processor/processor_odom_icp_3d.cpp

@@ -179,14 +179,24 @@ unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
  */
 bool ProcessorOdomIcp3d::voteForKeyFrame() const
 {
+    // 1. vote by time
     if (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_odom_icp_->max_time_span)
     {
+        WOLF_DEBUG("ProcessorOdomIcp3d: vote KF by time");
         return true;
     }
     // TODO:
-    //  - vote by distance
-    //  - vote by angle
-    //  - vote by nbr. of captures
+    //  2. vote by distance
+    //  3. vote by angle
+    //  4. vote by nbr. of captures
+
+    // 5. vote by fitness score
+    if (registration_solver_->getFitnessScore() > params_odom_icp_->max_fitness_score)
+    {
+        WOLF_DEBUG("ProcessorOdomIcp3d: vote KF by fitness score");
+        return true;
+    }
+    
     return false;
 };
 

test/gtest_processor_odom_icp_3d.cpp

@@ -49,6 +49,8 @@ using namespace wolf;
 std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
 
 
+
+
 class Test_ProcessorOdomIcp3D_yaml : public testing::Test
 {
   public:

test/yaml/problem_odom_icp_3d.yaml

@@ -37,6 +37,7 @@ config:
       voting_active: true
       min_features_for_keyframe : 0
       max_time_span: 0.99
+      max_fitness_score: 0.0001 # maximum Euclidean fitness score (e.g., mean of squared distances from the source to the target)
       angle_turned: 1.0
     state_getter: true
     state_priority: 1
@@ -44,7 +45,7 @@ config:
     pcl_downsample_voxel_size: 0.05
     icp_algorithm: "icp_nl"                        # "icp", "icp_nl", "gicp"
     icp_max_iterations:  20
-    icp_transformation_rotation_epsilon:  0.99    # this is cos(alpha)
+    icp_transformation_rotation_epsilon:  0.9999    # this is cos(alpha)
     icp_transformation_translation_epsilon: 1e-9  # this is translation squared
     icp_max_correspondence_distance:  0.05