No commit message

No commit message

sm/lib/gpc/gpc_utils.c

@@ -43,6 +43,8 @@ double m_det(const gsl_matrix*A) {
 	int sign;
 	gsl_linalg_LU_decomp (m, perm, &sign);
 	double det = gsl_linalg_LU_det(m, sign);
+	
+	gsl_permutation_free(perm);
 	gsl_matrix_free(m);
 	return det;
 }
@@ -63,12 +65,12 @@ double poly_greatest_real_root(unsigned int n, double*a) {
 	double lambda = 0;
 	unsigned int i;
 	for (i = 0; i < n-1; i++) {
-		printf ("z%d = %+.18f %+.18f\n", i, z[2*i], z[2*i+1]);
+//		printf ("z%d = %+.18f %+.18f\n", i, z[2*i], z[2*i+1]);
 		// XXX ==0 is bad
 		if( (z[2*i+1]==0) && (z[2*i]>lambda))
 			lambda = z[2*i];
 	}
-	printf ("lambda = %+.18f \n", lambda);
+//	printf ("lambda = %+.18f \n", lambda);
 	return lambda;
 }
 

sm/src/icp/icp_loop.c

@@ -136,7 +136,10 @@ void sm_icp(struct sm_params*params, struct sm_result*res) {
 	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();
 }
 
@@ -191,7 +194,7 @@ void icp_loop(struct sm_params*params, const gsl_vector*start, gsl_vector*x_new,
 			break;
 		}
 
-//		kill_outliers_double(params, x_old);
+		kill_outliers_double(params, x_old);
 		int num_corr2 = ld_num_valid_correspondences(laser_sens);
 
 		double error=0;