/**
* Fill in the particle analysis report.
* For a given image, fill in the particle analysis report structure.
* @param imageParam the image to use
* @param particleNumber the particle index in the set of particles for the image
* @param par the particle analysis report to be filled in
*/
int BinaryImage::ParticleAnalysis(Image* imageParam, int particleNumber, ParticleAnalysisReport* par)
{
int success = 0;
success = imaqGetImageSize(imageParam, &par->imageWidth, &par->imageHeight);
wpi_imaqAssert(success, "Error getting image size");
par->particleIndex = particleNumber;
success = imaqCountParticles(imageParam, TRUE, NULL);
wpi_imaqAssert(success, "Error counting particles");
par->center_mass_x = (int) particleMeasurement(imageParam, particleNumber, IMAQ_MT_CENTER_OF_MASS_X);
par->center_mass_y = (int) particleMeasurement(imageParam, particleNumber, IMAQ_MT_CENTER_OF_MASS_Y);
par->particleArea = (int) particleMeasurement(imageParam, particleNumber, IMAQ_MT_AREA);
par->boundingRect.top = (int) particleMeasurement(imageParam, particleNumber, IMAQ_MT_BOUNDING_RECT_TOP);
par->boundingRect.left = (int) particleMeasurement(imageParam, particleNumber, IMAQ_MT_BOUNDING_RECT_LEFT);
par->boundingRect.height = (int) particleMeasurement(imageParam, particleNumber, IMAQ_MT_BOUNDING_RECT_HEIGHT);
par->boundingRect.width = (int) particleMeasurement(imageParam, particleNumber, IMAQ_MT_BOUNDING_RECT_WIDTH);
par->particleToImagePercent = particleMeasurement(imageParam, particleNumber, IMAQ_MT_AREA_BY_IMAGE_AREA);
par->particleQuality = particleMeasurement(imageParam, particleNumber, IMAQ_MT_AREA_BY_PARTICLE_AND_HOLES_AREA);
/* normalized position (-1 to 1) */
par->center_mass_x_normalized = NormalizeFromRange(par->center_mass_x, par->imageWidth);
par->center_mass_y_normalized = NormalizeFromRange(par->center_mass_y, par->imageHeight);
return success;
}
IMAQdxError Camera::Stop() {
// stop image acquisition
imaqError = IMAQdxErrorSuccess;
if (ULONG_MAX != session) {
// Had trouble with imaqDrawTextOnImage
//const DrawTextOptions options = {"Arial", 12, 0, 0, 0, 0, IMAQ_CENTER, IMAQ_INVERT};
//int fontUsed;
//imaqDrawTextOnImage(frame, frame, {120, 80}, "X", &options, &fontUsed);
if (NULL != frame && camera == currentCamera) {
// Would really like to make this semi-transparent, but not apparent how.
int x, y;
imaqGetImageSize(frame, &x, &y);
//RGBValue color = IMAQ_RGB_YELLOW;
//color.G = 240;
//color.R = 240;
//imaqOverlayOval(frame, { y/3, x/3, y/3, x/3 }, &color, IMAQ_DRAW_VALUE, NULL);
//imaqMergeOverlay(NULL, frame, NULL, 0, NULL);
// imaqDrawShapeOnImage(frame, frame, { y/3, x/3, y/3, x/3 }, DrawMode::IMAQ_DRAW_INVERT, ShapeMode::IMAQ_SHAPE_OVAL,0.0);
LCameraServer::GetInstance()->SetImage(frame);
}
IMAQdxStopAcquisition(session);
IMAQdxCloseCamera(session);
printf("Camera %s closed on session %lu.\n",
camInfo[camera].InterfaceName, session);
session = ULONG_MAX;
}
return imaqError;
}
/**
* @brief Conduct measurements for a single particle in an images.
* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL.
*
* @param image image with the particle to analyze. This function modifies the source image.
* If you need the original image, create a copy of the image using frcCopy() before using this function.
* @param particleNumber The number of the particle to get information on
* @param par on return, a particle analysis report containing information about the particle. This structure must be created by the caller.
* @return On success: 1. On failure: 0. To get extended error information, call GetLastError().
*/
int frcParticleAnalysis(Image* image, int particleNumber, ParticleAnalysisReport* par)
{
int success = 0;
/* image information */
int height, width;
if ( ! imaqGetImageSize(image, &width, &height) ) { return success; }
par->imageWidth = width;
par->imageHeight = height;
par->particleIndex = particleNumber;
/* center of mass point of the largest particle */
double returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_CENTER_OF_MASS_X, &returnDouble);
if ( !success ) { return success; }
par->center_mass_x = (int)returnDouble; // pixel
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_CENTER_OF_MASS_Y, &returnDouble);
if ( !success ) { return success; }
par->center_mass_y = (int)returnDouble; // pixel
/* particle size statistics */
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_AREA, &returnDouble);
if ( !success ) { return success; }
par->particleArea = returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_BOUNDING_RECT_TOP, &returnDouble);
if ( !success ) { return success; }
par->boundingRect.top = (int)returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &returnDouble);
if ( !success ) { return success; }
par->boundingRect.left = (int)returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_BOUNDING_RECT_HEIGHT, &returnDouble);
if ( !success ) { return success; }
par->boundingRect.height = (int)returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_BOUNDING_RECT_WIDTH, &returnDouble);
if ( !success ) { return success; }
par->boundingRect.width = (int)returnDouble;
/* particle quality statistics */
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &returnDouble);
if ( !success ) { return success; }
par->particleToImagePercent = returnDouble;
success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_AREA_BY_PARTICLE_AND_HOLES_AREA, &returnDouble);
if ( !success ) { return success; }
par->particleQuality = returnDouble;
/* normalized position (-1 to 1) */
par->center_mass_x_normalized = RangeToNormalized(par->center_mass_x, width);
par->center_mass_y_normalized = RangeToNormalized(par->center_mass_y, height);
return success;
}
/**
* Computes the estimated distance to a target using the width of the particle in the image. For more information and graphics
* showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation.
*
* @param image The image to use for measuring the particle estimated rectangle
* @param report The Particle Analysis Report for the particle
* @param isLong Boolean indicating if the target is believed to be the long side of a tote
* @return The estimated distance to the target in feet.
*/
double computeDistance (Image *image, ParticleReport report, bool isLong) {
double normalizedWidth, targetWidth;
int xRes, yRes;
imaqGetImageSize(image, &xRes, &yRes);
normalizedWidth = 2*(report.BoundingRectRight - report.BoundingRectLeft)/xRes;
SmartDashboard::PutNumber("Width", normalizedWidth);
targetWidth = isLong ? 26.9 : 16.9;
return targetWidth/(normalizedWidth*12*tan(VIEW_ANGLE*M_PI/(180*2)));
}
/**
* Get a single particle analysis report.
* Get one (of possibly many) particle analysis reports for an image.
* This version could be more efficient when copying many reports.
* @param particleNumber Which particle analysis report to return.
* @param par the selected particle analysis report
*/
void BinaryImage::GetParticleAnalysisReport(int particleNumber, ParticleAnalysisReport *par)
{
int success;
int numParticles = 0;
success = imaqGetImageSize(m_imaqImage, &par->imageWidth, &par->imageHeight);
wpi_setImaqErrorWithContext(success, "Error getting image size");
if (StatusIsFatal())
return;
success = imaqCountParticles(m_imaqImage, 1, &numParticles);
wpi_setImaqErrorWithContext(success, "Error counting particles");
if (StatusIsFatal())
return;
if (particleNumber >= numParticles)
{
wpi_setWPIErrorWithContext(ParameterOutOfRange, "particleNumber");
return;
}
par->particleIndex = particleNumber;
// Don't bother measuring the rest of the particle if one fails
bool good = ParticleMeasurement(particleNumber, IMAQ_MT_CENTER_OF_MASS_X, &par->center_mass_x);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_CENTER_OF_MASS_Y, &par->center_mass_y);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA, &par->particleArea);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_TOP, &par->boundingRect.top);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_LEFT, &par->boundingRect.left);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_HEIGHT, &par->boundingRect.height);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_WIDTH, &par->boundingRect.width);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA_BY_IMAGE_AREA, &par->particleToImagePercent);
good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA_BY_PARTICLE_AND_HOLES_AREA, &par->particleQuality);
if (good)
{
/* normalized position (-1 to 1) */
par->center_mass_x_normalized = NormalizeFromRange(par->center_mass_x, par->imageWidth);
par->center_mass_y_normalized = NormalizeFromRange(par->center_mass_y, par->imageHeight);
}
}
// Feed video frams from the currently selected camera.
// Pass in the robot tick count -- it's just wasteful to
// send a frame every tick if the camera rate is slower
void Camera::Feed(int ticks) {
if (enabled && cameraCount > 0 && ticks % 2 == 0) {
IMAQdxError imaqError = cameras[currentCamera]->GetFrame();
if (cameras[currentCamera]->frame != NULL) {
if (IMAQdxErrorSuccess == imaqError) {
int error = ImageProcessing::IVA_ProcessImage(cameras[currentCamera]->frame);
if (error == 1) { //success
if (overlay) {
int x, y;
Image *frame = cameras[currentCamera]->frame;
imaqGetImageSize(frame, &x, &y);
// Draw box around image. Note -- because the camera is pointing up a little, it's not a rectangle
Point leftStart, leftEnd, rightStart, rightEnd;
// It's just magic numbers here. See the "Feeder station image measurements.xlsx" spreadsheet
leftStart.x = x * 0.387;
leftStart.y = y * 0.258;
leftEnd.x = x * 0.381;
leftEnd.y = y * (0.258 + 0.584);
imaqDrawLineOnImage(frame, frame,
DrawMode::IMAQ_DRAW_VALUE, leftStart, leftEnd,
1.0);
rightStart.x = x * (0.387 + 0.241);
rightStart.y = y * 0.258;
rightEnd.x = x * (0.381 + 0.249);
rightEnd.y = y * (0.258 + 0.584);
imaqDrawLineOnImage(frame, frame,
DrawMode::IMAQ_DRAW_VALUE, rightStart, rightEnd,
1.0);
}
LCameraServer::GetInstance()->SetImage(
cameras[currentCamera]->frame);
}
}
}
}
}
IMAQdxError Camera::GetFrame() {
imaqError = IMAQdxErrorSuccess;
// grab an image,
if (session != ULONG_MAX && frame != NULL) {
if (false && firstTime) {
printf("Dumping modes\n");
DumpModes();
printf("Dumping attrs\n");
DumpAttrs();
}
IMAQdxGrab(session, frame, false, NULL);
if (imaqError != IMAQdxErrorSuccess) {
printf("IMAQdxConfigureGrab error (%s): %x\n",
camInfo[camera].InterfaceName, imaqError);
} else if (firstTime) {
int xRes, yRes;
imaqGetImageSize(frame, &xRes, &yRes);
printf("Camera: first image grab is %d x %d frame=%x\n", xRes, yRes,
(unsigned int) frame);
}
firstTime = false;
}
return imaqError;
}
/**
* Gets the width of an image.
* @return The width of the image in pixels.
*/
int ImageBase::GetWidth()
{
int width;
imaqGetImageSize(m_imaqImage, &width, NULL);
return width;
}
/**
* Gets the height of an image.
* @return The height of the image in pixels.
*/
int ImageBase::GetHeight()
{
int height;
imaqGetImageSize(m_imaqImage, NULL, &height);
return height;
}
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);
}
}
