laser_data.c
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <stdlib.h>
00003 #include <string.h>
00004 
00005 #include "csm_all.h"
00006 
00007 double* alloc_double_array(int n, double def);
00008 int* alloc_int_array(int n, int def);
00009 
00010 /* -------------------------------------------------- */
00011 
00012 LDP ld_alloc_new(int nrays) {
00013         LDP ld = malloc(sizeof(struct laser_data));
00014         ld_alloc(ld, nrays);
00015         return ld;
00016 }
00017 
00018 double* alloc_double_array(int n, double def) {
00019         double *v = (double*) malloc(sizeof(double)*n);
00020         int i=0; for(i=0;i<n;i++) {
00021                 v[i] = def;
00022         }
00023         return v;
00024 }
00025 
00026 int* alloc_int_array(int n, int def) {
00027         int *v = (int*) malloc(sizeof(int)*n);
00028         int i=0; for(i=0;i<n;i++) {
00029                 v[i] = def;
00030         }
00031         return v;
00032 }
00033 
00034 void ld_alloc(LDP ld, int nrays) {
00035         ld->nrays = nrays;
00036         
00037         ld->valid        = alloc_int_array(nrays, 0);
00038         ld->readings     = alloc_double_array(nrays, GSL_NAN);
00039         ld->readings_sigma = alloc_double_array(nrays, GSL_NAN);
00040         ld->theta        = alloc_double_array(nrays, GSL_NAN);
00041         
00042         ld->min_theta = GSL_NAN;
00043         ld->max_theta = GSL_NAN;
00044         
00045         ld->cluster      = alloc_int_array(nrays, -1);
00046         ld->alpha        = alloc_double_array(nrays, GSL_NAN);
00047         ld->cov_alpha    = alloc_double_array(nrays, GSL_NAN);
00048         ld->alpha_valid  = alloc_int_array(nrays, 0);
00049 
00050         ld->true_alpha   = alloc_double_array(nrays, GSL_NAN);
00051         
00052         ld->up_bigger    = alloc_int_array(nrays, 0);
00053         ld->up_smaller   = alloc_int_array(nrays, 0);
00054         ld->down_bigger  = alloc_int_array(nrays, 0);
00055         ld->down_smaller = alloc_int_array(nrays, 0);
00056 
00057         ld->corr = (struct correspondence*) 
00058                 malloc(sizeof(struct correspondence)*nrays);
00059 
00060         int i;
00061         for(i=0;i<ld->nrays;i++) {
00062                 ld->corr[i].valid = 0;
00063                 ld->corr[i].j1 = -1;
00064                 ld->corr[i].j2 = -1;
00065         }
00066         
00067         for(i=0;i<3;i++) {
00068                 ld->odometry[i] = 
00069                 ld->estimate[i] = 
00070                 ld->true_pose[i] = GSL_NAN;
00071         }
00072         
00073         ld->points = (point2d*) malloc(nrays * sizeof(point2d));
00074         ld->points_w = (point2d*) malloc(nrays * sizeof(point2d));
00075         
00076         for(i=0;i<nrays;i++) {
00077                 ld->points[i].p[0] = 
00078                 ld->points[i].p[1] = 
00079                 ld->points[i].rho = 
00080                 ld->points[i].phi = GSL_NAN;
00081                 ld->points_w[i] = ld->points[i];
00082         }
00083         
00084         strcpy(ld->hostname, "CSM");
00085 }
00086 
00087 void ld_free(LDP ld) {
00088         ld_dealloc(ld);
00089         free(ld);
00090 }
00091 
00092 void ld_dealloc(struct laser_data*ld){  
00093         free(ld->valid);
00094         free(ld->readings);
00095         free(ld->readings_sigma);
00096         free(ld->theta);
00097         free(ld->cluster);
00098         free(ld->alpha);
00099         free(ld->alpha_valid);
00100         free(ld->true_alpha);
00101         free(ld->cov_alpha);
00102         free(ld->up_bigger);
00103         free(ld->up_smaller);
00104         free(ld->down_bigger);
00105         free(ld->down_smaller);
00106         free(ld->corr);
00107         
00108 /*      int i;
00109         for(i=0;i<ld->nrays;i++)
00110                 gsl_vector_free(ld->p[i]);
00111         free(ld->p);*/
00112 
00113         free(ld->points);
00114         free(ld->points_w);
00115 }
00116 
00117 
00118 void ld_compute_cartesian(LDP ld) {
00119         int i;
00120         for(i=0;i<ld->nrays;i++) {
00121 /*              if(!ld_valid_ray(ld,i)) continue;*/
00122                 double x = cos(ld->theta[i])*ld->readings[i];
00123                 double y = sin(ld->theta[i])*ld->readings[i];
00124                 
00125                 ld->points[i].p[0] = x, 
00126                 ld->points[i].p[1] = y;
00127                 ld->points[i].rho = GSL_NAN;
00128                 ld->points[i].phi = GSL_NAN;
00129         }
00130 }
00131 
00132 
00133 void ld_compute_world_coords(LDP ld, const double *pose) {
00134         double pose_x = pose[0];
00135         double pose_y = pose[1];
00136         double pose_theta = pose[2];
00137         double cos_theta = cos(pose_theta); 
00138         double sin_theta = sin(pose_theta);
00139         const int nrays = ld->nrays ;
00140 
00141         point2d * points = ld->points;
00142         point2d * points_w = ld->points_w;
00143         int i; for(i=0;i<nrays;i++) {
00144                 if(!ld_valid_ray(ld,i)) continue;
00145                 double x0 = points[i].p[0], 
00146                        y0 = points[i].p[1]; 
00147                 
00148                 if(is_nan(x0) || is_nan(y0)) {
00149                         sm_error("ld_compute_world_coords(): I expected that cartesian coords were already computed: ray #%d: %f %f.\n", i, x0, y0);
00150                 }
00151                 
00152                 points_w[i].p[0] = cos_theta * x0 -sin_theta*y0 + pose_x;
00153                 points_w[i].p[1] = sin_theta * x0 +cos_theta*y0 + pose_y;
00154                 /* polar coordinates */
00155         }
00156         
00157         for(i=0;i<nrays;i++) {
00158                 double x = points_w[i].p[0];
00159                 double y = points_w[i].p[1];
00160                 points_w[i].rho = sqrt( x*x+y*y);
00161                 points_w[i].phi = atan2(y, x);
00162         }
00163         
00164 }
00165 
00166 
00167 
00168 int ld_num_valid_correspondences(LDP ld) {
00169         int i; 
00170         int num = 0;
00171         for(i=0;i<ld->nrays;i++) {
00172                 if(ld->corr[i].valid)
00173                         num++;
00174         }
00175         return num;
00176 }
00177 
00178 
00179 int ld_valid_fields(LDP ld)  {
00180         if(!ld) {
00181                 sm_error("NULL pointer given as laser_data*.\n");       
00182                 return 0;
00183         }
00184         
00185         int min_nrays = 10;
00186         int max_nrays = 10000;
00187         if(ld->nrays < min_nrays || ld->nrays > max_nrays) {
00188                 sm_error("Invalid number of rays: %d\n", ld->nrays);
00189                 return 0;
00190         }
00191         if(is_nan(ld->min_theta) || is_nan(ld->max_theta)) {
00192                 sm_error("Invalid min / max theta: min_theta = %f max_theta = %f\n",
00193                         ld->min_theta, ld->max_theta);
00194                 return 0;
00195         }
00196         double min_fov = deg2rad(20.0); 
00197         double max_fov = 2.01 * M_PI;
00198         double fov = ld->max_theta - ld->min_theta;
00199         if( fov < min_fov || fov > max_fov) {
00200                 sm_error("Strange FOV: %f rad = %f deg \n", fov, rad2deg(fov));
00201                 return 0;
00202         }
00203         if(fabs(ld->min_theta - ld->theta[0]) > 1e-8) {
00204                 sm_error("Min_theta (%f) should be theta[0] (%f)\n",
00205                         ld->min_theta, ld->theta[0]);
00206                 return 0;
00207         }
00208         if(fabs(ld->max_theta - ld->theta[ld->nrays-1]) > 1e-8) {
00209                 sm_error("Min_theta (%f) should be theta[0] (%f)\n",
00210                         ld->max_theta, ld->theta[ld->nrays-1]);
00211                 return 0;
00212         }
00213         /* Check that there are valid rays */
00214         double min_reading = 0;
00215         double max_reading = 100;
00216         int i; for(i=0;i<ld->nrays;i++) {
00217                 double th = ld->theta[i];
00218                 if(ld->valid[i]) {
00219                         double r = ld->readings[i];
00220                         if(is_nan(r) || is_nan(th)) {
00221                                 sm_error("Ray #%d: r = %f  theta = %f but valid is %d\n",
00222                                         i, r, th, ld->valid[i]);
00223                                 return 0;
00224                         }
00225                         if( !( min_reading < r && r < max_reading ) ) {
00226                                 sm_error("Ray #%d: %f is not in interval (%f, %f) \n",
00227                                         i, r, min_reading, max_reading);
00228                                 return 0;
00229                         }               
00230                 } else {
00231                         /* ray not valid, but checking theta anyway */
00232                         if(is_nan(th)) {
00233                                 sm_error("Ray #%d: valid = %d  but theta = %f\n",
00234                                         i,  ld->valid[i], th);
00235                                 return 0;
00236                         }
00237 
00238                         if(ld->cluster[i] != -1 ) {
00239                                 sm_error("Invalid ray #%d has cluster %d\n.", i, ld->cluster[i]);
00240                                 return 0;
00241                         }
00242                 }
00243                 if(ld->cluster[i] < -1 ) {
00244                         sm_error("Ray #%d: Invalid cluster value %d\n.", i, ld->cluster[i]);
00245                         return 0;
00246                 }
00247                 
00248                 if(!is_nan(ld->readings_sigma[i]) && ld->readings_sigma[i] < 0) {
00249                         sm_error("Ray #%d: has invalid readings_sigma %f \n", i, ld->readings_sigma[i]);
00250                         return 0;
00251                 }
00252                 
00253         }
00254         /* Checks that there is at least 10% valid rays */
00255         int num_valid   = count_equal(ld->valid, ld->nrays, 1);
00256         int num_invalid = count_equal(ld->valid, ld->nrays, 0);
00257         if (num_valid < ld->nrays * 0.10) {
00258                 sm_error("Valid: %d/%d invalid: %d.\n", num_valid, ld->nrays, num_invalid);
00259                 return 0;
00260         }
00261 
00262         return 1;
00263 }
00264 
00265 
00267 unsigned int ld_corr_hash(LDP ld){
00268         unsigned int hash = 0;
00269         unsigned int i    = 0;
00270 
00271         for(i = 0; i < (unsigned)ld->nrays; i++) {
00272                 int str = ld_valid_corr(ld, (int)i) ? (ld->corr[i].j1 + 1000*ld->corr[i].j2) : -1;
00273                 hash ^= ((i & 1) == 0) ? (  (hash <<  7) ^ (str) ^ (hash >> 3)) :
00274                                          (~((hash << 11) ^ (str) ^ (hash >> 5)));
00275         }
00276 
00277         return (hash & 0x7FFFFFFF);
00278 }


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