/** This function is called periodically during operator control */
public void teleopInit() {
ColorImage image;
try {
image = camera.getImage();
image.write("unedited.jpg");
BinaryImage bImage = image.thresholdRGB(160, 255, 160, 255, 160, 255);
bImage.write("whitemask.jpg");
MonoImage mImage = image.getLuminancePlane();
mImage.write("luminancePlane.jpg");
image.free();
bImage.free();
} catch (NIVisionException e) {
System.out.println("Error retrieving image: NIVisionException");
e.printStackTrace();
} catch (AxisCameraException e) {
System.out.println("Error retrieving image: AxisCameraException");
e.printStackTrace();
}
}
public boolean processImage() {
boolean debugWriteImages = true;
boolean success = cam.freshImage();
if (success) {
try {
System.out.println("In Try loop");
ColorImage im = cam.getImage();
System.out.println("Got image");
if (debugWriteImages) {
im.write("image1.jpg");
System.out.println("Wrote color image");
}
BinaryImage thresholdIm =
im.thresholdRGB(redLow, redHigh, greenLow, greenHigh, blueLow, blueHigh);
if (debugWriteImages) {
thresholdIm.write("image2.jpg");
System.err.println("Wrote Threshold Image");
}
BinaryImage filteredBoxIm = thresholdIm.particleFilter(boxCriteria);
ParticleAnalysisReport[] xparticles = filteredBoxIm.getOrderedParticleAnalysisReports();
System.out.println(xparticles.length + " particles at " + Timer.getFPGATimestamp());
BinaryImage filteredInertiaIm = filteredBoxIm.particleFilter(inertiaCriteria);
ParticleAnalysisReport[] particles = filteredInertiaIm.getOrderedParticleAnalysisReports();
System.out.println(particles.length + " particles at " + Timer.getFPGATimestamp());
// Loop through targets, find highest one.
// Targets aren't found yet.
highTarget = Target.NullTarget;
target1 = Target.NullTarget;
target2 = Target.NullTarget;
target3 = Target.NullTarget;
target4 = Target.NullTarget;
System.out.println("Targets created");
double minY = IMAGE_HEIGHT; // Minimum y <-> higher in image.
for (int i = 0; i < particles.length; i++) {
Target t = new Target(i, particles[i]);
if (t.ratio > ratioMin && t.ratio < ratioMax) {
addTarget(t);
if (t.centerY <= minY) {
highTarget = t;
}
}
System.out.println(
"Target "
+ i
+ ": ("
+ t.centerX
+ ","
+ t.centerY
+ ") Distance: "
+ getDistance(t));
}
System.out.println("Best target: " + highTarget.index);
System.out.println("Distance to the target: " + getDistance(highTarget));
if (debugWriteImages) {
filteredBoxIm.write("image3.jpg");
filteredInertiaIm.write("image4.jpg");
System.out.println("Wrote Images");
}
// Free memory from images.
im.free();
thresholdIm.free();
filteredBoxIm.free();
filteredInertiaIm.free();
} catch (AxisCameraException ex) {
System.out.println("Axis Camera Exception Gotten" + ex.getMessage());
ex.printStackTrace();
} catch (NIVisionException ex) {
System.out.println("NIVision Exception Gotten - " + ex.getMessage());
ex.printStackTrace();
}
}
return success;
}
/**
* Analyzes an image taken by the Axis Camera, performing various functions to determine where the
* goal is and at what goal the robot is facing
*
* @param
* @return Array of doubles where the first value is the type of goal (2.0 high, 1.0 middle, 0.0
* no goal), the x-centered-normalized value, the y-centered-normalized value and the
* distance. If the distance is negative, no goal was found.
*/
public double[] analyze() {
double[] data = new double[3];
// [goal type, x-centered, y-centered, distance]
try {
ColorImage image = camera.getImage(); // Get image from camera
BinaryImage thresholdImage;
thresholdImage =
image.thresholdHSV(120, 120, 44, 80, 98, 100); // "Look" for objects in this HSV range
BinaryImage convexHullImage = thresholdImage.convexHull(false); // Fill in occluded rectangles
BinaryImage filteredImage =
convexHullImage.particleFilter(collection); // Find filled in rectangles
Scores scores[] = new Scores[filteredImage.getNumberParticles()];
for (int i = 0; i < scores.length; i++) {
ParticleAnalysisReport report =
filteredImage.getParticleAnalysisReport(i); // Get the report for each particle found
scores[i] = new Scores();
scores[i].rectangularity = scoreRectangularity(report);
scores[i].aspectRatioOuter = scoreAspectRatio(filteredImage, report, i, true);
scores[i].aspectRatioInner = scoreAspectRatio(filteredImage, report, i, false);
scores[i].xEdge = scoreXEdge(thresholdImage, report);
scores[i].yEdge = scoreYEdge(thresholdImage, report);
if (scoreCompare(scores[i], false)) {
double dist = computeDistance(thresholdImage, report, i, false);
System.out.println(
"particle: "
+ i
+ " is a High Goal centerX: "
+ report.center_mass_x_normalized
+ "centerY: "
+ report.center_mass_y_normalized);
System.out.println("Distance: " + dist);
data[0] = 2.0;
data[1] = report.center_mass_x_normalized;
data[2] = report.center_mass_y_normalized;
data[3] = dist;
} else if (scoreCompare(scores[i], true)) {
double dist = computeDistance(thresholdImage, report, i, false);
System.out.println(
"particle: "
+ i
+ " is a Middle Goal centerX: "
+ report.center_mass_x_normalized
+ "centerY: "
+ report.center_mass_y_normalized);
System.out.println("Distance: " + dist);
data[0] = 1.0;
data[1] = report.center_mass_x_normalized;
data[2] = report.center_mass_y_normalized;
data[3] = dist;
} else {
System.out.println(
"particle: "
+ i
+ " is not a goal centerX: "
+ report.center_mass_x_normalized
+ "centerY: "
+ report.center_mass_y_normalized);
data[0] = 0.0;
data[1] = report.center_mass_x_normalized;
data[2] = report.center_mass_y_normalized;
data[3] = -1.0;
}
System.out.println(
"rect: " + scores[i].rectangularity + "ARinner: " + scores[i].aspectRatioInner);
System.out.println(
"ARouter: "
+ scores[i].aspectRatioOuter
+ "xEdge: "
+ scores[i].xEdge
+ "yEdge: "
+ scores[i].yEdge);
}
/* MUST USE FREE FUNCTION: all images are currently allocated in C structures */
filteredImage.free();
convexHullImage.free();
thresholdImage.free();
image.free();
} // end analyze()
catch (AxisCameraException e) {
data[0] = 0.0;
data[1] = 0.0;
data[2] = 0.0;
data[3] = -1.0;
SmartDashboard.putString("ERROR: ", "Camera malfunction!");
} catch (NIVisionException ex) {
data[0] = 0.0;
data[1] = 0.0;
data[2] = 0.0;
data[3] = -1.0;
SmartDashboard.putString("ERROR: ", "NIVision Exception!");
}
return data;
}
