Enable LidarMeasurement and tune parameter

Author: Ryohei Sasaki

include/amcl_3d/type.hpp

@@ -1,4 +1,5 @@
 #pragma once
+#include <vector>
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
 #include <Eigen/Geometry>

launch/amcl_3d_rosbag.launch

@@ -14,7 +14,7 @@
   <arg name="amcl_param/augmented_mcl/noise_pitch_var" default="1.0" />
   <arg name="amcl_param/augmented_mcl/noise_yaw_var" default="1.0" />
   <arg name="amcl_param/init_pose/initial_particle_num" default="100" />
-  <arg name="amcl_param/resample_timing/ess_ratio_threshold" default="0.95" />
+  <arg name="amcl_param/resample_timing/ess_ratio_threshold" default="1.0" />
   <arg name="amcl_param/kld_sampling/min_particle_num" default="100" />
   <arg name="amcl_param/kld_sampling/max_particle_num" default="2000" />
   <arg name="amcl_param/kld_sampling/delta" default="0.5" />

src/mcl.cpp

@@ -36,16 +36,16 @@ bool Amcl::getParticles(std::shared_ptr<const Particles> &particles_ptr)
 
 bool Amcl::measureLidar(const Time &time, const pcl::PointCloud<pcl::PointXYZ>::Ptr measuement)
 {
-#if 0
+#if 1
     if (kd_map_ptr_ == nullptr || measuement == nullptr)
         return false;
     MeasurementState measurement_state;
     const size_t random_sample_num = 100;
-    const double max_dist = 1.0;
+    const double max_dist = 0.2;
     const double sigma = 1.0;
     std::shared_ptr<MeasurementModelInterface> model =
         std::make_shared<LidarMeasurementModel>(kd_map_ptr_, measuement, random_sample_num, max_dist, sigma);
-    pf_ptr_->measure(model, time, measurement_state);
+    pf_ptr_->measure(model, measurement_state);
 
     checkResample(measurement_state);
 #endif

src/measurement_model/lidar_measurement_model.cpp

@@ -26,6 +26,7 @@ bool LidarMeasurementModel::measure(std::shared_ptr<const Particles> mesurement_
     v_log_likelihood.reserve(particles_ptr->size());
     const double max_squared_dist = max_dist_ * max_dist_; // [m^2]
     const double denominator = 2 * sigma_ * sigma_;
+    const double distance_penelty = 0.2;
     for (const auto &state : *mesurement_point_particles_ptr)
     {
         /*
@@ -63,7 +64,7 @@ bool LidarMeasurementModel::measure(std::shared_ptr<const Particles> mesurement_
                 squared_dist = max_squared_dist;
             }
             // first step : log likelihood += -(x-mu)^2
-            log_likelihood += -1.0 * squared_dist;
+            log_likelihood += -1.0 * squared_dist * distance_penelty;
         }
         // second step : log likelihood *= 1.0/(2*sigma^2)
         log_likelihood /= denominator; // denominator = (2*sigma^2)