Add min_num_correspondences parameter

include/csm/algos.h

@@ -33,6 +33,8 @@ struct sm_params {
 	double max_correspondence_dist;
 	/** Use smart tricks for finding correspondences. Only influences speed; not convergence. */
 	int use_corr_tricks;
+        /** Minimum required number of correspondences, bail out if have less. */
+        int min_num_correspondences;
 
 	/** Restart if error under threshold (0 or 1)*/
 	int restart;

src/csm/sm_options.cpp

@@ -24,7 +24,11 @@ void sm_options(struct sm_params*p, struct option*ops) {
 	options_double(ops, "max_correspondence_dist", 
 		&(p->max_correspondence_dist), 2.0,
 		"dubious parameter (m)");
-
+
+	options_int(ops, "min_num_correspondences", 
+		&(p->min_num_correspondences), 20,
+		"minimum required number of correspondences");
+
 	options_double(ops, "sigma", 
 		&(p->sigma), 0.01,
 		"Noise in the scan (m)");

src/icp/icp_loop.cpp

@@ -51,8 +51,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 
 		/* If not many correspondences, bail out */
 		int num_corr = ld_num_valid_correspondences(laser_sens);
-		double fail_perc = 0.05;
-		if(num_corr < fail_perc * laser_sens->nrays) { /* TODO: arbitrary */
+		if(num_corr < params->min_num_correspondences) {
 			sm_error("	: before trimming, only %d correspondences.\n",num_corr);
 			all_is_okay = 0;
 			egsl_pop_named("icp_loop iteration"); /* loop context */
@@ -76,7 +75,7 @@ int icp_loop(struct sm_params*params, const double*q0, double*x_new,
 		sm_debug("  icp_loop: total error: %f  valid %d   mean = %f\n", *total_error, *valid, *total_error/ *valid);
 
 		/* If not many correspondences, bail out */
-		if(num_corr_after < fail_perc * laser_sens->nrays){
+		if(num_corr_after < params->min_num_correspondences){
 			sm_error("  icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
 			all_is_okay = 0;
 			egsl_pop_named("icp_loop iteration"); /* loop context */