makes beam_model likelihood configurable (#30)

at-wat · Feb 8, 2017 · f87d7ac · f87d7ac
1 parent 76877bc
commit f87d7ac
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp
@@ -99,6 +99,7 @@ class mcl_3dl_node
 		double unmatch_output_dist;
 		double bias_var_dist;
 		double bias_var_ang;
+		float beam_likelihood;
 		std::shared_ptr<ros::Duration> match_output_interval;
 	} params;
 	int cnt_measure;
@@ -608,7 +609,7 @@ class mcl_3dl_node
 							if(kdtree->radiusSearch(center, 
 										params.map_grid_min, id, sqdist, 1))
 							{
-								score_beam *= 0.8;
+								score_beam *= params.beam_likelihood;
 								break;
 							}
 						}
@@ -926,6 +927,10 @@ class mcl_3dl_node
 		nh.param("num_points", params.num_points, 32);
 		nh.param("num_points_beam", params.num_points_beam, 8);
 
+		double beam_likelihood_a;
+		nh.param("beam_likelihood", beam_likelihood_a, 0.5);
+		params.beam_likelihood = powf(beam_likelihood_a, 1.0 / (float)params.num_points_beam);
+
 		nh.param("resample_var_x", params.resample_var_x, 0.05);
 		nh.param("resample_var_y", params.resample_var_y, 0.05);
 		nh.param("resample_var_z", params.resample_var_z, 0.05);