icp_corr_dumb.c
Go to the documentation of this file.
00001 #include <gsl/gsl_vector.h>
00002 
00003 #include "icp.h"
00004 #include "../csm_all.h"
00005 
00006 int compatible(struct sm_params*params, int i, int j);
00007 
00008 int compatible(struct sm_params*params, int i, int j) {
00009         if(!params->do_alpha_test) return 1;
00010 
00011         double theta0 = 0; /* FIXME */
00012         if((params->laser_sens->alpha_valid[i]==0) ||
00013          (params->laser_ref->alpha_valid[j]==0))
00014          return 1;
00015 
00016         double alpha_i = params->laser_sens->alpha[i];
00017         double alpha_j = params->laser_ref->alpha[j];
00018         double tolerance = deg2rad(params->do_alpha_test_thresholdDeg);
00019 
00021         double theta = angleDiff(alpha_j, alpha_i);
00022         if(fabs(angleDiff(theta,theta0))>
00023                 tolerance+deg2rad(params->max_angular_correction_deg)) {
00024          return 0;
00025         } else {
00026          return 1;
00027         }
00028 }
00029 
00030 void find_correspondences(struct sm_params*params) {
00031         const LDP laser_ref  = params->laser_ref;
00032         const LDP laser_sens = params->laser_sens;
00033 
00034         int i;
00035         for(i=0;i<laser_sens->nrays;i++) {
00036                 if(!ld_valid_ray(laser_sens,i)) {
00037 /*                      sm_debug("dumb: i %d is invalid \n", i);*/
00038                         ld_set_null_correspondence(laser_sens, i);
00039                         continue; 
00040                 }
00041 
00042                 double *p_i_w = laser_sens->points_w[i].p;
00043                 
00044                 int j1 = -1;
00045                 double best_dist = 10000;
00046                 
00047                 int from; int to; int start_cell;
00048                 possible_interval(p_i_w, laser_ref, params->max_angular_correction_deg,
00049                         params->max_linear_correction, &from, &to, &start_cell);
00050 
00051 /*              sm_debug("dumb: i %d from  %d to %d \n", i, from, to); */
00052                 int j;
00053                 for(j=from;j<=to;j++) {
00054                         if(!ld_valid_ray(laser_ref,j)) {
00055 /*                              sm_debug("dumb: i %d      j %d invalid\n", i, j);*/
00056                                 continue;
00057                         }
00058                         double dist = distance_squared_d(p_i_w, laser_ref->points[j].p);
00059 /*                      sm_debug("dumb: i %d j1 %d j %d d %f\n", i,j1,j,dist);*/
00060                         if(dist>square(params->max_correspondence_dist)) continue;
00061                         
00062                         if( (-1 == j1) || (dist < best_dist) ) {
00063                                 if(compatible(params, i, j)) {
00064                                         j1 = j; 
00065                                         best_dist = dist;
00066                                 }
00067                         } 
00068                 }
00069                 
00070                 if(j1==-1) {/* no match */
00071                         ld_set_null_correspondence(laser_sens, i);
00072                         continue;
00073                 }
00074                 /* Do not match with extrema*/
00075                 if(j1==0 || (j1 == (laser_ref->nrays-1))) {/* no match */
00076                         ld_set_null_correspondence(laser_sens, i);
00077                         continue;
00078                 }
00079                 
00080                 int j2;
00081                 int j2up   = ld_next_valid_up   (laser_ref, j1);
00082                 int j2down = ld_next_valid_down (laser_ref, j1);
00083                 if((j2up==-1)&&(j2down==-1)) {
00084                         ld_set_null_correspondence(laser_sens, i);
00085                         continue;
00086                 }
00087                 if(j2up  ==-1) { j2 = j2down; } else
00088                 if(j2down==-1) { j2 = j2up; } else {
00089                         double dist_up   = distance_squared_d(p_i_w, laser_ref->points[j2up  ].p);
00090                         double dist_down = distance_squared_d(p_i_w, laser_ref->points[j2down].p);
00091                         j2 = dist_up < dist_down ? j2up : j2down;
00092                 }
00093                 
00094                 ld_set_correspondence(laser_sens, i, j1, j2);
00095                 laser_sens->corr[i].dist2_j1 = best_dist;
00096                 laser_sens->corr[i].type = 
00097                         params->use_point_to_line_distance ? corr_pl : corr_pp;
00098                 
00099         }
00100         
00101 }
00102 


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