icp_loop.c
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <string.h>
00003 
00004 #include <gsl/gsl_matrix.h>
00005 
00006 #include <gpc/gpc.h>
00007 #include <egsl/egsl_macros.h>
00008 
00009 #include "../csm_all.h"
00010 
00011 #include "icp.h"
00012 
00013 int icp_loop(struct sm_params*params, const double*q0, double*x_new, 
00014         double*total_error, int*valid, int*iterations) {
00015         if(any_nan(q0,3)) {
00016                 sm_error("icp_loop: Initial pose contains nan: %s\n", friendly_pose(q0));
00017                 return 0;
00018         }
00019                 
00020                 
00021         LDP laser_sens = params->laser_sens;
00022         double x_old[3], delta[3], delta_old[3] = {0,0,0};
00023         copy_d(q0, 3, x_old);
00024         unsigned int hashes[params->max_iterations];
00025         int iteration;
00026         
00027         sm_debug("icp: starting at  q0 =  %s  \n", friendly_pose(x_old));
00028         
00029         if(JJ) jj_loop_enter("iterations");
00030         
00031         int all_is_okay = 1;
00032         
00033         for(iteration=0; iteration<params->max_iterations;iteration++) {
00034                 if(JJ) jj_loop_iteration();
00035                 if(JJ) jj_add_double_array("x_old", x_old, 3);
00036 
00037                 egsl_push_named("icp_loop iteration");
00038                 sm_debug("== icp_loop: starting iteration. %d  \n", iteration);
00039 
00042                 ld_compute_world_coords(laser_sens, x_old);
00043 
00045                 if(params->use_corr_tricks)
00046                         find_correspondences_tricks(params);
00047                 else
00048                         find_correspondences(params);
00049 
00052                         if(params->debug_verify_tricks)
00053                                 debug_correspondences(params);
00054 
00055                 /* If not many correspondences, bail out */
00056                 int num_corr = ld_num_valid_correspondences(laser_sens);
00057                 double fail_perc = 0.05;
00058                 if(num_corr < fail_perc * laser_sens->nrays) { /* TODO: arbitrary */
00059                         sm_error("      : before trimming, only %d correspondences.\n",num_corr);
00060                         all_is_okay = 0;
00061                         egsl_pop_named("icp_loop iteration"); /* loop context */
00062                         break;
00063                 }
00064 
00065                 if(JJ) jj_add("corr0", corr_to_json(laser_sens->corr, laser_sens->nrays));
00066 
00067                 /* Kill some correspondences (using dubious algorithm) */
00068                 if(params->outliers_remove_doubles)
00069                         kill_outliers_double(params);
00070                 
00071                 int num_corr2 = ld_num_valid_correspondences(laser_sens);
00072 
00073                 if(JJ) jj_add("corr1", corr_to_json(laser_sens->corr, laser_sens->nrays));
00074                 
00075                 double error=0;
00076                 /* Trim correspondences */
00077                 kill_outliers_trim(params, &error);
00078                 int num_corr_after = ld_num_valid_correspondences(laser_sens);
00079                 
00080                 if(JJ) {
00081                         jj_add("corr2", corr_to_json(laser_sens->corr, laser_sens->nrays));
00082                         jj_add_int("num_corr0", num_corr);
00083                         jj_add_int("num_corr1", num_corr2);
00084                         jj_add_int("num_corr2", num_corr_after);
00085                 }
00086 
00087                 *total_error = error; 
00088                 *valid = num_corr_after;
00089 
00090                 sm_debug("  icp_loop: total error: %f  valid %d   mean = %f\n", *total_error, *valid, *total_error/ *valid);
00091                 
00092                 /* If not many correspondences, bail out */
00093                 if(num_corr_after < fail_perc * laser_sens->nrays){
00094                         sm_error("  icp_loop: failed: after trimming, only %d correspondences.\n",num_corr_after);
00095                         all_is_okay = 0;
00096                         egsl_pop_named("icp_loop iteration"); /* loop context */
00097                         break;
00098                 }
00099 
00100                 /* Compute next estimate based on the correspondences */
00101                 if(!compute_next_estimate(params, x_old, x_new)) {
00102                         sm_error("  icp_loop: Cannot compute next estimate.\n");
00103                         all_is_okay = 0;
00104                         egsl_pop_named("icp_loop iteration");
00105                         break;                  
00106                 }
00107 
00108                 pose_diff_d(x_new, x_old, delta);
00109                 
00110                 {
00111                         sm_debug("  icp_loop: killing. laser_sens has %d/%d rays valid,  %d corr found -> %d after double cut -> %d after adaptive cut \n", count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays, num_corr, num_corr2, num_corr_after);
00112                         if(JJ) {
00113                                 jj_add_double_array("x_new", x_new, 3);
00114                                 jj_add_double_array("delta", delta, 3);
00115                         }
00116                 }
00118                 hashes[iteration] = ld_corr_hash(laser_sens);
00119                 
00120                 {
00121                         sm_debug("  icp_loop: it. %d  hash=%d nvalid=%d mean error = %f, x_new= %s\n", 
00122                                 iteration, hashes[iteration], *valid, *total_error/ *valid, 
00123                                 friendly_pose(x_new));
00124                 }
00125 
00126                 
00128                 if(params->use_point_to_line_distance) {
00129                         int loop_detected = 0; /* TODO: make function */
00130                         int a; for(a=iteration-1;a>=0;a--) {
00131                                 if(hashes[a]==hashes[iteration]) {
00132                                         sm_debug("icpc: oscillation detected (cycle length = %d)\n", iteration-a);
00133                                         loop_detected = 1;
00134                                         break;
00135                                 }
00136                         }
00137                         if(loop_detected) {
00138                                 egsl_pop_named("icp_loop iteration");
00139                                 break;
00140                         } 
00141                 }
00142         
00143                 /* This termination criterium is useless when using
00144                    the point-to-line-distance; however, we put it here because
00145                    one can choose to use the point-to-point distance. */
00146                 if(termination_criterion(params, delta)) {
00147                         egsl_pop_named("icp_loop iteration");
00148                         break;
00149                 }
00150                 
00151                 copy_d(x_new, 3, x_old);
00152                 copy_d(delta, 3, delta_old);
00153                 
00154                 
00155                 egsl_pop_named("icp_loop iteration");
00156         }
00157 
00158         if(JJ) jj_loop_exit();
00159         
00160         *iterations = iteration+1;
00161         
00162         return all_is_okay;
00163 }
00164 
00165 int termination_criterion(struct sm_params*params, const double*delta){
00166         double a = norm_d(delta);
00167         double b = fabs(delta[2]);
00168         return (a<params->epsilon_xy) && (b<params->epsilon_theta);
00169 }
00170 
00171 int compute_next_estimate(struct sm_params*params, 
00172         const double x_old[3], double x_new[3]) 
00173 {
00174         LDP laser_ref  = params->laser_ref;
00175         LDP laser_sens = params->laser_sens;
00176         
00177         struct gpc_corr c[laser_sens->nrays];
00178 
00179         int i; int k=0;
00180         for(i=0;i<laser_sens->nrays;i++) {
00181                 if(!laser_sens->valid[i])
00182                         continue;
00183                         
00184                 if(!ld_valid_corr(laser_sens,i))
00185                         continue;
00186                 
00187                 int j1 = laser_sens->corr[i].j1;
00188                 int j2 = laser_sens->corr[i].j2;
00189 
00190                 c[k].valid = 1;
00191                 
00192                 if(laser_sens->corr[i].type == corr_pl) {
00193 
00194                         c[k].p[0] = laser_sens->points[i].p[0];
00195                         c[k].p[1] = laser_sens->points[i].p[1];
00196                         c[k].q[0] = laser_ref->points[j1].p[0];
00197                         c[k].q[1] = laser_ref->points[j1].p[1];
00198 
00200                         double diff[2];
00201                         diff[0] = laser_ref->points[j1].p[0]-laser_ref->points[j2].p[0];
00202                         diff[1] = laser_ref->points[j1].p[1]-laser_ref->points[j2].p[1];
00203                         double one_on_norm = 1 / sqrt(diff[0]*diff[0]+diff[1]*diff[1]);
00204                         double normal[2];
00205                         normal[0] = +diff[1] * one_on_norm;
00206                         normal[1] = -diff[0] * one_on_norm;
00207 
00208                         double cos_alpha = normal[0];
00209                         double sin_alpha = normal[1];
00210                                                 
00211                         c[k].C[0][0] = cos_alpha*cos_alpha;
00212                         c[k].C[1][0] = 
00213                         c[k].C[0][1] = cos_alpha*sin_alpha;
00214                         c[k].C[1][1] = sin_alpha*sin_alpha;
00215                         
00216 /*                      sm_debug("k=%d, i=%d sens_phi: %fdeg, j1=%d j2=%d,  alpha_seg=%f, cos=%f sin=%f \n", k,i,
00217                                 rad2deg(laser_sens->theta[i]), j1,j2, atan2(sin_alpha,cos_alpha), cos_alpha,sin_alpha);*/
00218                         
00219 #if 0
00220                         /* Note: it seems that because of numerical errors this matrix might be
00221                            not semidef positive. */
00222                         double det = c[k].C[0][0] * c[k].C[1][1] - c[k].C[0][1] * c[k].C[1][0];
00223                         double trace = c[k].C[0][0] + c[k].C[1][1];
00224                         
00225                         int semidef = (det >= 0) && (trace>0);
00226                         if(!semidef) {
00227         /*                      printf("%d: Adjusting correspondence weights\n",i);*/
00228                                 double eps = -det;
00229                                 c[k].C[0][0] += 2*sqrt(eps);
00230                                 c[k].C[1][1] += 2*sqrt(eps);
00231                         }
00232 #endif                  
00233                 } else {
00234                         c[k].p[0] = laser_sens->points[i].p[0];
00235                         c[k].p[1] = laser_sens->points[i].p[1];
00236                         
00237                         projection_on_segment_d(
00238                                 laser_ref->points[j1].p,
00239                                 laser_ref->points[j2].p,
00240                                 laser_sens->points_w[i].p,
00241                                 c[k].q);
00242 
00243                         /* Identity matrix */
00244                         c[k].C[0][0] = 1;
00245                         c[k].C[1][0] = 0;
00246                         c[k].C[0][1] = 0;
00247                         c[k].C[1][1] = 1;
00248                 }
00249                 
00250                 
00251                 double factor = 1;
00252                 
00253                 /* Scale the correspondence weight by a factor concerning the 
00254                    information in this reading. */
00255                 if(params->use_ml_weights) {
00256                         int have_alpha = 0;
00257                         double alpha = 0;
00258                         if(!is_nan(laser_ref->true_alpha[j1])) {
00259                                 alpha = laser_ref->true_alpha[j1];
00260                                 have_alpha = 1;
00261                         } else if(laser_ref->alpha_valid[j1]) {
00262                                 alpha = laser_ref->alpha[j1];;
00263                                 have_alpha = 1;
00264                         } else have_alpha = 0;
00265                         
00266                         if(have_alpha) {
00267                                 double pose_theta = x_old[2];
00272                                 double beta = alpha - (pose_theta + laser_sens->theta[i]);
00273                                 factor = 1 / square(cos(beta));
00274                         } else {
00275                                 static int warned_before = 0;
00276                                 if(!warned_before) {
00277                                         sm_error("Param use_ml_weights was active, but not valid alpha[] or true_alpha[]." 
00278                                                   "Perhaps, if this is a single ray not having alpha, you should mark it as inactive.\n");                                              
00279                                         sm_error("Writing laser_ref: \n");                                              
00280                                         ld_write_as_json(laser_ref, stderr);
00281                                         warned_before = 1;
00282                                 }
00283                         }
00284                 } 
00285                 
00286                 /* Weight the points by the sigma in laser_sens */
00287                 if(params->use_sigma_weights) {
00288                         if(!is_nan(laser_sens->readings_sigma[i])) {
00289                                 factor *= 1 / square(laser_sens->readings_sigma[i]);
00290                         } else {
00291                                 static int warned_before = 0;
00292                                 if(!warned_before) {
00293                                         sm_error("Param use_sigma_weights was active, but the field readings_sigma[] was not filled in.\n");                                            
00294                                         sm_error("Writing laser_sens: \n");                                             
00295                                         ld_write_as_json(laser_sens, stderr);
00296                                 }
00297                         }
00298                 }
00299                 
00300                 c[k].C[0][0] *= factor;
00301                 c[k].C[1][0] *= factor;
00302                 c[k].C[0][1] *= factor;
00303                 c[k].C[1][1] *= factor;
00304                 
00305                 k++;
00306         }
00307         
00308         /* TODO: use prior for odometry */
00309         double std = 0.11;
00310         const double inv_cov_x0[9] = 
00311                 {1/(std*std), 0, 0,
00312                  0, 1/(std*std), 0,
00313                  0, 0, 0};
00314         
00315         
00316         int ok = gpc_solve(k, c, 0, inv_cov_x0, x_new);
00317         if(!ok) {
00318                 sm_error("gpc_solve_valid failed\n");
00319                 return 0;
00320         }
00321 
00322         double old_error = gpc_total_error(c, k, x_old);
00323         double new_error = gpc_total_error(c, k, x_new);
00324 
00325         sm_debug("\tcompute_next_estimate: old error: %f  x_old= %s \n", old_error, friendly_pose(x_old));
00326         sm_debug("\tcompute_next_estimate: new error: %f  x_new= %s \n", new_error, friendly_pose(x_new));
00327         sm_debug("\tcompute_next_estimate: new error - old_error: %g \n", new_error-old_error);
00328 
00329         double epsilon = 0.000001;
00330         if(new_error > old_error + epsilon) {
00331                 sm_error("\tcompute_next_estimate: something's fishy here! Old error: %lf  new error: %lf  x_old %lf %lf %lf x_new %lf %lf %lf\n",old_error,new_error,x_old[0],x_old[1],x_old[2],x_new[0],x_new[1],x_new[2]);
00332         }
00333         
00334         return 1;
00335 }
00336         
00337 
00338 


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29