Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Representations/Perception/ColorTableTSL.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file ColorTableTSL.cpp
    00003 * Implementation of class ColorTableTSL.
    00004 *
    00005 * @author <A href=mailto:robocup@andreosterhues.de>André Osterhues</A>
    00006 */
    00007
    00008 #include "ColorTableTSL.h"
    00009 #include "Tools/Math/Common.h"
    00010 #include "Tools/Debugging/Debugging.h"
    00011
    00012 /**
    00013 * Constructor
    00014 */
    00015 ColorTableTSL::ColorTableTSL()
    00016 : corrector()
    00017 {
    00018 //  clear();
    00019 }
    00020
    00021 /**
    00022 * Destructor
    00023 */
    00024 ColorTableTSL::~ColorTableTSL()
    00025 {
    00026 }
    00027
    00028 /**
    00029 * Clear color tables
    00030 */
    00031 void ColorTableTSL::clear()
    00032 {
    00033   for (unsigned char y = 0; y < 32; y++)
    00034   {
    00035     for (unsigned char u = 0; u < 64; u++)
    00036     {
    00037       for (unsigned char v = 0; v < 64; v++)
    00038       {
    00039         colorClasses[y][u][v] = noColor;
    00040       }
    00041     }
    00042   }
    00043   for (int i = 0; i < numOfColors; i++)
    00044   {
    00045     tsl_threshold[i][0][0] = 0;
    00046     tsl_threshold[i][1][0] = 0;
    00047     tsl_threshold[i][0][1] = 0;
    00048     tsl_threshold[i][1][1] = 0;
    00049     tsl_threshold[i][0][2] = 0;
    00050     tsl_threshold[i][1][2] = 0;
    00051     tsl_order[i] = orange;
    00052   }
    00053   tsl_index = 0;
    00054 }
    00055
    00056 /**
    00057 * Reset to default TSL values
    00058 */
    00059 void ColorTableTSL::reset()
    00060 {
    00061   clear();
    00062   addColorClass(green,    15,  85,  30, 150,  45, 170);
    00063   addColorClass(skyblue, 230,  15,  45, 135,  75, 200);
    00064   addColorClass(blue,    195,  20,  15, 100,  45, 120);
    00065   addColorClass(orange,  120, 145,  45, 150, 120, 220);
    00066   addColorClass(yellow,   90, 120,  45, 130, 150, 245);
    00067   addColorClass(pink,    140, 180,  15,  65, 150, 245);
    00068   addColorClass(red,     135, 165,  45, 255,  45, 155);
    00069   addColorClass(gray,      0, 255,   0,  90,   0, 145);
    00070   addColorClass(white,     0, 255,   0,  30, 145, 255);
    00071   addColorClass(black,     0, 255,   0,  255, 0, 10);
    00072   addColorClass(noColor,   0, 255,   0, 255,   0, 255);
    00073 }
    00074
    00075 /**
    00076 * Add TSL thresholds for a color class
    00077 */
    00078 void ColorTableTSL::addColorClass(
    00079     colorClass colorClass,
    00080     unsigned char t_min,
    00081     unsigned char t_max,
    00082     unsigned char s_min,
    00083     unsigned char s_max,
    00084     unsigned char l_min,
    00085     unsigned char l_max
    00086     )
    00087 {
    00088   tsl_order[tsl_index++] = colorClass;
    00089   tsl_threshold[colorClass][0][0] = t_min;
    00090   tsl_threshold[colorClass][1][0] = t_max;
    00091   tsl_threshold[colorClass][0][1] = s_min;
    00092   tsl_threshold[colorClass][1][1] = s_max;
    00093   tsl_threshold[colorClass][0][2] = l_min;
    00094   tsl_threshold[colorClass][1][2] = l_max;
    00095 }
    00096
    00097 /**
    00098 * Calculates the YUV to color class look-up table
    00099 */
    00100 void ColorTableTSL::calculateLUT()
    00101 {
    00102   unsigned char t,
    00103                 s,
    00104                 l;
    00105
    00106   for (register unsigned char y = 0; y < 32; y++)
    00107   {
    00108     for (register unsigned char u = 0; u < 64; u++)
    00109     {
    00110       for (register unsigned char v = 0; v < 64; v++)
    00111       {
    00112         convertYUVToTSL((y << 3) | (y >> 2), (u << 2) | (u >> 4), (v << 2) | (v >> 4), &t, &s, &l);
    00113         colorClasses[y][u][v] = classifyTSL(t, s, l);
    00114       }
    00115     }
    00116   }
    00117 }
    00118
    00119 /**
    00120 * Returns the color class of an YUV pixel
    00121 */
    00122 colorClass ColorTableTSL::getColorClass(
    00123     const unsigned char y,
    00124     const unsigned char u,
    00125     const unsigned char v
    00126     ) const
    00127 {
    00128   /*unsigned char t;
    00129   unsigned char s;
    00130   unsigned char l;
    00131
    00132   convertYUVToTSL(y, u, v, &t, &s, &l);
    00133   return classifyTSL(t, s, l);
    00134   */ return (colorClass) colorClasses[y >> 3][u >> 2][v >> 2];
    00135 }
    00136
    00137 /**
    00138 * YUV->TSL conversion
    00139 */
    00140 void ColorTableTSL::convertYUVToTSL(
    00141     unsigned char y,
    00142     unsigned char u,
    00143     unsigned char v,
    00144     unsigned char *t,
    00145     unsigned char *s,
    00146     unsigned char *l
    00147   ) const
    00148 {
    00149   double  y_in,
    00150           u_in,
    00151           v_in,
    00152           tmp,
    00153           tmp_r,
    00154           tmp_g,
    00155           tmp_b,
    00156           t_out,
    00157           s_out;
    00158   const double pi2_div = 0.15915494309189533576888376337251;  /* 1.0 / (PI * 2.0) */
    00159
    00160   y_in = (double) y;
    00161   u_in = (double) v;
    00162   v_in = (double) u;
    00163   u_in -= 128.0;
    00164   v_in -= 128.0;
    00165   tmp = 1.0 / (4.3403 * y_in +  2.0 * u_in + v_in);
    00166   tmp_r = (-0.6697 * u_in + 1.6959 * v_in) * tmp;
    00167   tmp_g = (-1.168  * u_in - 1.3626 * v_in) * tmp;
    00168   tmp_b = ( 1.8613 * u_in - 0.331  * v_in) * tmp;
    00169   if (tmp_g > 0.0)
    00170   {
    00171     t_out = (atan2(tmp_r, tmp_g) * pi2_div + 0.25) * 255.0;
    00172   }
    00173   else if (tmp_g < 0.0)
    00174   {
    00175     t_out = (atan2(-tmp_r, -tmp_g) * pi2_div + 0.75) * 255.0;
    00176   }
    00177   else
    00178   {
    00179      t_out = 0.0;
    00180   }
    00181   s_out = sqrt(1.8 * (tmp_r * tmp_r + tmp_g * tmp_g + tmp_b * tmp_b)) * 255.0;
    00182
    00183   /* Crop T and S values */
    00184   if (t_out < 0.0)
    00185   {
    00186     t_out = 0.0;
    00187   }
    00188   else if (t_out > 255.0)
    00189   {
    00190     t_out = 255.0;
    00191   }
    00192   if (s_out < 0.0)
    00193   {
    00194     s_out = 0.0;
    00195   }
    00196   else if (s_out > 255.0)
    00197   {
    00198     s_out = 255.0;
    00199   }
    00200
    00201   *t = (unsigned char) t_out;
    00202   *s = (unsigned char) s_out;
    00203   *l = y;
    00204 }
    00205
    00206 /**
    00207 * TSL->colorClass classification
    00208 */
    00209 colorClass ColorTableTSL::classifyTSL(
    00210     unsigned char t,
    00211     unsigned char s,
    00212     unsigned char l
    00213   ) const
    00214 {
    00215   colorClass cl;
    00216
    00217   for (register int i = 0; i < tsl_index; i++)
    00218   {
    00219     cl = tsl_order[i];
    00220     if ((s >= tsl_threshold[cl][0][1]) && (s <= tsl_threshold[cl][1][1])
    00221      && (l >= tsl_threshold[cl][0][2]) && (l <= tsl_threshold[cl][1][2]))
    00222     {
    00223       if (tsl_threshold[cl][0][0] < tsl_threshold[cl][1][0])
    00224       {
    00225         if ((t >= tsl_threshold[cl][0][0]) && (t <= tsl_threshold[cl][1][0]))
    00226         {
    00227           return cl;
    00228         }
    00229       }
    00230       else
    00231       {
    00232         if ((t >= tsl_threshold[cl][0][0]) || (t <= tsl_threshold[cl][1][0]))
    00233         {
    00234           return cl;
    00235         }
    00236       }
    00237     }
    00238   }
    00239   return noColor;
    00240 }
    00241
    00242
    00243 /**
    00244 * Generates a segmented image
    00245 */
    00246 void ColorTableTSL::generateColorClassImage(const Image& image,
    00247     ColorClassImage& colorClassImage) const
    00248 {
    00249   unsigned char y,
    00250                 u,
    00251                 v;
    00252       //          t,
    00253       //        s,
    00254             //    l;
    00255
    00256   colorClassImage.width = image.cameraInfo.resolutionWidth;
    00257   colorClassImage.height = image.cameraInfo.resolutionHeight;
    00258
    00259   for (register int j = 0; j < image.cameraInfo.resolutionHeight; j++)
    00260   {
    00261     for (register int i = 0; i < image.cameraInfo.resolutionWidth; i++)
    00262     {
    00263       y = image.image[j][0][i];
    00264       u = image.image[j][1][i];
    00265     v = image.image[j][2][i];
    00266     corrector.getCorrectedPixel(i,j,y,u,v);
    00267
    00268     //sollte hier nicht benutzt werden, da hiermit eigentlich ja
    00269     //DEBUG-Drawings auf dem Roboter erzeugt werden sollen
    00270     //Bernd Schmidt
    00271     //convertYUVToTSL(y, u, v, &t, &s, &l);
    00272       colorClassImage.image[j][i] = getColorClass(y,u,v);
    00273                   //classifyTSL(t, s, l);
    00274     }
    00275   }
    00276 }
    00277
    00278
    00279 Out& operator << (Out& stream, const ColorTableTSL& colorTableTSL)
    00280 {
    00281   unsigned char tmp;
    00282   register int  i;
    00283
    00284   stream << (unsigned int) 0x004c5354;            // "TSL\0"
    00285   for (i = 0; i < numOfColors; i++)
    00286   {
    00287     tmp = colorTableTSL.tsl_order[i];
    00288     stream << tmp;
    00289     tmp = colorTableTSL.tsl_threshold[i][0][0];
    00290     stream << tmp;
    00291     tmp = colorTableTSL.tsl_threshold[i][1][0];
    00292     stream << tmp;
    00293     tmp = colorTableTSL.tsl_threshold[i][0][1];
    00294     stream << tmp;
    00295     tmp = colorTableTSL.tsl_threshold[i][1][1];
    00296     stream << tmp;
    00297     tmp = colorTableTSL.tsl_threshold[i][0][2];
    00298     stream << tmp;
    00299     tmp = colorTableTSL.tsl_threshold[i][1][2];
    00300     stream << tmp;
    00301   }
    00302
    00303   return stream;
    00304 }
    00305
    00306 In& operator>>(In& stream,ColorTableTSL& colorTableTSL)
    00307 {
    00308   unsigned int  tslMagic;
    00309   unsigned char tmp;
    00310   register int  i;
    00311
    00312   stream >> tslMagic;
    00313   if (tslMagic != (unsigned int) 0x004c5354)   // "TSL\0"
    00314   {
    00315     colorTableTSL.reset();
    00316   }
    00317   else
    00318   {
    00319     for (i = 0; i < numOfColors; i++)
    00320     {
    00321       stream >> tmp;
    00322       colorTableTSL.tsl_order[i] = (colorClass) tmp;
    00323       stream >> tmp;
    00324       colorTableTSL.tsl_threshold[i][0][0] = tmp;
    00325       stream >> tmp;
    00326       colorTableTSL.tsl_threshold[i][1][0] = tmp;
    00327       stream >> tmp;
    00328       colorTableTSL.tsl_threshold[i][0][1] = tmp;
    00329       stream >> tmp;
    00330       colorTableTSL.tsl_threshold[i][1][1] = tmp;
    00331       stream >> tmp;
    00332       colorTableTSL.tsl_threshold[i][0][2] = tmp;
    00333       stream >> tmp;
    00334       colorTableTSL.tsl_threshold[i][1][2] = tmp;
    00335     }
    00336   }
    00337
    00338   return stream;
    00339 }
    00340
    00341 void ColorTableTSL::generateTSLDialogImage(const Image &image, ColorClassImage &colorClassImage){
    00342     unsigned char         y,
    00343                 u,
    00344               v,
    00345                     t,
    00346             s,
    00347         l;
    00348
    00349   colorClassImage.width = image.cameraInfo.resolutionWidth;
    00350   colorClassImage.height = image.cameraInfo.resolutionHeight;
    00351
    00352   for (register int j = 0; j < image.cameraInfo.resolutionHeight; j++)
    00353   {
    00354     for (register int i = 0; i < image.cameraInfo.resolutionWidth; i++)
    00355     {
    00356       y = image.image[j][0][i];
    00357       u = image.image[j][1][i];
    00358       v = image.image[j][2][i];
    00359
    00360
    00361     convertYUVToTSL(y, u, v, &t, &s, &l);
    00362       colorClassImage.image[j][i] = classifyTSL(t, s, l);
    00363     }
    00364   }
    00365 }