visiondata.cc

来自「该文件是包含了机器人足球比赛中的整个系统的代码」· CC 代码 · 共 637 行 · 第 1/2 页

  deleteBlob->subsumed = true;
}

double VisionData::GetDistanceToPointUnreliable(double cx, double cy, double heightOfNeck) { //Gets the distance from a point on the camera image to the ground in that direction

//  double heightAboveGround = 9.0; //Check this, height of bigTilt pivot point above ground...can use other method to find?

	double horizontalImageAngle = atan2(currentImageWidth_/2.0-0.5-cx, EFFECTIVE_CAMERA_DISTANCE_IN_PIXELS);
	double verticalImageAngle = atan2(currentImageHeight_/2.0-0.5-cy, EFFECTIVE_CAMERA_DISTANCE_IN_PIXELS);

  double alpha = bodyTilt_ - headTiltBig_; //Combined large tilt
  double beta = headTiltSmall_ + verticalImageAngle; //Combined small tilt
  double gamma = headPan_ + horizontalImageAngle; //Combined pan

  //Distances of camera from pivot point of small tilt motor in cartesian coordinates - derived from TransformPosition() x1 -> x5
  double x = HEADLENGTH*cos(beta)*sin(gamma);
  double y = HEADLENGTH*(sin(beta)*cos(alpha) - sin(alpha)*cos(beta)*cos(gamma)) -1.46; //Camera plane is parallel to the plane of the small tilt but 1.46cm below it
  double z = HEADLENGTH*(sin(alpha)*sin(beta) + cos(alpha)*cos(beta)*cos(gamma));

  double angleToFront = atan2(x,z);
  double angleToHorizontal = atan2(y,z);
  double sinAngleToFront = sin(angleToFront);
  double sinAngleToHorizontal = sin(angleToHorizontal);

  double t = atan2(sinAngleToFront, sinAngleToHorizontal);
  double rotationalHypotenuse = sqrt(sinAngleToFront*sinAngleToFront + sinAngleToHorizontal*sinAngleToHorizontal);
  double sinTrueAngleToHorizontal = rotationalHypotenuse*cos(bodyRoll_ + t);

  if (sinTrueAngleToHorizontal >= 0) return -1;

//  double headDistanceFromGround = (NECKLENGTH*cos(headTiltBig_) + heightAboveGround)*cos(bodyRoll_); //Check 9.0, equal to height of bigTilt pivot point above ground...can find using other method?
//  double distance = headDistanceFromGround / (-sinTrueAngleToHorizontal) - HEADLENGTH;
//  return distance;

  double cameraDistanceAboveGround = ((NECKLENGTH - cos(headTiltSmall_)*CAMERA_OFFSET)*cos(alpha) + y)*cos(bodyRoll_) + heightOfNeck;
  double distance = cameraDistanceAboveGround / (-sinTrueAngleToHorizontal);
  return distance;



	// this maths does NOT work for tilt > 0.
	//double tilt = headTilt_;
	//double pan = headPan_;

	//double bodyTilt = atan2(accX,-accZ);
	//double bodyRoll = atan2(accY,-accZ);

	/*if (tilt >= 0) {
		return 1.0;
	}

	double r11 = cos(tilt)*cos(pan);
	double r12 = -cos(tilt)*sin(pan);
	double r13 = sin(tilt);

	double r21 =  cos(bodyRoll)*sin(pan)+sin(bodyRoll)*sin(tilt)*cos(pan);
	double r22 =  cos(bodyRoll)*cos(pan)-sin(bodyRoll)*sin(tilt)*sin(pan);
	double r23 = -sin(bodyRoll)*cos(tilt);

	double r31 = -cos(bodyRoll)*sin(tilt)*cos(pan)+sin(bodyRoll)*sin(pan);
	double r32 =  cos(bodyRoll)*sin(tilt)*sin(pan)+sin(bodyRoll)*cos(pan);
	double r33 =  cos(bodyRoll)*cos(tilt);

	double xRatio = 2.0*tan(FOVx/2.0)/currentImageWidth_;
	double yRatio = 2.0*tan(FOVy/2.0)/currentImageHeight_;

	double xi = 1.0;
	double yi = (currentImageWidth_/2.0-0.5-cx)*xRatio;
	double zi = (currentImageHeight_/2.0-0.5-(currentImageHeight_-cy))*yRatio;
	//double zi = (IMAGE_HEIGHT/2.0-0.5 -cy)*yRatio;

	double vectorX = r11*xi + r12*yi + r13*zi;
	double vectorY = r21*xi + r22*yi + r23*zi;
	double vectorZ = r31*xi + r32*yi + r33*zi;

	//double heightOfNeck = 13.0; // check this. in CM. note thestrut uses
	//mm... careful when translating!

	double translationX = cos(tilt)*HEADLENGTH*cos(pan) + sin(tilt)*NECKLENGTH;
	double translationY = cos(bodyRoll)*HEADLENGTH*sin(pan)-sin(bodyRoll)*cos(tilt)*NECKLENGTH + sin(bodyRoll)*sin(tilt)*HEADLENGTH*cos(pan);
	double translationZ = cos(bodyRoll)*cos(tilt)*NECKLENGTH-cos(bodyRoll)*sin(tilt)*HEADLENGTH*cos(pan)+ sin(bodyRoll)*HEADLENGTH*sin(pan)+heightOfNeck;

	double x = (translationX*vectorZ - translationZ*vectorX)/vectorZ;
	double y = (translationY*vectorZ - translationZ*vectorY)/vectorZ;

	return sqrt(x*x+y*y);*/
}

double VisionData::GetDistanceToPoint(double cx, double cy, double heightOfNeck) { //Gets the distance from a point on the camera image to the ground in that direction

  double cameraOffset = 1.46; //Camera plane is parallel to the plane of the small tilt but 1.46cm below it

  double horizontalPixels = currentImageWidth_/2.0-0.5-cx;
  double verticalPixels = currentImageHeight_/2.0-0.5-cy;
  double hypotenusePixels = sqrt(horizontalPixels*horizontalPixels + verticalPixels*verticalPixels);
  double theta = Normalise_PI(atan2(verticalPixels,horizontalPixels));
  double trueTheta = Normalise_PI(theta - effHeadRoll_ + bodyRoll_);
  double trueVerticalPixels = hypotenusePixels*sin(trueTheta);
  double verticalPixelAngle = atan2(trueVerticalPixels,EFFECTIVE_CAMERA_DISTANCE_IN_PIXELS);

  double alpha = bodyTilt_ - headTiltBig_; //Combined large tilt

  //Distances of camera from pivot point of small tilt motor in cartesian coordinates - derived from TransformPosition() x1 -> x5
  double x = HEADLENGTH*cos(headTiltSmall_)*sin(headPan_);
  double y = HEADLENGTH*(sin(headTiltSmall_)*cos(alpha) - sin(alpha)*cos(headTiltSmall_)*cos(headPan_));
  double z = HEADLENGTH*(sin(alpha)*sin(headTiltSmall_) + cos(alpha)*cos(headTiltSmall_)*cos(headPan_));

  double angleToFront = atan2(x,z);
  double angleToHorizontal = atan2(y,z);
  double sinAngleToFront = sin(angleToFront);
  double sinAngleToHorizontal = sin(angleToHorizontal);

  double t = atan2(sinAngleToFront, sinAngleToHorizontal);
  double rotationalHypotenuse = sqrt(sinAngleToFront*sinAngleToFront + sinAngleToHorizontal*sinAngleToHorizontal);
  double trueAngleToHorizontal = asin(rotationalHypotenuse*cos(bodyRoll_ + t)) + verticalPixelAngle;
  if (trueAngleToHorizontal >= 0) return -1;
  double sinTrueAngleToHorizontal = sin(trueAngleToHorizontal);

  double cameraDistanceAboveGround = ((NECKLENGTH - cos(headTiltSmall_)*cameraOffset)*cos(alpha) + y)*cos(bodyRoll_) + heightOfNeck;
  double distance = cameraDistanceAboveGround / (-sinTrueAngleToHorizontal);
  return distance;
}

 // Determines the distance using the infrared sensor. Returns -1 if it is unable to determine the distance of the object
double VisionData::GetInfraredDistance(Blob* currentBlob,const Colour BLOB_COLOUR) {
  // First verify that we can actually obtain a value from the sensor that is not past the extent of the sensor i.e. 90
  double distance = ((double)(infraRed_))/10000.0;
  if (distance >= 90.0)
    return -1;   
  else {
    // Check roughly that the infrared should hit the blob and if so use it. Note: the infrared sensor is offset from the centre of the image.
    int centreX = (currentBlob->maxX - currentBlob->minX)/2 + currentBlob->minX;
    int centreY = (currentBlob->maxY - currentBlob->minY)/2 + currentBlob->minY;

    if ((  (BLOB_COLOUR != c_BALL_ORANGE) 
        && (currentBlob->maxX > (INFRA_MAX_X)) 
        && (currentBlob->minX < (INFRA_MIN_X)) 
        && (currentBlob->maxY > (INFRA_MAX_Y)) 
        && (currentBlob->minY < (INFRA_MIN_Y))
        ) || (
           (BLOB_COLOUR == c_BALL_ORANGE) // and the distance from the centre of the blob to the sensor corner must be greater than the radius
        && (GetDistance(INFRA_MIN_X,INFRA_MIN_Y,centreX,centreY) < ((currentBlob->maxX - currentBlob->minX)/2)) 
        && (GetDistance(INFRA_MIN_X,INFRA_MAX_Y,centreX,centreY) < ((currentBlob->maxX - currentBlob->minX)/2)) 
        && (GetDistance(INFRA_MAX_X,INFRA_MIN_Y,centreX,centreY) < ((currentBlob->maxX - currentBlob->minX)/2)) 
        && (GetDistance(INFRA_MAX_X,INFRA_MAX_Y,centreX,centreY) < ((currentBlob->maxX - currentBlob->minX)/2))
        )) {
        return (distance); // NOTE: Distance is corrected to be the distance from the head and not the distance to the dog
    }        
    return -1;
  }
}

double VisionData::NonLinearScaling(double value, double min, double max) {
  // Return an appropriatly scaled factor based on the value, min and max values
  if (value <= min) {
    return 0.0;
  } else if (value >= max) {
    return 1.0;
  } else {
    return ((value - min)/(max - min));
  }
}

void VisionData::DrawOverlayLine(int startX, int startY, double angle, int length, int colour) {
#ifdef WIN32
  DrawOverlayLine(startX, startY, cos(Normalise_PI(angle)), sin(Normalise_PI(angle)), length, colour);
#endif
}

void VisionData::DrawOverlayLine(int startX, int startY, double xInc, double yInc, int length, int colour) {
#ifdef WIN32
  double xPos = startX;
  double yPos = startY;
  int xPosInt = (int)xPos;
  int yPosInt = (int)yPos;
  int counter = 0;
  while (counter < length && xPosInt < currentImageWidth_ && yPosInt < currentImageHeight_ && xPosInt >= 0 && yPosInt >= 0) {
    overlay[yPosInt * currentImageWidth_ + xPosInt] = colour;
    xPos+=xInc;
    yPos+=yInc;
    xPosInt = (int)xPos;
    yPosInt = (int)yPos;
    counter++;
  }
#endif
}


bool VisionData::CheckForColourLeft(Blob* currentBlob, int startX, int startY, int distance, int colour) {
  return CheckForColour(currentBlob, startX, startY, distance, colour, true);
}

bool VisionData::CheckForColourRight(Blob* currentBlob, int startX, int startY, int distance, int colour) {
  return CheckForColour(currentBlob, startX, startY, distance, colour, false);
}


bool VisionData::CheckForColour(Blob* currentBlob, int startX, int startY, int distance, int colour, bool left) {
  double xPos = startX;
  double yPos = startY;
  double xInc = 0.0;
  double yInc = 0.0;
  if (left==false) {
    xInc = cos(effHeadRoll_);
    yInc = sin(effHeadRoll_);
  } else {
    xInc = -cos(effHeadRoll_);
    yInc = -sin(effHeadRoll_);
  }
  int xPosInt = (int)xPos;
  int yPosInt = (int)yPos;
  while (xPosInt < currentBlob->maxX && xPosInt >= currentBlob->minX && yPosInt < currentBlob->maxY && yPosInt >= currentBlob->minY) {
    xPos+=xInc;
    yPos+=yInc;
    xPosInt = (int)xPos;
    yPosInt = (int)yPos;
  }
  bool foundColour = false;
  int j = 0;
  for (j = 0; j < distance; j++) {
    if (yPosInt >= currentImageHeight_-10 || xPosInt >= currentImageWidth_-10 || xPosInt < 10) {
      break;
    }
    if (yPosInt < 0) break;
    if (classified_[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] == colour) {
      foundColour=true;
      break;
    } 
    xPos+=xInc;
    yPos+=yInc;
    xPosInt = (int)xPos;
    yPosInt = (int)yPos;
          
  }
  return foundColour;
}

bool VisionData::CheckForColourAngle(Blob* currentBlob, int startX, int startY, int distance, int colour, double angle) {
  double xPos = startX;
  double yPos = startY;
  double xInc = 0.0;
  double yInc = 0.0;
  xInc = -sin(effHeadRoll_ + angle);
  yInc = cos(effHeadRoll_ + angle);
  int xPosInt = (int)xPos;
  int yPosInt = (int)yPos;
  while (xPosInt < currentBlob->maxX && xPosInt >= currentBlob->minX && yPosInt < currentBlob->maxY && yPosInt >= currentBlob->minY) {
    xPos+=xInc;
    yPos+=yInc;
    xPosInt = (int)xPos;
    yPosInt = (int)yPos;
  }
  bool foundColour = false;
  int j = 0;
  for (j = 0; j < distance; j++) {
    if (yPosInt >= currentImageHeight_ || xPosInt >= currentImageWidth_ || xPosInt < 0) {
      break;
    }
    if (yPosInt < 0) break;
    #ifdef WIN32
      overlay[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] = c_FIELD_GREEN;
    #endif
    if (classified_[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] == colour) {
      foundColour=true;
      break;
    } 
    xPos+=xInc;
    yPos+=yInc;
    xPosInt = (int)xPos;
    yPosInt = (int)yPos;
          
  }
  return foundColour;
}

bool VisionData::CheckForMultipleColourAngle(Blob* currentBlob, int startX, int startY, int distance, int colour, int numRequiredPixels, double angle) {
  double xPos = startX;
  double yPos = startY;
  double xInc = 0.0;
  double yInc = 0.0;
  xInc = -sin(effHeadRoll_ + angle);
  yInc = cos(effHeadRoll_ + angle);
  int xPosInt = (int)xPos;
  int yPosInt = (int)yPos;
  while (xPosInt < currentBlob->maxX && xPosInt >= currentBlob->minX && yPosInt < currentBlob->maxY && yPosInt >= currentBlob->minY) {
    xPos+=xInc;
    yPos+=yInc;
    xPosInt = (int)xPos;
    yPosInt = (int)yPos;
  }
  int foundColour = 0;
  int numWrong=0;
  int j = 0;
  for (j = 0; j < distance; j++) {
    if (yPosInt >= currentImageHeight_ || xPosInt >= currentImageWidth_ || xPosInt < 0) {
      break;
    }
    if (yPosInt < 0) break;
    int currentIndex = (int)(yPosInt)*currentImageWidth_+(int)(xPosInt);
    #ifdef WIN32
      overlay[currentIndex] = c_FIELD_GREEN;
    #endif
    if (classified_[currentIndex] == colour) {
      foundColour++;
      if (foundColour >= numRequiredPixels) return true;
    } else if (classified_[currentIndex] != c_UNKNOWN && classified_[currentIndex] != c_BALL_ORANGE) {
      numWrong++;
      if (numWrong > 1) return false;
    }
    xPos+=xInc;
    yPos+=yInc;
    xPosInt = (int)xPos;
    yPosInt = (int)yPos;
          
  }
  return (foundColour >= numRequiredPixels);
}