Now you can download a copy of these docs so you can use them offline! Download now
TrackAPI.cpp
1 /********************************************************************************
2 * Project : FIRST Motor Controller
3 * File Name : TrackAPI.cpp
4 * Contributors : ELF, DWD
5 * Creation Date : August 10, 2008
6 * Revision History : Source code & revision history maintained at sourceforge.WPI.edu
7 * File Description : Tracking Routines for FIRST Vision API
8 */
9 /*----------------------------------------------------------------------------*/
10 /* Copyright (c) FIRST 2008. All Rights Reserved. */
11 /* Open Source Software - may be modified and shared by FRC teams. The code */
12 /* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
13 /*----------------------------------------------------------------------------*/
14 
15 #include "string.h"
16 #include "vxWorks.h"
17 
18 #include "AxisCamera.h"
19 #include "FrcError.h"
20 #include "TrackAPI.h"
21 #include "VisionAPI.h"
22 
23 int TrackAPI_debugFlag = 0;
24 #define DPRINTF if(TrackAPI_debugFlag)dprintf
25 
32 bool InArea(Image* binaryImage, int particleIndex, Rect rect)
33 {
34  double position;
35 
36  imaqMeasureParticle(binaryImage, particleIndex, 0,
37  IMAQ_MT_BOUNDING_RECT_LEFT, &position);
38  if ( position < (rect.left ) ) return false; // outside left of rectangle?
39 
40  imaqMeasureParticle(binaryImage, particleIndex, 0,
41  IMAQ_MT_BOUNDING_RECT_TOP, &position);
42  if ( position < (rect.top ) ) return false; // outside top of rectangle ?
43 
44  imaqMeasureParticle(binaryImage, particleIndex, 0,
45  IMAQ_MT_BOUNDING_RECT_RIGHT, &position);
46  if (position > (rect.left + rect.width) ) return false; // outside right of rectangle ?
47 
48  imaqMeasureParticle(binaryImage, particleIndex, 0,
49  IMAQ_MT_BOUNDING_RECT_BOTTOM, &position);
50  if (position > (rect.top + rect.height) ) return false; // outside bottom of rectangle ?
51 
52  DPRINTF(LOG_INFO, "particle %i is in (%i %i) height %i width %i\n",
53  particleIndex, rect.left, rect.top, rect.height, rect.width);
54  return true;
55 }
56 
64 int GetLargestParticle(Image* binaryImage, int* largestParticleIndex)
65 { return GetLargestParticle(binaryImage, largestParticleIndex, IMAQ_NO_RECT); }
66 
67 int GetLargestParticle(Image* binaryImage, int* largestParticleIndex, Rect rect)
68 {
69  *largestParticleIndex = 0; // points to caller-provided variable
70 
71  /* determine number of particles in thresholded image */
72  int numParticles;
73  int success = frcCountParticles(binaryImage, &numParticles);
74  if ( !success ) { return success; }
75 
76  /* if no particles found we can quit here */
77  if (numParticles == 0) { return 0; } // unsuccessful if zero particles found
78 
79  // find the largest particle
80  double largestParticleArea = 0;
81  double particleArea;
82  for (int i = 0; i < numParticles; ++i) {
83  success = imaqMeasureParticle(binaryImage, i, 0, IMAQ_MT_AREA, &particleArea);
84  if ( !success ) { return success; }
85  if (particleArea > largestParticleArea) {
86  // see if is in the right area
87  if ( InArea(binaryImage, i, rect) ) {
88  largestParticleArea = particleArea;
89  *largestParticleIndex = i; // return index to caller
90  }
91  }
92  }
93 
94  return success;
95 }
96 
103 int FindColor(FrcHue color, ParticleAnalysisReport* trackReport)
104 {
105  int success = 0; // return: 0 = error
106 
107  /* track color */
108  // use ACTIVE_LIGHT or WHITE_LIGHT for brightly lit objects
109  TrackingThreshold td = GetTrackingData(color, PASSIVE_LIGHT);
110 
111  success = FindColor(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, trackReport);
112  if ( !success ) {
113  DPRINTF (LOG_INFO, "did not find color - errorCode= %i",GetLastVisionError());
114  return success;
115  }
116 
117  //PrintReport(par);
118 
119  /* set an image quality restriction */
120  if (trackReport->particleToImagePercent < PARTICLE_TO_IMAGE_PERCENT) {
121  imaqSetError(ERR_PARTICLE_TOO_SMALL, __FUNCTION__);
122  success = 0;
123  }
124  return success;
125 }
126 
133 int FindColor(const Range* hueRange, ParticleAnalysisReport *trackReport)
134 { return FindColor(hueRange, DEFAULT_SATURATION_THRESHOLD, trackReport); }
135 
143 int FindColor(const Range* hueRange, int minSaturation, ParticleAnalysisReport *trackReport)
144 {
145  Range satRange;
146  satRange.minValue = minSaturation;
147  satRange.maxValue = 255;
148  Range lumRange;
149  lumRange.minValue = 0;
150  lumRange.maxValue = 255;
151  ColorMode cmode = IMAQ_HSL;
152  return FindColor(cmode, hueRange, &satRange, &lumRange, trackReport);
153 }
154 
164 int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range,
165  const Range* plane3Range, ParticleAnalysisReport *trackReport)
166 {
167  return FindColor(mode, plane1Range, plane2Range, plane3Range, trackReport, NULL);
168 }
169 
180 int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range,
181  const Range* plane3Range, ParticleAnalysisReport *trackReport,
182  ColorReport *colorReport)
183 {
184  return FindColor(mode, plane1Range, plane2Range, plane3Range, trackReport,
185  NULL, IMAQ_NO_RECT);
186 }
187 
199 int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range,
200  const Range* plane3Range, ParticleAnalysisReport *trackReport,
201  ColorReport *colorReport, Rect rect)
202 {
203  int errorCode = 0;
204  int success = 0;
205 
206  /* create an image object */
207  Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL);
208  if (!cameraImage) { return success; }
209 
210  /* get image from camera - if the camera has not finished initializing,
211  * this will fail
212  */
213  double imageTime;
214  success = GetImage(cameraImage, &imageTime);
215  if (!success){
216  DPRINTF(LOG_INFO, "No camera Image available Error = %i %s",
217  errorCode, GetVisionErrorText(errorCode));
218  frcDispose(cameraImage);
219  imaqSetError(errorCode, __FUNCTION__); //reset error code for the caller
220  return success;
221  }
222 
223  /* save a copy of the image to another image for color thresholding later */
224  Image* histImage = frcCreateImage(IMAQ_IMAGE_HSL);
225  if (!histImage) { frcDispose(cameraImage); return success; }
226  success = frcCopyImage(histImage,cameraImage);
227  if ( !success ) {
228  errorCode = GetLastVisionError();
229  frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
230  return success;
231  }
232 
233  /* Color threshold the image */
234  success = frcColorThreshold(cameraImage, cameraImage, mode, plane1Range, plane2Range, plane3Range);
235  if ( !success ) {
236  errorCode = GetLastVisionError();
237  DPRINTF (LOG_DEBUG, "Error = %i %s ", errorCode, GetVisionErrorText(errorCode));
238  frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
239  return success;
240  }
241 
242  int largestParticleIndex = 0;
243  success = GetLargestParticle(cameraImage, &largestParticleIndex, rect );
244  if ( !success ) {
245  errorCode = GetLastVisionError();
246  DPRINTF (LOG_DEBUG, "Error after GetLargestParticle = %i %s ", errorCode, GetVisionErrorText(errorCode));
247  frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
248  imaqSetError(ERR_COLOR_NOT_FOUND, __FUNCTION__);
249  return success;
250  }
251  DPRINTF(LOG_INFO, "largestParticleIndex = %i\n", largestParticleIndex);
252 
253  /* Particles were found */
254  /*
255  * Fill in report information for largest particle found
256  */
257  success = frcParticleAnalysis(cameraImage, largestParticleIndex, trackReport);
258  trackReport->imageTimestamp = imageTime;
259 
260  /* clean up */
261  if (!success) {frcDispose(__FUNCTION__,cameraImage,histImage,NULL); return success;}
262 
263  /* particle color statistics */
264  /* only if a color report requested */
265  if (colorReport != NULL)
266  {
267  /* first filter out the other particles */
268  ParticleFilterCriteria2 criteria;
269  ParticleFilterOptions* options = NULL;
270  Rect rect;
271  int numParticles;
272  success = frcParticleFilter(cameraImage, cameraImage, &criteria, 1, options,
273  rect, &numParticles);
274  if ( !success ) {
275  DPRINTF(LOG_INFO, "frcParticleFilter errorCode %i", GetLastVisionError());
276  }
277 
278  /* histogram the original image using the thresholded image as a mask */
279  int numClasses = 10; //how many classes?
280  ColorHistogramReport* chrep = imaqColorHistogram2(histImage, numClasses, IMAQ_HSL,
281  NULL, cameraImage);
282  if (chrep == NULL) {
283  DPRINTF(LOG_INFO, "NULL Color Histogram");
284  errorCode = GetLastVisionError();
285  } else {
286  colorReport->particleHueMax = chrep->plane1.max;
287  colorReport->particleHueMin = chrep->plane1.min;
288  colorReport->particleHueMean = chrep->plane1.mean;
289  colorReport->particleSatMax = chrep->plane2.max;
290  colorReport->particleSatMin = chrep->plane2.min;
291  colorReport->particleSatMean = chrep->plane2.mean;
292  colorReport->particleLumMax = chrep->plane3.max;
293  colorReport->particleLumMin = chrep->plane3.min;
294  colorReport->particleLumMean = chrep->plane3.mean;
295  colorReport->numberParticlesFound = numParticles;
296  frcDispose(chrep);
297  }
298  }
299 
300  /* clean up */
301  frcDispose(__FUNCTION__,cameraImage,histImage,NULL);
302 
303  return success;
304 }
305 
306 
319 TrackingThreshold GetTrackingData(FrcHue hue, FrcLight light)
320 {
321  TrackingThreshold trackingData;
322 
323  //set saturation & luminance
324  switch (light) {
325  default:
326  case FLUORESCENT:
327  trackingData.saturation.minValue = 100;
328  trackingData.saturation.maxValue = 255;
329  trackingData.luminance.minValue = 40;
330  trackingData.luminance.maxValue = 255;
331  if (hue == GREEN) trackingData.luminance.minValue = 100;
332  if (hue == PINK) trackingData.saturation.minValue = 80;
333  if (hue == PINK) trackingData.luminance.minValue = 60;
334  if (hue == PINK) trackingData.luminance.maxValue = 155;
335  break;
336  case PASSIVE_LIGHT:
337  trackingData.saturation.minValue = 50;
338  trackingData.saturation.maxValue = 255;
339  trackingData.luminance.minValue = 20;
340  trackingData.luminance.maxValue = 255;
341  break;
342  case BRIGHT_LIGHT:
343  trackingData.saturation.minValue = 0;
344  trackingData.saturation.maxValue = 100;
345  trackingData.luminance.minValue = 100;
346  trackingData.luminance.maxValue = 255;
347  break;
348  case ACTIVE_LIGHT:
349  trackingData.saturation.minValue = 0;
350  trackingData.saturation.maxValue = 50;
351  trackingData.luminance.minValue = 150;
352  trackingData.luminance.maxValue = 255;
353  break;
354  case WHITE_LIGHT:
355  trackingData.saturation.minValue = 0;
356  trackingData.saturation.maxValue = 20;
357  trackingData.luminance.minValue = 200;
358  trackingData.luminance.maxValue = 255;
359  break;
360  }
361 
362  //set hue
363  switch (hue){
364  default:
365  case WHITE:
366  strcpy (trackingData.name, "WHITE");
367  trackingData.hue.minValue = 0;
368  trackingData.hue.maxValue = 255;
369  break;
370  case ORANGE:
371  strcpy (trackingData.name, "ORANGE");
372  trackingData.hue.minValue = 5;
373  trackingData.hue.maxValue = 25;
374  break;
375  case YELLOW:
376  strcpy (trackingData.name, "YELLOW");
377  trackingData.hue.minValue = 30;
378  trackingData.hue.maxValue = 50;
379  break;
380  case GREEN:
381  strcpy (trackingData.name, "GREEN");
382  if (light == FLUORESCENT) {
383  trackingData.hue.minValue = 60;
384  trackingData.hue.maxValue = 110;
385  } else {
386  trackingData.hue.minValue = 90;
387  trackingData.hue.maxValue = 125;
388  }
389  break;
390  case BLUE:
391  strcpy (trackingData.name, "BLUE");
392  trackingData.hue.minValue = 140;
393  trackingData.hue.maxValue = 170;
394  break;
395  case PURPLE:
396  strcpy (trackingData.name, "PURPLE");
397  trackingData.hue.minValue = 180;
398  trackingData.hue.maxValue = 200;
399  break;
400  case PINK:
401  strcpy (trackingData.name, "PINK");
402  trackingData.hue.minValue = 210;
403  trackingData.hue.maxValue = 250;
404  break;
405  case RED:
406  strcpy (trackingData.name, "RED");
407  trackingData.hue.minValue = 240;
408  trackingData.hue.maxValue = 255;
409  break;
410  }
411  return(trackingData);
412 }
413 
414 
419 void PrintReport(ParticleAnalysisReport* myReport)
420 {
421  dprintf(LOG_INFO, "particle analysis:\n %s%i %s%i\n %s%lf\n %s%i %s%i\n %s%g %s%g\n %s%g\n %s%i %s%i\n %s%i %s%i\n",
422  "imageHeight = ", myReport->imageHeight,
423  "imageWidth = ", myReport->imageWidth,
424  "imageTimestamp = ", myReport->imageTimestamp,
425  "center_mass_x = ", myReport->center_mass_x,
426  "center_mass_y = ", myReport->center_mass_y,
427  "center_mass_x_normalized = ", myReport->center_mass_x_normalized,
428  "center_mass_y_normalized = ", myReport->center_mass_y_normalized,
429  "particleArea = ", myReport->particleArea,
430  "boundingRectangleTop = ", myReport->boundingRect.top,
431  "boundingRectangleLeft = ", myReport->boundingRect.left,
432  "boundingRectangleHeight = ", myReport->boundingRect.height,
433  "boundingRectangleWidth = ", myReport->boundingRect.width);
434 
435  dprintf(LOG_INFO, "quality statistics: \n %s%g %s%g \n",
436  "particleToImagePercent = ", myReport->particleToImagePercent,
437  "particleQuality = ", myReport->particleQuality);
438 }
439 
444 void PrintReport(ColorReport* myReport)
445 {
446  dprintf(LOG_INFO, "particle ranges for %i particles: ",
447  "numberParticlesFound = ", myReport->numberParticlesFound);
448  ;
449  dprintf(LOG_INFO, "\n %s%f %s%f %s%f\n %s%f %s%f %s%f\n %s%f %s%f %s%f\n -------",
450  "particleHueMax = ", myReport->particleHueMax,
451  "particleHueMin = ", myReport->particleHueMin,
452  "particleHueMean = ", myReport->particleHueMean,
453  "particleSatMax = ", myReport->particleSatMax,
454  "particleSatMin = ", myReport->particleSatMin,
455  "particleSatMean = ", myReport->particleSatMean,
456  "particleLumMax = ", myReport->particleLumMax,
457  "particleLumMin = ", myReport->particleLumMin,
458  "particleLumMean = ", myReport->particleLumMean);
459 
460 }
461 
466 void PrintReport(TrackingThreshold* myReport)
467 {
468  dprintf(LOG_INFO, "name of color: %s", myReport->name);
469 
470  dprintf(LOG_INFO, "\n %s%i %s%i\n %s%i %s%i\n %s%i %s%i\n -------",
471  "hueMin = ", myReport->hue.minValue,
472  "hueMax = ", myReport->hue.maxValue,
473  "satMin = ", myReport->saturation.minValue,
474  "satMax = ", myReport->saturation.maxValue,
475  "lumMin = ", myReport->luminance.minValue,
476  "lumMax = ", myReport->luminance.maxValue );
477 
478 }
479 
480 
481 
ParticleFilterCriteria2_struct
Definition: nivision.h:3910
ColorReport_struct
Definition: VisionAPI.h:59
Range_struct
Definition: nivision.h:4381
Rect_struct
Definition: nivision.h:3971
TrackingThreshold_struct
Definition: TrackAPI.h:30
ParticleFilterOptions_struct
Definition: nivision.h:3233
ColorHistogramReport_struct
Definition: nivision.h:4293
ParticleAnalysisReport_struct
Definition: VisionAPI.h:35
Generated on Sat Apr 26 2014 12:26:45 for WPILibC++ by doxygen 1.8.6