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

# Representations/Perception/ColorTable32K.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file ColorTable32K.cpp
    00003 * Implementation of class ColorTable32K.
    00004 *
    00005 * @author <A href=mailto:walter.nistico@uni-dortmund.de>Walter Nistico</A>
    00006 */
    00007
    00008 #include "ColorTable32K.h"
    00009 #include "Platform/GTAssert.h"
    00010
    00011 void ColorTable32K::unpack(unsigned char* unpackedCube, const unsigned char* packedCube){
    00012   int lpos1, lpos2, pos;
    00013   unsigned char mask1, mask2;
    00014   for (int y=0; y<8; y++)
    00015     for (int u=0; u<64; u++)
    00016       for (int v=0; v<64; v++){
    00017         lpos1 = ((2*y)<<12)|(u<<6)|v;
    00018         lpos2 = ((2*y+1)<<12)|(u<<6)|v;
    00019         pos = (y<<12)|(u<<6)|v;
    00020         mask1 = 0x0f&packedCube[pos];
    00021         mask2 = 0x0f&(packedCube[pos]>>4);
    00022         unpackedCube[lpos1] = mask1;
    00023         unpackedCube[lpos2] = mask2;
    00024       }
    00025 }
    00026
    00027 colorClass ColorTable32K::getColorClass(const unsigned char y,
    00028     const unsigned char u,
    00029     const unsigned char v) const
    00030 {
    00031   return getColorClassFast(y, u, v);
    00032 }
    00033
    00034 void ColorTable32K::generateColorClassImage(const Image& image,
    00035     ColorClassImage& colorClassImage) const
    00036 {
    00037   colorClassImage.width = image.cameraInfo.resolutionWidth;
    00038   colorClassImage.height = image.cameraInfo.resolutionHeight;
    00039
    00040   for (register int y = 0; y < image.cameraInfo.resolutionHeight; y++) {
    00041     for (register int x = 0; x < image.cameraInfo.resolutionWidth; x++) {
    00042       colorClassImage.image[y][x]
    00043         = getColorClassFast(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
    00044     }
    00045   }
    00046 }
    00047
    00048 void ColorTable32K::generateColorClassImage(const Image& image,
    00049     ColorClassImage& colorClassImage,
    00050     colorClass colorClass
    00051     ) const
    00052 {
    00053   colorClassImage.width = image.cameraInfo.resolutionWidth;
    00054   colorClassImage.height = image.cameraInfo.resolutionHeight;
    00055
    00056   for (register int y = 0; y < image.cameraInfo.resolutionHeight; y++) {
    00057     for (register int x = 0; x < image.cameraInfo.resolutionWidth; x++) {
    00058       if(getColorClassFast(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]) == colorClass)
    00059       {
    00060         colorClassImage.image[y][x] = colorClass;
    00061       }
    00062       else
    00063       {
    00064         colorClassImage.image[y][x] = noColor;
    00065       }
    00066     }
    00067   }
    00068 }
    00069
    00070 void ColorTable32K::generateHighResColorClassImage(const Image& image,
    00071     ColorClassImage& colorClassImage
    00072     ) const
    00073 {
    00074   colorClassImage.width = image.cameraInfo.resolutionWidth * 2;
    00075   colorClassImage.height = image.cameraInfo.resolutionHeight * 2;
    00076
    00077   for (register int y = 0; y < image.cameraInfo.resolutionHeight * 2; y++) {
    00078     for (register int x = 0; x < image.cameraInfo.resolutionWidth * 2; x++) {
    00079       colorClassImage.image[y][x] =
    00080         getColorClassFast(image.getHighResY(x, y),
    00081         image.image[y / 2][1][x / 2],
    00082         image.image[y / 2][2][x / 2]);
    00083     }
    00084   }
    00085 }
    00086
    00087 void ColorTable32K::generateHighResColorClassImage(const Image& image,
    00088     ColorClassImage& colorClassImage,
    00089     colorClass colorClass
    00090     ) const
    00091 {
    00092   colorClassImage.width = image.cameraInfo.resolutionWidth * 2;
    00093   colorClassImage.height = image.cameraInfo.resolutionHeight * 2;
    00094
    00095   for (register int y = 0; y < image.cameraInfo.resolutionHeight * 2; y++) {
    00096     for (register int x = 0; x < image.cameraInfo.resolutionWidth * 2; x++) {
    00097       if(getColorClassFast(image.getHighResY(x, y),
    00098         image.image[y / 2][1][x / 2],
    00099         image.image[y / 2][2][x / 2])
    00100         == colorClass)
    00101       {
    00102         colorClassImage.image[y][x] = colorClass;
    00103       }
    00104       else
    00105       {
    00106         colorClassImage.image[y][x] = noColor;
    00107       }
    00108     }
    00109   }
    00110 }
    00111
    00112 void ColorTable32K::getBoxAroundColorClass(colorClass color,
    00113                                           Vector3<int>& pNear, Vector3<int>& pFar)
    00114 {
    00115   pNear = Vector3<int>(0,0,0);
    00116   pFar = Vector3<int>(255,255,255);
    00117   unsigned char y(0),u(0),v(0);
    00118   while(y<16)
    00119   {
    00120     u = 0;
    00121     while(u<64)
    00122     {
    00123       v = 0;
    00124       while(v<64)
    00125       {
    00126         if(colorClassesUnpacked[(y<<12)|(v<<6)|u] == color)
    00127         {
    00128           Vector3<int> first(4*y,u,v);
    00129           pNear = first;
    00130           pFar = first;
    00131           v++;
    00132           goto expandBox;
    00133         }
    00134         v++;
    00135       }
    00136       u++;
    00137     }
    00138     y++;
    00139   }
    00140   return; //Return if the color has not been found
    00141   expandBox:
    00142   while(y<16)
    00143   {
    00144     u = 0;
    00145     while(u<64)
    00146     {
    00147       v = 0;
    00148       while(v<64)
    00149       {
    00150         if(colorClassesUnpacked[(y<<12)|(v<<6)|u] == color)
    00151         {
    00152           Vector3<int> expand(4*y,u,v);
    00153           if(expand.x < pNear.x)
    00154           {
    00155             pNear.x = expand.x;
    00156           }
    00157           else if(expand.x > pFar.x)
    00158           {
    00159             pFar.x = expand.x;
    00160           }
    00161           if(expand.y < pNear.y)
    00162           {
    00163             pNear.y = expand.y;
    00164           }
    00165           else if(expand.y > pFar.y)
    00166           {
    00167             pFar.y = expand.y;
    00168           }
    00169           if(expand.z < pNear.z)
    00170           {
    00171             pNear.z = expand.z;
    00172           }
    00173           else if(expand.z > pFar.z)
    00174           {
    00175             pFar.y = expand.y;
    00176           }
    00177         }
    00178         v++;
    00179       }
    00180       u++;
    00181     }
    00182     y++;
    00183   }
    00184   pNear *= 4;
    00185   pFar *= 4;
    00186 }
    00187
    00188 ColorTable32K::~ColorTable32K()
    00189 {
    00190 }
    00191
    00192 ColorTable32K::ColorTable32K()
    00193 {
    00194   clear();
    00195 }
    00196
    00197 /**
    00198  * Currently, interactive calibration through RobotControl is unsupported.
    00199  * For that purpose, you should use AIBOVision.
    00200  */
    00201 void ColorTable32K::addColorClass
    00202 (
    00203  colorClass colorClass,
    00204  unsigned char y,
    00205  unsigned char u,
    00206  unsigned char v
    00207  )
    00208 {
    00209   //~ this->colorClasses[y/4][u/4][v/4] = colorClass;
    00210 }
    00211
    00212 /**
    00213  * Currently, interactive calibration through RobotControl is unsupported.
    00214  * For that purpose, you should use AIBOVision.
    00215  */
    00216 void ColorTable32K::addColorClass
    00217 (
    00218  colorClass colorClass,
    00219  unsigned char y,
    00220  unsigned char u,
    00221  unsigned char v,
    00222  unsigned char range
    00223  )
    00224 {
    00225   //~ y /= 4;
    00226   //~ u /= 4;
    00227   //~ v /= 4;
    00228
    00229   //~ if (y < range / 2) y = range / 2;
    00230   //~ if (u < range / 2) u = range / 2;
    00231   //~ if (v < range / 2) v = range / 2;
    00232
    00233   //~ if (y + range / 2 > 63) y = 63 + range / 2 - range;
    00234   //~ if (u + range / 2 > 63) u = 63 + range / 2 - range;
    00235   //~ if (v + range / 2 > 63) v = 63 + range / 2 - range;
    00236
    00237   //~ for(unsigned char currentY = y - range / 2; currentY < y - range / 2 + range + 1; currentY++)
    00238   //~ {
    00239     //~ for(unsigned char currentU = u - range / 2; currentU < u - range / 2 + range + 1; currentU++)
    00240     //~ {
    00241       //~ for(unsigned char currentV = v - range / 2; currentV < v - range / 2 + range + 1; currentV++)
    00242       //~ {
    00243         //~ this->colorClasses[currentY][currentU][currentV] = colorClass;
    00244         //~ ASSERT(currentY < 64);
    00245         //~ ASSERT(currentU < 64);
    00246         //~ ASSERT(currentV < 64);
    00247       //~ }
    00248     //~ }
    00249   //~ }
    00250 }
    00251
    00252 /**
    00253  * Currently, interactive calibration through RobotControl is unsupported.
    00254  * For that purpose, you should use AIBOVision.
    00255  */
    00256 void ColorTable32K::addCuboidToColorClass
    00257 (
    00258  colorClass colorClass,
    00259  unsigned char yMin,
    00260  unsigned char uMin,
    00261  unsigned char vMin,
    00262  unsigned char yMax,
    00263  unsigned char uMax,
    00264  unsigned char vMax
    00265  )
    00266 {
    00267   //~ yMin /= 4; uMin /= 4; vMin /= 4;
    00268   //~ yMax /= 4; uMax /= 4; vMax /= 4;
    00269
    00270   //~ for(unsigned char currentY = yMin; currentY <= yMax + 1; currentY++)
    00271   //~ {
    00272     //~ for(unsigned char currentU = uMin; currentU <= uMax + 1; currentU++)
    00273     //~ {
    00274       //~ for(unsigned char currentV = vMin; currentV <= vMax + 1; currentV++)
    00275       //~ {
    00276         //~ this->colorClasses[currentY][currentU][currentV] = colorClass;
    00277         //~ ASSERT(currentY < 64);
    00278         //~ ASSERT(currentU < 64);
    00279         //~ ASSERT(currentV < 64);
    00280       //~ }
    00281     //~ }
    00282   //~ }
    00283 }
    00284
    00285 /**
    00286  * Currently, interactive calibration through RobotControl is unsupported.
    00287  * For that purpose, you should use AIBOVision.
    00288  */
    00289 void ColorTable32K::removeColorClass
    00290 (
    00291  unsigned char y,
    00292  unsigned char u,
    00293  unsigned char v,
    00294  unsigned char range
    00295  )
    00296 {
    00297   //~ y /= 4;
    00298   //~ u /= 4;
    00299   //~ v /= 4;
    00300
    00301   //~ if (y < range / 2) y = range / 2;
    00302   //~ if (u < range / 2) u = range / 2;
    00303   //~ if (v < range / 2) v = range / 2;
    00304
    00305   //~ if (y + range / 2 > 63) y = 63 + range / 2 - range;
    00306   //~ if (u + range / 2 > 63) u = 63 + range / 2 - range;
    00307   //~ if (v + range / 2 > 63) v = 63 + range / 2 - range;
    00308
    00309   //~ for(unsigned char currentY = y - range / 2; currentY < y - range / 2 + range + 1; currentY++)
    00310   //~ {
    00311     //~ for(unsigned char currentU = u - range / 2; currentU < u - range / 2 + range + 1; currentU++)
    00312     //~ {
    00313       //~ for(unsigned char currentV = v - range / 2; currentV < v - range / 2 + range + 1; currentV++)
    00314       //~ {
    00315         //~ this->colorClasses[currentY][currentU][currentV] = noColor;
    00316       //~ }
    00317     //~ }
    00318   //~ }
    00319 }
    00320
    00321
    00322 /**
    00323  * Currently, interactive calibration through RobotControl is unsupported.
    00324  * For that purpose, you should use AIBOVision.
    00325  */
    00326 void ColorTable32K::clearChannel(colorClass colorClass)
    00327 {
    00328   //~ for (unsigned char y = 0; y < 64; y++)
    00329   //~ {
    00330     //~ for (unsigned char u = 0; u < 64; u++)
    00331     //~ {
    00332       //~ for (unsigned char v = 0; v < 64; v++)
    00333       //~ {
    00334         //~ if (colorClasses[y][u][v] == colorClass) colorClasses[y][u][v] = noColor;
    00335       //~ }
    00336     //~ }
    00337   //~ }
    00338 }
    00339
    00340 void ColorTable32K::clear()
    00341 {
    00342   for (int i = 0; i < 8*64*64; i++)
    00343   {
    00344     colorClasses[i] = noColor;
    00345   }
    00346 }
    00347
    00348 Out& operator << (Out& stream, const ColorTable32K& colorTable32K)
    00349 {
    00350   // the old version of the streaming operator, can be used for converting
    00351   // old colortables to new ones (with the VC debugger)
    00352 //  stream.write(&colorTable32K.colorClasses,sizeof(colorTable32K.colorClasses));
    00353 //  return stream;
    00354
    00355
    00356   // the current color class in the color table
    00357   unsigned char currentColorClass = (unsigned char)(colorTable32K.colorClasses[0]);
    00358
    00359   // the next color class to be compared with the last one
    00360   unsigned char nextColorClass;
    00361
    00362   // a pointer to the begin of the color table
    00363   const unsigned char* colorTable = (const unsigned char*)&colorTable32K.colorClasses;
    00364
    00365   // the number of bytes in one sequence that have the same color class
    00366   int currentLength = 1;
    00367
    00368   for (unsigned int i=1;i<sizeof(colorTable32K.colorClasses);i++)
    00369   {
    00370     nextColorClass = colorTable[i];
    00371
    00372     if (nextColorClass != currentColorClass)
    00373     {
    00374       stream << currentLength << currentColorClass;
    00375       currentColorClass = nextColorClass;
    00376       currentLength = 1;
    00377     }
    00378     else
    00379     {
    00380       currentLength++;
    00381     }
    00382   }
    00383   stream << currentLength << currentColorClass;
    00384
    00385   // write a 0 at the end of the stream as a marker for the In streaming operator
    00386   stream << (int)0;
    00387
    00388   return stream;
    00389 }
    00390
    00391 In& operator>>(In& stream,ColorTable32K& colorTable32K)
    00392 {
    00393   // the old version of the streaming operator, can be used for converting
    00394   // old colortables to new ones (with the VC debugger)
    00395 //  stream.read(&colorTable32K.colorClasses,sizeof(colorTable32K.colorClasses));
    00396 //  return stream;
    00397
    00398   // a pointer to the begin of the color table
    00399   unsigned char* colorTable = (unsigned char*)&colorTable32K.colorClasses;
    00400
    00401   // the current color class in the color table
    00402   unsigned char currentColorClass;
    00403
    00404   // the number of bytes in one sequence that have the same color class
    00405   unsigned int currentLength;
    00406
    00407   // the position in the color table
    00408   unsigned int pos=0;
    00409
    00410   while(pos < sizeof(colorTable32K.colorClasses))
    00411   {
    00412     stream >> currentLength;
    00413     if (currentLength == 0)
    00414       if(!pos) // support for old format that contains table size
    00415         stream >> currentLength >> currentLength; // skip length
    00416       else
    00417         break;
    00418     stream >> currentColorClass;
    00419
    00420     for (unsigned int i=0;i<=currentLength && i + pos < sizeof(colorTable32K.colorClasses);i++)
    00421     {
    00422       colorTable[i+pos] = currentColorClass;
    00423       if (i+pos > sizeof(colorTable32K.colorClasses) - 1)
    00424         break;
    00425     }
    00426
    00427     pos += currentLength;
    00428   }
    00429
    00430   unsigned char* colorTableUnpacked = (unsigned char*)&colorTable32K.colorClassesUnpacked;
    00431   colorTable32K.unpack(colorTableUnpacked, colorTable);
    00432
    00433   return stream;
    00434 }