laser_data_bbox.c
Go to the documentation of this file.
00001 /* This algorithm was created by Cyrill Stachniss 
00002         http://www.informatik.uni-freiburg.de/~stachnis/ */
00003 #include "laser_data_drawing.h"
00004 #include "math_utils.h"
00005 #include "logging.h"
00006 
00007 // the 2d-point structure for the input
00008 typedef struct {
00009   double x;
00010   double y;
00011 } BB_Point;
00012 
00013 // computes the area of minimal bounding box for a set of 2d-points
00014 double getBoundingBoxArea(BB_Point* p, int nOfPoints);
00015 
00016 // computes the minimal bounding box for a set of 2d-points
00017 // ul = upper left  point, ll = lower left point, 
00018 // ur = upper right point, ul = upper left point
00019 int getBoundingBox(BB_Point* p, int nOfPoints, 
00020         double ul[2], double ur[2], double ll[2], double lr[2]);
00021 
00022 struct bbfind_imp {
00023         int num;
00024         
00025         int buf_size;
00026         BB_Point * buf;
00027 };
00028 
00029 /* Initialize structure */
00030 bbfind * bbfind_new(void);
00031 
00032 /* -------------------------------------- */
00033 
00034 bbfind * bbfind_new() {
00035         bbfind * bbf = malloc(sizeof(bbfind));
00036         bbf->buf_size = 1000;
00037         bbf->buf = malloc(sizeof(BB_Point)*bbf->buf_size);
00038         bbf->num = 0;
00039         return bbf;
00040 }
00041 
00042 int bbfind_add_point(bbfind*bbf, double point[2]) {
00043         return bbfind_add_point2(bbf, point[0], point[1]);
00044 }
00045 
00046 int bbfind_add_point2(bbfind*bbf, double x, double y) {
00047         if(bbf->num > bbf->buf_size - 2) {
00048                 bbf->buf_size   *= 2;
00049                 if(! (bbf->buf = (BB_Point*) realloc(bbf->buf, sizeof(BB_Point)*bbf->buf_size)) ) {
00050                         sm_error("Cannot allocate (size=%d)\n", bbf->buf_size);
00051                         return 0;
00052                 }
00053         }
00054         bbf->buf[bbf->num].x = x;
00055         bbf->buf[bbf->num].y = y;
00056         bbf->num++;
00057         return 1;
00058 }
00059 
00060 void oriented_bbox_compute_corners(const BB2 obbox,
00061         double ul[2], double ur[2], double ll[2], double lr[2]) {
00062         
00063         ll[0] = obbox->pose[0];
00064         ll[1] = obbox->pose[1];
00065         lr[0] = obbox->pose[0] + obbox->size[0] * cos(obbox->pose[2]);
00066         lr[1] = obbox->pose[1] + obbox->size[0] * sin(obbox->pose[2]);
00067         ul[0] = obbox->pose[0] + obbox->size[1] * cos(obbox->pose[2] + M_PI/2);
00068         ul[1] = obbox->pose[1] + obbox->size[1] * sin(obbox->pose[2] + M_PI/2);
00069         ur[0] = ul[0] + obbox->size[0] * cos(obbox->pose[2]);
00070         ur[1] = ul[1] + obbox->size[0] * sin(obbox->pose[2]);
00071                 
00072 }
00073 
00074 int bbfind_add_bbox(bbfind*bbf, const BB2 bbox) {
00075         double ul[2], ur[2], ll[2], lr[2];
00076         oriented_bbox_compute_corners(bbox, ul, ur, ll, lr);
00077         return
00078                 bbfind_add_point(bbf, ul) &&
00079                 bbfind_add_point(bbf, ur) &&
00080                 bbfind_add_point(bbf, ll) &&
00081                 bbfind_add_point(bbf, lr);
00082 }
00083 
00084 
00085 int bbfind_compute(bbfind*bbf, BB2 bbox) {
00086         double ul[2], ur[2], ll[2], lr[2];
00087         
00088         if(1) {
00089                 if(!getBoundingBox(bbf->buf, bbf->num, ul, ur, ll, lr)) {
00090                         sm_error("Could not compute bounding box.\n");
00091                         return 0;
00092                 }
00093                 bbox->pose[0] = ll[0];
00094                 bbox->pose[1] = ll[1];
00095                 bbox->pose[2] = atan2(lr[1]-ll[1], lr[0]-ll[0]);
00096                 bbox->size[0] = distance_d(lr, ll);
00097                 bbox->size[1] = distance_d(ll, ul);
00098         } else {
00099                 double bb_min[2] = {bbf->buf[0].x,bbf->buf[0].y}, 
00100                                 bb_max[2] = {bbf->buf[0].x,bbf->buf[0].y};
00101                 int i; for(i=0;i<bbf->num; i++) {
00102                         bb_min[0] = GSL_MIN(bb_min[0], bbf->buf[i].x);
00103                         bb_min[1] = GSL_MIN(bb_min[1], bbf->buf[i].y);
00104                         bb_max[0] = GSL_MAX(bb_max[0], bbf->buf[i].x);
00105                         bb_max[1] = GSL_MAX(bb_max[1], bbf->buf[i].y);
00106                 }
00107                 bbox->pose[0] = bb_min[0];
00108                 bbox->pose[1] = bb_min[1];
00109                 bbox->pose[2] = 0;
00110                 bbox->size[0] = bb_max[0] - bb_min[0];
00111                 bbox->size[1] = bb_max[1] - bb_min[1];
00112         }
00113         return 1;
00114 }
00115 
00116 void bbfind_free(bbfind* bbf) {
00117         free(bbf->buf);
00118         free(bbf);
00119 }
00120 
00121 void ld_get_oriented_bbox(LDP ld, double horizon, oriented_bbox*obbox) {
00122         bbfind * bbf = bbfind_new();
00123         int i; for(i=0;i<ld->nrays;i++) {
00124                 if(!ld->valid[i]) continue;
00125                 if(ld->readings[i]>horizon) continue;
00126 
00127                 double p0[2] = {
00128                         cos(ld->theta[i]) * ld->readings[i],
00129                         sin(ld->theta[i]) * ld->readings[i]
00130                 };
00131                 
00132                 bbfind_add_point(bbf, p0);
00133         }
00134         bbfind_compute(bbf, obbox);
00135         bbfind_free(bbf);
00136 }
00137 
00138 // computes the area of minimal bounding box for a set of 2d-points
00139 double getBoundingBoxArea(BB_Point* p, int nOfPoints) {
00140         double ul[2], ur[2], ll[2], lr[2];
00141         
00142         int wasOk = getBoundingBox(p, nOfPoints, ul, ur, ll, lr);
00143         double vol = (!wasOk) ? -1.0 : distance_d(ul,ll)*distance_d(ul,ur);
00144   return vol;
00145 }
00146 
00147 // computes the minimal bounding box for a set of 2d-points
00148 // ul = upper left  point, ll = lower left point, 
00149 // ur = upper right point, ul = upper left point
00150 int getBoundingBox(BB_Point* p, int nOfPoints, 
00151         double ul[2], double ur[2], double ll[2], double lr[2]) {
00152 
00153   // calculate the center of all points (schwerpunkt)
00154   // -------------------------------------------------
00155   double centerx = 0;
00156   double centery = 0;
00157   for (int i=0; i < nOfPoints; i++) {
00158          centerx += p[i].x;
00159          centery += p[i].y;
00160   }      
00161   centerx /= (double) nOfPoints;
00162   centery /= (double) nOfPoints;
00163 
00164 
00165   
00166   // calcutae the covariance matrix
00167   // -------------------------------
00168   // covariance matrix (x1 x2, x3 x4) 
00169   double x1 = 0.0;
00170   double x2 = 0.0;
00171   double x3 = 0.0;
00172   double x4 = 0.0;
00173 
00174   for (int i=0; i < nOfPoints; i++) {
00175          double cix = p[i].x - centerx;
00176          double ciy = p[i].y - centery;
00177          
00178          x1 += cix*cix;
00179          x2 += cix*ciy;  
00180          x4 += ciy*ciy;
00181   }
00182   x1 /= (double) nOfPoints;
00183   x2 /= (double) nOfPoints;
00184   x3 = x2;
00185   x4 /= (double) nOfPoints;
00186   // covariance & center  done
00187 
00188 
00189   // calculate the eigenvectors
00190   // ---------------------------
00191   // catch 1/0 or sqrt(<0)
00192   if ((x3 == 0) || (x2 == 0)|| (x4*x4-2*x1*x4+x1*x1+4*x2*x3 < 0 ))  {
00193         sm_error("Cyrill: Could not compute bounding box.\n");
00194         return 0;
00195 }
00196 
00197  // eigenvalues
00198   double lamda1 = 0.5* (x4 + x1 + sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3));
00199   double lamda2 = 0.5* (x4 + x1 - sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3));
00200   
00201   // eigenvector 1  with  (x,y)
00202   double v1x = - (x4-lamda1) * (x4-lamda1) * (x1-lamda1) / (x2 * x3 * x3);
00203   double v1y = (x4-lamda1) * (x1-lamda1) / (x2 * x3);
00204   // eigenvector 2 with  (x,y)
00205   double v2x = - (x4-lamda2) * (x4-lamda2) * (x1-lamda2) / (x2 * x3 * x3);
00206   double v2y = (x4-lamda2) * (x1-lamda2) / (x2 * x3);
00207 
00208   // norm the eigenvectors
00209   double lv1 = sqrt ( (v1x*v1x) + (v1y*v1y) );
00210   double lv2 = sqrt ( (v2x*v2x) + (v2y*v2y) );
00211   v1x /= lv1;
00212   v1y /= lv1;
00213   v2x /= lv2;
00214   v2y /= lv2;
00215   // eigenvectors done
00216 
00217   // get the points with maximal dot-product 
00218   double x = 0.0;
00219   double y = 0.0;
00220   double xmin = 1e20;
00221   double xmax = -1e20;
00222   double ymin = 1e20;
00223   double ymax = -1e20;
00224   for(int i = 0; i< nOfPoints; i++) {
00225          // dot-product of relativ coordinates of every point
00226          x = (p[i].x - centerx) * v1x + (p[i].y - centery) * v1y;
00227          y = (p[i].x - centerx) * v2x + (p[i].y - centery) * v2y;
00228 
00229          if( x > xmax) xmax = x;
00230          if( x < xmin) xmin = x;
00231          if( y > ymax) ymax = y;
00232          if( y < ymin) ymin = y;
00233   }
00234 
00235   // now we can compute the corners of the bounding box
00236         if(ul) {
00237                 ul[0] = centerx + xmin * v1x + ymin * v2x;
00238                 ul[1] = centery + xmin * v1y + ymin * v2y;
00239         }
00240 
00241         if(ur) {
00242                 ur[0] = centerx + xmax * v1x + ymin * v2x;
00243                 ur[1] = centery + xmax * v1y + ymin * v2y;
00244         }
00245         
00246         if(ll) {
00247                 ll[0] = centerx + xmin * v1x + ymax * v2x;
00248                 ll[1] = centery + xmin * v1y + ymax * v2y;
00249         }
00250         
00251         if(lr) {
00252                 lr[0] = centerx + xmax * v1x + ymax * v2x;
00253                 lr[1] = centery + xmax * v1y + ymax * v2y;
00254         }
00255         return 1;
00256 }
00257 
00258 
00259 
00260 


csm_ros
Author(s): Gaƫl Ecorchard , Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Sat Jun 8 2019 19:52:42