Modules/SensorBehaviorControl/MotionRecognition.cpp

Go to the documentation of this file.

/**
* @file MotionRecognition.cpp
*
* Implementation of class MotionRecognition
*
* @author <a href="mailto:ordyniak@informatik.hu-berlin.de">Sebastian Ordyniak</a>
* @author <a href="mailto:richert@informatik.hu-berlin.de">Marten Richert</a>
*/

#include "MotionRecognition.h"

MotionRecognition::MotionRecognition(const SensorBehaviorControlInterfaces& interfaces)
: SensorBehaviorControl(interfaces)
{
  pixelDimensions = 
    Vector2<double>
    (
    sin(image.cameraInfo.openingAngleWidth / 2.0)*BRENNWEITE / 
    (image.cameraInfo.resolutionWidth / 2.0),
    sin(image.cameraInfo.openingAngleHeight / 2.0)*BRENNWEITE / 
    (image.cameraInfo.resolutionHeight / 2.0)
    );
  currentImage = 0;
  start = 1;
  imagesLeft = IMAGEBUFFERSIZE;
}

double MotionRecognition::getCameraZ(Vector2<double> angleYZ) {
  double ztranslation = cameraMatrix.translation.z;
  double back;
  back = - ztranslation /
      (cameraMatrix.rotation.c[0].z + cameraMatrix.rotation.c[1].z*sin(angleYZ.y) + cameraMatrix.rotation.c[2].z*sin(angleYZ.x));
  if (back < 0)
    back = DISTANCE_INFINITY;
  return back;
}

Vector2<double> MotionRecognition::getCoorInmm(Vector2<int> p) {
  return Vector2<double>(p.x*pixelDimensions.x,p.y*pixelDimensions.y);
}

Vector2<int> MotionRecognition::getCoorInPixel(Vector2<double> p) {
  return Vector2<int>((int)(p.x/pixelDimensions.x), (int)(p.y/pixelDimensions.y));
}


Vector2<int> MotionRecognition::getCenteredCoor(Vector2<int> p) {
  // zentrieren
  Vector2<int> back = Vector2<int>(p.x - image.cameraInfo.resolutionWidth / 2,
                    image.cameraInfo.resolutionHeight / 2 - p.y);
  return back;
}


int MotionRecognition::isDiff(Image& image1, Image& image2, Vector2<int> p1, Vector2<int> p2) {
  int diff = image1.image[p1.y][0][p1.x] - image2.image[p2.y][0][p2.x];
  if (diff > 50)
    return 1;
  return 0;
}

Vector2<double> MotionRecognition::getAngleYZ(Vector2<int> pCentered) {
  double angleY, angleZ;
  angleY = (double)pCentered.y / (image.cameraInfo.resolutionHeight / 2.0) * (image.cameraInfo.openingAngleHeight / 2.0);
  angleZ = -(double)pCentered.x / (image.cameraInfo.resolutionWidth / 2.0) * (image.cameraInfo.openingAngleWidth / 2.0);
  return Vector2<double>(angleY, angleZ);
}

Vector2<int> MotionRecognition::getPixelFlow(Vector2<double> pCenteredmm, double z, Vector3<double> cameraTranslation, Vector3<double> cameraRotation) {
  /** translationskoeffizienten */
  double tx_z = cameraTranslation.x/z;
  double ty_z = cameraTranslation.y/z;
  double tz_z = cameraTranslation.z/z;

  /** Brennweite der Kamera */
  double f = 2.18;

  /** rotationskoeffizenten*/
  double xy_f = pCenteredmm.x*(-pCenteredmm.y)/f;
  double xx_f_f = pCenteredmm.x*pCenteredmm.x/f+f;
  double yy_f_f = pCenteredmm.y*pCenteredmm.y/f+f;

  Vector2<double> back = Vector2<double>(0,0);
  back.x = (ty_z*f + tx_z*pCenteredmm.x + xy_f*cameraRotation.y - xx_f_f*cameraRotation.z + pCenteredmm.y*cameraRotation.x);
  back.y = (tz_z*f + tx_z*(-pCenteredmm.y) + yy_f_f*cameraRotation.y - xy_f*cameraRotation.z - pCenteredmm.x*cameraRotation.x);
  return getCoorInPixel(back);
}

Vector2<int> MotionRecognition::getNewPixelPos(Vector2<int> p, Vector3<double> cameraTranslation, Vector3<double> cameraRotation) {
  /** transformiere p in Kamerazentrierte */
  Vector2<int> cCentered = getCenteredCoor(p);

  /** transformiere in Grad */
  Vector2<double> angleYZ = getAngleYZ(cCentered);
  
  /** berechne z Koordinate der xy-Ebene */
  double cameraZ = getCameraZ(angleYZ);
  
  /** berechne Fluss in Pixeln */
  Vector2<int> back = getPixelFlow(getCoorInmm(cCentered), cameraZ, cameraTranslation, cameraRotation);
  return p + back;
}

Vector3<double> MotionRecognition::getCameraTranslation(Vector3<double> robotTranslation) {
  RotationMatrix rm = cameraMatrix.rotation;
  return rm.invert()*robotTranslation;
}

Vector3<double> MotionRecognition::getRobotTranslationForRotation(double robotRotationZ) {
  double d = cameraMatrix.translation.x;
  return Vector3<double>(cos(robotRotationZ)*d - d, sin(robotRotationZ)*d, 0); 
}

void MotionRecognition::drawPixelFlow(Vector3<double> cameraTranslation, Vector3<double> cameraRotation) {
  for (int x = 10;x <= image.cameraInfo.resolutionWidth - 10;x+=10) {
    for (int y = 10;y <= image.cameraInfo.resolutionHeight - 10;y+=10) {
      Vector2<int> p = Vector2<int>(x,y);
      Vector2<int> pt = getNewPixelPos(p, cameraTranslation, cameraRotation);
      LINE(sketch, p.x, p.y, pt.x, pt.y,1,1,Drawings::yellow);
      DOT(sketch, pt.x,pt.y, Drawings::red, Drawings::red);
    }
  }
}

int MotionRecognition::pixelInImage(Vector2<int> p) {
  if (p.x >= 0 && p.y >= 0 && p.x < image.cameraInfo.resolutionWidth && p.y < image.cameraInfo.resolutionHeight)
    return 1;
  return 0;
}

Vector3<double> MotionRecognition::getRobotTranslationFromOdometry() {
  Vector2<double> translationdiff = odometryData.translation - previousOdometry.translation;
  double rotation = odometryData.rotation - previousOdometry.rotation;

  Vector3<double> robotTranslation = Vector3<double>(translationdiff.x, translationdiff.y, 0) +
    getRobotTranslationForRotation(rotation);
  return robotTranslation;
}

Vector3<double> MotionRecognition::getRobotTranslationFromBounce(double bounce) {
  return Vector3<double>(0,0,0);
}

Vector3<double> MotionRecognition::getRobotRotationFromOdometry() {
  return Vector3<double>(0,0, previousOdometry.rotation - odometryData.rotation);
}

Vector3<double> MotionRecognition::getRobotRotationFromBounce(double bounce) {
  return Vector3<double>(0,fromDegrees(bounce),0);
}

Vector3<double> MotionRecognition::getCameraTranslation(double bounce) {
  Vector3<double> robotTranslation = getRobotTranslationFromOdometry() + 
                  getRobotTranslationFromBounce(bounce);
  return getCameraTranslation(robotTranslation);
}

Vector3<double> MotionRecognition::getCameraRotation(double bounce) {
  Vector3<double> robotRotation = getRobotRotationFromOdometry() +
                  getRobotRotationFromBounce(bounce);
  return getCameraTranslation(robotRotation);
}

double MotionRecognition::getPixelDiff(Vector2<int> raster, double bounce, Image& image1, Image& image2, int draw) {
  Vector3<double> cameraTranslation = getCameraTranslation(bounce);
  Vector3<double> cameraRotation = getCameraRotation(bounce);

  int diffSum = 0;
  int countPixel = 0;
  for (int x = 0;x < image.cameraInfo.resolutionWidth;x+=raster.x) {
    for (int y = 0;y < image.cameraInfo.resolutionHeight;y+=raster.y) {
      Vector2<int> p = Vector2<int>(x,y);
      Vector2<int> pt = getNewPixelPos(p, cameraTranslation, cameraRotation);
      if (pixelInImage(pt)) {
        countPixel++;
        if (isDiff(image1, image2, p, pt)) {
          diffSum++;
          if (draw)
            motionRecognitionImage.image[pt.y][0][pt.x] = 200;
        }
      }
      if (draw) {
        if (isDiff(image1, image2, p, p))
          processorGradientsImage.image[p.y][0][p.x] = 200;
      }
    }
  }
  return (double)diffSum / (double)countPixel;
}

double MotionRecognition::getPixelDiff(Vector2<int> raster, double bounce, Image& image1, Image& image2) {
  return getPixelDiff(raster, bounce, image1, image2, 0);
}

void MotionRecognition::drawDynamicDiff(Image& image1, Image& image2, double timeDiff) {
  
  motionRecognitionImage = image2;
    int x;
  for (x = 0;x < image.cameraInfo.resolutionWidth;x++) {
    for (int y = 0;y < image.cameraInfo.resolutionHeight;y++) {
      motionRecognitionImage.image[y][0][x] = 0;
    }
  }
  processorGradientsImage = image2;
  for (x = 0;x < image.cameraInfo.resolutionWidth;x++) {
    for (int y = 0;y < image.cameraInfo.resolutionHeight;y++) {
      processorGradientsImage.image[y][0][x] = 0;
    }
  }
  double minDiff = 65000;
  int minIndex = 0;
  int z;
  for (z = -15;z <=15;z+=5) {
    double relDiffSum = getPixelDiff(Vector2<int>(4,1), (z / 3.0), image1, image2);
    if (relDiffSum < minDiff) {
      minDiff = relDiffSum;minIndex = z;
    }
  }
  
  minDiff = 65000;
  int fastIndex = minIndex;
  for (z = fastIndex-3;z <=fastIndex+3;z++) {
    double relDiffSum = getPixelDiff(Vector2<int>(1,1), (z / 3.0), image1, image2);
    if (relDiffSum < minDiff) {
      minDiff = relDiffSum;minIndex = z;
    }
  }
  
  getPixelDiff(Vector2<int>(1,1), (minIndex / 3.0), image1, image2, 1);
  INIT_DEBUG_IMAGE(imageMotionRecognition, motionRecognitionImage);
  SEND_DEBUG_IMAGE(imageMotionRecognition);
  INIT_DEBUG_IMAGE(imageProcessorGradients, processorGradientsImage);
  SEND_DEBUG_IMAGE(imageProcessorGradients);

  drawPixelFlow(getCameraTranslation((minIndex / 3.0)),getCameraRotation((minIndex / 3.0)));
}

void MotionRecognition::execute()
{
  if (start) {
    start = 0;
    headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
    motionRequest.motionType = MotionRequest::getup;
  } else {
    imageBuffer[currentImage] = image;
    imageTimes[currentImage] = SystemCall::getCurrentSystemTime();
    if (!imagesLeft) {
      int prevImage = currentImage - 1;
      if (prevImage < 0)
        prevImage = IMAGEBUFFERSIZE - 1;
      drawDynamicDiff(imageBuffer[prevImage], imageBuffer[currentImage], (imageTimes[currentImage] - imageTimes[prevImage]) / 1000.0);
    }
    else
      imagesLeft--;
    ++currentImage %= (IMAGEBUFFERSIZE);
  }

  previousOdometry = odometryData;
  DEBUG_DRAWING_FINISHED(sketch);
}

bool MotionRecognition::handleMessage(InMessage& message)
{
  bool handled = true;
  return handled;
}

/*
* Change log :
* 
* $Log: MotionRecognition.cpp,v $
* Revision 1.1.1.1  2004/05/22 17:20:52  cvsadm
* created new repository GT2004_WM
*
* Revision 1.17  2004/03/08 02:11:49  roefer
* Interfaces should be const
*
* Revision 1.16  2004/01/10 17:03:09  roefer
* Warnings removed
*
* Revision 1.15  2004/01/07 17:16:00  ordyniak
* debugimages angepasst
*
* Revision 1.14  2004/01/07 16:53:41  ordyniak
* umgestellt auf odometrydata für logfiles
*
* Revision 1.13  2004/01/07 14:26:40  richert
* no message
*
* Revision 1.12  2004/01/07 09:54:42  schumann
* corrected misdeclaration of variable z and removed compiler error in _GT2004
*
* Revision 1.11  2004/01/07 04:35:34  richert
* bugfix
*
* Revision 1.10  2004/01/07 04:09:44  richert
* added HeadControlMode "direct"
*
* Revision 1.9  2004/01/04 10:10:27  juengel
* Errors and warnings removed.
*
* Revision 1.8  2004/01/03 23:12:51  ordyniak
* new concept
*
* Revision 1.7  2003/12/31 23:50:38  roefer
* Removed inclusion of RobotDimensions.h because it is not used
*
* 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/26 19:58:58  richert
* better angel for horizon (hardcoded)
*
* Revision 1.4  2003/12/26 00:53:08  ordyniak
* z bewegung
*
* Revision 1.3  2003/12/16 19:43:06  roefer
* Warnings removed
*
* Revision 1.2  2003/12/12 17:21:13  richert
* the beginning of a new solution: MotionRegcognition
*
*
*
*/

---