Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/Athena/src/Vision/BinaryImage.cpp b/wpilibc/Athena/src/Vision/BinaryImage.cpp
new file mode 100644
index 0000000..ea14cbc
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/BinaryImage.cpp
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+}