Modules/SensorBehaviorControl/SimpleMotionRecognition.cpp

Go to the documentation of this file.

/**
* @file SimpleMotionRecognition.cpp
*
* Implementation of class SimpleMotionRecognition
*
* @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 "SimpleMotionRecognition.h"

SimpleMotionRecognition::SimpleMotionRecognition(const SensorBehaviorControlInterfaces& interfaces)
: SensorBehaviorControl(interfaces)
{
  currentImage = 0;
  start = 1;
  imagesLeft = SIMPLEIMAGEBUFFERSIZE;
  motion = 0;
  wait = WAITMIN;
  halfCameraResolutionHeigth = image.cameraInfo.resolutionHeight / 2;
  halfCameraResolutionWidth = image.cameraInfo.resolutionWidth / 2;
  halfCameraOpeningAngleHeigth = image.cameraInfo.openingAngleHeight / 2.0;
  halfCameraOpeningAngleWidth = image.cameraInfo.openingAngleWidth / 2.0;
  reset = RESETCONST;
  searchStatus=0;
}

int SimpleMotionRecognition::searchTilt() {
  if (searchStatus==-1)
    return headControlMode.directTilt/2;
  else 
    return 0;
}

int SimpleMotionRecognition::searchPan() {
  int back = 0;
  switch (searchStatus)
  {
    case -1:  back = headControlMode.directPan/2; wait = WAITMIN*5/4; break;
    case  0:  back = 0;           wait = WAITMIN*3/2; break;
    case  1:  back = -585390;         wait = WAITMIN*5/4; break;
    case  2:  back = -1370790;        wait = WAITMIN*5/4; break;
    case  3:  back = 0;           wait = WAITMIN*3/2; break;
    case  4:  back = 585390;          wait = WAITMIN*5/4; break;
    case  5:  back = 1370790;         wait = WAITMIN*5/4; break;
  }
  searchStatus = (searchStatus+1)%6;
  return back;
}

bool SimpleMotionRecognition::headIsNotInMotion() {
//vergleiche previousOdometry und odometryData
  if (previousCameraMatrix == cameraMatrix) {
    if (wait>0)
      wait -= frameDiff;
  }
  else  
    if (motion) {
      if (abs(motion[1].x) > abs(motion[1].y))
        wait = WAITMIN + abs(motion[1].x)*WAITCONST/toMicroRad(halfCameraOpeningAngleWidth);
      else
        wait = WAITMIN + abs(motion[1].y)*WAITCONST/toMicroRad(halfCameraOpeningAngleHeigth);
    }
//    else
//      wait = WAITMIN;
  
  return (wait<=0);
}

Vector2<long> SimpleMotionRecognition::getAngleYZ(Vector2<int> point) {
  long tilt, pan;
  point = point - Vector2<int>(halfCameraResolutionWidth,halfCameraResolutionHeigth);
  point.y = -point.y;
  tilt = toMicroRad(/*atan*/((double)point.y / halfCameraResolutionHeigth) * halfCameraOpeningAngleHeigth);
  pan = -toMicroRad(/*atan*/((double)point.x/ halfCameraResolutionWidth) * halfCameraOpeningAngleWidth);
  return Vector2<long>(tilt, pan);
}

Vector2<int> *SimpleMotionRecognition::getMotion() {
  int diff[2] = {0,0};
  int BPscount[2] = {0,0};
  Vector2<int> BPs[2] = { Vector2<int>(0,0), Vector2<int>(0,0) };
  

  for (int y = 0;y < imageBuffer[currentImage].cameraInfo.resolutionHeight;y+=3) {
    for (int x = 0;x < imageBuffer[currentImage].cameraInfo.resolutionWidth;x+=5) {
      for (int i=0;i < 2;i++)
        diff[i] = abs(imageBuffer[(currentImage+SIMPLEIMAGEBUFFERSIZE-2)%SIMPLEIMAGEBUFFERSIZE].image[y][0][x]
                      - imageBuffer[(currentImage+SIMPLEIMAGEBUFFERSIZE-1+i)%SIMPLEIMAGEBUFFERSIZE].image[y][0][x]) 
               > MINDIFF;

      if (diff[0] && diff[1]) {
        BPscount[0]++;
        BPs[0] += Vector2<int>(x,y);
      } else {
        if (!diff[0] && diff[1]) {
          BPscount[1]++;
          BPs[1] += Vector2<int>(x,y);
        }
      }
    }
  }
  if (BPscount[0] && BPscount[1]) {
    Vector2<int> *back = new Vector2<int>[2];
    back[0] = BPs[0] / BPscount[0];
    back[1] = BPs[1] / BPscount[1];
    return back;
  }
  return 0;
}

void SimpleMotionRecognition::execute()
{
  
  if (start) {
    start = 0;
    headControlMode.headControlMode = HeadControlMode::direct;
    headControlMode.directTilt = 0;
    headControlMode.directPan = 0;
    headControlMode.directRoll = 0;
    motionRequest.motionType = MotionRequest::getup;
  } else {
    imageBuffer[currentImage] = image;
    frameDiff = (int)(imageBuffer[currentImage].frameNumber - imageBuffer[(currentImage+1)%SIMPLEIMAGEBUFFERSIZE].frameNumber);
    if (!imagesLeft) {
      if (headIsNotInMotion()) {
        motion = getMotion();
        if (motion) {
          motion[1].x=motion[1].x + (motion[1].x-motion[0].x)*frameDiff*SPEEDFAKTOR.x/100;
          motion[1].y=motion[1].y + (motion[1].y-motion[0].y)*frameDiff*SPEEDFAKTOR.y/100;
          ARROW(sketch, motion[0].x, motion[0].y, motion[1].x, motion[1].y, 3, Drawings::ps_solid, Drawings::yellow);
          newHeadPos = getAngleYZ(motion[1]);
          headControlMode.headControlMode = HeadControlMode::direct;
          headControlMode.directTilt = headControlMode.directTilt + newHeadPos.x;
          headControlMode.directPan = headControlMode.directPan + newHeadPos.y;
          //headControlMode.directTilt = newHeadPos.x;
          //headControlMode.directPan = newHeadPos.y;
          searchStatus=-1;
          reset = RESETCONST/2;
        }
        else {
          reset -= frameDiff;
          if(reset<=0) {
            headControlMode.headControlMode = HeadControlMode::direct;
            headControlMode.directTilt = searchTilt();
            headControlMode.directPan = searchPan();
            headControlMode.directRoll = 0;
            reset = RESETCONST;
          }
        }
      }
    }
    else {
      imagesLeft--;
    }
    ++currentImage %= (SIMPLEIMAGEBUFFERSIZE);
  }
  previousCameraMatrix = cameraMatrix;
  DEBUG_DRAWING_FINISHED(sketch);
}

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

/*
* Change log :
* 
* $Log: SimpleMotionRecognition.cpp,v $
* Revision 1.2  2004/05/23 12:08:26  loetzsch
* clean up in class HeadControlMode
*
* Revision 1.1.1.1  2004/05/22 17:20:53  cvsadm
* created new repository GT2004_WM
*
* Revision 1.8  2004/03/08 02:11:50  roefer
* Interfaces should be const
*
* Revision 1.7  2004/01/13 02:17:01  richert
* finetuning SimpleMotionDetector
*
* Revision 1.6  2004/01/10 17:03:09  roefer
* Warnings removed
*
* Revision 1.5  2004/01/10 10:11:36  roefer
* Corrected
*
* Revision 1.4  2004/01/09 20:09:50  richert
* some improovements
*
* Revision 1.3  2004/01/09 16:57:35  loetzsch
* now also compiles under vc .NET
*
* Revision 1.2  2004/01/08 22:01:26  richert
* warning removed
*
* Revision 1.1  2004/01/08 21:51:40  richert
* beginning of "old" SimpleMotionRecognition
*
*/

---