You must use always the biggest real root

'

sm/csm/icp/icp_loop.c

@@ -174,6 +174,8 @@ int compute_next_estimate(struct sm_params*params,
 		int j1 = laser_sens->corr[i].j1;
 		int j2 = laser_sens->corr[i].j2;
 
+		c[k].valid = 1;
+		
 /*		if(params->use_point_to_line_distance) {*/
 		if(laser_sens->corr[i].type == corr_pl) {
 
@@ -249,7 +251,7 @@ int compute_next_estimate(struct sm_params*params,
 		 0, 0, 0};
 	
 	
-	int ok = gpc_solve_valid(k, c, 0, 0, inv_cov_x0, x_new);
+	int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
 	if(!ok) {
 		sm_error("gpc_solve_valid failed");
 		return 0;

sm/lib/gpc/gpc.c

@@ -21,16 +21,12 @@
 #include "gpc.h"
 #include "gpc_utils.h"
 
-int gpc_solve(int K, const struct gpc_corr*c, double *x) {
-	return gpc_solve_valid(K,c,0,0,0,x);
-}
-
 #define M(matrix, rows, col) static gsl_matrix*matrix=0; if(!matrix) matrix = gsl_matrix_alloc(rows,col);
 /*#define MF(matrix) gsl_matrix_free(matrix)*/
 #define MF(matrix) /*gsl_matrix_free(matrix)*/
 
 
-int gpc_solve_valid(int K, const struct gpc_corr*c, const int*valid, 
+int gpc_solve(int K, const struct gpc_corr*c,
 	const double*x0, const double *cov_x0,
 	double *x_out) 
 {
@@ -48,7 +44,7 @@ int gpc_solve_valid(int K, const struct gpc_corr*c, const int*valid,
 	double d_g[4] = {0, 0, 0, 0};
 	int k;
 	for(k=0;k<K;k++) {
-		if(valid && !valid[k]) continue;
+		if(!c[k].valid) continue;
 		
 		double C00 = c[k].C[0][0];
 		double C01 = c[k].C[0][1];
@@ -201,30 +197,74 @@ int gpc_solve_valid(int K, const struct gpc_corr*c, const int*valid,
 		fprintf(stderr, "q = %f %f %f %f %f \n", q[4],  q[3],  q[2], q[1], q[0]);
 	}
 
-	double lambda;
-	
-	if(!poly_greatest_real_root(5,q,&lambda)) {
+	/*
+	double lambdas[4];
+	if(!poly_real_roots(5, q, lambdas)) {
 		fprintf(stderr, "Cannot solve polynomial.\n");
 		return 0;
 	}
+
+	double lambdas_error[4];
+	double lambdas_pose[4][3];
 	
-	if(TRACE_ALGO) {
-		fprintf(stderr, "lambda = %f \n", lambda);
-	}	
+	for(int i=0;i<4;i++) {
+		double lambda = lambdas[i];
+		
+		if(TRACE_ALGO) {
+			fprintf(stderr, "lambda = %f \n", lambda);
+		}	
+	
+		M(W,4,4); gsl_matrix_set_zero(W); gms(W,2,2,1.0); gms(W,3,3,1.0);
+		M(x,4,1);
+	
+		m_scale(2*lambda, W);
+		gsl_matrix_add(bigM,W);
+		m_inv(bigM, temp44);
+		m_mult(temp44, g, x);
+		m_scale(-1.0, x);
+	
+		lambdas_pose[i][0] = gmg(x,0,0);
+		lambdas_pose[i][1] = gmg(x,1,0);
+		lambdas_pose[i][2] = atan2(gmg(x,3,0),gmg(x,2,0));
+		
+		lambdas_error[i] = gpc_total_error(c, K, lambdas_pose[i]);
+	}
+	
+	int lowest_error = 0;
+	for(int i=0;i<4;i++) {
+		printf("#%d lambda=%lf error=%lf\n",i,lambdas[i],lambdas_error[i]);
+		if(lambdas_error[i] < lambdas_error[lowest_error])
+			lowest_error = i;
+	}
+	
+	double lr;
+	poly_greatest_real_root(5,q,&lr);
+	printf("Choose %d: lambda = %lf   bigger real root = %lf \n",lowest_error,lambdas[lowest_error],lr);
+	
+	x_out[0]=lambdas_pose[lowest_error][0];
+	x_out[1]=lambdas_pose[lowest_error][1];
+	x_out[2]=lambdas_pose[lowest_error][2];
+	*/
+	
+	double lambda;
+	if(!poly_greatest_real_root(5,q,&lambda)) return 0;
 	
 	M(W,4,4); gsl_matrix_set_zero(W); gms(W,2,2,1.0); gms(W,3,3,1.0);
 	M(x,4,1);
-	
+
 	m_scale(2*lambda, W);
 	gsl_matrix_add(bigM,W);
 	m_inv(bigM, temp44);
 	m_mult(temp44, g, x);
 	m_scale(-1.0, x);
-	
+
 	x_out[0] = gmg(x,0,0);
 	x_out[1] = gmg(x,1,0);
 	x_out[2] = atan2(gmg(x,3,0),gmg(x,2,0));
+
+	
 	
+		
 	if(TRACE_ALGO) {
 		fprintf(stderr, "x =  %f  %f %f deg\n", x_out[0], x_out[1],x_out[2]*180/M_PI);
 	}
@@ -262,6 +302,7 @@ double gpc_error(const struct gpc_corr*co, const double*x) {
 double gpc_total_error(const struct gpc_corr*co, int n, const double*x){
 	double error = 0;
 	for(int i=0;i<n;i++) {
+		if(!co[i].valid) continue;
 		error += gpc_error(co+i,x);
 		if(error<0) {
 			fprintf(stderr, "Something fishy!\n");

sm/lib/gpc/gpc.h

@@ -25,6 +25,8 @@ struct gpc_corr {
 	double q[2];
 
 	double C[2][2];
+	
+	int valid;
 };
 
 /**
@@ -39,11 +41,8 @@ struct gpc_corr {
 
 #define TRACE_ALGO 0
 
-int gpc_solve(int K, const struct gpc_corr*, double *x);
-
-/** if valid[k] is 0, the correspondence is not used */
-int gpc_solve_valid(int K, const struct gpc_corr*, 
-	const int*valid, 
+/** if c[k].valid is 0, the correspondence is not used */
+int gpc_solve(int K, const struct gpc_corr*, 
 	const double*x0, const double *cov_x0,
 	double *x);
 

sm/lib/gpc/gpc_utils.c

@@ -58,10 +58,28 @@ double m_dot(const gsl_matrix*A,const gsl_matrix*B) {
 	return sum;
 }
 
+int poly_real_roots(unsigned int n, const double*a, double *roots) {
+	double z[(n-1)*2];
+	gsl_poly_complex_workspace * w  = gsl_poly_complex_workspace_alloc(n);
+	if(GSL_SUCCESS != gsl_poly_complex_solve (a, n, w, z)) {
+		return 0;
+	}
+	gsl_poly_complex_workspace_free (w);
+	
+	unsigned int i=0;
+	for(i=0;i<n-1;i++) {
+		roots[i] = z[2*i];
+	}	
+	return 1;
+}
+
+
 int poly_greatest_real_root(unsigned int n, const double*a, double *root) {
 	double z[(n-1)*2];
 	gsl_poly_complex_workspace * w  = gsl_poly_complex_workspace_alloc(n);
-	gsl_poly_complex_solve (a, n, w, z);
+	if(GSL_SUCCESS != gsl_poly_complex_solve (a, n, w, z)) {
+		return 0;
+	}
 	gsl_poly_complex_workspace_free (w);
 	if(TRACE_ALGO) {
 		printf("Solving the equation\n a = [");

sm/lib/gpc/gpc_utils.h

@@ -21,6 +21,9 @@ double m_dot(const gsl_matrix*A,const gsl_matrix*B);
 void m_inv(const gsl_matrix*A, gsl_matrix*invA);
 double m_det(const gsl_matrix*A);
 
+/* Returns the real part of the roots in roots */
+int poly_real_roots(unsigned int n, const double*a, double *roots);
+
 int poly_greatest_real_root(unsigned int n, const double*a, double *root);
 
 void m_display(const char*str, gsl_matrix*m);

tests/failure1/Makefile

@@ -2,7 +2,11 @@
 only_corr=-sens_countour_draw 0 -ref_countour_draw 0 -sens_points_draw 0 -ref_points_draw 0 -sens_rays_draw 0 -ref_rays_draw 0 -write_info 1
 
 all:
-	log2pdf -config hokuyo.config -use odometry -in stallo2.log
+	json_extract -nth 0 -in stallo2.log -out stallo2-0.json
+	json_extract -nth 1 -in stallo2.log -out stallo2-1.json
+	log2pdf -config hokuyo.config -use odometry -in stallo2-0.json
+	log2pdf -config hokuyo.config -use odometry -in stallo2-1.json
+	log2pdf -config hokuyo.config -use odometry -in stallo2.log -distance_xy 0
 	sm2 -restart 0 -in stallo2.log -out stallo2_plicp.log -max_iterations 10 -file_jj stallo2_plicp.journal
 	sm_animate $(only_corr) -in  stallo2_plicp.journal -out 'stallo2_plicp_%02d.pdf'
 	sm2 -restart 0  -in stallo2.log -out stallo2_icp.log  -use_point_to_line_distance  0 -max_iterations 10 -file_jj stallo2_icp.journal