Representations/Perception/ColorTable32K.cpp

Go to the documentation of this file.

/** 
* @file ColorTable32K.cpp
* Implementation of class ColorTable32K.
*
* @author <A href=mailto:walter.nistico@uni-dortmund.de>Walter Nistico</A>
*/

#include "ColorTable32K.h"
#include "Platform/GTAssert.h"

void ColorTable32K::unpack(unsigned char* unpackedCube, const unsigned char* packedCube){
  int lpos1, lpos2, pos; 
  unsigned char mask1, mask2;
  for (int y=0; y<8; y++)
    for (int u=0; u<64; u++)
      for (int v=0; v<64; v++){
        lpos1 = ((2*y)<<12)|(u<<6)|v;
        lpos2 = ((2*y+1)<<12)|(u<<6)|v;
        pos = (y<<12)|(u<<6)|v;
        mask1 = 0x0f&packedCube[pos];
        mask2 = 0x0f&(packedCube[pos]>>4);
        unpackedCube[lpos1] = mask1;
        unpackedCube[lpos2] = mask2;
      }
}

colorClass ColorTable32K::getColorClass(const unsigned char y, 
    const unsigned char u, 
    const unsigned char v) const 
{
  return getColorClassFast(y, u, v);
}

void ColorTable32K::generateColorClassImage(const Image& image, 
    ColorClassImage& colorClassImage) const
{
  colorClassImage.width = image.cameraInfo.resolutionWidth;
  colorClassImage.height = image.cameraInfo.resolutionHeight;

  for (register int y = 0; y < image.cameraInfo.resolutionHeight; y++) {
    for (register int x = 0; x < image.cameraInfo.resolutionWidth; x++) {
      colorClassImage.image[y][x] 
        = getColorClassFast(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
    }
  }
}

void ColorTable32K::generateColorClassImage(const Image& image, 
    ColorClassImage& colorClassImage,
    colorClass colorClass
    ) const
{
  colorClassImage.width = image.cameraInfo.resolutionWidth;
  colorClassImage.height = image.cameraInfo.resolutionHeight;

  for (register int y = 0; y < image.cameraInfo.resolutionHeight; y++) {
    for (register int x = 0; x < image.cameraInfo.resolutionWidth; x++) {
      if(getColorClassFast(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]) == colorClass)
      {
        colorClassImage.image[y][x] = colorClass;
      }
      else
      {
        colorClassImage.image[y][x] = noColor;
      }
    }
  }
}

void ColorTable32K::generateHighResColorClassImage(const Image& image, 
    ColorClassImage& colorClassImage
    ) const
{
  colorClassImage.width = image.cameraInfo.resolutionWidth * 2;
  colorClassImage.height = image.cameraInfo.resolutionHeight * 2;

  for (register int y = 0; y < image.cameraInfo.resolutionHeight * 2; y++) {
    for (register int x = 0; x < image.cameraInfo.resolutionWidth * 2; x++) {
      colorClassImage.image[y][x] = 
        getColorClassFast(image.getHighResY(x, y), 
        image.image[y / 2][1][x / 2], 
        image.image[y / 2][2][x / 2]);
    }
  }
}

void ColorTable32K::generateHighResColorClassImage(const Image& image, 
    ColorClassImage& colorClassImage,
    colorClass colorClass
    ) const
{
  colorClassImage.width = image.cameraInfo.resolutionWidth * 2;
  colorClassImage.height = image.cameraInfo.resolutionHeight * 2;

  for (register int y = 0; y < image.cameraInfo.resolutionHeight * 2; y++) {
    for (register int x = 0; x < image.cameraInfo.resolutionWidth * 2; x++) {
      if(getColorClassFast(image.getHighResY(x, y), 
        image.image[y / 2][1][x / 2], 
        image.image[y / 2][2][x / 2])
        == colorClass)
      {
        colorClassImage.image[y][x] = colorClass;
      }
      else
      {
        colorClassImage.image[y][x] = noColor;
      }
    }
  }
}

void ColorTable32K::getBoxAroundColorClass(colorClass color, 
                                          Vector3<int>& pNear, Vector3<int>& pFar)
{
  pNear = Vector3<int>(0,0,0); 
  pFar = Vector3<int>(255,255,255); 
  unsigned char y(0),u(0),v(0);
  while(y<16)
  {
    u = 0;
    while(u<64)
    {
      v = 0;
      while(v<64)
      {
        if(colorClassesUnpacked[(y<<12)|(v<<6)|u] == color)
        {
          Vector3<int> first(4*y,u,v);
          pNear = first;
          pFar = first;
          v++;
          goto expandBox;
        }
        v++;
      }
      u++;
    }
    y++;
  }
  return; //Return if the color has not been found
  expandBox:
  while(y<16)
  {
    u = 0;
    while(u<64)
    {
      v = 0;
      while(v<64)
      {
        if(colorClassesUnpacked[(y<<12)|(v<<6)|u] == color)
        {
          Vector3<int> expand(4*y,u,v);
          if(expand.x < pNear.x)
          {
            pNear.x = expand.x;
          }
          else if(expand.x > pFar.x)
          {
            pFar.x = expand.x;
          }
          if(expand.y < pNear.y)
          {
            pNear.y = expand.y;
          }
          else if(expand.y > pFar.y)
          {
            pFar.y = expand.y;
          }
          if(expand.z < pNear.z)
          {
            pNear.z = expand.z;
          }
          else if(expand.z > pFar.z)
          {
            pFar.y = expand.y;
          }
        }
        v++;
      }
      u++;
    }
    y++;
  }
  pNear *= 4;
  pFar *= 4;
}

ColorTable32K::~ColorTable32K()
{
}

ColorTable32K::ColorTable32K()
{
  clear();
}

/**
 * Currently, interactive calibration through RobotControl is unsupported.
 * For that purpose, you should use AIBOVision.
 */
void ColorTable32K::addColorClass
(
 colorClass colorClass,
 unsigned char y,
 unsigned char u,
 unsigned char v
 )
{
  //~ this->colorClasses[y/4][u/4][v/4] = colorClass;
}

/**
 * Currently, interactive calibration through RobotControl is unsupported.
 * For that purpose, you should use AIBOVision.
 */
void ColorTable32K::addColorClass
(
 colorClass colorClass,
 unsigned char y,
 unsigned char u,
 unsigned char v,
 unsigned char range
 )
{
  //~ y /= 4;
  //~ u /= 4;
  //~ v /= 4;

  //~ if (y < range / 2) y = range / 2;
  //~ if (u < range / 2) u = range / 2;
  //~ if (v < range / 2) v = range / 2;

  //~ if (y + range / 2 > 63) y = 63 + range / 2 - range;
  //~ if (u + range / 2 > 63) u = 63 + range / 2 - range;
  //~ if (v + range / 2 > 63) v = 63 + range / 2 - range;

  //~ for(unsigned char currentY = y - range / 2; currentY < y - range / 2 + range + 1; currentY++)
  //~ {
    //~ for(unsigned char currentU = u - range / 2; currentU < u - range / 2 + range + 1; currentU++)
    //~ {
      //~ for(unsigned char currentV = v - range / 2; currentV < v - range / 2 + range + 1; currentV++)
      //~ {
        //~ this->colorClasses[currentY][currentU][currentV] = colorClass;
        //~ ASSERT(currentY < 64);
        //~ ASSERT(currentU < 64);
        //~ ASSERT(currentV < 64);
      //~ }
    //~ }
  //~ }
}

/**
 * Currently, interactive calibration through RobotControl is unsupported.
 * For that purpose, you should use AIBOVision.
 */
void ColorTable32K::addCuboidToColorClass
(
 colorClass colorClass,
 unsigned char yMin, 
 unsigned char uMin, 
 unsigned char vMin, 
 unsigned char yMax, 
 unsigned char uMax, 
 unsigned char vMax
 )
{
  //~ yMin /= 4; uMin /= 4; vMin /= 4;
  //~ yMax /= 4; uMax /= 4; vMax /= 4;

  //~ for(unsigned char currentY = yMin; currentY <= yMax + 1; currentY++)
  //~ {
    //~ for(unsigned char currentU = uMin; currentU <= uMax + 1; currentU++)
    //~ {
      //~ for(unsigned char currentV = vMin; currentV <= vMax + 1; currentV++)
      //~ {
        //~ this->colorClasses[currentY][currentU][currentV] = colorClass;
        //~ ASSERT(currentY < 64);
        //~ ASSERT(currentU < 64);
        //~ ASSERT(currentV < 64);
      //~ }
    //~ }
  //~ }
}

/**
 * Currently, interactive calibration through RobotControl is unsupported.
 * For that purpose, you should use AIBOVision.
 */
void ColorTable32K::removeColorClass
(
 unsigned char y,
 unsigned char u,
 unsigned char v,
 unsigned char range
 )
{
  //~ y /= 4;
  //~ u /= 4;
  //~ v /= 4;

  //~ if (y < range / 2) y = range / 2;
  //~ if (u < range / 2) u = range / 2;
  //~ if (v < range / 2) v = range / 2;

  //~ if (y + range / 2 > 63) y = 63 + range / 2 - range;
  //~ if (u + range / 2 > 63) u = 63 + range / 2 - range;
  //~ if (v + range / 2 > 63) v = 63 + range / 2 - range;

  //~ for(unsigned char currentY = y - range / 2; currentY < y - range / 2 + range + 1; currentY++)
  //~ {
    //~ for(unsigned char currentU = u - range / 2; currentU < u - range / 2 + range + 1; currentU++)
    //~ {
      //~ for(unsigned char currentV = v - range / 2; currentV < v - range / 2 + range + 1; currentV++)
      //~ {
        //~ this->colorClasses[currentY][currentU][currentV] = noColor;
      //~ }
    //~ }
  //~ }
}


/**
 * Currently, interactive calibration through RobotControl is unsupported.
 * For that purpose, you should use AIBOVision.
 */
void ColorTable32K::clearChannel(colorClass colorClass)
{
  //~ for (unsigned char y = 0; y < 64; y++)
  //~ {
    //~ for (unsigned char u = 0; u < 64; u++)
    //~ {
      //~ for (unsigned char v = 0; v < 64; v++)
      //~ {
        //~ if (colorClasses[y][u][v] == colorClass) colorClasses[y][u][v] = noColor;
      //~ }
    //~ }
  //~ }
}

void ColorTable32K::clear()
{
  for (int i = 0; i < 8*64*64; i++)
  {
    colorClasses[i] = noColor;
  }
}

Out& operator << (Out& stream, const ColorTable32K& colorTable32K)
{
  // the old version of the streaming operator, can be used for converting
  // old colortables to new ones (with the VC debugger)
//  stream.write(&colorTable32K.colorClasses,sizeof(colorTable32K.colorClasses));
//  return stream;


  // the current color class in the color table
  unsigned char currentColorClass = (unsigned char)(colorTable32K.colorClasses[0]);

  // the next color class to be compared with the last one 
  unsigned char nextColorClass;

  // a pointer to the begin of the color table
  const unsigned char* colorTable = (const unsigned char*)&colorTable32K.colorClasses;

  // the number of bytes in one sequence that have the same color class
  int currentLength = 1;

  for (unsigned int i=1;i<sizeof(colorTable32K.colorClasses);i++)
  {
    nextColorClass = colorTable[i];
    
    if (nextColorClass != currentColorClass)
    {
      stream << currentLength << currentColorClass;
      currentColorClass = nextColorClass;
      currentLength = 1;
    }
    else
    {
      currentLength++;
    }
  }
  stream << currentLength << currentColorClass;

  // write a 0 at the end of the stream as a marker for the In streaming operator
  stream << (int)0;

  return stream;
}

In& operator>>(In& stream,ColorTable32K& colorTable32K)
{
  // the old version of the streaming operator, can be used for converting
  // old colortables to new ones (with the VC debugger)
//  stream.read(&colorTable32K.colorClasses,sizeof(colorTable32K.colorClasses));
//  return stream;

  // a pointer to the begin of the color table
  unsigned char* colorTable = (unsigned char*)&colorTable32K.colorClasses;

  // the current color class in the color table
  unsigned char currentColorClass;

  // the number of bytes in one sequence that have the same color class
  unsigned int currentLength;

  // the position in the color table
  unsigned int pos=0;

  while(pos < sizeof(colorTable32K.colorClasses))
  {
    stream >> currentLength;
    if (currentLength == 0)
      if(!pos) // support for old format that contains table size
        stream >> currentLength >> currentLength; // skip length
      else
        break;
    stream >> currentColorClass;
    
    for (unsigned int i=0;i<=currentLength && i + pos < sizeof(colorTable32K.colorClasses);i++)
    {
      colorTable[i+pos] = currentColorClass;
      if (i+pos > sizeof(colorTable32K.colorClasses) - 1)
        break;
    }

    pos += currentLength;
  }
  
  unsigned char* colorTableUnpacked = (unsigned char*)&colorTable32K.colorClassesUnpacked;  
  colorTable32K.unpack(colorTableUnpacked, colorTable);
  
  return stream;
}

---