Representations/Perception/ColorTableTSL.cpp

Go to the documentation of this file.

/** 
* @file ColorTableTSL.cpp
* Implementation of class ColorTableTSL.
*
* @author <A href=mailto:robocup@andreosterhues.de>André Osterhues</A>
*/

#include "ColorTableTSL.h"
#include "Tools/Math/Common.h"
#include "Tools/Debugging/Debugging.h"

/**
* Constructor
*/
ColorTableTSL::ColorTableTSL()
: corrector()
{
//  clear();
}

/**
* Destructor
*/
ColorTableTSL::~ColorTableTSL()
{
}

/**
* Clear color tables
*/
void ColorTableTSL::clear()
{
  for (unsigned char y = 0; y < 32; y++)
  {
    for (unsigned char u = 0; u < 64; u++)
    {
      for (unsigned char v = 0; v < 64; v++)
      {
        colorClasses[y][u][v] = noColor;
      }
    }
  }
  for (int i = 0; i < numOfColors; i++)
  {
    tsl_threshold[i][0][0] = 0;
    tsl_threshold[i][1][0] = 0;
    tsl_threshold[i][0][1] = 0;
    tsl_threshold[i][1][1] = 0;
    tsl_threshold[i][0][2] = 0;
    tsl_threshold[i][1][2] = 0;
    tsl_order[i] = orange;
  }
  tsl_index = 0;
}

/**
* Reset to default TSL values
*/
void ColorTableTSL::reset()
{
  clear();
  addColorClass(green,    15,  85,  30, 150,  45, 170);
  addColorClass(skyblue, 230,  15,  45, 135,  75, 200);
  addColorClass(blue,    195,  20,  15, 100,  45, 120);
  addColorClass(orange,  120, 145,  45, 150, 120, 220);
  addColorClass(yellow,   90, 120,  45, 130, 150, 245);
  addColorClass(pink,    140, 180,  15,  65, 150, 245);
  addColorClass(red,     135, 165,  45, 255,  45, 155);
  addColorClass(gray,      0, 255,   0,  90,   0, 145);
  addColorClass(white,     0, 255,   0,  30, 145, 255);
  addColorClass(black,     0, 255,   0,  255, 0, 10);
  addColorClass(noColor,   0, 255,   0, 255,   0, 255);
}

/**
* Add TSL thresholds for a color class
*/
void ColorTableTSL::addColorClass(
    colorClass colorClass,
    unsigned char t_min,
    unsigned char t_max,
    unsigned char s_min,
    unsigned char s_max,
    unsigned char l_min,
    unsigned char l_max
    )
{
  tsl_order[tsl_index++] = colorClass;
  tsl_threshold[colorClass][0][0] = t_min;
  tsl_threshold[colorClass][1][0] = t_max;
  tsl_threshold[colorClass][0][1] = s_min;
  tsl_threshold[colorClass][1][1] = s_max;
  tsl_threshold[colorClass][0][2] = l_min;
  tsl_threshold[colorClass][1][2] = l_max;
}

/**
* Calculates the YUV to color class look-up table
*/
void ColorTableTSL::calculateLUT()
{
  unsigned char t,
                s,
                l;

  for (register unsigned char y = 0; y < 32; y++)
  {
    for (register unsigned char u = 0; u < 64; u++)
    {
      for (register unsigned char v = 0; v < 64; v++)
      {
        convertYUVToTSL((y << 3) | (y >> 2), (u << 2) | (u >> 4), (v << 2) | (v >> 4), &t, &s, &l);
        colorClasses[y][u][v] = classifyTSL(t, s, l);
      }
    }
  }
}

/** 
* Returns the color class of an YUV pixel
*/
colorClass ColorTableTSL::getColorClass(
    const unsigned char y, 
    const unsigned char u, 
    const unsigned char v
    ) const
{
  /*unsigned char t;
  unsigned char s;
  unsigned char l;

  convertYUVToTSL(y, u, v, &t, &s, &l);
  return classifyTSL(t, s, l);
  */ return (colorClass) colorClasses[y >> 3][u >> 2][v >> 2];
}

/**
* YUV->TSL conversion
*/
void ColorTableTSL::convertYUVToTSL(
    unsigned char y,
    unsigned char u,
    unsigned char v,
    unsigned char *t,
    unsigned char *s,
    unsigned char *l
  ) const
{
  double  y_in,
          u_in,
          v_in,
          tmp,
          tmp_r,
          tmp_g,
          tmp_b,
          t_out,
          s_out;
  const double pi2_div = 0.15915494309189533576888376337251;  /* 1.0 / (PI * 2.0) */

  y_in = (double) y;
  u_in = (double) v;
  v_in = (double) u;
  u_in -= 128.0;
  v_in -= 128.0;
  tmp = 1.0 / (4.3403 * y_in +  2.0 * u_in + v_in);
  tmp_r = (-0.6697 * u_in + 1.6959 * v_in) * tmp;
  tmp_g = (-1.168  * u_in - 1.3626 * v_in) * tmp;
  tmp_b = ( 1.8613 * u_in - 0.331  * v_in) * tmp;
  if (tmp_g > 0.0)
  {
    t_out = (atan2(tmp_r, tmp_g) * pi2_div + 0.25) * 255.0;
  }
  else if (tmp_g < 0.0)
  {
    t_out = (atan2(-tmp_r, -tmp_g) * pi2_div + 0.75) * 255.0;
  }
  else
  {
     t_out = 0.0;
  }
  s_out = sqrt(1.8 * (tmp_r * tmp_r + tmp_g * tmp_g + tmp_b * tmp_b)) * 255.0;

  /* Crop T and S values */
  if (t_out < 0.0)
  {
    t_out = 0.0;
  }
  else if (t_out > 255.0)
  {
    t_out = 255.0;
  }
  if (s_out < 0.0)
  {
    s_out = 0.0;
  }
  else if (s_out > 255.0)
  {
    s_out = 255.0;
  }

  *t = (unsigned char) t_out;
  *s = (unsigned char) s_out;
  *l = y;
}

/**
* TSL->colorClass classification
*/
colorClass ColorTableTSL::classifyTSL(
    unsigned char t,
    unsigned char s,
    unsigned char l
  ) const
{
  colorClass cl;

  for (register int i = 0; i < tsl_index; i++)
  {
    cl = tsl_order[i];
    if ((s >= tsl_threshold[cl][0][1]) && (s <= tsl_threshold[cl][1][1])
     && (l >= tsl_threshold[cl][0][2]) && (l <= tsl_threshold[cl][1][2]))
    {
      if (tsl_threshold[cl][0][0] < tsl_threshold[cl][1][0])
      {
        if ((t >= tsl_threshold[cl][0][0]) && (t <= tsl_threshold[cl][1][0]))
        {
          return cl;
        }
      }
      else
      {
        if ((t >= tsl_threshold[cl][0][0]) || (t <= tsl_threshold[cl][1][0]))
        {
          return cl;
        }
      }
    }
  }
  return noColor;
}


/**
* Generates a segmented image
*/
void ColorTableTSL::generateColorClassImage(const Image& image, 
    ColorClassImage& colorClassImage) const
{
  unsigned char y,
                u,
                v;
      //          t,
      //        s,
            //    l;
  
  colorClassImage.width = image.cameraInfo.resolutionWidth;
  colorClassImage.height = image.cameraInfo.resolutionHeight;

  for (register int j = 0; j < image.cameraInfo.resolutionHeight; j++) 
  {
    for (register int i = 0; i < image.cameraInfo.resolutionWidth; i++) 
    {
      y = image.image[j][0][i];
      u = image.image[j][1][i];
    v = image.image[j][2][i];
    corrector.getCorrectedPixel(i,j,y,u,v);
      
    //sollte hier nicht benutzt werden, da hiermit eigentlich ja
    //DEBUG-Drawings auf dem Roboter erzeugt werden sollen
    //Bernd Schmidt
    //convertYUVToTSL(y, u, v, &t, &s, &l);
      colorClassImage.image[j][i] = getColorClass(y,u,v);
                  //classifyTSL(t, s, l);    
    }
  }
}


Out& operator << (Out& stream, const ColorTableTSL& colorTableTSL)
{
  unsigned char tmp;
  register int  i;

  stream << (unsigned int) 0x004c5354;            // "TSL\0"
  for (i = 0; i < numOfColors; i++)
  {
    tmp = colorTableTSL.tsl_order[i];
    stream << tmp;
    tmp = colorTableTSL.tsl_threshold[i][0][0];
    stream << tmp;
    tmp = colorTableTSL.tsl_threshold[i][1][0];
    stream << tmp;
    tmp = colorTableTSL.tsl_threshold[i][0][1];
    stream << tmp;
    tmp = colorTableTSL.tsl_threshold[i][1][1];
    stream << tmp;
    tmp = colorTableTSL.tsl_threshold[i][0][2];
    stream << tmp;
    tmp = colorTableTSL.tsl_threshold[i][1][2];
    stream << tmp;
  }

  return stream;
}

In& operator>>(In& stream,ColorTableTSL& colorTableTSL)
{
  unsigned int  tslMagic;
  unsigned char tmp;
  register int  i;

  stream >> tslMagic;
  if (tslMagic != (unsigned int) 0x004c5354)   // "TSL\0"
  {
    colorTableTSL.reset();
  }
  else
  {
    for (i = 0; i < numOfColors; i++)
    {
      stream >> tmp;
      colorTableTSL.tsl_order[i] = (colorClass) tmp;
      stream >> tmp;
      colorTableTSL.tsl_threshold[i][0][0] = tmp;
      stream >> tmp;
      colorTableTSL.tsl_threshold[i][1][0] = tmp;
      stream >> tmp;
      colorTableTSL.tsl_threshold[i][0][1] = tmp;
      stream >> tmp;
      colorTableTSL.tsl_threshold[i][1][1] = tmp;
      stream >> tmp;
      colorTableTSL.tsl_threshold[i][0][2] = tmp;
      stream >> tmp;
      colorTableTSL.tsl_threshold[i][1][2] = tmp;
    }
  }

  return stream;
}

void ColorTableTSL::generateTSLDialogImage(const Image &image, ColorClassImage &colorClassImage){
    unsigned char         y,
                u,
              v,
                    t,
            s,
        l;
  
  colorClassImage.width = image.cameraInfo.resolutionWidth;
  colorClassImage.height = image.cameraInfo.resolutionHeight;

  for (register int j = 0; j < image.cameraInfo.resolutionHeight; j++) 
  {
    for (register int i = 0; i < image.cameraInfo.resolutionWidth; i++) 
    {
      y = image.image[j][0][i];
      u = image.image[j][1][i];
      v = image.image[j][2][i];
      

    convertYUVToTSL(y, u, v, &t, &s, &l);
      colorClassImage.image[j][i] = classifyTSL(t, s, l);    
    }
  }
}

---