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

# Modules/ImageProcessor/SlamImageProcessor/SlamColorCorrector.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file SlamColorCorrector.cpp
    00003 *
    00004 * Implementation of class SlamColorCorrector
    00005 *
    00006 * @author Max Risler
    00007 */
    00008 #include "SlamColorCorrector.h"
    00009 #include "Tools/Location.h"
    00010
    00011 SlamColorCorrector::SlamColorCorrector()
    00012 {
    00013   if(getRobotConfiguration().getRobotDesign() == RobotDesign::ERS7)
    00014     init();
    00015   else
    00016     disable();
    00017 }
    00018
    00019 void SlamColorCorrector::init()
    00020 {
    00021   //setup radius table
    00022   for (int x = 0; x < cameraResolutionWidth_ERS7; x++)
    00023     for (int y = 0; y < cameraResolutionHeight_ERS7; y++)
    00024       radiusTable[x][y] = getRadius(x,y);
    00025
    00026   int maxRadius = radiusTable[0][0];
    00027
    00028   InBinaryFile file(getLocation().getModelFilename("white.img"));
    00029   if (file.exists())
    00030   {
    00031     int i,c,j,x,y;
    00032     long pixelSum[cameraResolutionWidth_ERS7][3];
    00033     int pixelNum[cameraResolutionWidth_ERS7];
    00034
    00035     for (i = 0; i <= maxRadius; i++)
    00036     {
    00037       for (j = 0; j < 3; j++)
    00038         pixelSum[i][j] = 0;
    00039       pixelNum[i] = 0;
    00040     }
    00041
    00042     do
    00043     {
    00044       Image image;
    00045       //file >> image;
    00046       file >> image.cameraInfo.resolutionWidth
    00047            >> image.cameraInfo.resolutionHeight
    00048            >> image.frameNumber;
    00049       for(y = 0; y < image.cameraInfo.resolutionHeight; ++y)
    00050         for(c = 0; c < 3; ++c)
    00051           file.read(&image.image[y][c][0], image.cameraInfo.resolutionWidth);
    00052
    00053       for (x = 0; x < image.cameraInfo.resolutionWidth; x++)
    00054         for (y = 0; y < image.cameraInfo.resolutionHeight; y++)
    00055         {
    00056           int r = radiusTable[x][y];
    00057           pixelSum[r][0] += image.image[y][0][x];
    00058           pixelSum[r][1] += image.image[y][1][x];
    00059           pixelSum[r][2] += image.image[y][2][x];
    00060           pixelNum[r]++;
    00061         }
    00062     }
    00063     while(!file.eof());
    00064
    00065     if (pixelNum[0] == 0)
    00066     {
    00067       OUTPUT(idText, text, "SlamColorCorrector: white.img contains no images.");
    00068       disable();
    00069       return;
    00070     }
    00071
    00072     for (i = 0; i <= maxRadius; i++)
    00073       for (j = 0; j < 3; j++)
    00074       {
    00075         pixelSum[i][j] /= pixelNum[i];
    00076         for (c = 0; c < 256; c++)
    00077         {
    00078           long x = c * pixelSum[0][j] / pixelSum[i][j];
    00079           if (x > 255) x = 255;
    00080           correctionTable[i][c][j] = (unsigned char) x;
    00081         }
    00082       }
    00083
    00084   } else {
    00085     OUTPUT(idText, text, "SlamColorCorrector: white.img not found.");
    00086     disable();
    00087   }
    00088 }
    00089
    00090 void SlamColorCorrector::disable()
    00091 {
    00092   for (int x = 0; x < cameraResolutionWidth_ERS7; x++)
    00093     for (int y = 0; y < cameraResolutionHeight_ERS7; y++)
    00094       radiusTable[x][y] = 0;
    00095   for (int c=0; c<256; c++)
    00096   {
    00097     correctionTable[0][c][0] = c;
    00098     correctionTable[0][c][1] = c;
    00099     correctionTable[0][c][2] = c;
    00100   }
    00101 }