Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.cpp

Go to the documentation of this file.

/**
* @file Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.cpp
* 
* This file contains a class that represents a table used for color correction.
* It is a mixture among the BB2004, DDD2004 and MSH2004 correction.
* Everything is static so it is loaded only once in the simulator.
*
* @author <A href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</A>
* @author Max Risler
* @author <A href="mailto:roefer@tzi.de">Thomas Röfer</A>
*/

#include "ColorCorrector.h"
#include "Platform/GTAssert.h"
#include "Tools/Location.h"
#include "Tools/Streams/InStreams.h"

bool ColorCorrector::loaded = false;
unsigned char ColorCorrector::correctionTable[ColorCorrector::maxRadius + 1][256][3];

#ifdef MSH
int ColorCorrector::radialOrder = 0;
int ColorCorrector::colorOrder = 0;
double ColorCorrector::radialP[3*maxRadialOrder];
double ColorCorrector::colorP[3*maxColorOrder];
const double centralPointX = 102.0;
const double centralPointY = 75.0;

#endif

#ifdef _WIN32  
unsigned char ColorCorrector::distortionTable[ColorCorrector::maxRadius + 1][256][3];
#endif

void ColorCorrector::load()
{
#ifndef MSH
  //setup radius table
  if(getRobotConfiguration().getRobotDesign() == RobotDesign::ERS7)
    for (int x = 0; x < cameraResolutionWidth_ERS7; x++)
      for (int y = 0; y < cameraResolutionHeight_ERS7; y++)
        radiusTable[y][x] = calcRadius(x,y);
  else
    disable();

  if(loaded) 
    return;

  int maxRadius = radiusTable[0][0];
  InBinaryFile file(getLocation().getModelFilename("white.img"));
  if(file.exists())
  {
    loaded = true;
    int i, c, j, x, y;
    long pixelSum[cameraResolutionWidth_ERS7][3];
    int pixelNum[cameraResolutionWidth_ERS7];

    for(i = 0; i <= maxRadius; i++)
    {
      for(j = 0; j < 3; j++)
        pixelSum[i][j] = 0;
      pixelNum[i] = 0;
    }

    do 
    {
      Image image;
      file >> image.cameraInfo.resolutionWidth 
           >> image.cameraInfo.resolutionHeight 
           >> image.frameNumber;
      for(y = 0; y < image.cameraInfo.resolutionHeight; ++y)
        for(c = 0; c < 3; ++c)
          file.read(&image.image[y][c][0], image.cameraInfo.resolutionWidth);

      for(x = 0; x < image.cameraInfo.resolutionWidth; ++x)
        for(y = 0; y < image.cameraInfo.resolutionHeight; ++y)
        {
          int r = radiusTable[y][x];
          pixelSum[r][0] += image.image[y][0][x];
          pixelSum[r][1] += image.image[y][1][x];
          pixelSum[r][2] += image.image[y][2][x];
          ++pixelNum[r];
        }
    }
    while(!file.eof());

    if(pixelNum[0] == 0)
      disable();
    else
      for(i = 0; i <= maxRadius; i++)
        for(j = 0; j < 3; j++)
        {
          pixelSum[i][j] /= pixelNum[i];
          for (c = 0; c < 256; c++)
          {
            long x = c * pixelSum[0][j] / pixelSum[i][j];
            if (x > 255) x = 255;
            correctionTable[i][c][j] = (unsigned char) x;
#ifdef _WIN32  
            x = c * pixelSum[i][j] / pixelSum[0][j];
            if (x > 255) x = 255;
            distortionTable[i][c][j] = (unsigned char) x;
#endif
          }
        }
  }
  else
    disable();
}
#else
  if(getRobotConfiguration().getRobotDesign() == RobotDesign::ERS7)
  {
    double dx, dy;
    for (int y=0; y<cameraResolutionHeight_ERS7; y++)
      for (int x=0; x<cameraResolutionWidth_ERS7; x++)
      {
        dx = x - centralPointX;
        dy = y - centralPointY;
        double radius = sqrt(dx*dx + dy*dy);
        radiusTable[y][x] = (unsigned char) radius;
        ASSERT(radiusTable[y][x] < maxRadius);
      }
  }
  else
    disable();

  if(loaded)
    return;

  const char* channels[3] = {"coeff.cy", "coeff.cu", "coeff.cv"};
  double dummy;
  bool channelLoaded[3] = {false, false, false};
  for(int channel = 0; channel < 3; ++channel)
  {
    InBinaryFile stream(getLocation().getModelFilename(channels[channel]));
    if(stream.exists())
    {
      loaded = true;
      channelLoaded[channel] = true;
      stream >> radialOrder;
      stream >> colorOrder;
      int i;
      if(radialOrder < maxRadialOrder)
      { //source array bigger than file
        for(i = 0; i < radialOrder; ++i)
          stream >> radialP[i + channel * maxRadialOrder];
        for(; i < maxRadialOrder; i++)
          radialP[i] = 0.0;
      }
      else
      {
        for(i = 0; i < maxRadialOrder; ++i)
          stream >> radialP[i + channel * maxRadialOrder];
        for(; i < radialOrder; i++) //discard higher order coeff that doesnt fit in the array size
          stream >> dummy;
      }
      if (colorOrder < maxColorOrder)
      { //source array bigger than file
        for (i = 0; i < colorOrder; ++i)
          stream >> colorP[i + channel * maxColorOrder];
        for (; i < maxColorOrder; ++i)
          colorP[i] = 0.0;
      }
      else
      {
        for (i = 0; i < maxColorOrder; ++i)
          stream >> colorP[i + channel * maxColorOrder];
        for (; i < colorOrder; ++i) //discard higher order coeff that doesnt fit in the array size
          stream >> dummy;
      }
    }
    else
    {
      int i;
      for(i = 0; i < radialOrder; ++i)
        radialP[i + channel * maxRadialOrder] = 0.0;
      for(i = 0; i < colorOrder; ++i)
        colorP[i + channel * maxColorOrder] = 0.0;
    }
  }
  for (int radius=0; radius < maxRadius; ++radius)
  {
    int color;
    for(color = 0; color < 256; ++color)
      for(int channel = 0; channel < 3; ++channel)
        correctionTable[radius][color][channel] = colorDistortionCorrection(radius, color, channel);
#ifdef _WIN32
    for(color = 0; color < 256; ++color)
      for(int channel = 0; channel < 3; ++channel)
      {
        int best = 0;
        for(int i = 1; i < 256; ++i)
          if((radius == 0 || abs(i - distortionTable[radius - 1][color][channel]) < 3) &&
             abs(correctionTable[radius][i][channel] - color) < abs(correctionTable[radius][best][channel] - color))
            best = i;
        distortionTable[radius][color][channel] = (unsigned char) best;
      }
#endif
  }
  for(int c = 0; c < 256; ++c)
    for(int j = 0; j < 3; ++j)
    {
      correctionTable[maxRadius][c][j] = c;
#ifdef _WIN32
      distortionTable[maxRadius][c][j] = c;
#endif
    }
}

unsigned char ColorCorrector::colorDistortionCorrection(const unsigned char radius_i, 
                                                        const unsigned char color, const unsigned char channel)
{
  int radOffsetBase = channel * maxRadialOrder;
  int colOffsetBase = channel * maxColorOrder;
  double radius = radius_i;
  double radial_base;
  double color_base;
  double rad_correction = 0;
  double col_correction = 0;
  int i;
  for(i = 0; i < radialOrder; ++i)
  {
    radial_base = 1;
    for (int k = 0; k < i; ++k)
      radial_base *= radius;
    rad_correction += radial_base*radialP[radOffsetBase + i];
  }
  for(i = 0; i < colorOrder; ++i)
  {
    color_base = 1;
    for(int k = 0; k < i; ++k)
      color_base *= color;
    col_correction += color_base * colorP[colOffsetBase + i];
  }
  double result = color + rad_correction * col_correction;
  if(result < 0)
    result = 0;
  else if(result > 255)
    result = 255;
  return (unsigned char) result;
}
#endif

void ColorCorrector::disable()
{
  for(int y = 0; y < cameraResolutionHeight_ERS7; ++y)
    for(int x = 0; x < cameraResolutionWidth_ERS7; ++x)
      radiusTable[y][x] = maxRadius;
}

void ColorCorrector::correct(Image& image) const
{
  for(int x = 0; x < image.cameraInfo.resolutionWidth; ++x)
    for(int y = 0; y < image.cameraInfo.resolutionHeight; ++y)
      correct(x, y, image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
}

#ifdef _WIN32
void ColorCorrector::distort(Image& image) const
{
  for(int x = 0; x < image.cameraInfo.resolutionWidth; ++x)
    for(int y = 0; y < image.cameraInfo.resolutionHeight; ++y)
      distort(x, y, image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
}
#endif

---