Representations/Perception/Image.cpp

Go to the documentation of this file.

/**
 * @file Image.cpp
 *
 * Implementation of class Image.
 */

#include "Platform/GTAssert.h"
#include "Tools/Streams/InOut.h"
#include "Image.h"
#include "Tools/Math/Common.h"


Image::Image() : frameNumber(0)
{
  for(int y = 0; y < cameraInfo.resolutionHeight; ++y)
    for(int x = 0; x < cameraInfo.resolutionWidth; ++x)
    {
      image[y][0][x] = 0;
      image[y][1][x] = 128;
      image[y][2][x] = 128;
      image[y][3][x] = 128;
      image[y][4][x] = 128;
      image[y][5][x] = 128;
    }
}

Image::~Image()
{
}

bool Image::hasColorTable(void)
{
  if (colorTable) return true;
  else return false;
}


//int Image::getHeight(void)
//{  return height; }

void Image::setColorTable(const ColorTable* ct)
{
  colorTable = ct;
}


char Image::getClassifiedColor(int x, int y)
{
  if (colorTable) 
    return colorTable->getColorClass(image[y][0][x], image[y][1][x], image[y][2][x]);
  else return -1;
}

void Image::convertFromYUVToRGB(const Image& yuvImage)
{
  Image::convertFromYCbCrToRGB(yuvImage);
  /*
  int Y,U,V;
  int R,G,B;
  for(int y=0; y < yuvImage.cameraInfo.resolutionHeight; y++)
  {
    for(int x=0; x < yuvImage.cameraInfo.resolutionWidth; x++)
    {
      Y = yuvImage.image[y][0][x];
      U = yuvImage.image[y][1][x];
      V = yuvImage.image[y][2][x];
      R = (int)(Y + 1.140 * (U - 128));
      G = (int)(Y - 0.394 * (V - 128) - 0.581 * (U - 128));
      B = (int)(Y + 2.028 * (V - 128));
      if(R < 0) R = 0; if(R > 255) R = 255;
      if(G < 0) G = 0; if(G > 255) G = 255;
      if(B < 0) B = 0; if(B > 255) B = 255;
      image[y][0][x] = (unsigned char)R;
      image[y][1][x] = (unsigned char)G;
      image[y][2][x] = (unsigned char)B;
    }
  }
  this->cameraInfo = yuvImage.cameraInfo;
  */
}

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

void Image::convertFromRGBToYUV(const Image& rgbImage)
{
  int Y,U,V;
  int R,G,B;
  
  for(int y=0; y < rgbImage.cameraInfo.resolutionHeight; y++)
  {
    for(int x=0; x < rgbImage.cameraInfo.resolutionWidth; x++)
    {
      R = rgbImage.image[y][0][x];
      G = rgbImage.image[y][1][x];
      B = rgbImage.image[y][2][x];
      
      // RGB2YUV Method 1
      //Y =       (int) ( 0.299 * R + 0.587 * G + 0.114 * B);
      //V = 127 + (int) (-0.147 * R - 0.289 * G + 0.437 * B);
      //U = 127 + (int) ( 0.615 * R - 0.515 * G - 0.100 * B);
      
      // RGB2YUV Method 2
      Y =       (int)( 0.2990 * R + 0.5870 * G + 0.1140 * B);
      V = 127 + (int)(-0.1687 * R - 0.3313 * G + 0.5000 * B);
      U = 127 + (int)( 0.5000 * R - 0.4187 * G - 0.0813 * B);
      
//     if (R+G+B != 3*204)
//         int i=0;

      if(Y < 0) Y = 0; if(Y > 255) Y = 255;
      if(U < 0) U = 0; if(U > 255) U = 255;
      if(V < 0) V = 0; if(V > 255) V = 255;

      image[y][0][x] = (unsigned char)Y;
      image[y][1][x] = (unsigned char)U;
      image[y][2][x] = (unsigned char)V;
    }
  }
  this->cameraInfo = rgbImage.cameraInfo;
}

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

void Image::convertFromYUVToTSL(const Image& yuvImage)
{
  int Y,U,V;
  int T,S,L;
  double  y_in, u_in, v_in, tmp, tmp_r, tmp_g, tmp_b, t_out, s_out;
  
  for(int y = 0; y < yuvImage.cameraInfo.resolutionHeight; y++)
  {
    for(int x = 0; x < yuvImage.cameraInfo.resolutionWidth; x++)
    {
      Y = yuvImage.image[y][0][x];
      U = yuvImage.image[y][1][x];
      V = yuvImage.image[y][2][x];
      
      
      const double pi2_div = 0.15915494309189533576888376337251;  /* 1.0 / (PI * 2.0) */
      
      y_in = (double) Y;
      u_in = (double) U;
      v_in = (double) V;
      u_in -= 128.0;
      v_in -= 128.0;
      tmp = 1.0 / (4.3403 * y_in +  2.0 * u_in + v_in);
      tmp_r = (-0.6697 * u_in + 1.6959 * v_in) * tmp;
      tmp_g = (-1.168  * u_in - 1.3626 * v_in) * tmp;
      tmp_b = ( 1.8613 * u_in - 0.331  * v_in) * tmp;
      if (tmp_g > 0.0)
      {
        t_out = (atan2(tmp_r, tmp_g) * pi2_div + 0.25) * 255.0;
      }
      else if (tmp_g < 0.0)
      {
        t_out = (atan2(-tmp_r, -tmp_g) * pi2_div + 0.75) * 255.0;
      }
      else
      {
        t_out = 0.0;
      }
      s_out = sqrt(1.8 * (tmp_r * tmp_r + tmp_g * tmp_g + tmp_b * tmp_b)) * 255.0;
      
      /* Crop T and S values */
      if (t_out < 0.0)
      {
        t_out = 0.0;
      }
      else if (t_out > 255.0)
      {
        t_out = 255.0;
      }
      if (s_out < 0.0)
      {
        s_out = 0.0;
      }
      else if (s_out > 255.0)
      {
        s_out = 255.0;
      }
      
      T = (unsigned char) t_out;
      S = (unsigned char) s_out;
      L = Y;
      
      image[y][0][x] = (unsigned char)T;
      image[y][1][x] = (unsigned char)S;
      image[y][2][x] = (unsigned char)L;
    }
  }
  this->cameraInfo = yuvImage.cameraInfo;
}

unsigned char Image::getHighResY(int x, int y) const
{
  if ((x & 1) == 0)
    if ((y & 1) == 0)
    {
      // Pixel Top Left       = LL - LH - HL + HH 
      y=y>>1;x=x>>1;
      return (unsigned char) ((int)image[y][0][x] - (int)image[y][3][x] - 128 - (int)image[y][4][x] - 128 + (int)image[y][5][x] - 128);
    } 
    else
    {
      // Pixel Bottom Left    = LL + LH - HL - HH
      y=y>>1;x=x>>1;
      return (unsigned char)((int)image[y][0][x] + (int)image[y][3][x] - 128 - (int)image[y][4][x] - 128 - (int)image[y][5][x] - 128);
    }
  else
    if ((y & 1) == 0)
    {
      // Pixel Top Right      = LL - LH + HL - HH
      y=y>>1;x=x>>1;
      return (unsigned char)((int)image[y][0][x] - (int)image[y][3][x] - 128 + (int)image[y][4][x] - 128 - (int)image[y][5][x] - 128);
    }
    else
    {
      // Pixel Bottom Right   = LL + LH + HL + HH
      y=y>>1;x=x>>1;
      return (unsigned char)((int)image[y][0][x] + (int)image[y][3][x] - 128 + (int)image[y][4][x] - 128 + (int)image[y][5][x] - 128);
    }
}

void Image::setHighResY(int x, int y, unsigned char tl, unsigned char bl, unsigned char tr, unsigned char br) 
{
  image[y][0][x] = ((int) tl + (int)bl + (int)tr + (int)br) / 4;
  image[y][3][x] = ((int)-tl + (int)bl - (int)tr + (int)br) / 4 + 128;
  image[y][4][x] = ((int)-tl - (int)bl + (int)tr + (int)br) / 4 + 128;
  image[y][5][x] = ((int) tl - (int)bl - (int)tr + (int)br) / 4 + 128;
}

Out& operator<<(Out& stream, const Image& image)
{
  stream 
    << image.cameraInfo.resolutionWidth 
    << image.cameraInfo.resolutionHeight 
    << image.frameNumber;

  for(int y = 0; y < image.cameraInfo.resolutionHeight; ++y)
    for(int c = 0; c < 6; ++c)
      stream.write(&image.image[y][c][0], image.cameraInfo.resolutionWidth);

  return stream;
}

void Image::setCameraInfo()
{
  if(cameraInfo.resolutionWidth == cameraResolutionWidth_ERS210)
  {
    cameraInfo.openingAngleWidth = openingAngleWidth_ERS210;
    cameraInfo.openingAngleHeight = openingAngleHeight_ERS210;
    cameraInfo.focalLength = focalLength_ERS210;
    cameraInfo.opticalCenter.x = opticalCenterX_ERS210;
    cameraInfo.opticalCenter.y = opticalCenterY_ERS210;
    cameraInfo.secondOrderRadialDistortion = secondOrderRadialDistortion_ERS210;
    cameraInfo.fourthOrderRadialDistortion = fourthOrderRadialDistortion_ERS210;
  }
  else if(cameraInfo.resolutionWidth == cameraResolutionWidth_ERS7)
  {
    cameraInfo.openingAngleWidth = openingAngleWidth_ERS7;
    cameraInfo.openingAngleHeight = openingAngleHeight_ERS7;
    cameraInfo.focalLength = focalLength_ERS7;
    cameraInfo.opticalCenter.x = opticalCenterX_ERS7;
    cameraInfo.opticalCenter.y = opticalCenterY_ERS7;
    cameraInfo.secondOrderRadialDistortion = secondOrderRadialDistortion_ERS7;
    cameraInfo.fourthOrderRadialDistortion = fourthOrderRadialDistortion_ERS7;
  }
  else
  {
    ASSERT(false); // unknown image size
  }
#ifdef _WIN32
  if(frameNumber > 0x10000000) // is simulated image
  {
    frameNumber -= 0x10000000;
    cameraInfo.focalLength = cameraInfo.resolutionHeight / tan(cameraInfo.openingAngleHeight / 2.0) / 2.0;
    cameraInfo.opticalCenter = Vector2<double>(cameraInfo.resolutionWidth / 2, cameraInfo.resolutionHeight / 2);
    cameraInfo.secondOrderRadialDistortion = cameraInfo.fourthOrderRadialDistortion = 0;
    cameraInfo.simulated = true;
  }
#endif
  cameraInfo.focalLengthInv = 1/cameraInfo.focalLength;
  cameraInfo.focalLenPow2 = cameraInfo.focalLength*cameraInfo.focalLength;  
  cameraInfo.focalLenPow4 = cameraInfo.focalLenPow2*cameraInfo.focalLenPow2;
}

/*
 * Change log :
 * 
 * $Log: Image.cpp,v $
 * Revision 1.2  2004/05/26 14:06:32  roefer
 * Corrected initialization of addition y channels in default constructor
 * Flag for simulated images added
 *
 * Revision 1.1.1.1  2004/05/22 17:25:50  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.13  2004/05/13 06:56:23  roefer
 * Simulation of PSDs added
 * Special intrisic parameters for simulated images
 *
 * Revision 1.12  2004/05/07 09:29:55  nistico
 * Bug fixed
 *
 * Revision 1.11  2004/05/01 17:09:33  roefer
 * Streamlined HSI stuff
 *
 * Revision 1.10  2004/04/28 13:24:25  thomas
 * modified: conversion from yuv now uses ycbcr-transformation
 *
 * Revision 1.9  2004/04/27 17:22:06  thomas
 * added convertFromYCbCr2RGB for images
 *
 * Revision 1.8  2004/04/07 13:00:44  risler
 * ddd checkin after go04 - second part
 *
 * Revision 1.5  2004/04/07 11:44:05  risler
 * added sending low res images
 * added Image::setCameraInfo
 *
 * Revision 1.4  2004/04/06 13:19:35  risler
 * cleaned up and improved high resolution image support
 *
 * Revision 1.3  2004/03/29 20:45:16  risler
 * bugfix getHighResY
 *
 * Revision 1.2  2004/03/29 15:19:03  Marc
 * Intruduced the Black and White Image
 * Normal Images (not Jpeg) images were now send as Color Image with BW
 *
 * Revision 1.7  2004/01/04 16:28:46  juengel
 * Conversion methods copy cameraInfo now.
 *
 * Revision 1.6  2003/12/30 20:12:03  roefer
 * Image size is now 208 x 160. Smaller images are placed in the upper left corner
 *
 * Revision 1.5  2003/12/15 11:47:06  juengel
 * Introduced CameraInfo
 *
 * Revision 1.4  2003/12/09 21:17:55  roefer
 * Platform-dependent code moved to GUI (release code) project
 *
 * Revision 1.3  2003/12/02 19:30:44  roefer
 * yuv<->rgb conversion speed improved
 *
 * Revision 1.2  2003/10/26 01:19:55  kindler
 * - fixed a clamping bug in convertFromRGBToYUV()  (an Y was mistyped as a V)
 * - tabs converted to spaces
 *
 * Revision 1.1  2003/10/07 10:09:36  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.1.1.1  2003/07/02 09:40:22  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.12  2003/05/05 11:59:21  dueffert
 * spelling corrected
 *
 * Revision 1.11  2003/03/19 17:20:20  roefer
 * Nonsense streaming operator removed
 *
 * Revision 1.10  2003/03/06 11:57:32  dueffert
 * re-order warning removed
 *
 * Revision 1.9  2003/02/24 22:30:54  juengel
 * Added convertion methods for TSL and HSI
 *
 * Revision 1.8  2003/02/19 14:59:54  roefer
 * pColorTable -> colorTable
 *
 * Revision 1.7  2003/02/18 21:29:17  osterhues
 * Changed all instances of ColorTable64 to new base class ColorTable
 *
 * Revision 1.6  2003/02/17 11:06:49  dueffert
 * warnings removed, greenshills backport
 *
 * Revision 1.5  2003/01/09 16:23:58  dueffert
 * no message
 *
 * Revision 1.4  2003/01/09 13:07:16  jhoffman
 * no message
 *
 * Revision 1.3  2002/12/15 23:33:02  dueffert
 * rgb-yuv-convertion moved to Image
 *
 * Revision 1.2  2002/09/17 23:55:20  loetzsch
 * - unraveled several datatypes
 * - changed the WATCH macro
 * - completed the process restructuring
 *
 * Revision 1.1  2002/09/10 15:26:40  cvsadm
 * Created new project GT2003 (M.L.)
 * - Cleaned up the /Src/DataTypes directory
 * - Removed Challenge Code
 * - Removed processing of incoming audio data
 * - Renamed AcousticMessage to SoundRequest
 *
 * Revision 1.4  2002/08/30 13:33:05  dueffert
 * removed unused includes
 *
 * Revision 1.3  2002/08/29 15:28:31  loetzsch
 * removed #include <iostream.h>
 *
 * Revision 1.2  2002/08/29 14:03:12  dueffert
 * includes in correct case, system includes in <>
 *
 * Revision 1.1.1.1  2002/05/10 12:40:13  cvsadm
 * Moved GT2002 Project from ute to tamara.
 *
 * Revision 1.7  2002/04/03 14:18:52  juengel
 * camera matrix and odometry data added to streaming operator for images
 *
 * Revision 1.6  2002/04/02 13:10:18  dueffert
 * big change: odometryData and cameraMatrix in image now, old logfiles may be obsolete
 *
 * Revision 1.5  2002/02/27 07:00:53  roefer
 * Image initialized
 *
 * Revision 1.4  2001/12/14 16:03:58  juengel
 * Im Konstruktor werden width( fixed ), height( fixed ), frameNumber(0) gesetzt.
 *
 * Revision 1.3  2001/12/12 18:08:55  loetzsch
 * Streaming- Operatoren für Bilder eingebaut, DebugKeyTable nicht- statisch gemacht, Debuggin Mechanismen weitergemacht, Bilder aus Logfiles in RobotControl anzeigen, Logfiles in HU1/Debug auf den Stick schreiben
 *
 * Revision 1.2  2001/12/10 17:47:05  risler
 * change log added
 *
 */

---