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;
}
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());
}
