gpm.c
Go to the documentation of this file.
00001 #include <gsl/gsl_histogram.h>
00002 #include <gsl/gsl_matrix.h>
00003 
00004 
00005 #include "../csm_all.h"
00006 #include "gpm.h"
00007 
00008 #include <egsl/egsl_macros.h>
00009 
00010         
00011 void sm_gpm(struct sm_params*params, struct sm_result*res) {
00012         res->valid = 0;
00013         /* Check for well-formedness of the input data */
00014         if(!ld_valid_fields(params->laser_ref) || 
00015            !ld_valid_fields(params->laser_sens)) {
00016                 return;
00017         }
00018         
00019         LDP laser_ref  = params->laser_ref;
00020         LDP laser_sens = params->laser_sens;
00021         
00022         /* We need to compute cartesian points */
00023         ld_compute_cartesian(laser_ref);
00024         /* ... and orientation */
00025         ld_simple_clustering(laser_ref, params->clustering_threshold);
00026         ld_compute_orientation(laser_ref, params->orientation_neighbourhood, params->sigma);
00027         /* ... for both scans. */
00028         ld_compute_cartesian(laser_sens);
00029         ld_simple_clustering(laser_sens, params->clustering_threshold);
00030         ld_compute_orientation(laser_sens, params->orientation_neighbourhood, params->sigma);
00031 
00032         /* Create an histogram whose bin is large `theta_bin_size` */
00033         double theta_bin_size = deg2rad(params->gpm_theta_bin_size_deg);
00034         double hist_min = -M_PI-theta_bin_size; /* be robust */
00035         double hist_max = +M_PI+theta_bin_size;
00036         size_t nbins = (size_t) ceil(  (hist_max-hist_min) / theta_bin_size);
00037         gsl_histogram*hist = gsl_histogram_alloc(nbins);
00038         gsl_histogram_set_ranges_uniform(hist, hist_min, hist_max);
00039         
00040         /* Fill the histogram with samples */
00041         double u[3]; copy_d(params->first_guess, 3, u);
00042         sm_debug("gpm 1/2: old u = : %s \n", friendly_pose(u) );
00043         
00044         int interval = params->gpm_interval;
00045         
00046         int num_correspondences_theta=-1;
00047         
00048         
00049         ght_find_theta_range(laser_ref, laser_sens,
00050                 u, params->max_linear_correction,
00051                 params->max_angular_correction_deg, interval, hist, &num_correspondences_theta);
00052                 
00053         if(num_correspondences_theta < laser_ref->nrays) {
00054                 sm_error("sm_gpm(): I found only %d correspondences in the first pass of GPM. I consider it a failure.\n",
00055                         num_correspondences_theta);
00056                 return;
00057         }
00058         
00059         /* Find the bin with most samples */
00060         size_t max_bin = gsl_histogram_max_bin(hist);
00061         
00062         /* Around that value will be the range admissible for theta */
00063         double min_range, max_range;
00064         gsl_histogram_get_range(hist,max_bin,&min_range,&max_range);
00065         
00066         /* Extend the range of the search */
00067         double extend_range = deg2rad(params->gpm_extend_range_deg);
00068         min_range += -extend_range;
00069         max_range += +extend_range;
00070 
00071         /*      if(jf()) fprintf(jf(), "iteration 0\n");
00072         journal_pose("x_old", u);*/
00073 
00074 
00075         /*      if(jf()) fprintf(jf(), "iteration 1\n");
00076         journal_pose("x_old", u);*/
00077 
00078 
00079         /* Now repeat the samples generation with a smaller domain */
00080         u[2] = 0.5 * (max_range + min_range);
00081         double new_range_deg = rad2deg( 0.5*(max_range - min_range) );
00082         
00083         double x_new[3];
00084         int num_correspondences=-1;
00085         ght_one_shot(laser_ref, laser_sens,
00086                         u, params->max_linear_correction*2,
00087                         new_range_deg, interval, x_new, &num_correspondences) ;
00088                         
00089         if(num_correspondences < laser_ref->nrays) {
00090                 sm_error("sm_gpm(): I found only %d correspondences in the second pass of GPM. I consider it a failure.\n",
00091                         num_correspondences);
00092                 return;
00093         }
00094 
00095         /* Et voila, in x_new we have the answer */
00096 
00097         {
00098                 sm_debug("gpm : max_correction_lin %f def %f\n", params->max_linear_correction,                 params->max_angular_correction_deg);
00099                 sm_debug("gpm : acceptable range for theta: [%f, %f]\n", min_range,max_range);
00100                 sm_debug("gpm : 1) Num correspondences for theta: %d\n", num_correspondences_theta);
00101 
00102                 sm_debug("gpm 1/2: new u = : %s \n", friendly_pose(u) );
00103                 sm_debug("gpm 1/2: New range: %f to %f\n",rad2deg(min_range),rad2deg(max_range));
00104 
00105                 sm_debug("gpm 2/2: Solution: %s \n", friendly_pose(x_new));
00106         /*      if(jf()) fprintf(jf(), "iteration 2\n");
00107                 journal_pose("x_old", x_new);   */
00108         }
00109 
00110         /* Administrivia */
00111 
00112         res->valid = 1;
00113         copy_d(x_new, 3, res->x);
00114         
00115         res->iterations = 0;
00116         
00117         gsl_histogram_free(hist);
00118 }
00119 
00120 void ght_find_theta_range(LDP laser_ref, LDP laser_sens,
00121         const double*x0, double max_linear_correction,
00122         double max_angular_correction_deg, int interval, gsl_histogram*hist, int*num_correspondences) 
00123 {
00125         ld_compute_world_coords(laser_sens, x0);
00126         
00127         int count = 0;
00128         int i;
00129         for(i=0;i<laser_sens->nrays;i++) {
00130                 if(!laser_sens->alpha_valid[i]) continue;
00131                 if(i % interval) continue;
00132                 
00133                 const double * p_i = laser_sens->points[i].p;
00134                 
00135                 const double * p_i_w = laser_sens->points_w[i].p;
00136                 int from; int to; int start_cell;
00137                 possible_interval(p_i_w, laser_ref, max_angular_correction_deg,
00138                         max_linear_correction, &from, &to, &start_cell);
00139 
00140 //              printf("\n i=%d interval = [%d,%d] ", i, from, to);
00141                 int j;
00142                 for(j=from;j<=to;j++) {
00143                         if(!laser_ref->alpha_valid[j]) continue;
00144                         if(j % interval) continue;
00145                         
00146                         double theta = angleDiff(laser_ref->alpha[j], laser_sens->alpha[i]);
00147                         double theta_diff = angleDiff(theta,x0[2]); 
00148                         if( fabs(theta_diff) > deg2rad(max_angular_correction_deg) )
00149                                 continue;
00150                         theta = x0[2] + theta_diff; // otherwise problems near +- PI
00151         
00152                         const double * p_j = laser_ref->points[j].p;
00153                         
00154                         double c = cos(theta); double s = sin(theta);
00155                         double t_x = p_j[0] - (c*p_i[0]-s*p_i[1]);
00156                         double t_y = p_j[1] - (s*p_i[0]+c*p_i[1]);
00157                         double t_dist = sqrt( square(t_x-x0[0]) + square(t_y-x0[1]) );
00158 
00159                         if(t_dist > max_linear_correction)
00160                                 continue;
00161                                 
00162                         /*double weight = 1/(laser_sens->cov_alpha[i]+laser_ref->cov_alpha[j]);*/
00163                         double weight = 1;
00164                         gsl_histogram_accumulate(hist, theta, weight);
00165                         gsl_histogram_accumulate(hist, theta+2*M_PI, weight); /* be robust */
00166                         gsl_histogram_accumulate(hist, theta-2*M_PI, weight);
00167                         count ++;
00168                 }
00169         }
00170         *num_correspondences = count;
00171         sm_debug(" correspondences = %d\n",count);
00172 }
00173 
00174 void ght_one_shot(LDP laser_ref, LDP laser_sens,
00175                 const double*x0, double max_linear_correction,
00176         double max_angular_correction_deg, int interval, double*x, int*num_correspondences) 
00177 {
00179         ld_compute_world_coords(laser_sens, x0);
00180         
00181         double L[3][3]  = {{0,0,0},{0,0,0},{0,0,0}};
00182         double z[3] = {0,0,0};
00183         
00184         int count = 0;
00185         int i;
00186         for(i=0;i<laser_sens->nrays;i++) {
00187                 if(!laser_sens->alpha_valid[i]) continue;
00188                 if(i % interval) continue;
00189                 
00190 
00191                 const double * p_i = laser_sens->points_w[i].p;
00192 
00193                 const double * p_i_w = laser_sens->points_w[i].p;
00194                 int from; int to; int start_cell;
00195                 possible_interval(p_i_w, laser_ref, max_angular_correction_deg,
00196                         max_linear_correction, &from, &to, &start_cell);
00197 //              from = 0; to = laser_ref->nrays-1;
00198                 
00199 
00200                 int j;
00201                 for(j=from;j<=to;j++) {
00202                         if(j % interval) continue;
00203                         if(!laser_ref->alpha_valid[j]) continue;
00204                         
00205                         double theta = angleDiff(laser_ref->alpha[j], laser_sens->alpha[i]);
00206                         double theta_diff = angleDiff(theta,x0[2]); 
00207                         if( fabs(theta_diff) > deg2rad(max_angular_correction_deg) )
00208                                 continue;
00209                         theta = x0[2] + theta_diff; // otherwise problems near +- PI
00210         
00211                         const double * p_j = laser_ref->points[j].p;
00212                         
00213                         double c = cos(theta); double s = sin(theta);
00214                         double t_x = p_j[0] - (c*p_i[0]-s*p_i[1]);
00215                         double t_y = p_j[1] - (s*p_i[0]+c*p_i[1]);
00216                         double t_dist = sqrt( square(t_x-x0[0]) + square(t_y-x0[1]) );
00217 
00218                         if(t_dist > max_linear_correction)
00219                                 continue;
00220 
00221                         /*double weight = 1/(laser_sens->cov_alpha[i]+laser_ref->cov_alpha[j]);
00222                         double weight = exp( -square(t_dist) - 5 * square(theta-x0[2]) );*/
00223                         
00224                         double weight = 1;
00225                         
00226                         double alpha = laser_ref->alpha[j];
00227                         double ca = cos(alpha); double sa=sin(alpha);
00228 
00229 //                              printf("%d ", (int) rad2deg(theta));
00230 
00231 /*                      printf("valid %d alpha %f weight %f t_x %f t_y %f\n",
00232                                 laser_ref->alpha_valid[j],alpha,weight,
00233                                 t_x, t_y); */
00234                         z[0] += weight*(ca*ca*t_x + sa*ca*t_y);
00235                         z[1] += weight*(sa*ca*t_x + sa*sa*t_y);
00236                         z[2] += weight*theta;
00237                         L[0][0] += weight* ca * ca;
00238                         L[0][1] += weight* sa * ca;
00239                         L[1][0] += weight* sa * ca;
00240                         L[1][1] += weight* sa * sa;
00241                         L[2][2] += weight;
00242                         
00243                         count += 1;
00244                 }
00245         }
00246         
00247         *num_correspondences = count;
00248 
00249         if(1) {
00250                 
00251                 double weight = 0.5 * count;
00252                 z[0] += x0[0] * weight;
00253                 z[1] += x0[1] * weight;
00254                 L[0][0] += weight;
00255                 L[0][1] += 0;
00256                 L[1][0] += 0;
00257                 L[1][1] += weight;
00258         }
00259         
00260 
00261         egsl_push();
00262                 val eL = egsl_alloc(3,3);
00263                         size_t a,b; 
00264                         for(a=0;a<3;a++) 
00265                         for(b=0;b<3;b++) 
00266                                 *egsl_atmp(eL,a,b) = L[a][b];
00267 
00268 /*              egsl_print("eL", eL);*/
00269                 val ez = egsl_vFa(3,z);
00270                 
00271                 val ex = m(inv(eL), ez);
00272                 
00273                 egsl_v2a(ex, x);
00274                 
00275 
00276 /*              egsl_print("eL", eL);
00277                 egsl_print("ez", ez);
00278                 egsl_print("ex", ex); */
00279 
00280         egsl_pop();
00281 
00282 //      sm_debug("gpm: second step: theta = %f   %f / %d = %f \n", rad2deg(x[2]), rad2deg(z[2]), count, rad2deg(z[2]) / count);
00283         sm_debug("gpm: second step: found %d correspondences\n",count);
00284         
00285 }
00286 


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