/**
* @brief Applies a threshold to the Red, Green, and Blue values of a RGB image
* or the Hue,
* Saturation, Luminance values for a HSL image.
* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL.
* This simpler version filters based on a hue range in the HSL mode.
*
* @param dest The destination image. This must be a IMAQ_IMAGE_U8 image.
* @param source The image to threshold
* @param mode The color space to perform the threshold in. valid values are:
* IMAQ_RGB, IMAQ_HSL.
* @param plane1Range The selection range for the first plane of the image. Set
* this parameter to nullptr to use a selection range from 0 to 255.
* @param plane2Range The selection range for the second plane of the image. Set
* this parameter to nullptr to use a selection range from 0 to 255.
* @param plane3Range The selection range for the third plane of the image. Set
* this parameter to nullptr to use a selection range from 0 to 255.
*
* @return On success: 1. On failure: 0. To get extended error information, call
* GetLastError().
* */
int frcColorThreshold(Image* dest, const Image* source, ColorMode mode,
const Range* plane1Range, const Range* plane2Range,
const Range* plane3Range) {
int replaceValue = 1;
return imaqColorThreshold(dest, source, replaceValue, mode, plane1Range,
plane2Range, plane3Range);
}
////////////////////////////////////////////////////////////////////////////////
//
// Function Name: IVA_CLRThreshold
//
// Description : Thresholds a color image.
//
// Parameters : image - Input image
// min1 - Minimum range for the first plane
// max1 - Maximum range for the first plane
// min2 - Minimum range for the second plane
// max2 - Maximum range for the second plane
// min3 - Minimum range for the third plane
// max3 - Maximum range for the third plane
// colorMode - Color space in which to perform the threshold
//
// Return Value : success
//
////////////////////////////////////////////////////////////////////////////////
static int IVA_CLRThreshold(Image* image, int min1, int max1, int min2, int max2, int min3, int max3, int colorMode)
{
int success = 1;
Image* thresholdImage;
Range plane1Range;
Range plane2Range;
Range plane3Range;
//-------------------------------------------------------------------//
// Color Threshold //
//-------------------------------------------------------------------//
// Creates an 8 bit image for the thresholded image.
VisionErrChk(thresholdImage = imaqCreateImage(IMAQ_IMAGE_U8, 7));
// Set the threshold range for the 3 planes.
plane1Range.minValue = min1;
plane1Range.maxValue = max1;
plane2Range.minValue = min2;
plane2Range.maxValue = max2;
plane3Range.minValue = min3;
plane3Range.maxValue = max3;
// Thresholds the color image.
VisionErrChk(imaqColorThreshold(thresholdImage, image, 1, colorMode, &plane1Range, &plane2Range, &plane3Range));
// Copies the threshold image in the souce image.
VisionErrChk(imaqDuplicate(image, thresholdImage));
Error:
imaqDispose(thresholdImage);
return success;
}
/**
* Perform a threshold operation on a ColorImage.
*
* Perform a threshold operation on a ColorImage using the ColorMode supplied
* as a parameter.
*
* @param colorMode The type of colorspace this operation should be performed in
* @return a pointer to a binary image
*/
BinaryImage* ColorImage::ComputeThreshold(ColorMode colorMode, int low1,
int high1, int low2, int high2,
int low3, int high3) {
auto result = new BinaryImage();
Range range1 = {low1, high1}, range2 = {low2, high2}, range3 = {low3, high3};
int success = imaqColorThreshold(result->GetImaqImage(), m_imaqImage, 1,
colorMode, &range1, &range2, &range3);
wpi_setImaqErrorWithContext(success, "ImaqThreshold error");
return result;
}
int frcHueThreshold(Image* dest, const Image* source, const Range* hueRange, int minSaturation)
{
// assume HSL mode
ColorMode mode = IMAQ_HSL;
// Set saturation 100 - 255
Range satRange; satRange.minValue = minSaturation; satRange.maxValue = 255;
// Set luminance 100 - 255
Range lumRange; lumRange.minValue = 100; lumRange.maxValue = 255;
// Replace pixels with 1 if pass threshold filter
int replaceValue = 1;
return imaqColorThreshold(dest, source, replaceValue, mode, hueRange, &satRange, &lumRange);
}
void Autonomous() override {
while (IsAutonomous() && IsEnabled())
{
//read file in from disk. For this example to run you need to copy image20.jpg from the SampleImages folder to the
//directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp
imaqError = imaqReadFile(frame, "//NewDir//image2.jpg", NULL, NULL);
//Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished.
TOTE_HUE_RANGE.minValue = SmartDashboard::GetNumber("Tote hue min", TOTE_HUE_RANGE.minValue);
TOTE_HUE_RANGE.maxValue = SmartDashboard::GetNumber("Tote hue max", TOTE_HUE_RANGE.maxValue);
TOTE_SAT_RANGE.minValue = SmartDashboard::GetNumber("Tote sat min", TOTE_SAT_RANGE.minValue);
TOTE_SAT_RANGE.maxValue = SmartDashboard::GetNumber("Tote sat max", TOTE_SAT_RANGE.maxValue);
TOTE_VAL_RANGE.minValue = SmartDashboard::GetNumber("Tote val min", TOTE_VAL_RANGE.minValue);
TOTE_VAL_RANGE.maxValue = SmartDashboard::GetNumber("Tote val max", TOTE_VAL_RANGE.maxValue);
//Threshold the image looking for yellow (tote color)
imaqError = imaqColorThreshold(binaryFrame, frame, 255, IMAQ_HSV, &TOTE_HUE_RANGE, &TOTE_SAT_RANGE, &TOTE_VAL_RANGE);
//Send particle count to dashboard
int numParticles = 0;
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Masked particles", numParticles);
//Send masked image to dashboard to assist in tweaking mask.
SendToDashboard(binaryFrame, imaqError);
//filter out small particles
float areaMin = SmartDashboard::GetNumber("Area min %", AREA_MINIMUM);
criteria[0] = {IMAQ_MT_AREA_BY_IMAGE_AREA, areaMin, 100, false, false};
imaqError = imaqParticleFilter4(binaryFrame, binaryFrame, criteria, 1, &filterOptions, NULL, NULL);
//Send particle count after filtering to dashboard
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Filtered particles", numParticles);
if(numParticles > 0)
{
//Measure particles and sort by particle size
std::vector<ParticleReport> particles;
for(int particleIndex = 0; particleIndex < numParticles; particleIndex++)
{
ParticleReport par;
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &(par.PercentAreaToImageArea));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA, &(par.Area));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_CONVEX_HULL_AREA, &(par.ConvexHullArea));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &(par.BoundingRectTop));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &(par.BoundingRectLeft));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &(par.BoundingRectBottom));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &(par.BoundingRectRight));
particles.push_back(par);
}
sort(particles.begin(), particles.end(), CompareParticleSizes);
//This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise
//for the reader. Note that the long and short side scores expect a single tote and will not work for a stack of 2 or more totes.
//Modification of the code to accommodate 2 or more stacked totes is left as an exercise for the reader.
scores.Trapezoid = TrapezoidScore(particles.at(0));
SmartDashboard::PutNumber("Trapezoid", scores.Trapezoid);
scores.LongAspect = LongSideScore(particles.at(0));
SmartDashboard::PutNumber("Long Aspect", scores.LongAspect);
scores.ShortAspect = ShortSideScore(particles.at(0));
SmartDashboard::PutNumber("Short Aspect", scores.ShortAspect);
scores.AreaToConvexHullArea = ConvexHullAreaScore(particles.at(0));
SmartDashboard::PutNumber("Convex Hull Area", scores.AreaToConvexHullArea);
bool isTote = scores.Trapezoid > SCORE_MIN && (scores.LongAspect > SCORE_MIN || scores.ShortAspect > SCORE_MIN) && scores.AreaToConvexHullArea > SCORE_MIN;
bool isLong = scores.LongAspect > scores.ShortAspect;
//Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
SmartDashboard::PutBoolean("IsTote", isTote);
SmartDashboard::PutNumber("Distance", computeDistance(binaryFrame, particles.at(0), isLong));
} else {
SmartDashboard::PutBoolean("IsTote", false);
}
Wait(0.005); // wait for a motor update time
}
}
Vision_Out Run(Vision_In in)
{
Vision_Out out;
//read file in from disk. For this example to run you need to copy image.jpg from the SampleImages folder to the
//directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp
//imaqError = imaqReadFile(frame, "//home//lvuser//SampleImages//image.jpg", NULL, NULL);
imaqError = camera->GetImage(frame);
//Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished.
/*
RING_HUE_RANGE.minValue = SmartDashboard::GetNumber("Tote hue min", RING_HUE_RANGE.minValue);
RING_HUE_RANGE.maxValue = SmartDashboard::GetNumber("Tote hue max", RING_HUE_RANGE.maxValue);
RING_SAT_RANGE.minValue = SmartDashboard::GetNumber("Tote sat min", RING_SAT_RANGE.minValue);
RING_SAT_RANGE.maxValue = SmartDashboard::GetNumber("Tote sat max", RING_SAT_RANGE.maxValue);
RING_VAL_RANGE.minValue = SmartDashboard::GetNumber("Tote val min", RING_VAL_RANGE.minValue);
RING_VAL_RANGE.maxValue = SmartDashboard::GetNumber("Tote val max", RING_VAL_RANGE.maxValue);
*/
if(in.shouldProcess)
{
//Threshold the image looking for ring light color
imaqError = imaqColorThreshold(binaryFrame, frame, 255, IMAQ_HSV, &RING_HUE_RANGE, &RING_SAT_RANGE, &RING_VAL_RANGE);
//Send particle count to dashboard
int numParticles = 0;
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Masked particles", numParticles);
//Send masked image to dashboard to assist in tweaking mask.
SendToDashboard(binaryFrame, imaqError);
//filter out small particles
float areaMin = SmartDashboard::GetNumber("Area min %", AREA_MINIMUM);
criteria[0] = {IMAQ_MT_AREA_BY_IMAGE_AREA, areaMin, 100, false, false};
imaqError = imaqParticleFilter4(binaryFrame, binaryFrame, criteria, 1, &filterOptions, NULL, NULL);
//Send particle count after filtering to dashboard
imaqError = imaqCountParticles(binaryFrame, 1, &numParticles);
SmartDashboard::PutNumber("Filtered particles", numParticles);
if(numParticles > 0)
{
//Measure particles and sort by particle size
std::vector<ParticleReport> particles;
for(int particleIndex = 0; particleIndex < numParticles; particleIndex++)
{
ParticleReport par;
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &(par.PercentAreaToImageArea));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA, &(par.Area));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &(par.BoundingRectTop));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &(par.BoundingRectLeft));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &(par.BoundingRectBottom));
imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &(par.BoundingRectRight));
particles.push_back(par);
}
sort(particles.begin(), particles.end(), CompareParticleSizes);
//This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise
//for the reader. Note that this scores and reports information about a single particle (single L shaped target). To get accurate information
//about the location of the tote (not just the distance) you will need to correlate two adjacent targets in order to find the true center of the tote.
scores.Aspect = AspectScore(particles.at(0));
SmartDashboard::PutNumber("Aspect", scores.Aspect);
scores.Area = AreaScore(particles.at(0));
SmartDashboard::PutNumber("Area", scores.Area);
bool isTarget = scores.Area > SCORE_MIN && scores.Aspect > SCORE_MIN;
//Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote
SmartDashboard::PutBoolean("IsTarget", isTarget);
double distance = computeDistance(binaryFrame, particles.at(0));
SmartDashboard::PutNumber("Distance", computeDistance(binaryFrame, particles.at(0)));
out.returnDistanceToTote = distance;
out.returnIsTote = true;
return out;
}
else
{
out.returnIsTote = false;
SmartDashboard::PutBoolean("IsTarget", false);
}
}
return out;
}
void Camera::GetInfo() {
Image* binFrame;
binFrame = imaqCreateImage(IMAQ_IMAGE_U8, 0);
Range Hue = {hueMin, hueMax};
Range Sat = {satMin, satMax};
Range Val = {valMin, valMax};
ParticleFilterCriteria2 criteria[1];
//ParticleFilterOptions2 filterOptions = {0, 0, 1, 1};
criteria[0] = {IMAQ_MT_AREA, 0, (float) aireMin, false, true};
int nbParticles(0);
IMAQdxGrab(session, frame, true, NULL);
imaqScale(frame, frame, 2, 2, ScalingMode::IMAQ_SCALE_SMALLER, IMAQ_NO_RECT);
imaqColorThreshold(binFrame, frame, 255, IMAQ_HSV, &Hue, &Sat, &Val);
imaqMorphology(binFrame, binFrame, IMAQ_DILATE, NULL);
//imaqParticleFilter4(binFrame, binFrame, &criteria[0], 1, &filterOptions, NULL, &nbParticles);
imaqCountParticles(binFrame, 0, &nbParticles);
CameraServer::GetInstance()->SetImage(binFrame);
int indexMax(0);
double aireMax(0);
if(nbParticles > 0) {
for(int particleIndex = 0; particleIndex < nbParticles; particleIndex++){
double aire (0);
imaqMeasureParticle(binFrame, particleIndex, 0, IMAQ_MT_AREA, &aire);
if (aire > aireMax){
aireMax = aire;
indexMax = particleIndex;
}
}
double hypothenuse(0);
int hauteurImage(0);
int largeurImage(0);
double centreX(0);
double centreY(0);
double angleX(0);
double angleY(0);
double offset(0);
imaqMeasureParticle(binFrame, indexMax, 0, IMAQ_MT_CENTER_OF_MASS_X, ¢reX);
imaqMeasureParticle(binFrame, indexMax, 0, IMAQ_MT_CENTER_OF_MASS_Y, ¢reY);
imaqGetImageSize(binFrame, &largeurImage, &hauteurImage);
double dHauteurImage(hauteurImage);
double dLargeurImage(largeurImage);
//Normalisation
centreX = ((2 * centreX) / dLargeurImage) - 1;
centreY = ((-2 * centreY) / dHauteurImage) + 1;
angleX = atan(centreX * tan(FOV_X * acos(-1) / 180.0));
angleY = atan(centreY * tan(FOV_Y * acos(-1) / 180.0));
distance = (TARGET_HEIGHT - CAMERA_HEIGHT) / tan(CAMERA_ANGLE * acos(-1) / 180 + angleY);
hypothenuse = sqrt(pow(distance, 2) + pow(TARGET_HEIGHT - CAMERA_HEIGHT, 2));
offset = hypothenuse * tan(angleX);
ecart = offset - CAMERA_OFFSET;
SmartDashboard::PutNumber("Distance Cible", distance);
SmartDashboard::PutNumber("Angle Cible", ecart);
SmartDashboard::PutNumber("Aire Particule", aireMax);
SmartDashboard::PutNumber("Nombre Particules", nbParticles);
SmartDashboard::PutNumber("Hypothenuse", hypothenuse);
SmartDashboard::PutNumber("Largeur image", largeurImage);
}
}