예제 #1
0
  public Mat process(Mat srcImage) {
    if (stage == 0) return srcImage;

    List<PolygonCv> targets = new ArrayList<>();
    PolygonCv bestTarget;

    Mat workingImage = srcImage.clone();

    _ColorSpace.process(workingImage);
    _ColorRange.process(workingImage);
    _Erode.process(workingImage);
    _Dilate.process(workingImage);
    //		_GrayScale.process(workingImage);
    //		_BlackWhite.process(workingImage);

    if (stage == 1) return workingImage;

    targets = findTargets(workingImage);
    workingImage = srcImage.clone();

    addTargetingRectangle(workingImage);

    if (targets.size() > 0) {
      bestTarget = findBestTarget(targets);

      if (networkTable != null) {
        targetAnalysis(bestTarget); // no return as it simply writes data to netTables
        networkTable.putNumber("FrameCount", frameCount++);
      }
      targetAnalysis(bestTarget, false);

      if (stage == 2) return workingImage; // commandline, so don't bother drawing anything

      if (stage == 3) {
        _OtherTargets.setPolygon(targets);
        _OtherTargets.process(workingImage);

        _BestTarget.setPolygon(bestTarget);
        _BestTarget.process(workingImage);

        _Reticle.setCenter(bestTarget.getCenterX(), bestTarget.getMaxY());
        _Reticle.setSize(Render.RETICLE_SIZE, Render.RETICLE_SIZE);
        _Reticle.process(workingImage);
      }

      if (stage == 4) {

        _BoundingBox.setCenter(bestTarget.getCenterX(), bestTarget.getCenterY());
        _BoundingBox.setSize(bestTarget.getHeight() / 2, bestTarget.getWidth() / 2);
        _BoundingBox.process(workingImage);
      }
    }

    // _CrossHair.process(workingImage);

    return workingImage;
  }
예제 #2
0
  private void targetAnalysis(
      PolygonCv foundTarget, boolean printToNetWorkTable) { // tells the robo info about the target
    double offsetXDegrees,
        offsetXDegreesIdeal,
        targetDistanceInches,
        baseDistanceInches,
        cameraAngleElevationRadians,
        targetAngleRadians,
        targetAngleFactor;

    // calculates how far off center the target is from the center of the camera
    // offsetXDegrees = Math.atan((400 - foundTarget.getCenterX()) *
    // Math.tan(Camera.FOV_X_RADIANS/2) / (Camera.RESOLUTION_X_PIXELS/2));
    offsetXDegrees =
        Math.atan(
            (400 - foundTarget.getMinX())
                * Math.tan(Camera.FOV_X_RADIANS / 2)
                / (Camera.RESOLUTION_X_PIXELS / 2));

    // converts from radians (output of Math) to degrees
    offsetXDegrees = Math.toDegrees(offsetXDegrees);

    // used to determine size of the target in radians
    targetAngleFactor = 2 * Math.tan(Camera.FOV_X_RADIANS / 2) / Camera.RESOLUTION_X_PIXELS;

    /*   	//gets size of target in Radians
        	targetAngleRadians = Math.atan(((Camera.RESOLUTION_X_PIXELS/2) - foundTarget.getMaxX()) * targetAngleFactor) -
        				  		 Math.atan(((Camera.RESOLUTION_X_PIXELS/2) - foundTarget.getMinX()) * targetAngleFactor);
        	//gets degree value of top and bottom points, and finds difference

        	//targetAngleRadians could be negative, compensates for this
        	targetAngleRadians = Math.abs(targetAngleRadians);

        	//gets distance to target
        	targetDistanceInches = (Target.TAPE_WIDTH_INCHES / 2) / Math.tan(targetAngleRadians);
    */
    // use perspective height rather than targetTapeHeight?

    // ye olde algorithm
    /*    	targetDistanceInches = (Target.TAPE_WIDTH_INCHES / 2) / Math.tan((Target.TAPE_WIDTH_INCHES / Camera.RESOLUTION_X_PIXELS) * (Camera.FOV_X_RADIANS / 2));
     */

    // new old algo
    double dpx = (Camera.RESOLUTION_X_PIXELS / 2) / Math.tan(Camera.FOV_X_RADIANS / 2);
    double tta = (foundTarget.getWidth() / 2) / dpx;
    targetDistanceInches = (Target.TAPE_WIDTH_INCHES) / Math.tan(tta);

    // System.out.println(dpx + " " + tta + " " + targetDistanceInches);

    // gets elevation of target to camera relative to ground
    cameraAngleElevationRadians =
        Math.asin((Target.TOWER_HEIGHT_INCHES - Camera.OFFSET_Y_INCHES) / targetDistanceInches);

    // gets distance to the base of the target
    baseDistanceInches = Math.cos(cameraAngleElevationRadians) * targetDistanceInches;

    /* gets 'ideal' target off center angle - because camera is to the side,
     * a perfectly zeroed robot will be slightly off from the camera
     */
    // if(Camera.OFFSET_X_INCHES != 0) {
    // offsetXDegreesIdeal = Math.toDegrees(Math.atan(targetDistanceInches /
    // Camera.OFFSET_X_INCHES));
    // } else { 								//will be positive if cameraCenterOffset is negative
    //	offsetXDegreesIdeal = 0;
    // }

    // compensates for Ideal angle offset
    // offsetXDegrees = offsetXDegrees - offsetXDegreesIdeal;

    double camAngle =
        fovCalc.pixelFromCenterToDeg(foundTarget.getCenterX() - (Camera.RESOLUTION_X_PIXELS / 2));
    double rot = -(robotAngleOffset - camAngle);

    // If rotation estimator enabled, use it as a better guess as how much to rotate
    if (rotEst != null) {
      // Guess at camera position somewhere between 75 and 120 inches from plane
      // containing target
      double distEst = (75 + 120) / 2;
      rot = rotEst.computeRotation(camAngle, distEst);
    }

    offsetXDegrees = rot; // TODO temporary substitution

    // determines if angle values are reasonable
    if (offsetXDegrees < (Camera.FOV_X_DEGREES / 2)
        && offsetXDegrees > (-Camera.FOV_X_DEGREES / 2)
        && printToNetWorkTable) {
      networkTable.putNumber(
          "OffCenterDegreesX",
          -Math.toDegrees(distanceOffset(foundTarget.getCenterX(), foundTarget.getCenterY())));
    }

    System.out.println("Angle: " + offsetXDegrees);

    // writes calculated data to network tables
    System.out.println("Distance: " + baseDistanceInches);
    if (printToNetWorkTable) {
      networkTable.putNumber("DistanceToBase", baseDistanceInches);
      networkTable.putNumber("DistanceToTarget", targetDistanceInches);
    }

    // Newest Algorithm
    distanceOffset(foundTarget.getCenterX(), foundTarget.getCenterY());
  }