add cm_icp_xy from https://github.com/ccny-ros-pkg/scan_tools/tree/in…

…digo/laser_scan_matcher/csm/patches

sm/csm/algos.h

@@ -159,6 +159,7 @@ struct sm_result {
 
 
 void sm_icp(struct sm_params*input, struct sm_result*output);
+void sm_icp_xy(struct sm_params*input, struct sm_result*output);
 void sm_gpm(struct sm_params*input, struct sm_result*output);
 void sm_hsm(struct sm_params*input, struct sm_result*output);
 

sm/csm/icp/icp.c

@@ -185,3 +185,167 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 
 	if(JJ) jj_context_exit();
 }
+
+void sm_icp_xy(struct sm_params*params, struct sm_result*res) 
+{
+	res->valid = 0;
+
+	LDP laser_ref  = params->laser_ref;
+	LDP laser_sens = params->laser_sens;
+
+	if(!ld_valid_fields(laser_ref) || 
+	   !ld_valid_fields(laser_sens)) {
+		return;
+	}
+
+/*
+	sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid\n",
+		count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
+		count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays);
+	*/
+
+	/** Mark as invalid the rays outside of (min_reading, max_reading] */
+	ld_invalid_if_outside(laser_ref, params->min_reading, params->max_reading);
+	ld_invalid_if_outside(laser_sens, params->min_reading, params->max_reading);
+
+/*
+	sm_debug("sm_icp:  laser_sens has %d/%d; laser_ref has %d/%d rays valid (after removing outside interval [%f, %f])\n",
+		count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
+		count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays,
+   	   params->min_reading, params->max_reading);
+	
+	if(JJ) jj_context_enter("sm_icp");
+	
+	egsl_push_named("sm_icp");
+	*/
+
+	if(params->use_corr_tricks || params->debug_verify_tricks)
+		ld_create_jump_tables(laser_ref);
+/*		
+	ld_compute_cartesian(laser_ref);
+	ld_compute_cartesian(laser_sens);
+*/
+
+	if(params->do_alpha_test) {
+		ld_simple_clustering(laser_ref, params->clustering_threshold);
+		ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
+		ld_simple_clustering(laser_sens, params->clustering_threshold);
+		ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);
+	}
+
+	if(JJ) jj_add("laser_ref",  ld_to_json(laser_ref));
+	if(JJ) jj_add("laser_sens", ld_to_json(laser_sens));
+
+	gsl_vector * x_new = gsl_vector_alloc(3);
+	gsl_vector * x_old = vector_from_array(3, params->first_guess);
+
+	if(params->do_visibility_test) {
+		sm_debug("laser_ref:\n");
+		visibilityTest(laser_ref, x_old);
+
+		sm_debug("laser_sens:\n");
+		gsl_vector * minus_x_old = gsl_vector_alloc(3);
+		ominus(x_old,minus_x_old);
+		visibilityTest(laser_sens, minus_x_old);
+		gsl_vector_free(minus_x_old);
+	}
+
+	double error;
+	int iterations;
+	int nvalid;
+	if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
+		sm_error("icp: ICP failed for some reason. \n");
+		res->valid = 0;
+		res->iterations = iterations;
+		res->nvalid = 0;
+
+	} else {
+		/* It was succesfull */
+
+		double best_error = error;
+		gsl_vector * best_x = gsl_vector_alloc(3);
+		gsl_vector_memcpy(best_x, x_new);
+
+		if(params->restart && 
+			(error/nvalid)>(params->restart_threshold_mean_error) ) {
+			sm_debug("Restarting: %f > %f \n",(error/nvalid),(params->restart_threshold_mean_error));
+			double dt  = params->restart_dt;
+			double dth = params->restart_dtheta;
+			sm_debug("icp_loop: dt = %f dtheta= %f deg\n",dt,rad2deg(dth));
+
+			double perturb[6][3] = {
+				{dt,0,0}, {-dt,0,0},
+				{0,dt,0}, {0,-dt,0},
+				{0,0,dth}, {0,0,-dth}
+			};
+
+			int a; for(a=0;a<6;a++){
+				sm_debug("-- Restarting with perturbation #%d\n", a);
+				struct sm_params my_params = *params;
+				gsl_vector * start = gsl_vector_alloc(3);
+					gvs(start, 0, gvg(x_new,0)+perturb[a][0]);
+					gvs(start, 1, gvg(x_new,1)+perturb[a][1]);
+					gvs(start, 2, gvg(x_new,2)+perturb[a][2]);
+				gsl_vector * x_a = gsl_vector_alloc(3);
+				double my_error; int my_valid; int my_iterations;
+				if(!icp_loop(&my_params, start->data, x_a->data, &my_error, &my_valid, &my_iterations)){
+					sm_error("Error during restart #%d/%d. \n", a, 6);
+					break;
+				}
+				iterations+=my_iterations;
+
+				if(my_error < best_error) {
+					sm_debug("--Perturbation #%d resulted in error %f < %f\n", a,my_error,best_error);
+					gsl_vector_memcpy(best_x, x_a);
+					best_error = my_error;
+				}
+				gsl_vector_free(x_a); gsl_vector_free(start);
+			}
+		}
+
+
+		/* At last, we did it. */
+		res->valid = 1;
+		vector_to_array(best_x, res->x);
+		sm_debug("icp: final x =  %s  \n", gsl_friendly_pose(best_x));
+
+
+		if(params->do_compute_covariance)  {
+
+			val cov0_x, dx_dy1, dx_dy2;
+			compute_covariance_exact(
+				laser_ref, laser_sens, best_x,
+				&cov0_x, &dx_dy1, &dx_dy2);
+
+			val cov_x = sc(square(params->sigma), cov0_x); 
+//			egsl_v2da(cov_x, res->cov_x); 
+
+			res->cov_x_m = egsl_v2gslm(cov_x);
+			res->dx_dy1_m = egsl_v2gslm(dx_dy1);
+			res->dx_dy2_m = egsl_v2gslm(dx_dy2);
+
+			if(0) {
+				egsl_print("cov0_x", cov0_x);
+				egsl_print_spectrum("cov0_x", cov0_x);
+
+				val fim = ld_fisher0(laser_ref);
+				val ifim = inv(fim);
+				egsl_print("fim", fim);
+				egsl_print_spectrum("ifim", ifim);
+			}
+		}
+
+		res->error = best_error;
+		res->iterations = iterations;
+		res->nvalid = nvalid;
+
+		gsl_vector_free(x_new);
+		gsl_vector_free(x_old);
+		gsl_vector_free(best_x);
+	}
+/*
+	egsl_pop_named("sm_icp");
+
+	if(JJ) jj_context_exit();
+*/
+}

sm/csm/icp/icp.h

@@ -8,6 +8,8 @@
 /** This sets the stage.  */
 void sm_icp(struct sm_params*params, struct sm_result*res);
 
+void sm_icp_xy(struct sm_params*params, struct sm_result*res);
+
 /** This is the meat */
 int icp_loop(struct sm_params*params, const double*q0, double*x_new, 
  	double*total_error, int*nvalid, int*iterations);