Vote by fitness score

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

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/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