icp.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 
00014 void sm_journal_open(const char* file) {
00015         file = 0;
00016 /*      journal_open(file);*/
00017 }
00018 
00019 void ld_invalid_if_outside(LDP ld, double min_reading, double max_reading) {
00020         int i;
00021         for(i=0;i<ld->nrays;i++) {
00022                 if(!ld_valid_ray(ld, i)) continue;
00023                 double r = ld->readings[i];
00024                 if( r <= min_reading || r > max_reading)
00025                         ld->valid[i] = 0;
00026         }
00027 }
00028 
00029 void sm_icp(struct sm_params*params, struct sm_result*res) {
00030         res->valid = 0;
00031 
00032         LDP laser_ref  = params->laser_ref;
00033         LDP laser_sens = params->laser_sens;
00034         
00035         if(!ld_valid_fields(laser_ref) || 
00036            !ld_valid_fields(laser_sens)) {
00037                 return;
00038         }
00039         
00040         sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid\n",
00041                 count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
00042                 count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays);
00043         
00044         
00046         ld_invalid_if_outside(laser_ref, params->min_reading, params->max_reading);
00047         ld_invalid_if_outside(laser_sens, params->min_reading, params->max_reading);
00048         
00049         sm_debug("sm_icp:  laser_sens has %d/%d; laser_ref has %d/%d rays valid (after removing outside interval [%f, %f])\n",
00050                 count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
00051                 count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays,
00052            params->min_reading, params->max_reading);
00053         
00054         if(JJ) jj_context_enter("sm_icp");
00055         
00056         egsl_push_named("sm_icp");
00057         
00058                         
00059         if(params->use_corr_tricks || params->debug_verify_tricks)
00060                 ld_create_jump_tables(laser_ref);
00061                 
00062         ld_compute_cartesian(laser_ref);
00063         ld_compute_cartesian(laser_sens);
00064 
00065         if(params->do_alpha_test) {
00066                 ld_simple_clustering(laser_ref, params->clustering_threshold);
00067                 ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
00068                 ld_simple_clustering(laser_sens, params->clustering_threshold);
00069                 ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);
00070         }
00071 
00072         if(JJ) jj_add("laser_ref",  ld_to_json(laser_ref));
00073         if(JJ) jj_add("laser_sens", ld_to_json(laser_sens));
00074         
00075         gsl_vector * x_new = gsl_vector_alloc(3);
00076         gsl_vector * x_old = vector_from_array(3, params->first_guess);
00077         
00078         if(params->do_visibility_test) {
00079                 sm_debug("laser_ref:\n");
00080                 visibilityTest(laser_ref, x_old);
00081 
00082                 sm_debug("laser_sens:\n");
00083                 gsl_vector * minus_x_old = gsl_vector_alloc(3);
00084                 ominus(x_old,minus_x_old);
00085                 visibilityTest(laser_sens, minus_x_old);
00086                 gsl_vector_free(minus_x_old);
00087         }
00088         
00089         double error;
00090         int iterations;
00091         int nvalid;
00092         if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
00093                 sm_error("icp: ICP failed for some reason. \n");
00094                 res->valid = 0;
00095                 res->iterations = iterations;
00096                 res->nvalid = 0;
00097                 
00098         } else {
00099                 /* It was succesfull */
00100 
00101                 int restarted = 0;              
00102                 double best_error = error;
00103                 gsl_vector * best_x = gsl_vector_alloc(3);
00104                 gsl_vector_memcpy(best_x, x_new);
00105 
00106                 if(params->restart && 
00107                         (error/nvalid)>(params->restart_threshold_mean_error) ) {
00108                         sm_debug("Restarting: %f > %f \n",(error/nvalid),(params->restart_threshold_mean_error));
00109                         restarted = 1;
00110                         double dt  = params->restart_dt;
00111                         double dth = params->restart_dtheta;
00112                         sm_debug("icp_loop: dt = %f dtheta= %f deg\n",dt,rad2deg(dth));
00113                 
00114                         double perturb[6][3] = {
00115                                 {dt,0,0}, {-dt,0,0},
00116                                 {0,dt,0}, {0,-dt,0},
00117                                 {0,0,dth}, {0,0,-dth}
00118                         };
00119 
00120                         int a; for(a=0;a<6;a++){
00121                                 sm_debug("-- Restarting with perturbation #%d\n", a);
00122                                 struct sm_params my_params = *params;
00123                                 gsl_vector * start = gsl_vector_alloc(3);
00124                                         gvs(start, 0, gvg(x_new,0)+perturb[a][0]);
00125                                         gvs(start, 1, gvg(x_new,1)+perturb[a][1]);
00126                                         gvs(start, 2, gvg(x_new,2)+perturb[a][2]);
00127                                 gsl_vector * x_a = gsl_vector_alloc(3);
00128                                 double my_error; int my_valid; int my_iterations;
00129                                 if(!icp_loop(&my_params, start->data, x_a->data, &my_error, &my_valid, &my_iterations)){
00130                                         sm_error("Error during restart #%d/%d. \n", a, 6);
00131                                         break;
00132                                 }
00133                                 iterations+=my_iterations;
00134                 
00135                                 if(my_error < best_error) {
00136                                         sm_debug("--Perturbation #%d resulted in error %f < %f\n", a,my_error,best_error);
00137                                         gsl_vector_memcpy(best_x, x_a);
00138                                         best_error = my_error;
00139                                 }
00140                                 gsl_vector_free(x_a); gsl_vector_free(start);
00141                         }
00142                 }
00143         
00144         
00145                 /* At last, we did it. */
00146                 res->valid = 1;
00147                 vector_to_array(best_x, res->x);
00148                 sm_debug("icp: final x =  %s  \n", gsl_friendly_pose(best_x));
00149         
00150                 if (restarted) { // recompute correspondences in case of restarts
00151                         ld_compute_world_coords(laser_sens, res->x);
00152                         if(params->use_corr_tricks)
00153                                 find_correspondences_tricks(params);
00154                         else
00155                                 find_correspondences(params);
00156                 }
00157 
00158                 if(params->do_compute_covariance)  {
00159 
00160                         val cov0_x, dx_dy1, dx_dy2;
00161                         compute_covariance_exact(
00162                                 laser_ref, laser_sens, best_x,
00163                                 &cov0_x, &dx_dy1, &dx_dy2);
00164                 
00165                         val cov_x = sc(square(params->sigma), cov0_x); 
00166 /*                      egsl_v2da(cov_x, res->cov_x); */
00167                 
00168                         res->cov_x_m = egsl_v2gslm(cov_x);
00169                         res->dx_dy1_m = egsl_v2gslm(dx_dy1);
00170                         res->dx_dy2_m = egsl_v2gslm(dx_dy2);
00171                 
00172                         if(0) {
00173                                 egsl_print("cov0_x", cov0_x);
00174                                 egsl_print_spectrum("cov0_x", cov0_x);
00175                 
00176                                 val fim = ld_fisher0(laser_ref);
00177                                 val ifim = inv(fim);
00178                                 egsl_print("fim", fim);
00179                                 egsl_print_spectrum("ifim", ifim);
00180                         }
00181                 }
00182         
00183                 res->error = best_error;
00184                 res->iterations = iterations;
00185                 res->nvalid = nvalid;
00186 
00187                 gsl_vector_free(best_x);
00188         }
00189         gsl_vector_free(x_new);
00190         gsl_vector_free(x_old);
00191 
00192 
00193         egsl_pop_named("sm_icp");
00194 
00195         if(JJ) jj_context_exit();
00196 }
00197 
00198 void sm_icp_xy(struct sm_params*params, struct sm_result*res) 
00199 {
00200         res->valid = 0;
00201 
00202         LDP laser_ref  = params->laser_ref;
00203         LDP laser_sens = params->laser_sens;
00204         
00205         if(!ld_valid_fields(laser_ref) || 
00206            !ld_valid_fields(laser_sens)) {
00207                 return;
00208         }
00209         
00210 /*
00211         sm_debug("sm_icp: laser_sens has %d/%d; laser_ref has %d/%d rays valid\n",
00212                 count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
00213                 count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays);
00214         */
00215         
00217         ld_invalid_if_outside(laser_ref, params->min_reading, params->max_reading);
00218         ld_invalid_if_outside(laser_sens, params->min_reading, params->max_reading);
00219         
00220 /*
00221         sm_debug("sm_icp:  laser_sens has %d/%d; laser_ref has %d/%d rays valid (after removing outside interval [%f, %f])\n",
00222                 count_equal(laser_sens->valid, laser_sens->nrays, 1), laser_sens->nrays,
00223                 count_equal(laser_ref->valid, laser_ref->nrays, 1), laser_ref->nrays,
00224            params->min_reading, params->max_reading);
00225         
00226         if(JJ) jj_context_enter("sm_icp");
00227         
00228         egsl_push_named("sm_icp");
00229         */
00230                         
00231         if(params->use_corr_tricks || params->debug_verify_tricks)
00232                 ld_create_jump_tables(laser_ref);
00233 /*              
00234         ld_compute_cartesian(laser_ref);
00235         ld_compute_cartesian(laser_sens);
00236 */
00237 
00238         if(params->do_alpha_test) {
00239                 ld_simple_clustering(laser_ref, params->clustering_threshold);
00240                 ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
00241                 ld_simple_clustering(laser_sens, params->clustering_threshold);
00242                 ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);
00243         }
00244 
00245         if(JJ) jj_add("laser_ref",  ld_to_json(laser_ref));
00246         if(JJ) jj_add("laser_sens", ld_to_json(laser_sens));
00247         
00248         gsl_vector * x_new = gsl_vector_alloc(3);
00249         gsl_vector * x_old = vector_from_array(3, params->first_guess);
00250         
00251         if(params->do_visibility_test) {
00252                 sm_debug("laser_ref:\n");
00253                 visibilityTest(laser_ref, x_old);
00254 
00255                 sm_debug("laser_sens:\n");
00256                 gsl_vector * minus_x_old = gsl_vector_alloc(3);
00257                 ominus(x_old,minus_x_old);
00258                 visibilityTest(laser_sens, minus_x_old);
00259                 gsl_vector_free(minus_x_old);
00260         }
00261         
00262         double error;
00263         int iterations;
00264         int nvalid;
00265         if(!icp_loop(params, x_old->data, x_new->data, &error, &nvalid, &iterations)) {
00266                 sm_error("icp: ICP failed for some reason. \n");
00267                 res->valid = 0;
00268                 res->iterations = iterations;
00269                 res->nvalid = 0;
00270                 
00271         } else {
00272                 /* It was succesfull */
00273                 
00274                 double best_error = error;
00275                 gsl_vector * best_x = gsl_vector_alloc(3);
00276                 gsl_vector_memcpy(best_x, x_new);
00277 
00278                 if(params->restart && 
00279                         (error/nvalid)>(params->restart_threshold_mean_error) ) {
00280                         sm_debug("Restarting: %f > %f \n",(error/nvalid),(params->restart_threshold_mean_error));
00281                         double dt  = params->restart_dt;
00282                         double dth = params->restart_dtheta;
00283                         sm_debug("icp_loop: dt = %f dtheta= %f deg\n",dt,rad2deg(dth));
00284                 
00285                         double perturb[6][3] = {
00286                                 {dt,0,0}, {-dt,0,0},
00287                                 {0,dt,0}, {0,-dt,0},
00288                                 {0,0,dth}, {0,0,-dth}
00289                         };
00290 
00291                         int a; for(a=0;a<6;a++){
00292                                 sm_debug("-- Restarting with perturbation #%d\n", a);
00293                                 struct sm_params my_params = *params;
00294                                 gsl_vector * start = gsl_vector_alloc(3);
00295                                         gvs(start, 0, gvg(x_new,0)+perturb[a][0]);
00296                                         gvs(start, 1, gvg(x_new,1)+perturb[a][1]);
00297                                         gvs(start, 2, gvg(x_new,2)+perturb[a][2]);
00298                                 gsl_vector * x_a = gsl_vector_alloc(3);
00299                                 double my_error; int my_valid; int my_iterations;
00300                                 if(!icp_loop(&my_params, start->data, x_a->data, &my_error, &my_valid, &my_iterations)){
00301                                         sm_error("Error during restart #%d/%d. \n", a, 6);
00302                                         break;
00303                                 }
00304                                 iterations+=my_iterations;
00305                 
00306                                 if(my_error < best_error) {
00307                                         sm_debug("--Perturbation #%d resulted in error %f < %f\n", a,my_error,best_error);
00308                                         gsl_vector_memcpy(best_x, x_a);
00309                                         best_error = my_error;
00310                                 }
00311                                 gsl_vector_free(x_a); gsl_vector_free(start);
00312                         }
00313                 }
00314         
00315         
00316                 /* At last, we did it. */
00317                 res->valid = 1;
00318                 vector_to_array(best_x, res->x);
00319                 sm_debug("icp: final x =  %s  \n", gsl_friendly_pose(best_x));
00320         
00321         
00322                 if(params->do_compute_covariance)  {
00323 
00324                         val cov0_x, dx_dy1, dx_dy2;
00325                         compute_covariance_exact(
00326                                 laser_ref, laser_sens, best_x,
00327                                 &cov0_x, &dx_dy1, &dx_dy2);
00328                 
00329                         val cov_x = sc(square(params->sigma), cov0_x); 
00330 //                      egsl_v2da(cov_x, res->cov_x); 
00331                 
00332                         res->cov_x_m = egsl_v2gslm(cov_x);
00333                         res->dx_dy1_m = egsl_v2gslm(dx_dy1);
00334                         res->dx_dy2_m = egsl_v2gslm(dx_dy2);
00335                 
00336                         if(0) {
00337                                 egsl_print("cov0_x", cov0_x);
00338                                 egsl_print_spectrum("cov0_x", cov0_x);
00339                 
00340                                 val fim = ld_fisher0(laser_ref);
00341                                 val ifim = inv(fim);
00342                                 egsl_print("fim", fim);
00343                                 egsl_print_spectrum("ifim", ifim);
00344                         }
00345                 }
00346         
00347                 res->error = best_error;
00348                 res->iterations = iterations;
00349                 res->nvalid = nvalid;
00350 
00351                 gsl_vector_free(x_new);
00352                 gsl_vector_free(x_old);
00353                 gsl_vector_free(best_x);
00354         }
00355 /*
00356         egsl_pop_named("sm_icp");
00357 
00358         if(JJ) jj_context_exit();
00359 */
00360 }


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