visiondata.cc

该文件是包含了机器人足球比赛中的整个系统的代码

#include <math.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include "VisionData.h"
#include "../Common/VisionObject.h"
#include "../Common/VisionLine.h"
#include "../Common/Common.h"
#include "../Globals.h"

using namespace std;

VisionData::VisionData() {
  classified_ = new uchar[DOUBLE_IMAGE_WIDTH*DOUBLE_IMAGE_HEIGHT]; // Stores the classified image
  classifiedTight_ = new uchar[DOUBLE_IMAGE_WIDTH*DOUBLE_IMAGE_HEIGHT]; // Stores the classified image

#ifdef WIN32
  overlay = new uchar[DOUBLE_IMAGE_WIDTH*DOUBLE_IMAGE_HEIGHT];
#endif
  runs_ = new Run*[DOUBLE_IMAGE_HEIGHT]; // Array of arrays of runs used to store the runs which have been found so far
  numRuns_ = new int[DOUBLE_IMAGE_HEIGHT]; // Array of int which stores the number of runs
  ball_ = NULL;
  for (int i = 0; i < DOUBLE_IMAGE_HEIGHT; i++) { // Initalise each array of the runs array to contain an array of run
    runs_[i] = new Run[MAXRUNSPERLINE];
    numRuns_[i] = 0; // Initalises the array of ints to be all 0s
  }
  blobs_ = new Blob*[NUMCOLOURS]; // An array of arrays of blobs equal in size to the number of colours
  numBlobs_ = new int[NUMCOLOURS]; // Array of int equal in size to the number of colours
  for (int j = 0; j < NUMCOLOURS; j++) { // Initalise the two arrays
    blobs_[j] = new Blob[MAXBLOBS]; 
    numBlobs_[j] = 0;
  }
  realElevation_ = 0.0;
}
                                            
// updates variables based on new head position data.
void VisionData::UpdateLookupTrigonometry(int infraRed, double headTilt, double headPan, double headRoll, double _accX, double _accY, double _accZ) {
  accX = _accX;
  accY = _accY;
  accZ = _accZ;
  infraRed_=infraRed;

//bodyTilt_ = atan2(accX,-accZ);
  bodyTilt_ = DEG_TO_RAD(8.5);
	bodyRoll_ = atan2(accY,-accZ);
  
//  bodyTilt_ = DEG_TO_RAD(8.0);
//  bodyRoll_ = 0.0;

#ifdef ERS_7
  headPan_=headPan; 
  headRoll_=0;
  headTiltBig_=headTilt; 
  headTiltSmall_=headRoll;

  headTiltCombined_=headTiltBig_+headTiltSmall_;


  double tempHeadTilt = (-headTiltCombined_ + bodyTilt_);
  if (tempHeadTilt > DEG_TO_RAD(65)) {
    tempHeadTilt = DEG_TO_RAD(65);
    headTiltCombined_ = bodyTilt_ - tempHeadTilt;
  }

  cosHeadPan_ = cos((double)headPan_);
  sinHeadPan_ = sin((double)headPan_);

  cosHeadTiltBig_ = cos((double)(-headTiltBig_) + bodyTilt_);
  sinHeadTiltBig_ = sin((double)(-headTiltBig_) + bodyTilt_);

  cosHeadTiltSmall_ = cos((double)(-headTiltSmall_));
  sinHeadTiltSmall_ = sin((double)(-headTiltSmall_));

  cosHeadTiltCombined_ = cos((double)(-headTiltCombined_)+bodyTilt_);
  sinHeadTiltCombined_ = sin((double)(-headTiltCombined_)+bodyTilt_);

  cosHeadRoll_ = cos((double)headRoll_);
  sinHeadRoll_ = sin((double)headRoll_);

  
  effHeadTilt_ = (asin(sinHeadTiltSmall_*cosHeadPan_))+(asin(sinHeadTiltBig_*cosHeadPan_));
  sinEffHeadTilt_ = sin(effHeadTilt_);
  cosEffHeadTilt_ = cos(effHeadTilt_);
  
  effHeadRoll_ = (asin(sinHeadPan_*-1*sinHeadTiltBig_)); 
  //effHeadRoll_ = (asin(sinHeadPan_*sin(headTiltBig_))) -(asin(sinHeadPan_*sin(headTiltSmall_)));
  sinEffHeadRoll_ = sin(effHeadRoll_);
  cosEffHeadRoll_ = cos(effHeadRoll_);
#endif
#ifdef ERS_210
  headPan_=headPan; 
  headRoll_ = headRoll;
  headTiltBig_=headTilt; 
  headTiltSmall_=0;
  headTiltCombined_=headTilt;

  double tempHeadTilt = (-headTiltCombined_ + bodyTilt_);
  if (tempHeadTilt > DEG_TO_RAD(65)) {
    tempHeadTilt = DEG_TO_RAD(65);
    headTiltCombined_ = bodyTilt_ - tempHeadTilt;
  }

  cosHeadPan_ = cos((double)headPan_);
  sinHeadPan_ = sin((double)headPan_);

  cosHeadTiltBig_ = cos((double)(-headTiltBig_) + bodyTilt_);
  sinHeadTiltBig_ = sin((double)(-headTiltBig_) + bodyTilt_);

  cosHeadTiltSmall_ = cos((double)(-headTiltSmall_));
  sinHeadTiltSmall_ = sin((double)(-headTiltSmall_));

  cosHeadTiltCombined_ = cos((double)(-headTiltCombined_)+bodyTilt_);
  sinHeadTiltCombined_ = sin((double)(-headTiltCombined_)+bodyTilt_);

  cosHeadRoll_ = cos((double)headRoll_);
  sinHeadRoll_ = sin((double)headRoll_);

  effHeadTilt_ = ABS(asin(sinHeadTiltCombined_*cosHeadPan_));
  sinEffHeadTilt_ = sin(effHeadTilt_);
  cosEffHeadTilt_ = cos(effHeadTilt_);
  
  effHeadRoll_ = (asin(sinHeadPan_*-1*sinHeadTiltCombined_)); 
  sinEffHeadRoll_ = sin(effHeadRoll_);
  cosEffHeadRoll_ = cos(effHeadRoll_);
#endif
}


// returns heading of an object in the image, based on the centre (the parameter)
// always returns radians.
double VisionData::CalculateHeading(int cx) {
  return atan((currentImageWidth_/2.0-cx)/(currentImageWidth_/(2.0*tan(FOVx/2.0))));
}

double VisionData::CalculateHeading(int cx, int cy, double sinEffRoll, double cosEffRoll) {
  return atan(((currentImageWidth_/2.0-cx)*cosEffRoll+(currentImageHeight_/2.0-cy)*sinEffRoll) * 2.0 * tan(FOVx/2.0)/currentImageWidth_);
}

// same as above for elevation always returns rads
double VisionData::CalculateElevation(int cy) {
  return atan((currentImageHeight_/2.0-cy)/(currentImageHeight_/(2.0*tan(FOVy/2.0))));
}

double VisionData::CalculateElevation(int cx, int cy, double sinEffRoll, double cosEffRoll) {
  return atan(((currentImageWidth_/2.0-cx)*sinEffRoll+(currentImageHeight_/2.0-cy)*cosEffRoll)*2.0*tan(FOVy/2.0)/currentImageHeight_);
}

// Calculates the distance between two coordinators using the distance formula.
// The points are obviously taken as parameters
double VisionData::GetDistance(int x, int y, int x2, int y2) {
  return sqrt((double)((x-x2)*(x-x2)+(y-y2)*(y-y2)));
}

// Transforms a vision object based on the tilt, pan and roll
void VisionData::TransformPositionObject(void* vo_) {
  VisionObject* vo = (VisionObject*)vo_;
  TransformPosition(&(vo->distance_), &(vo->heading_), &(vo->elevation_));
}

// Transforms a vision line based on the tilt, pan and roll
void VisionData::TransformPositionLine(void* vl_) {
  VisionLine* vl = (VisionLine*)vl_;
  TransformPosition(&(vl->startDistance_), &(vl->startHeading_), &(vl->startElevation_));
  TransformPosition(&(vl->endDistance_), &(vl->endHeading_), &(vl->endElevation_));

  // Transform coordinates of the end of the line from spherical coordinates back into cartesian coordinates so they are robot relative and specified in robot space
  vl->startXRobot_ = (int)(vl->startDistance_ * sin(PI/2.0 + vl->startElevation_) * cos(vl->startHeading_));
  vl->startYRobot_ = (int)(vl->startDistance_ * sin(PI/2.0 + vl->startElevation_) * sin(vl->startHeading_));
  vl->startZRobot_ = (int)(vl->startDistance_ * cos(PI/2.0 + vl->startElevation_));

  vl->endXRobot_ = (int)(vl->endDistance_ * sin(PI/2.0 + vl->endElevation_) * cos(vl->endHeading_));
  vl->endYRobot_ = (int)(vl->endDistance_ * sin(PI/2.0 + vl->endElevation_) * sin(vl->endHeading_));
  vl->endZRobot_ = (int)(vl->endDistance_ * cos(PI/2.0 + vl->endElevation_));

  // If we make it to here then we have fitted a side line through a set of points which fall within 
  // the range specified between startX and endX - calculate the vertical based on the horizon line
  double m = horizonLine.m;
//double b = horizonLine.b;

  // Calculate the gradient of the sideline
  double m2 = (vl->endY_ - vl->startY_)/(vl->endX_ - vl->startX_);

  // We want to calculate the angle between sideline and the line orthogonal to the horizon line
  if ((m == 0.0) || (m2 = 0.0)) {
    vl->angle_ = DBL_MAX;
    return;
  }
   
  double m1 = -1.0/m; 

  // Calculate the angle between the two lines - note: the angle between the two lines assumes that the horizon is the lower of the two lines
  vl->angle_ = atan2((m2-m1),(1 + m1*m2));   
}

void VisionData::TransformPosition(double* distance, double* heading, double* elevation) {
    double x = 0;
    double y = 0;
    double z = *distance;

    // image heading
    double x2 = x*cos(*heading) + z*sin(*heading); // As x is 0 this is effectivly z*sin(heading_)
    double y2 = y;
    double z2 = -x*sin(*heading) + z*cos(*heading); // As x is 0 this is effectivly z*cos(heading_)

    // image elevation
    double x3 = x2;
    double y3 = y2*cos(*elevation) + z2*sin(*elevation);
    double z3 = -y2*sin(*elevation) + z2*cos(*elevation);

    // Note: In the following calculations we are only componsating for tilt and pan and not roll
    // as the head is fixed such that roll cannot occur. This allows us to remove one dimension from
    // the system and hence simplify the maths

    // robot HEAD length and camera offset
    double x4 = x3;
    double y4 = y3 - CAMERA_OFFSET;
    double z4 = z3 + HEADLENGTH;

    // robot head tilt
    double x45 = x4;
    double y45 = y4 * cosHeadTiltSmall_ - z4 * sinHeadTiltSmall_;
    double z45 = y4 * sinHeadTiltSmall_ + z4 * cosHeadTiltSmall_;


    // robot head pan - fixed in the y direction
    double x5 = x45 * cosHeadPan_ + z45 * sinHeadPan_;
    double y5 = y45;
    double z5 = -x45 * sinHeadPan_ + z45 * cosHeadPan_;

    // robot neck length and distance from ground to neck
    double x6 = x5;
    double y6 = y5 + NECKLENGTH;
    double z6 = z5;

    // rotation for head tilt - fixed in the x direction
    double x7 = x6;
    double y7 = y6 * cosHeadTiltBig_ - z6 * sinHeadTiltBig_;
    double z7 = y6 * sinHeadTiltBig_ + z6 * cosHeadTiltBig_;

    // calculate actual distance and heading to robot
    *distance = sqrt(x7*x7 + y7*y7 + z7*z7);
    if (z7 == 0.0) {
      *heading = PI/2;
    } else {
      //*heading = atan2(x7, z7/ABS(z7)*sqrt(y7*y7 + z7*z7));
      *heading = atan2(x7, z7);
    }
    *elevation = atan2(y7,sqrt(x7*x7 + z7*z7));   
//    double timElevation = atan2(y7,z7);   
//    double tempDenom = x7*x7 + y7*y7 + z7*z7;
//    if (tempDenom == 0.0) {
//      tempDenom = 0.001;
//    }
//    double tempElevation = acos(sqrt((x7*x7+z7*z7)/(tempDenom)));
//    if (y7 >= 0)
//      *elevation = tempElevation;
//    else
//      *elevation = -tempElevation;

    realElevation_ = atan2((4.0-HEIGHTOFNECK),sqrt(x7*x7 + z7*z7));
}

// Calculates the end points of the horizon for the image based on the head angles (CS/CM - 19/02/03)
// Based on the algorithm specified in the German Report
void VisionData::GenerateHorizon() {

  //double bodyTilt = atan2(accX,-accZ);
  //double bodyRoll = atan2(accY,-accZ); // Note the negative value had to change here other wise the slope was in the wrong direction

  double bigTilt = bodyTilt_-headTiltBig_;
  double smallTilt = -headTiltSmall_;
  double pan = headPan_;
  double s = currentImageWidth_/2.0;
  double alpha = (FOVx/2.0);
  double cotAlpha = 1.0/tan(alpha);

  // Equations generated by multiplying the y and z components of a 3D rotation matrix together
  // but which does not take into account body roll ! -> replaced by code below which used body roll
  //double r31 = -sin(tilt)*cos(pan);
  //double r32 = sin(tilt)*sin(pan);
  //double r33 = cos(tilt);

  // The following equations take into account the body roll of the dog
  // ERS-210 calcs
  double r31 = -cos(bodyRoll_)*sin(bigTilt)*cos(pan) + sin(bodyRoll_)*sin(pan);
  double r32 = cos(bodyRoll_)*sin(bigTilt)*sin(pan)+sin(bodyRoll_)*cos(pan);
  double r33 = cos(bodyRoll_)*cos(bigTilt);

#ifdef ERS_7
  double oldR31 = r31;
  //double oldR32 = r32;
  double oldR33 = r33;
 
  r31 = oldR31*cos(smallTilt)-oldR33*sin(smallTilt);
  r33 = +sin(smallTilt)*oldR31 + oldR33*cos(smallTilt);
#endif


  double zl = currentImageHeight_/2 + s*(r32  + r31*cotAlpha)/r33;
  double zr = currentImageHeight_/2 + s*(-r32 + r31*cotAlpha)/r33;

  horizonLine.m = (double)(zr-zl)/(double)(currentImageWidth_-1.0);
  horizonLine.b = zl;
  horizonLine.exists = true;
}

void VisionData::DeleteBlob(Blob* deleteBlob) {
  // note for front sentinel, this doesn't actually do anything (since deleteBlob->prevBlob == deleteBlob)
  if (deleteBlob->prevBlob != NULL) {
	 deleteBlob->prevBlob->nextBlob = deleteBlob->nextBlob;
  }
  // note again: front sentinel always part of the structure (must check if it's subsumed)
  if (deleteBlob->nextBlob != NULL) {
    deleteBlob->nextBlob->prevBlob = deleteBlob->prevBlob;
  }
  if (deleteBlob->prevBlob != deleteBlob) {
   deleteBlob->prevBlob = NULL;
  }