Refactor likelihood calculation (#189)

* Make embedded magic parameter configurable

* Decrease kdtree epsilon

* Add document for parameter tuning

README.md

@@ -19,7 +19,8 @@ The node provides classic MCL; currently, it doesn't implement adaptive feature
 A fundamental algorithm of *mcl_3dl* node is Monte Carlo localization (MCL), aka particle filter localization.
 MCL represents a probabilistic distribution of estimated pose as density and weight of particles and estimates the pose from the distribution.
 
-See [details](doc/Algorithms.md).
+- [Algorithm details](doc/Algorithms.md)
+- [Parameters](doc/Parameters.md)
 
 ## Install
 

doc/Parameters.md

@@ -0,0 +1,10 @@
+## Parameters
+
+Description of the node parameters and how to tune them.
+
+**This document is work-in-progress.**
+
+- **match_dist_min** (float, default: `0.2`, unit: meters):\
+    Point to map distance larger than *match_dist_min* is ignored in the likelihood calculation.
+- **match_dist_flat** (float, default: `0.05`, unit: meters):\
+    Point-to-map distance smaller than *match_dist_flat* is clipped to *match_dist_flat*. Set to two or three-sigma of the input pointcloud noise.

src/mcl_3dl.cpp

@@ -117,6 +117,7 @@ class MCL3dlNode
     double expansion_var_yaw;
     double match_ratio_thresh;
     double match_dist_min;
+    double match_dist_flat;
     double match_weight;
     double jump_dist;
     double jump_ang;
@@ -749,10 +750,11 @@ class MCL3dlNode
     float match_ratio_min = 1.0;
     float match_ratio_max = 0.0;
     const float match_dist_min = params_.match_dist_min;
+    const float match_dist_flat = params_.match_dist_flat;
     const float match_weight = params_.match_weight;
     mcl_3dl::NormalLikelihood<float> odom_error_lin_nd(params_.odom_err_integ_lin_sigma);
     mcl_3dl::NormalLikelihood<float> odom_error_ang_nd(params_.odom_err_integ_ang_sigma);
-    auto measure_func = [this, &match_dist_min, &match_weight,
+    auto measure_func = [this, &match_dist_min, &match_dist_flat, &match_weight,
                          &id, &sqdist, &pc_particle, &pc_local,
                          &pc_particle_beam, &pc_local_beam, &origins,
                          &odom_error_lin_nd,
@@ -767,10 +769,7 @@ class MCL3dlNode
       {
         if (kdtree_->radiusSearch(p, match_dist_min, id, sqdist, 1))
         {
-          float dist = sqdist[0];
-          if (dist < 0.05 * 0.05)
-            dist = 0.05 * 0.05;
-          dist = match_dist_min - sqrtf(dist);
+          const float dist = match_dist_min - std::max(sqrtf(sqdist[0]), match_dist_flat);
           if (dist < 0.0)
             continue;
 
@@ -1246,7 +1245,8 @@ class MCL3dlNode
       {
         pf_->resizeParticle(params_.num_particles);
       }
-      global_localization_fix_cnt_ = 1 + ceil(params_.lpf_step) * 3.0;  // wait 99.7% fix (three-sigma)
+      // wait 99.7% fix (three-sigma)
+      global_localization_fix_cnt_ = 1 + ceil(params_.lpf_step) * 3.0;
     }
     if (global_localization_fix_cnt_)
     {
@@ -1526,6 +1526,7 @@ class MCL3dlNode
     params_.map_update_interval.reset(new ros::Duration(map_update_interval_t));
 
     pnh_.param("match_dist_min", params_.match_dist_min, 0.2);
+    pnh_.param("match_dist_flat", params_.match_dist_flat, 0.05);
     pnh_.param("match_weight", params_.match_weight, 5.0);
 
     double weight[3];
@@ -1547,7 +1548,7 @@ class MCL3dlNode
             std::max(params_.match_dist_min, params_.map_grid_max * 4.0)));
     ROS_DEBUG("max_search_radius: %0.3f", max_search_radius);
     kdtree_.reset(new mcl_3dl::ChunkedKdtree<pcl::PointXYZI>(map_chunk, max_search_radius));
-    kdtree_->setEpsilon(params_.map_grid_min / 4);
+    kdtree_->setEpsilon(params_.map_grid_min / 16);
     kdtree_->setPointRepresentation(
         boost::dynamic_pointer_cast<
             pcl::PointRepresentation<pcl::PointXYZI>,