Modules/SensorDataProcessor/GT2004SensorDataProcessor.cpp

Go to the documentation of this file.

/**
* @file GT2004SensorDataProcessor.cpp
* 
* This file contains a class for Sensor Data Processing.
*/

#include "GT2004SensorDataProcessor.h"
#include "Tools/Actorics/RobotDimensions.h"
#include "Tools/Actorics/Kinematics.h"
#include "Tools/Math/Common.h"
#include "Platform/SystemCall.h"

#include "Tools/Debugging/Debugging.h"

GT2004SensorDataProcessor::GT2004SensorDataProcessor
(const SensorDataProcessorInterfaces& interfaces)
: SensorDataProcessor(interfaces),
lastImageFrameNumber(0),
gravity(0,0,0),
accelerationWithGrav(0,0,0),
leftRollStartTime(SystemCall::getCurrentSystemTime()),
tempSysTime(0),
rightRollStartTime(SystemCall::getCurrentSystemTime())
{
  sensorDataRingBuffer.init();
  cameraMatrixRingBuffer.init();
  bodyPostureRingBuffer.init();
  for (int i = 0; i<3; i++) {
    previousAverage[i] = 0.0;
    actAverage[i] = 0.0;
  }
}

void GT2004SensorDataProcessor::execute()
{
  int i;
  CameraMatrix frameCameraMatrix;
  BodyPosture frameBodyPosture;

  psdPercept.numOfPercepts = 0;

  for (i = 0; i < sensorDataBuffer.numOfFrames; i++)
  {
    sensorDataRingBuffer.add(sensorDataBuffer.frame[i]);
  
    sensorDataRingBuffer.updateAverage(SensorData::accelerationX,10,gravity.x);
    sensorDataRingBuffer.updateAverage(SensorData::accelerationY,10,gravity.y);
    sensorDataRingBuffer.updateAverage(SensorData::accelerationZ,10,gravity.z);
  
    sensorDataRingBuffer.updateAverage(SensorData::accelerationX,2,accelerationWithGrav.x);
    sensorDataRingBuffer.updateAverage(SensorData::accelerationY,2,accelerationWithGrav.y);
    sensorDataRingBuffer.updateAverage(SensorData::accelerationZ,2,accelerationWithGrav.z);


    frameBodyPosture.frameNumber = sensorDataBuffer.frame[i].frameNumber;
    calculateBodyPostureFromLegSensors(sensorDataBuffer.frame[i], frameBodyPosture);
    calculateBodyPostureFromAccelerationSensors(sensorDataBuffer.frame[i], frameBodyPosture);

    if (1) // if motion provides valid data...
    {
      frameBodyPosture.bodyTiltProvidedByMotionControl = motionInfo.bodyTilt;
      frameBodyPosture.neckHeightProvidedByMotionControl = motionInfo.neckHeight;
      frameBodyPosture.bodyRollProvidedByMotionControl = 0;
    }
    else
    {
      /** @todo copy data from "calculated from..."*/
    }

    buildCameraMatrix(sensorDataBuffer.frame[i], frameBodyPosture, frameCameraMatrix);

    cameraMatrixRingBuffer.add(frameCameraMatrix);
    bodyPostureRingBuffer.add(frameBodyPosture);

    buildPSDPercept(
      sensorDataBuffer.frame[i], 
      cameraMatrixRingBuffer.getEntry(9), // 9: hack
      psdPercept[psdPercept.numOfPercepts++]);
  }

  double bodyRollAngle = atan2((double)-gravity.y,(double)-gravity.z);
  if ((bodyRollAngle >= -gt2004SensorDataProcessorRolledOnWallAngle)||(bodyRollAngle <= -gt2004SensorDataProcessorFalldownRollAngle))
    leftRollStartTime = SystemCall::getCurrentSystemTime();
  if ((bodyRollAngle <= gt2004SensorDataProcessorRolledOnWallAngle)||(bodyRollAngle >= gt2004SensorDataProcessorFalldownRollAngle))
    rightRollStartTime = SystemCall::getCurrentSystemTime();
  
  if (lastImageFrameNumber == imageFrameNumber) 
  {
    // The image is not new, so the camera matrix should not be built
    return;
  }
  lastImageFrameNumber = imageFrameNumber;

  // search image frame in sensor data
  for (i = 0; i < sensorDataRingBuffer.getNumberOfEntries(); i++)
  {
    if (sensorDataRingBuffer.getEntry(i).frameNumber <= imageFrameNumber) break;
  }
  cameraMatrix = cameraMatrixRingBuffer.getEntry(i);
  bodyPosture = bodyPostureRingBuffer.getEntry(i);
  bodyPosture.bodyTiltProvidedByMotionControl = motionInfo.bodyTilt;
  bodyPosture.bodyRollProvidedByMotionControl = 0;
  bodyPosture.neckHeightProvidedByMotionControl = motionInfo.neckHeight;
 
  Vector3<double> acceleration = 
    accelerationWithGrav - gravity;
  bodyPercept.acceleration = acceleration;
  
  int switches = detectSwitches();

  bodyPercept.setFrameNumber(sensorDataBuffer.frame[0].frameNumber);
  
  bodyPercept.setMouthState(detectMouthState());

  bodyPercept.setBodyPSDHighValue(sensorDataBuffer.lastFrame().data[SensorData::bodyPsd]>150000); //BBWalk: 110000
  
  bodyPercept.setBodyPSDDistance(getDistanceToSIFOC());
  
    bodyPercept.setSwitches(switches);
  if (!motionInfo.motionIsStable) 
  {
    bodyPercept.setState(BodyPercept::standing);
  }
  else if (detectFallDown())
  {
    bodyPercept.setState(BodyPercept::crashed);
    //OUTPUT (idText, text, "GT2004SensorDataProcessor : crashed");
  }
  else if (SystemCall::getTimeSince(leftRollStartTime)>1500)
  {
    bodyPercept.setState(BodyPercept::rollLeft);
    //OUTPUT (idText, text, "GT2004SensorDataProcessor : roll left");
  }
  else if (SystemCall::getTimeSince(rightRollStartTime)>1500)
  {
    bodyPercept.setState(BodyPercept::rollRight);
    //OUTPUT (idText, text, "GT2004SensorDataProcessor : roll right");
  }
  else if (detectPickup(gravity,acceleration))
  {
    bodyPercept.setState(BodyPercept::pickedUp);
  }
  else
  {
    bodyPercept.setState(BodyPercept::standing);
  }
  
  //double camSpeed = getCameraVelocity();
}


void GT2004SensorDataProcessor::buildCameraMatrix(const SensorData& sensorData, const BodyPosture& bP, CameraMatrix& cameraMatrix)
{
  double tilt = fromMicroRad(sensorData.data[SensorData::neckTilt]);
  double pan  = fromMicroRad(sensorData.data[SensorData::headPan]);
  double tilt2 = fromMicroRad(sensorData.data[SensorData::headTilt]);
  Kinematics::calculateCameraMatrix(tilt, pan, tilt2, bP.bodyTiltCalculatedFromLegSensors, bP.neckHeightCalculatedFromLegSensors, cameraMatrix);
  cameraMatrix.setFrameNumber(sensorData.frameNumber);
  //~ cameraMatrix.setFrameNumber(imageFrameNumber);
  cameraMatrix.isValid=motionInfo.motionIsStable;
}

void GT2004SensorDataProcessor::buildPSDPercept(const SensorData& sensorData, const CameraMatrix& cameraMatrix, SinglePSDPercept& psdPercept)
{
  // Achtung !!!
  // Hier wird zunächst einmal die eventuell kritische Annahme getroffen, 
  // dass optische Achse und "PSD-Strahl"- Achse identisch sind. 
  // Das bleibt erst mal so, bis genaueres bekannt ist. (M.L.)

  psdPercept.isValid = cameraMatrix.isValid;

  // A vector in the camera coordinate system with the length of the measured psd distance
  Vector3<double> psd;
  psd.y = 0; psd.z = 0;
  
  if(sensorData.data[SensorData::psd]/1000 < 300)
    psd.x = sensorData.data[SensorData::psd]/1000 - 20; 
  else
    psd.x = sensorData.data[SensorData::headPsdFar]/1000 - 30; 
  // mark the percept to be out of scope 
  psdPercept.tooFarAway = (psd.x >= 1300 - 30);
  psdPercept.body = sensorData.data[SensorData::bodyPsd]/1000;

  //
  psdPercept.neckTilt = fromMicroRad(sensorData.data[SensorData::neckTilt]);

  // Transformation of the vector into the robot's coordinate system
  Vector3<double> objectRelativeToCamera = cameraMatrix.rotation * psd;

  //OUTPUT(idText,text,"x: " << objectRelativeToCamera.x << ", y: " << objectRelativeToCamera.y << ", z: " << objectRelativeToCamera.z);

  // calculates the spot detected by the PSD Sensor relative to the point on the
  // ground below the shoulder joints.
  (Vector3<double>&)psdPercept = objectRelativeToCamera + cameraMatrix.translation;

  // OUTPUT(idText,text,"x2 " << psdPercept.x << ", y2 " << psdPercept.y << ", z2 " << psdPercept.z);

  psdPercept.setFrameNumber(sensorData.frameNumber);
}

int GT2004SensorDataProcessor::detectSwitches()
{
  int backMiddle, backFront, backBack,
      head, mouth, chin;

    backMiddle= (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::back]) > 10  ) << BodyPercept::backMiddle;
    backFront = (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::backF]) > 10  ) << BodyPercept::backFront;
    backBack  = (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::backR]) > 10  ) << BodyPercept::backBack;
    head      = (int)( (int)(sensorDataBuffer.frame[0].data[SensorData::head]) > 10  ) << BodyPercept::head;
    mouth     = (int)(0 != (int)(sensorDataRingBuffer.getAverage(SensorData::mouth,     5)/-80000)) << BodyPercept::mouth;
    chin      = (int)(0 != (int)(sensorDataRingBuffer.getAverage(SensorData::chin,      5)*10)) << BodyPercept::chin;

  return 
    backMiddle | backFront | backBack |
    head | mouth | chin;
}

bool GT2004SensorDataProcessor::detectFallDown()
{
  double bodyTiltAngle = atan2((double)gravity.x,(double)-gravity.z);
  double bodyRollAngle = atan2((double)-gravity.y,(double)-gravity.z);

  return
    (bodyTiltAngle >  gt2004SensorDataProcessorFalldownTiltAngle  ||
    bodyTiltAngle < -gt2004SensorDataProcessorFalldownTiltAngle  ||
    bodyRollAngle >  gt2004SensorDataProcessorFalldownRollAngle  ||
    bodyRollAngle < -gt2004SensorDataProcessorFalldownRollAngle);
}

bool GT2004SensorDataProcessor::detectPickup(const Vector3<double>& gravity, const Vector3<double>& acceleration)
{
  double accZ = acceleration * gravity;
  
  return (accZ < -50000000000000.0);
}

BodyPercept::MouthStates 
GT2004SensorDataProcessor::detectMouthState()
{
  if(sensorDataBuffer.lastFrame().data[SensorData::mouth] < -370000) //mouth open
  {
    return BodyPercept::mouthOpen;
  }
  else
  {
    return BodyPercept::mouthClosed;
  }
}

/**************************/
/***  SensorDataBuffer  ***/

double GT2004SensorDataProcessor::SensorDataRingBuffer::getAverage (SensorData::sensors sensor, int ticks)
{
  if (ticks>getNumberOfEntries()) ticks=getNumberOfEntries();
  if (ticks<1) return 0.0;
  double sum=0.0;
  for (int i=0;i<ticks;i++)
    sum+=getEntry(i).data[sensor];
  return sum/(double)ticks;
}

void GT2004SensorDataProcessor::SensorDataRingBuffer::updateAverage(SensorData::sensors sensor, int ticks, double& average)
{
  if (ticks<1) return;
  if (ticks<getNumberOfEntries())
    average-=((double)getEntry(ticks).data[sensor])/(double)ticks;
  average+=((double)getEntry(0).data[sensor])/(double)ticks;
}

long GT2004SensorDataProcessor::SensorDataRingBuffer::interpolate (SensorData::sensors sensor, unsigned long imageFrame)
{
  long frame = (long) imageFrame,
    currentValue = getEntry(0).data[sensor],
    currentFrame = (long) getEntry(0).frameNumber;
  if(currentFrame == frame)
    return currentValue;
  else
  {
    long oldValue = getEntry(1).data[sensor],
      oldFrame = (long) getEntry(1).frameNumber;
    if(currentFrame == oldFrame)
      return currentValue;
    else
      return currentValue + (oldValue - currentValue) * (frame - currentFrame) / (oldFrame - currentFrame);
  }
}

void GT2004SensorDataProcessor::calculateBodyPostureFromLegSensors(const SensorData& sensorData, BodyPosture& bP)
{
  RobotVertices robotVertices;
  Kinematics::calcNeckAndLegPositions(sensorDataBuffer.lastFrame(), robotVertices);

  bP.bodyTiltCalculatedFromLegSensors = robotVertices.bodyTilt;
  bP.bodyRollCalculatedFromLegSensors = robotVertices.bodyRoll;
  bP.neckHeightCalculatedFromLegSensors = robotVertices.neckHeight;
}

void GT2004SensorDataProcessor::calculateBodyPostureFromAccelerationSensors(const SensorData& sensorData, BodyPosture& bP)
{
  //calculate body angles
  long accX = sensorDataBuffer.lastFrame().data[SensorData::accelerationX]/1000;
  long accY = sensorDataBuffer.lastFrame().data[SensorData::accelerationY]/1000;
  long accZ = sensorDataBuffer.lastFrame().data[SensorData::accelerationZ]/1000;
  bP.bodyTiltCalculatedFromAccelerationSensors = atan2((double)accX,(double)-accZ);
  bP.bodyRollCalculatedFromAccelerationSensors = atan2((double)-accY,(double)-accZ);
}

double GT2004SensorDataProcessor::getCameraVelocity(const SensorData::sensors joint=SensorData::headPan)
{
  const int numOfValues=4;
  int jointNum = 1;
  sensorDataRingBuffer.updateAverage(SensorData::neckTilt, numOfValues, actAverage[0]);
  sensorDataRingBuffer.updateAverage(SensorData::headPan, numOfValues, actAverage[1]);
  sensorDataRingBuffer.updateAverage(SensorData::headTilt, numOfValues, actAverage[2]);
  switch (joint)
  {
  case SensorData::headTilt: jointNum = 2;
    break;
  case SensorData::headPan: jointNum = 1;
    break;
  case SensorData::neckTilt: jointNum = 0;
    break;
  }
  double cameraSpeed = toDegrees(fromMicroRad((long) (actAverage[jointNum] - previousAverage[jointNum]))) / RobotDimensions::motionCycleTime;
  for (int index = 0; index < 3; index++)
    previousAverage[index] = actAverage[index];
  /** 
  if (abs ((int) cameraSpeed) > 50)
    if(SystemCall::getCurrentSystemTime()-tempSysTime > 250)
    {
      tempSysTime=SystemCall::getCurrentSystemTime();
      OUTPUT(idText, text, cameraSpeed);
    }
  */
  return cameraSpeed;
}

double GT2004SensorDataProcessor::getDistanceToSIFOC()
{
  double value = 50;
  double psdval = 0;
  for (int i = 0; i < psdPercept.numOfPercepts; ++i)
    psdval += psdPercept[i].body;
  psdval /= (double)(psdPercept.numOfPercepts);
  if (psdval >= 170)
    psdval = 170;

  // from 160 to 170 equals from 1cm to 0cm
  if (psdval >= 160)
    value = 10-(psdval-160);
  else
  // from 145 to 160 equals from 2cm to 1cm
  if (psdval >= 145)
    value = ((15-(psdval-145))/1.5)+10;
  else
  // from 130 to 145 equals from 4cm to 2cm
  if (psdval >= 130)
    value = ((15-(psdval-130))/0.75)+20;

  return value;
}


/*
* Change log :
* 
* $Log: GT2004SensorDataProcessor.cpp,v $
* Revision 1.1  2004/07/10 00:18:31  spranger
* renamed (readded) for coderelease
*
* Revision 1.16  2004/06/29 15:13:59  risler
* removed touch sensor averaging and minimum press time, not necessary for ers7 buttons
*
* Revision 1.15  2004/06/17 15:52:06  nistico
* Added visualization of camera motion vector on image
* Fixed cameraMatrix.frameNumber=0 problem when playing logfiles
* (the image frame number is copied)
*
* Revision 1.14  2004/06/17 12:17:14  schmitt
* Modified getCameraVelocity()
* Selection of the speed each head joint is now possible
* Default is headPan
*
* Revision 1.13  2004/06/16 17:07:33  cesarz
* Moved body PSD calculations
*
* Revision 1.12  2004/06/16 12:40:02  cesarz
* implemented getCameraVelocity()
*
* Revision 1.11  2004/05/27 17:13:37  jhoffman
* - renaming: tilt1 -> neckTilt,  pan -> headPan,  tilt2 -> headTilt
* - clipping included for setJoints
* - removed some microrad/rad-bugs
* - bodyPosture constructor and "=" operator fixed
*
* Revision 1.10  2004/05/27 15:45:44  juengel
* bug fixed
*
* Revision 1.9  2004/05/26 21:20:02  juengel
* bodyTiltProvidedByMotionControl etc. is written to BodyPosture.
*
* Revision 1.8  2004/05/26 20:18:05  juengel
* Calculation of BodyPosture.
*
* Revision 1.7  2004/05/26 17:21:47  dueffert
* better data types used
*
* Revision 1.6  2004/05/26 17:12:22  juengel
* Added bodyPosture.frameNumber.
*
* Revision 1.5  2004/05/25 19:11:31  goehring
* PSD-threshold for grab ball changed
*
* Revision 1.4  2004/05/25 14:55:33  tim
* changed parameters for PSD ball detection
*
* Revision 1.3  2004/05/25 12:36:51  tim
* added body PSD data
*
* Revision 1.2  2004/05/24 14:14:26  juengel
* New button evaluation.
*
* Revision 1.1.1.1  2004/05/22 17:21:30  cvsadm
* created new repository GT2004_WM
*
* Revision 1.9  2004/04/07 12:28:59  risler
* ddd checkin after go04 - first part
*
* Revision 1.3  2004/04/03 16:42:27  risler
* removed OUTPUTs
*
* Revision 1.2  2004/04/02 19:10:48  risler
* robot no longer is crashed when using dash
*
* Revision 1.1.1.1  2004/03/29 08:28:47  Administrator
* initial transfer from tamara
*
* Revision 1.8  2004/03/10 14:16:33  risler
* body psd value added to PSDPercept and ObstaclesModel
*
* Revision 1.7  2004/03/08 02:11:51  roefer
* Interfaces should be const
*
* Revision 1.6  2004/01/28 08:33:27  dueffert
* acceleration added
*
* Revision 1.5  2004/01/25 21:06:54  juengel
* headPsdFar is used to calculate PSDPercept, if headPsdNear is greater than 300 mm.
*
* Revision 1.4  2004/01/24 11:29:10  juengel
* Added ERS7 switches (head, backFront, backBack).
*
* Revision 1.3  2004/01/12 12:24:31  juengel
* back pressed for ERS7
*
* Revision 1.2  2003/11/14 19:02:26  goehring
* frameNumber added
*
* Revision 1.1  2003/10/06 14:10:14  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.4  2003/09/30 10:51:13  dueffert
* typos fixed
*
* Revision 1.3  2003/09/26 11:38:52  juengel
* - sorted tools
* - clean-up in DataTypes
*
* Revision 1.2  2003/07/06 12:05:31  schumann
* added foreleg opening angle for ball challenge
*
* Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.28  2003/06/23 12:06:38  dueffert
* consequetively roll on wall detection improved
*
* Revision 1.27  2003/06/21 12:53:44  juengel
* psdPercept.isValid = cameraMatrix.isValid
*
* Revision 1.26  2003/06/20 15:32:32  dueffert
* getting from down from wall by rolling added
*
* Revision 1.25  2003/06/20 14:34:31  juengel
* Bug fixed.
*
* Revision 1.24  2003/06/20 13:27:20  risler
* added tailLeft tailRight messages
*
* Revision 1.23  2003/06/12 17:35:25  risler
* remeasured psd delay
*
* Revision 1.22  2003/06/12 16:49:56  risler
* psd sensor data is delayed about 10 frames -> use stored camera matrix
*
* Revision 1.21  2003/05/16 14:06:18  risler
* removed warning
*
* Revision 1.20  2003/05/14 19:54:42  risler
* PSDPercept contains all points from one frame
*
* Revision 1.19  2003/05/02 18:26:18  risler
* SensorDataBuffer added
* replaced SensorData with SensorDataBuffer
* full SensorData resolution now accessible
*
* Revision 1.18  2003/04/07 16:50:11  jhoffman
* added PSD sensor workaround
*
* Revision 1.17  2003/04/04 17:51:23  hebbel
* Removed not needed OUTPUTs
*
* Revision 1.16  2003/04/04 16:08:59  cesarz
* mouth values adepted
*
* Revision 1.15  2003/04/01 22:40:44  cesarz
* added mouth states
*
* Revision 1.14  2003/04/01 16:51:09  roefer
* CameraMatrix contains validity and is calculated in Geometry
*
* Revision 1.13  2003/03/30 14:12:54  juengel
* changed buildPSDPercept
*
* Revision 1.12  2003/03/28 14:32:10  juengel
* Added offset to psdPercept.
*
* Revision 1.11  2003/03/19 17:29:05  roefer
* detectFallDown uses own calculation of tilt and roll
*
* Revision 1.10  2003/01/30 11:26:47  juengel
* Added tailPosition to bodyPercept
*
* Revision 1.9  2002/10/10 18:33:50  loetzsch
* only PSD measurements less than 90 cm are used
*
* Revision 1.8  2002/10/10 16:18:39  loetzsch
* some minor improvements and better doxygen comments
*
* Revision 1.7  2002/10/10 13:09:49  loetzsch
* First experiments with the PSD Sensor
* - SensorDataProcessor now calculates PSDPercept
* - Added the PerceptBehaviorControl solution PSDTest
* - Added the RadarViewer3D to RobotControl, which can display the Points3D structure
*
* Revision 1.6  2002/09/22 18:40:53  risler
* added new math functions, removed GTMath library
*
* Revision 1.5  2002/09/17 23:55:22  loetzsch
* - unraveled several datatypes
* - changed the WATCH macro
* - completed the process restructuring
*
* Revision 1.4  2002/09/12 12:24:09  juengel
* continued change of module/solution mechanisms
*
* Revision 1.3  2002/09/10 21:07:30  loetzsch
* continued change of module/solution mechanisms
*
* Revision 1.2  2002/09/10 17:53:40  loetzsch
* First draft of new Module/Solution Mechanisms
*
* Revision 1.1  2002/09/10 15:36:16  cvsadm
* Created new project GT2003 (M.L.)
* - Cleaned up the /Src/DataTypes directory
* - Removed challenge related source code
* - Removed processing of incoming audio data
* - Renamed AcousticMessage to SoundRequest
*
* Revision 1.2  2002/08/30 14:34:37  dueffert
* removed unused includes
*
* Revision 1.1.1.1  2002/05/10 12:40:15  cvsadm
* Moved GT2002 Project from ute to tamara.
*
* Revision 1.32  2002/04/23 10:38:30  risler
* renamed headOdometry to headState
*
* Revision 1.31  2002/04/18 10:25:32  roefer
* Bremen GO
*
* Revision 1.3  2001/12/01 21:21:59  roefer
* HeadOdometry used in simulation
*
* Revision 1.2  2001/11/30 13:59:17  roefer
* CMU kicking
*
* Revision 1.29  2002/04/08 17:48:28  risler
* added tilt angle calculation, pickedUp detection
* some cleaning up
*
* Revision 1.28  2002/04/02 13:10:20  dueffert
* big change: odometryData and cameraMatrix in image now, old logfiles may be obsolete
*
* Revision 1.27  2002/03/29 23:09:53  roefer
* SensorData initialized to avoid detection of falling down state
*
* Revision 1.26  2002/03/28 15:19:20  risler
* implemented switch messages in RobotStateDetector
*
* Revision 1.25  2002/03/24 20:17:49  roefer
* Warning on noFrameNumber removed
*
* Revision 1.24  2002/03/24 18:15:00  loetzsch
* continued change to blocking sensor data receivers
*
* Revision 1.23  2002/03/24 14:19:46  roefer
* Input now waits for SensorData
*
* Revision 1.22  2002/03/05 17:58:43  risler
* added detectPickup
*
* Revision 1.21  2002/02/27 11:28:44  roefer
* Better CameraMatrix calculation reactivated
*
* Revision 1.20  2002/02/16 16:24:28  roefer
* SensorData interpolation corrected
*
* Revision 1.19  2002/02/14 16:45:00  petters
* no message
*
* Revision 1.18  2002/02/11 11:13:06  roefer
* BallPerceptor and BallLocator inserted
*
* Revision 1.17  2002/02/05 20:26:46  petters
* ... uncommented
*
* Revision 1.16  2002/02/05 17:34:18  petters
* interpolation functions in sensor data buffer added (not tested -> commented out)
*
* Revision 1.15  2002/02/03 09:32:54  risler
* RingBuffer from DefaultSensorDataProcessor now is seperate class
*
* Revision 1.14  2002/02/01 15:03:23  risler
* Removed pickup detection, as not working with paw sensors
*
* Revision 1.13  2002/01/29 18:11:04  mkunz
* buildCameraMatrix pre-calculated (correctly, i hope)
*
* Revision 1.12  2002/01/29 12:53:33  risler
* optimised buildCameraMatrix with new Pose3D functions
*
* Revision 1.11  2002/01/22 14:52:23  juengel
* Compiler braucht nun nicht mehr so viel Speicher
*
* Revision 1.10  2002/01/20 23:35:06  loetzsch
* Added parameter imageFrameNumber to execute(...)
*
* Revision 1.9  2002/01/18 12:51:39  petters
* no message
*
* Revision 1.8  2002/01/18 12:46:40  petters
* buildcameramatrix fixed
*
* Revision 1.7  2002/01/18 12:24:17  mkunz
* build camera matrix added
*
* Revision 1.6  2002/01/18 11:23:17  petters
* return bodypercept added
*
* Revision 1.5  2001/12/28 22:39:25  roefer
* abs replaced by fabs
*
* Revision 1.4  2001/12/23 23:45:29  petters
* sensordatabuffer implemented, state detector implemented, switch detector implemented
*
* Revision 1.3  2001/12/12 20:21:12  petters
* Streaming for SensorData / Image implemented; Conflict solved
*
* Revision 1.2  2001/12/10 17:47:07  risler
* change log added
*
*/

---