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

# Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.cpp
    00003 *
    00004 * This file contains a class that represents a table used for color correction.
    00005 * It is a mixture among the BB2004, DDD2004 and MSH2004 correction.
    00006 * Everything is static so it is loaded only once in the simulator.
    00007 *
    00008 * @author <A href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</A>
    00009 * @author Max Risler
    00010 * @author <A href="mailto:roefer@tzi.de">Thomas Röfer</A>
    00011 */
    00012
    00013 #include "ColorCorrector.h"
    00014 #include "Platform/GTAssert.h"
    00015 #include "Tools/Location.h"
    00016 #include "Tools/Streams/InStreams.h"
    00017
    00018 bool ColorCorrector::loaded = false;
    00019 unsigned char ColorCorrector::correctionTable[ColorCorrector::maxRadius + 1][256][3];
    00020
    00021 #ifdef MSH
    00022 int ColorCorrector::radialOrder = 0;
    00023 int ColorCorrector::colorOrder = 0;
    00024 double ColorCorrector::radialP[3*maxRadialOrder];
    00025 double ColorCorrector::colorP[3*maxColorOrder];
    00026 const double centralPointX = 102.0;
    00027 const double centralPointY = 75.0;
    00028
    00029 #endif
    00030
    00031 #ifdef _WIN32
    00032 unsigned char ColorCorrector::distortionTable[ColorCorrector::maxRadius + 1][256][3];
    00033 #endif
    00034
    00035 void ColorCorrector::load()
    00036 {
    00037 #ifndef MSH
    00038   //setup radius table
    00039   if(getRobotConfiguration().getRobotDesign() == RobotDesign::ERS7)
    00040     for (int x = 0; x < cameraResolutionWidth_ERS7; x++)
    00041       for (int y = 0; y < cameraResolutionHeight_ERS7; y++)
    00042         radiusTable[y][x] = calcRadius(x,y);
    00043   else
    00044     disable();
    00045
    00046   if(loaded)
    00047     return;
    00048
    00049   int maxRadius = radiusTable[0][0];
    00050   InBinaryFile file(getLocation().getModelFilename("white.img"));
    00051   if(file.exists())
    00052   {
    00053     loaded = true;
    00054     int i, c, j, x, y;
    00055     long pixelSum[cameraResolutionWidth_ERS7][3];
    00056     int pixelNum[cameraResolutionWidth_ERS7];
    00057
    00058     for(i = 0; i <= maxRadius; i++)
    00059     {
    00060       for(j = 0; j < 3; j++)
    00061         pixelSum[i][j] = 0;
    00062       pixelNum[i] = 0;
    00063     }
    00064
    00065     do
    00066     {
    00067       Image image;
    00068       file >> image.cameraInfo.resolutionWidth
    00069            >> image.cameraInfo.resolutionHeight
    00070            >> image.frameNumber;
    00071       for(y = 0; y < image.cameraInfo.resolutionHeight; ++y)
    00072         for(c = 0; c < 3; ++c)
    00073           file.read(&image.image[y][c][0], image.cameraInfo.resolutionWidth);
    00074
    00075       for(x = 0; x < image.cameraInfo.resolutionWidth; ++x)
    00076         for(y = 0; y < image.cameraInfo.resolutionHeight; ++y)
    00077         {
    00078           int r = radiusTable[y][x];
    00079           pixelSum[r][0] += image.image[y][0][x];
    00080           pixelSum[r][1] += image.image[y][1][x];
    00081           pixelSum[r][2] += image.image[y][2][x];
    00082           ++pixelNum[r];
    00083         }
    00084     }
    00085     while(!file.eof());
    00086
    00087     if(pixelNum[0] == 0)
    00088       disable();
    00089     else
    00090       for(i = 0; i <= maxRadius; i++)
    00091         for(j = 0; j < 3; j++)
    00092         {
    00093           pixelSum[i][j] /= pixelNum[i];
    00094           for (c = 0; c < 256; c++)
    00095           {
    00096             long x = c * pixelSum[0][j] / pixelSum[i][j];
    00097             if (x > 255) x = 255;
    00098             correctionTable[i][c][j] = (unsigned char) x;
    00099 #ifdef _WIN32
    00100             x = c * pixelSum[i][j] / pixelSum[0][j];
    00101             if (x > 255) x = 255;
    00102             distortionTable[i][c][j] = (unsigned char) x;
    00103 #endif
    00104           }
    00105         }
    00106   }
    00107   else
    00108     disable();
    00109 }
    00110 #else
    00111   if(getRobotConfiguration().getRobotDesign() == RobotDesign::ERS7)
    00112   {
    00113     double dx, dy;
    00114     for (int y=0; y<cameraResolutionHeight_ERS7; y++)
    00115       for (int x=0; x<cameraResolutionWidth_ERS7; x++)
    00116       {
    00117         dx = x - centralPointX;
    00118         dy = y - centralPointY;
    00119         double radius = sqrt(dx*dx + dy*dy);
    00120         radiusTable[y][x] = (unsigned char) radius;
    00121         ASSERT(radiusTable[y][x] < maxRadius);
    00122       }
    00123   }
    00124   else
    00125     disable();
    00126
    00127   if(loaded)
    00128     return;
    00129
    00130   const char* channels[3] = {"coeff.cy", "coeff.cu", "coeff.cv"};
    00131   double dummy;
    00132   bool channelLoaded[3] = {false, false, false};
    00133   for(int channel = 0; channel < 3; ++channel)
    00134   {
    00135     InBinaryFile stream(getLocation().getModelFilename(channels[channel]));
    00136     if(stream.exists())
    00137     {
    00138       loaded = true;
    00139       channelLoaded[channel] = true;
    00140       stream >> radialOrder;
    00141       stream >> colorOrder;
    00142       int i;
    00143       if(radialOrder < maxRadialOrder)
    00144       { //source array bigger than file
    00145         for(i = 0; i < radialOrder; ++i)
    00146           stream >> radialP[i + channel * maxRadialOrder];
    00147         for(; i < maxRadialOrder; i++)
    00148           radialP[i] = 0.0;
    00149       }
    00150       else
    00151       {
    00152         for(i = 0; i < maxRadialOrder; ++i)
    00153           stream >> radialP[i + channel * maxRadialOrder];
    00154         for(; i < radialOrder; i++) //discard higher order coeff that doesnt fit in the array size
    00155           stream >> dummy;
    00156       }
    00157       if (colorOrder < maxColorOrder)
    00158       { //source array bigger than file
    00159         for (i = 0; i < colorOrder; ++i)
    00160           stream >> colorP[i + channel * maxColorOrder];
    00161         for (; i < maxColorOrder; ++i)
    00162           colorP[i] = 0.0;
    00163       }
    00164       else
    00165       {
    00166         for (i = 0; i < maxColorOrder; ++i)
    00167           stream >> colorP[i + channel * maxColorOrder];
    00168         for (; i < colorOrder; ++i) //discard higher order coeff that doesnt fit in the array size
    00169           stream >> dummy;
    00170       }
    00171     }
    00172     else
    00173     {
    00174       int i;
    00175       for(i = 0; i < radialOrder; ++i)
    00176         radialP[i + channel * maxRadialOrder] = 0.0;
    00177       for(i = 0; i < colorOrder; ++i)
    00178         colorP[i + channel * maxColorOrder] = 0.0;
    00179     }
    00180   }
    00181   for (int radius=0; radius < maxRadius; ++radius)
    00182   {
    00183     int color;
    00184     for(color = 0; color < 256; ++color)
    00185       for(int channel = 0; channel < 3; ++channel)
    00186         correctionTable[radius][color][channel] = colorDistortionCorrection(radius, color, channel);
    00187 #ifdef _WIN32
    00188     for(color = 0; color < 256; ++color)
    00189       for(int channel = 0; channel < 3; ++channel)
    00190       {
    00191         int best = 0;
    00192         for(int i = 1; i < 256; ++i)
    00193           if((radius == 0 || abs(i - distortionTable[radius - 1][color][channel]) < 3) &&
    00194              abs(correctionTable[radius][i][channel] - color) < abs(correctionTable[radius][best][channel] - color))
    00195             best = i;
    00196         distortionTable[radius][color][channel] = (unsigned char) best;
    00197       }
    00198 #endif
    00199   }
    00200   for(int c = 0; c < 256; ++c)
    00201     for(int j = 0; j < 3; ++j)
    00202     {
    00203       correctionTable[maxRadius][c][j] = c;
    00204 #ifdef _WIN32
    00205       distortionTable[maxRadius][c][j] = c;
    00206 #endif
    00207     }
    00208 }
    00209
    00210 unsigned char ColorCorrector::colorDistortionCorrection(const unsigned char radius_i,
    00211                                                         const unsigned char color, const unsigned char channel)
    00212 {
    00213   int radOffsetBase = channel * maxRadialOrder;
    00214   int colOffsetBase = channel * maxColorOrder;
    00215   double radius = radius_i;
    00216   double radial_base;
    00217   double color_base;
    00218   double rad_correction = 0;
    00219   double col_correction = 0;
    00220   int i;
    00221   for(i = 0; i < radialOrder; ++i)
    00222   {
    00223     radial_base = 1;
    00224     for (int k = 0; k < i; ++k)
    00225       radial_base *= radius;
    00226     rad_correction += radial_base*radialP[radOffsetBase + i];
    00227   }
    00228   for(i = 0; i < colorOrder; ++i)
    00229   {
    00230     color_base = 1;
    00231     for(int k = 0; k < i; ++k)
    00232       color_base *= color;
    00233     col_correction += color_base * colorP[colOffsetBase + i];
    00234   }
    00235   double result = color + rad_correction * col_correction;
    00236   if(result < 0)
    00237     result = 0;
    00238   else if(result > 255)
    00239     result = 255;
    00240   return (unsigned char) result;
    00241 }
    00242 #endif
    00243
    00244 void ColorCorrector::disable()
    00245 {
    00246   for(int y = 0; y < cameraResolutionHeight_ERS7; ++y)
    00247     for(int x = 0; x < cameraResolutionWidth_ERS7; ++x)
    00248       radiusTable[y][x] = maxRadius;
    00249 }
    00250
    00251 void ColorCorrector::correct(Image& image) const
    00252 {
    00253   for(int x = 0; x < image.cameraInfo.resolutionWidth; ++x)
    00254     for(int y = 0; y < image.cameraInfo.resolutionHeight; ++y)
    00255       correct(x, y, image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
    00256 }
    00257
    00258 #ifdef _WIN32
    00259 void ColorCorrector::distort(Image& image) const
    00260 {
    00261   for(int x = 0; x < image.cameraInfo.resolutionWidth; ++x)
    00262     for(int y = 0; y < image.cameraInfo.resolutionHeight; ++y)
    00263       distort(x, y, image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
    00264 }
    00265 #endif