/
Vision.cpp
332 lines (291 loc) · 14.2 KB
/
Vision.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
#include "WPILib.h"
#include "Vision/RGBImage.h"
#include "Vision/BinaryImage.h"
#include "Math.h"
/**
* Sample program to use NIVision to find rectangles in the scene that are illuminated
* by a LED ring light (similar to the model from FIRSTChoice). The camera sensitivity
* is set very low so as to only show light sources and remove any distracting parts
* of the image.
*
* The CriteriaCollection is the set of criteria that is used to filter the set of
* rectangles that are detected. In this example we're looking for rectangles with
* a minimum width of 30 pixels and maximum of 400 pixels.
*
* The algorithm first does a color threshold operation that only takes objects in the
* scene that have a bright green color component. Then a small object filter
* removes small particles that might be caused by green reflection scattered from other
* parts of the scene. Finally all particles are scored on rectangularity, and aspect ratio,
* to determine if they are a target.
*
* Look in the VisionImages directory inside the project that is created for the sample
* images.
*/
//Camera constants used for distance calculation
#define Y_IMAGE_RES 480 //X Image resolution in pixels, should be 120, 240 or 480
#define VIEW_ANGLE 49 //Axis M1013
//#define VIEW_ANGLE 41.7 //Axis 206 camera
//#define VIEW_ANGLE 37.4 //Axis M1011 camera
#define PI 3.141592653
//Score limits used for target identification
#define RECTANGULARITY_LIMIT 40
#define ASPECT_RATIO_LIMIT 55
//Score limits used for hot target determination
#define TAPE_WIDTH_LIMIT 50
#define VERTICAL_SCORE_LIMIT 50
#define LR_SCORE_LIMIT 50
//Minimum area of particles to be considered
#define AREA_MINIMUM 150
//Maximum number of particles to process
#define MAX_PARTICLES 8
//Structure to represent the scores for the various tests used for target identification
struct Scores {
double rectangularity;
double aspectRatioVertical;
double aspectRatioHorizontal;
};
struct TargetReport {
int verticalIndex;
int horizontalIndex;
bool Hot;
double totalScore;
double leftScore;
double rightScore;
double tapeWidthScore;
double verticalScore;
};
/**
* Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise
* linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2
*/
inline double ratioToScore(double ratio)
{
return (max(0, min(100*(1-fabs(1-ratio)), 100)));
}
/**
* Computes the estimated distance to a target using the height 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
* @return The estimated distance to the target in feet.
*/
inline double computeDistance (BinaryImage *image, ParticleAnalysisReport *report) {
double rectLong, height;
int targetHeight;
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
//using the smaller of the estimated rectangle long side and the bounding rectangle height results in better performance
//on skewed rectangles
height = min(report->boundingRect.height, rectLong);
targetHeight = 32;
return Y_IMAGE_RES * targetHeight / (height * 12 * 2 * tan(VIEW_ANGLE*PI/(180*2)));
}
/**
* Computes a score (0-100) comparing the aspect ratio to the ideal aspect ratio for the target. This method uses
* the equivalent rectangle sides to determine aspect ratio as it performs better as the target gets skewed by moving
* to the left or right. The equivalent rectangle is the rectangle with sides x and y where particle area= x*y
* and particle perimeter= 2x+2y
*
* @param image The image containing the particle to score, needed to perform additional measurements
* @param report The Particle Analysis Report for the particle, used for the width, height, and particle number
* @param outer Indicates whether the particle aspect ratio should be compared to the ratio for the inner target or the outer
* @return The aspect ratio score (0-100)
*/
inline double scoreAspectRatio(BinaryImage *image, ParticleAnalysisReport *report, bool vertical){
double rectLong, rectShort, idealAspectRatio, aspectRatio;
idealAspectRatio = vertical ? (4.0/32) : (23.5/4); //Vertical reflector 4" wide x 32" tall, horizontal 23.5" wide x 4" tall
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &rectLong);
imaqMeasureParticle(image->GetImaqImage(), report->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &rectShort);
//Divide width by height to measure aspect ratio
if(report->boundingRect.width > report->boundingRect.height){
//particle is wider than it is tall, divide long by short
aspectRatio = ratioToScore(((rectLong/rectShort)/idealAspectRatio));
} else {
//particle is taller than it is wide, divide short by long
aspectRatio = ratioToScore(((rectShort/rectLong)/idealAspectRatio));
}
return aspectRatio; //force to be in range 0-100
}
/**
* Compares scores to defined limits and returns true if the particle appears to be a target
*
* @param scores The structure containing the scores to compare
* @param vertical True if the particle should be treated as a vertical target, false to treat it as a horizontal target
*
* @return True if the particle meets all limits, false otherwise
*/
inline bool scoreCompare(Scores scores, bool vertical){
bool isTarget = true;
isTarget &= scores.rectangularity > RECTANGULARITY_LIMIT;
if(vertical){
isTarget &= scores.aspectRatioVertical > ASPECT_RATIO_LIMIT;
} else {
isTarget &= scores.aspectRatioHorizontal > ASPECT_RATIO_LIMIT;
}
return isTarget;
}
/**
* Computes a score (0-100) estimating how rectangular the particle is by comparing the area of the particle
* to the area of the bounding box surrounding it. A perfect rectangle would cover the entire bounding box.
*
* @param report The Particle Analysis Report for the particle to score
* @return The rectangularity score (0-100)
*/
inline double scoreRectangularity(ParticleAnalysisReport *report){
if(report->boundingRect.width*report->boundingRect.height !=0){
return 100*report->particleArea/(report->boundingRect.width*report->boundingRect.height);
} else {
return 0;
}
}
/**
* Takes in a report on a target and compares the scores to the defined score limits to evaluate
* if the target is a hot target or not.
*
* Returns True if the target is hot. False if it is not.
*/
inline bool hotOrNot(TargetReport target)
{
bool isHot = true;
isHot &= target.tapeWidthScore >= TAPE_WIDTH_LIMIT;
isHot &= target.verticalScore >= VERTICAL_SCORE_LIMIT;
isHot &= (target.leftScore > LR_SCORE_LIMIT) | (target.rightScore > LR_SCORE_LIMIT);
return isHot;
}
inline void processTaskFunc(UINT32 hotGoalPtr...)
{
bool hotGoal = (bool *) hotGoalPtr;
Scores *scores;
TargetReport target;
int verticalTargets[MAX_PARTICLES];
int horizontalTargets[MAX_PARTICLES];
int verticalTargetCount, horizontalTargetCount;
Threshold threshold(0, 255, 0, 255, 220, 255); //HSV threshold criteria, ranges are in that order ie. Hue is 60-100
ParticleFilterCriteria2 criteria[] = {
{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
}; //Particle filter criteria, used to filter out small particles
//AxisCamera &camera = AxisCamera::GetInstance(); //To use the Axis camera uncomment this line
/**
* Do the image capture with the camera and apply the algorithm described above. This
* sample will either get images from the camera or from an image file stored in the top
* level directory in the flash memory on the cRIO. The file name in this case is "testImage.jpg"
*/
ColorImage *image;
image = new RGBImage("/testImage.jpg"); // get the sample image from the cRIO flash
//image = camera.GetImage(); //To get the images from the camera comment the line above and uncomment this one
BinaryImage *thresholdImage = image->ThresholdHSV(threshold); // get just the green target pixels
//thresholdImage->Write("/threshold.bmp");
BinaryImage *filteredImage = thresholdImage->ParticleFilter(criteria, 1); //Remove small particles
//filteredImage->Write("Filtered.bmp");
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports(); //get a particle analysis report for each particle
verticalTargetCount = horizontalTargetCount = 0;
//Iterate through each particle, scoring it and determining whether it is a target or not
if(reports->size() > 0)
{
scores = new Scores[reports->size()];
for (unsigned int i = 0; i < MAX_PARTICLES && i < reports->size(); i++) {
ParticleAnalysisReport *report = &(reports->at(i));
//Score each particle on rectangularity and aspect ratio
scores[i].rectangularity = scoreRectangularity(report);
scores[i].aspectRatioVertical = scoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioHorizontal = scoreAspectRatio(filteredImage, report, false);
//Check if the particle is a horizontal target, if not, check if it's a vertical target
if(scoreCompare(scores[i], false))
{
printf("particle: %d is a Horizontal Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
horizontalTargets[horizontalTargetCount++] = i; //Add particle to target array and increment count
} else if (scoreCompare(scores[i], true)) {
printf("particle: %d is a Vertical Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
verticalTargets[verticalTargetCount++] = i; //Add particle to target array and increment count
} else {
printf("particle: %d is not a Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
}
printf("Scores rect: %f ARvert: %f \n", scores[i].rectangularity, scores[i].aspectRatioVertical);
printf("ARhoriz: %f \n", scores[i].aspectRatioHorizontal);
}
//Zero out scores and set verticalIndex to first target in case there are no horizontal targets
target.totalScore = target.leftScore = target.rightScore = target.tapeWidthScore = target.verticalScore = 0;
target.verticalIndex = verticalTargets[0];
for (int i = 0; i < verticalTargetCount; i++)
{
ParticleAnalysisReport *verticalReport = &(reports->at(verticalTargets[i]));
for (int j = 0; j < horizontalTargetCount; j++)
{
ParticleAnalysisReport *horizontalReport = &(reports->at(horizontalTargets[j]));
double horizWidth, horizHeight, vertWidth, leftScore, rightScore, tapeWidthScore, verticalScore, total;
//Measure equivalent rectangle sides for use in score calculation
imaqMeasureParticle(filteredImage->GetImaqImage(), horizontalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &horizWidth);
imaqMeasureParticle(filteredImage->GetImaqImage(), verticalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &vertWidth);
imaqMeasureParticle(filteredImage->GetImaqImage(), horizontalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &horizHeight);
//Determine if the horizontal target is in the expected location to the left of the vertical target
leftScore = ratioToScore(1.2*(verticalReport->boundingRect.left - horizontalReport->center_mass_x)/horizWidth);
//Determine if the horizontal target is in the expected location to the right of the vertical target
rightScore = ratioToScore(1.2*(horizontalReport->center_mass_x - verticalReport->boundingRect.left - verticalReport->boundingRect.width)/horizWidth);
//Determine if the width of the tape on the two targets appears to be the same
tapeWidthScore = ratioToScore(vertWidth/horizHeight);
//Determine if the vertical location of the horizontal target appears to be correct
verticalScore = ratioToScore(1-(verticalReport->boundingRect.top - horizontalReport->center_mass_y)/(4*horizHeight));
total = leftScore > rightScore ? leftScore:rightScore;
total += tapeWidthScore + verticalScore;
//If the target is the best detected so far store the information about it
if(total > target.totalScore)
{
target.horizontalIndex = horizontalTargets[j];
target.verticalIndex = verticalTargets[i];
target.totalScore = total;
target.leftScore = leftScore;
target.rightScore = rightScore;
target.tapeWidthScore = tapeWidthScore;
target.verticalScore = verticalScore;
}
}
//Determine if the best target is a Hot target
target.Hot = hotOrNot(target);
}
if(verticalTargetCount > 0)
{
//Information about the target is contained in the "target" structure
//To get measurement information such as sizes or locations use the
//horizontal or vertical index to get the particle report as shown below
ParticleAnalysisReport *distanceReport = &(reports->at(target.verticalIndex));
double distance = computeDistance(filteredImage, distanceReport);
if(target.Hot)
{
printf("Hot target located \n");
printf("Distance: %f \n", distance);
hotGoal = true;
} else {
printf("No hot target present \n");
printf("Distance: %f \n", distance);
hotGoal = false;
}
}
}
// be sure to delete images after using them
delete filteredImage;
delete thresholdImage;
delete image;
//delete allocated reports and Scores objects also
delete scores;
delete reports;
}
class Vision
{
Task processTask;
bool hotGoal;
public:
Vision () :
processTask ("Process", (FUNCPTR) processTaskFunc)
{ }
/**
* Image processing code to identify 2013 Vision targets
*/
void Process(void)
{
processTask.Start((UINT32) &hotGoal);
}
bool GoalIsHot()
{
return hotGoal;
}
};