Fawkes API  Fawkes Development Version
multi_color.cpp
00001 
00002 /***************************************************************************
00003  *  multi_color.cpp - Implementation of a MultiColorClassifier
00004  *
00005  *  Created: Sat Apr 02 09:57:14 2011
00006  *  Copyright  2005-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 #include <fvclassifiers/multi_color.h>
00025 
00026 #include <fvutils/color/colorspaces.h>
00027 #include <fvutils/color/yuv.h>
00028 #include <fvutils/color/color_object_map.h>
00029 
00030 #include <fvmodels/scanlines/scanlinemodel.h>
00031 #include <fvmodels/color/colormodel.h>
00032 
00033 #include <core/exceptions/software.h>
00034 
00035 #include <cstdlib>
00036 
00037 namespace firevision {
00038 #if 0 /* just to make Emacs auto-indent happy */
00039 }
00040 #endif
00041 
00042 /** @class MultiColorClassifier <fvclassifiers/multi_color.h>
00043  * Simple multi-color classifier.
00044  * @author Tim Niemueller
00045  */
00046 
00047 /** Constructor.
00048  * @param scanline_model scanline model
00049  * @param color_model color model
00050  * @param min_num_points minimum number of points in ROI to be considered
00051  * @param box_extent basic extent of a new ROI
00052  * @param upward set to true if you have an upward scanline model, this means that
00053  * points are traversed from the bottom to the top. In this case the ROIs are initially
00054  * extended towards the top instead of the bottom.
00055  * @param neighbourhood_min_match minimum number of object pixels to grow neighbourhood
00056  * @param grow_by grow region by that many pixels
00057  */
00058 MultiColorClassifier::MultiColorClassifier(ScanlineModel *scanline_model,
00059                                            ColorModel *color_model,
00060                                            unsigned int min_num_points,
00061                                            unsigned int box_extent,
00062                                            bool upward,
00063                                            unsigned int neighbourhood_min_match,
00064                                            unsigned int grow_by)
00065   : Classifier("MultiColorClassifier")
00066 {
00067   if (scanline_model == NULL) {
00068     throw fawkes::NullPointerException("MultiColorClassifier: scanline_model "
00069                                        "may not be NULL");
00070   }
00071   if (color_model == NULL) {
00072     throw fawkes::NullPointerException("MultiColorClassifier: color_model "
00073                                        "may not be NULL");
00074   }
00075 
00076   modified = false;
00077   this->scanline_model = scanline_model;
00078   this->color_model = color_model;
00079   this->min_num_points = min_num_points;
00080   this->box_extent = box_extent;
00081   this->upward = upward;
00082   this->grow_by = grow_by;
00083   this->neighbourhood_min_match = neighbourhood_min_match;
00084 }
00085 
00086 
00087 unsigned int
00088 MultiColorClassifier::consider_neighbourhood( unsigned int x, unsigned int y , color_t what)
00089 {
00090   color_t c;
00091   unsigned int num_what = 0;
00092 
00093   unsigned char yp = 0, up = 0, vp = 0;
00094   int start_x = -2, start_y = -2;
00095   int end_x = 2, end_y = 2;
00096 
00097   if (x < (unsigned int)abs(start_x)) {
00098     start_x = 0;
00099   }
00100   if (y < (unsigned int)abs(start_y)) {
00101     start_y = 0;
00102   }
00103 
00104   if (x > _width - end_x) {
00105     end_x = 0;
00106   }
00107   if (y == _height - end_y) {
00108     end_y = 0;
00109   }
00110 
00111   for (int dx = start_x; dx <= end_x ; dx += 2) {
00112     for (int dy = start_y; dy <= end_y; ++dy) {
00113       if ((dx == 0) && (dy == 0)) {
00114         continue;
00115       }
00116 
00117       //      cout << "x=" << x << "  dx=" << dx << "  +=" << x+dx
00118       //   << "  y=" << y << "  dy=" << dy << "  +2=" << y+dy << endl;
00119 
00120       YUV422_PLANAR_YUV(_src, _width, _height, x+dx, y+dy, yp, up, vp);
00121       c = color_model->determine(yp, up, vp);
00122 
00123       if (c == what) {
00124         ++num_what;
00125       }
00126     }
00127   }
00128 
00129   return num_what;
00130 }
00131 
00132 std::list< ROI > *
00133 MultiColorClassifier::classify()
00134 {
00135 
00136   if (_src == NULL) {
00137     return new std::list< ROI >;
00138   }
00139 
00140   std::map<color_t, std::list<ROI> > rois;
00141   std::map<color_t, std::list<ROI> >::iterator map_it;
00142   std::list<ROI>::iterator roi_it, roi_it2;
00143   color_t c;
00144 
00145   unsigned int  x = 0, y = 0;
00146   unsigned char yp = 0, up = 0, vp = 0;
00147   unsigned int num_what = 0;
00148 
00149   ROI r;
00150 
00151   scanline_model->reset();
00152   while (! scanline_model->finished()) {
00153 
00154     x = (*scanline_model)->x;
00155     y = (*scanline_model)->y;
00156 
00157     YUV422_PLANAR_YUV(_src, _width, _height, x, y, yp, up, vp);
00158 
00159     c = color_model->determine(yp,up, vp);
00160 
00161     if ((c != C_BACKGROUND) && (c != C_OTHER)) {
00162       // Yeah, found something, make it big and name it a ROI
00163       // Note that this may throw out a couple of ROIs for just one ball,
00164       // as the name suggests this one is really ABSOLUTELY simple and not
00165       // useful for anything else than quick testing
00166 
00167       if (neighbourhood_min_match) {
00168         num_what =
00169           consider_neighbourhood((*scanline_model)->x, (*scanline_model)->y, c);
00170       }
00171       if (num_what >= neighbourhood_min_match) {
00172         bool ok = false;
00173 
00174         for (roi_it = rois[c].begin(); roi_it != rois[c].end(); ++roi_it) {
00175           if ( (*roi_it).contains(x, y) ) {
00176             // everything is fine, this point is already in another ROI
00177             ok = true;
00178             break;
00179           }
00180         }
00181         if (! ok) {
00182           for (roi_it = rois[c].begin(); roi_it != rois[c].end(); ++roi_it) {
00183             if ( (*roi_it).neighbours(x, y, scanline_model->get_margin()) ) {
00184               // ROI is neighbour of this point, extend region
00185               (*roi_it).extend(x, y);
00186               ok = true;
00187               break;
00188             }
00189           }
00190         }
00191 
00192         if (! ok) {
00193           if ( upward ) {
00194             if ( x < box_extent ) {
00195               x = 0;
00196             } else {
00197               x -= box_extent;
00198             }
00199             if ( y < box_extent ) {
00200               y = 0;
00201             } else {
00202               y -= box_extent;
00203             }
00204           }
00205           r.start.x = x;
00206           r.start.y = y;
00207 
00208           unsigned int to_x = (*scanline_model)->x + box_extent;
00209           unsigned int to_y = (*scanline_model)->y + box_extent;
00210           if (to_x > _width)  to_x = _width;
00211           if (to_y > _height) to_y = _height;
00212           r.width = to_x - r.start.x;
00213           r.height = to_y - r.start.y;
00214           r.hint = c;
00215           r.color = c;
00216 
00217           r.line_step = _width;
00218           r.pixel_step = 1;
00219 
00220           r.image_width  = _width;
00221           r.image_height = _height;
00222 
00223           if ( (r.start.x + r.width) > _width ) {
00224             r.width -= (r.start.x + r.width) - _width;
00225           }
00226           if ( (r.start.y + r.height) > _height ) {
00227             r.height -= (r.start.y + r.height) - _height;
00228           }
00229 
00230           rois[c].push_back( r );
00231         }
00232       } // End if enough neighbours
00233     } // end if is orange
00234 
00235     ++(*scanline_model);
00236   }
00237 
00238   // Grow regions
00239   if (grow_by > 0) {
00240     for (map_it = rois.begin(); map_it != rois.end(); ++map_it) {
00241       for (roi_it = map_it->second.begin();
00242            roi_it != map_it->second.end(); ++roi_it)
00243       {
00244         (*roi_it).grow( grow_by );
00245       }
00246     }
00247   }
00248 
00249   // Merge neighbouring regions
00250   for (map_it = rois.begin(); map_it != rois.end(); ++map_it) {
00251     for (roi_it = map_it->second.begin(); roi_it != map_it->second.end(); ++roi_it)
00252     {
00253       roi_it2 = roi_it;
00254       ++roi_it2;
00255 
00256       while ( roi_it2 != map_it->second.end() ) {
00257         if ((roi_it != roi_it2) &&
00258             roi_it->neighbours(&(*roi_it2), scanline_model->get_margin()))
00259         {
00260           *roi_it += *roi_it2;
00261           map_it->second.erase(roi_it2);
00262           roi_it2 = map_it->second.begin(); //restart
00263         } else {
00264           ++roi_it2;
00265         }
00266       }
00267     }
00268   }
00269 
00270   // Throw away all ROIs that have not enough classified points
00271   for (map_it = rois.begin(); map_it != rois.end(); ++map_it) {
00272     for (roi_it = map_it->second.begin(); roi_it != map_it->second.end(); ++roi_it)
00273     {
00274       while ( (roi_it != map_it->second.end()) &&
00275               ((*roi_it).num_hint_points < min_num_points ))
00276       {
00277         roi_it = map_it->second.erase(roi_it);
00278       }
00279     }
00280   }
00281 
00282   // sort ROIs by number of hint points, descending (and thus call reverse)
00283 
00284   std::list<ROI> *rv = new std::list<ROI>();
00285   for (map_it = rois.begin(); map_it != rois.end(); ++map_it) {
00286     map_it->second.sort();
00287     rv->merge(map_it->second);
00288   }
00289   rv->reverse();
00290   return rv;
00291 }
00292 
00293 
00294 /** Get mass point of primary color.
00295  * @param roi ROI to consider
00296  * @param massPoint contains mass point upon return
00297  */
00298 void
00299 MultiColorClassifier::get_mass_point_of_color( ROI *roi,
00300                                                fawkes::point_t *massPoint )
00301 {
00302   unsigned int nrOfOrangePixels;
00303   nrOfOrangePixels = 0;
00304   massPoint->x     = 0;
00305   massPoint->y     = 0;
00306 
00307   // for accessing ROI pixels
00308   register unsigned int h = 0;
00309   register unsigned int w = 0;
00310   // planes
00311   register unsigned char *yp   = _src + (roi->start.y * roi->line_step) + (roi->start.x * roi->pixel_step);
00312   register unsigned char *up   = YUV422_PLANAR_U_PLANE(_src, roi->image_width, roi->image_height)
00313     + ((roi->start.y * roi->line_step) / 2 + (roi->start.x * roi->pixel_step) / 2) ;
00314   register unsigned char *vp   = YUV422_PLANAR_V_PLANE(_src, roi->image_width, roi->image_height)
00315     + ((roi->start.y * roi->line_step) / 2 + (roi->start.x * roi->pixel_step) / 2);
00316   // line starts
00317   unsigned char *lyp  = yp;
00318   unsigned char *lup  = up;
00319   unsigned char *lvp  = vp;
00320 
00321   color_t dcolor;
00322 
00323   // consider each ROI pixel
00324   for (h = 0; h < roi->height; ++h) {
00325     for (w = 0; w < roi->width; w += 2) {
00326       // classify its color
00327       dcolor = color_model->determine(*yp++, *up++, *vp++);
00328       yp++;
00329       // ball pixel?
00330       if (dcolor == roi->color) {
00331         // take into account its coordinates
00332         massPoint->x += w;
00333         massPoint->y += h;
00334         nrOfOrangePixels++;
00335       }
00336     }
00337     // next line
00338     lyp  += roi->line_step;
00339     lup  += roi->line_step / 2;
00340     lvp  += roi->line_step / 2;
00341     yp    = lyp;
00342     up    = lup;
00343     vp    = lvp;
00344   }
00345 
00346   // to obtain mass point, divide by number of pixels that were added up
00347   massPoint->x = (unsigned int) (float(massPoint->x) / float(nrOfOrangePixels));
00348   massPoint->y = (unsigned int) (float(massPoint->y) / float(nrOfOrangePixels));
00349 
00350   /* shift mass point
00351    such that it is relative to image
00352    (not relative to ROI) */
00353   massPoint->x += roi->start.x;
00354   massPoint->y += roi->start.y;
00355 }
00356 
00357 } // end namespace firevision