| /*----------------------------------------------------------------------------*/ |
| /* Copyright (c) FIRST 2014. All Rights Reserved. |
| */ |
| /* Open Source Software - may be modified and shared by FRC teams. The code */ |
| /* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */ |
| /*----------------------------------------------------------------------------*/ |
| |
| #include "Vision/BinaryImage.h" |
| #include "WPIErrors.h" |
| #include <cstring> |
| |
| using namespace std; |
| |
| /** |
| * Get then number of particles for the image. |
| * @returns the number of particles found for the image. |
| */ |
| int BinaryImage::GetNumberParticles() { |
| int numParticles = 0; |
| int success = imaqCountParticles(m_imaqImage, 1, &numParticles); |
| wpi_setImaqErrorWithContext(success, "Error counting particles"); |
| return numParticles; |
| } |
| |
| /** |
| * Get a single particle analysis report. |
| * Get one (of possibly many) particle analysis reports for an image. |
| * @param particleNumber Which particle analysis report to return. |
| * @returns the selected particle analysis report |
| */ |
| ParticleAnalysisReport BinaryImage::GetParticleAnalysisReport( |
| int particleNumber) { |
| ParticleAnalysisReport par; |
| GetParticleAnalysisReport(particleNumber, &par); |
| return par; |
| } |
| |
| /** |
| * 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); |
| } |
| } |
| |
| /** |
| * Get an ordered vector of particles for the image. |
| * Create a vector of particle analysis reports sorted by size for an image. |
| * The vector contains the actual report structures. |
| * @returns a pointer to the vector of particle analysis reports. The caller |
| * must delete the |
| * vector when finished using it. |
| */ |
| vector<ParticleAnalysisReport> * |
| BinaryImage::GetOrderedParticleAnalysisReports() { |
| auto particles = new vector<ParticleAnalysisReport>; |
| int particleCount = GetNumberParticles(); |
| for (int particleIndex = 0; particleIndex < particleCount; particleIndex++) { |
| particles->push_back(GetParticleAnalysisReport(particleIndex)); |
| } |
| // TODO: This is pretty inefficient since each compare in the sort copies |
| // both reports being compared... do it manually instead... while we're |
| // at it, we should provide a version that allows a preallocated buffer of |
| // ParticleAnalysisReport structures |
| sort(particles->begin(), particles->end(), CompareParticleSizes); |
| return particles; |
| } |
| |
| /** |
| * Write a binary image to flash. |
| * Writes the binary image to flash on the cRIO for later inspection. |
| * @param fileName the name of the image file written to the flash. |
| */ |
| void BinaryImage::Write(const char *fileName) { |
| RGBValue colorTable[256]; |
| memset(colorTable, 0, sizeof(colorTable)); |
| colorTable[0].R = 0; |
| colorTable[1].R = 255; |
| colorTable[0].G = colorTable[1].G = 0; |
| colorTable[0].B = colorTable[1].B = 0; |
| colorTable[0].alpha = colorTable[1].alpha = 0; |
| imaqWriteFile(m_imaqImage, fileName, colorTable); |
| } |
| |
| /** |
| * Measure a single parameter for an image. |
| * Get the measurement for a single parameter about an image by calling the |
| * imaqMeasureParticle |
| * function for the selected parameter. |
| * @param particleNumber which particle in the set of particles |
| * @param whatToMeasure the imaq MeasurementType (what to measure) |
| * @param result the value of the measurement |
| * @returns false on failure, true on success |
| */ |
| bool BinaryImage::ParticleMeasurement(int particleNumber, |
| MeasurementType whatToMeasure, |
| int *result) { |
| double resultDouble; |
| bool success = |
| ParticleMeasurement(particleNumber, whatToMeasure, &resultDouble); |
| *result = (int)resultDouble; |
| return success; |
| } |
| |
| /** |
| * Measure a single parameter for an image. |
| * Get the measurement for a single parameter about an image by calling the |
| * imaqMeasureParticle |
| * function for the selected parameter. |
| * @param particleNumber which particle in the set of particles |
| * @param whatToMeasure the imaq MeasurementType (what to measure) |
| * @param result the value of the measurement |
| * @returns true on failure, false on success |
| */ |
| bool BinaryImage::ParticleMeasurement(int particleNumber, |
| MeasurementType whatToMeasure, |
| double *result) { |
| int success; |
| success = imaqMeasureParticle(m_imaqImage, particleNumber, 0, whatToMeasure, |
| result); |
| wpi_setImaqErrorWithContext(success, "Error measuring particle"); |
| return !StatusIsFatal(); |
| } |
| |
| // Normalizes to [-1,1] |
| double BinaryImage::NormalizeFromRange(double position, int range) { |
| return (position * 2.0 / (double)range) - 1.0; |
| } |
| |
| /** |
| * The compare helper function for sort. |
| * This function compares two particle analysis reports as a helper for the sort |
| * function. |
| * @param particle1 The first particle to compare |
| * @param particle2 the second particle to compare |
| * @returns true if particle1 is greater than particle2 |
| */ |
| bool BinaryImage::CompareParticleSizes(ParticleAnalysisReport particle1, |
| ParticleAnalysisReport particle2) { |
| // we want descending sort order |
| return particle1.particleToImagePercent > particle2.particleToImagePercent; |
| } |
| |
| BinaryImage *BinaryImage::RemoveSmallObjects(bool connectivity8, int erosions) { |
| auto result = new BinaryImage(); |
| int success = imaqSizeFilter(result->GetImaqImage(), m_imaqImage, |
| connectivity8, erosions, IMAQ_KEEP_LARGE, nullptr); |
| wpi_setImaqErrorWithContext(success, "Error in RemoveSmallObjects"); |
| return result; |
| } |
| |
| BinaryImage *BinaryImage::RemoveLargeObjects(bool connectivity8, int erosions) { |
| auto result = new BinaryImage(); |
| int success = imaqSizeFilter(result->GetImaqImage(), m_imaqImage, |
| connectivity8, erosions, IMAQ_KEEP_SMALL, nullptr); |
| wpi_setImaqErrorWithContext(success, "Error in RemoveLargeObjects"); |
| return result; |
| } |
| |
| BinaryImage *BinaryImage::ConvexHull(bool connectivity8) { |
| auto result = new BinaryImage(); |
| int success = |
| imaqConvexHull(result->GetImaqImage(), m_imaqImage, connectivity8); |
| wpi_setImaqErrorWithContext(success, "Error in convex hull operation"); |
| return result; |
| } |
| |
| BinaryImage *BinaryImage::ParticleFilter(ParticleFilterCriteria2 *criteria, |
| int criteriaCount) { |
| auto result = new BinaryImage(); |
| int numParticles; |
| ParticleFilterOptions2 filterOptions = {0, 0, 0, 1}; |
| int success = |
| imaqParticleFilter4(result->GetImaqImage(), m_imaqImage, criteria, |
| criteriaCount, &filterOptions, nullptr, &numParticles); |
| wpi_setImaqErrorWithContext(success, "Error in particle filter operation"); |
| return result; |
| } |