1
|
#include <iostream>
|
2
|
#include <stdio.h>
|
3
|
#include <opencv2/opencv.hpp>
|
4
|
using namespace std;
|
5
|
using namespace cv;
|
6
|
|
7
|
#define WIDTH 320
|
8
|
#define HEIGHT 240
|
9
|
|
10
|
double width = 320;
|
11
|
double height = 240;
|
12
|
|
13
|
double im_width;
|
14
|
double im_height;
|
15
|
|
16
|
struct blob_info {
|
17
|
uchar blobCnt;
|
18
|
uint *blobSize;
|
19
|
uint *AvgX;
|
20
|
uint *AvgY;
|
21
|
};
|
22
|
|
23
|
blob_info *capture_image(uchar debug,VideoCapture& capture);
|
24
|
void get_image(Mat imgInput);
|
25
|
vector<Mat> convert_to_hsv(Mat imgBGR);
|
26
|
void write_hsv_images(vector<Mat> hsv);
|
27
|
void write_bgr_images(vector<Mat> bgr);
|
28
|
Mat negate_image(Mat im);
|
29
|
Mat threshold_image(Mat im,uchar threshold,uchar maxVal);
|
30
|
Mat get_intersection(Mat imA, Mat imB);
|
31
|
Mat gaussian_blur(Mat im, uchar bwidth);
|
32
|
blob_info *blob_detect(Mat img);
|
33
|
void print_blobs(blob_info *Blobs);
|
34
|
void sort_blobs(blob_info *blobs);
|
35
|
uint dist1(int x1, int x2, int y1, int y2);
|
36
|
|
37
|
|
38
|
uchar is_start_condition(blob_info *BlobInfo,uchar debug);
|
39
|
uchar is_first_nibble(blob_info *StartBlob, blob_info *FirstBlob,uchar debug);
|
40
|
uchar is_second_nibble(blob_info *StartBlob, blob_info *SecondBlob);
|
41
|
uchar get_nibble(blob_info *StartBlob, blob_info *ImgBlob);
|
42
|
|
43
|
int main(){
|
44
|
uchar debug = 0;
|
45
|
|
46
|
VideoCapture cap(-1);
|
47
|
cap.set(CV_CAP_PROP_FRAME_WIDTH,width);
|
48
|
cap.set(CV_CAP_PROP_FRAME_HEIGHT,height);
|
49
|
im_width = cap.get(CV_CAP_PROP_FRAME_WIDTH);
|
50
|
im_height = cap.get(CV_CAP_PROP_FRAME_HEIGHT);
|
51
|
|
52
|
blob_info *start_blobs;
|
53
|
while(1){
|
54
|
start_blobs = capture_image(debug,cap);
|
55
|
|
56
|
uchar ctr = 0;
|
57
|
blob_info *blobs = start_blobs;
|
58
|
while(1){
|
59
|
if(is_start_condition(blobs,debug)){
|
60
|
if(debug)
|
61
|
printf("Found start condition on frame %d\n",ctr);
|
62
|
break;
|
63
|
}
|
64
|
blobs = capture_image(debug,cap);
|
65
|
ctr++;
|
66
|
}
|
67
|
|
68
|
ctr = 0;
|
69
|
blob_info *start_blob = blobs;
|
70
|
blob_info *nibble_blob = capture_image(debug,cap);
|
71
|
while(1){
|
72
|
if(is_first_nibble(start_blob, nibble_blob,debug)){
|
73
|
if(debug)
|
74
|
printf("Found first nibble on frame %d\n",ctr);
|
75
|
break;
|
76
|
}
|
77
|
nibble_blob = capture_image(debug,cap);
|
78
|
ctr++;
|
79
|
}
|
80
|
|
81
|
|
82
|
uchar nib = get_nibble(start_blob, nibble_blob);
|
83
|
uchar RXbyte = nib << 4;
|
84
|
if(debug)
|
85
|
printf("Upper Nibble:%x\n",nib);
|
86
|
ctr = 0;
|
87
|
nibble_blob = capture_image(debug,cap);
|
88
|
while(1){
|
89
|
if(is_second_nibble(start_blob, nibble_blob)){
|
90
|
if(debug)
|
91
|
printf("Found second nibble on frame %d\n",ctr);
|
92
|
break;
|
93
|
}
|
94
|
nibble_blob = capture_image(debug,cap);
|
95
|
ctr++;
|
96
|
}
|
97
|
|
98
|
nib = get_nibble(start_blob, nibble_blob);
|
99
|
if(debug)
|
100
|
printf("Lower Nibble:%x\n",nib);
|
101
|
RXbyte |= nib;
|
102
|
printf("Received:%x\n",RXbyte);
|
103
|
}
|
104
|
return 0;
|
105
|
}
|
106
|
|
107
|
|
108
|
blob_info *capture_image(uchar debug,VideoCapture& capture){
|
109
|
|
110
|
Mat imgBGR;
|
111
|
if(!capture.isOpened()){
|
112
|
cout << "Camera not Working" << endl;
|
113
|
}
|
114
|
capture >> imgBGR;
|
115
|
|
116
|
|
117
|
vector<Mat> bgr;
|
118
|
split(imgBGR, bgr);
|
119
|
if(debug){
|
120
|
imwrite("imgB.png",bgr[0]);
|
121
|
imwrite("imgG.png",bgr[1]);
|
122
|
imwrite("imgR.png",bgr[2]);
|
123
|
}
|
124
|
|
125
|
|
126
|
Mat imgB_T = Mat::zeros(bgr[0].size(),bgr[0].type());
|
127
|
Mat imgG_T = Mat::zeros(bgr[1].size(),bgr[1].type());
|
128
|
Mat imgR_T = Mat::zeros(bgr[2].size(),bgr[2].type());
|
129
|
threshold(bgr[0],imgB_T,250,255,THRESH_BINARY);
|
130
|
threshold(bgr[1],imgG_T,250,255,THRESH_BINARY);
|
131
|
threshold(bgr[2],imgR_T,250,255,THRESH_BINARY);
|
132
|
|
133
|
if(debug){
|
134
|
imwrite("imgBT.png",imgB_T);
|
135
|
imwrite("imgGT.png",imgG_T);
|
136
|
imwrite("imgRT.png",imgR_T);
|
137
|
}
|
138
|
|
139
|
|
140
|
Mat B_G = Mat::zeros(imgB_T.size(),imgB_T.type());
|
141
|
bitwise_and(imgB_T,imgG_T,B_G);
|
142
|
Mat B_G_R = Mat::zeros(imgB_T.size(),imgB_T.type());
|
143
|
bitwise_and(B_G,imgR_T,B_G_R);
|
144
|
|
145
|
if(debug)
|
146
|
imwrite("imgBGR_I.png",B_G_R);
|
147
|
|
148
|
|
149
|
Mat BGR_B = Mat::zeros(B_G_R.size(),B_G_R.type());
|
150
|
GaussianBlur(B_G_R,BGR_B,Size(5,5),0,0);
|
151
|
|
152
|
if(debug)
|
153
|
imwrite("imgBGR_B.png",BGR_B);
|
154
|
|
155
|
|
156
|
Mat BGR_B_T = Mat::zeros(BGR_B.size(),BGR_B.type());
|
157
|
threshold(BGR_B,BGR_B_T,250,1,THRESH_BINARY);
|
158
|
|
159
|
if(debug)
|
160
|
imwrite("imgBGR_BT.png",BGR_B_T*255);
|
161
|
|
162
|
|
163
|
blob_info *Blobs = blob_detect(BGR_B_T);
|
164
|
sort_blobs(Blobs);
|
165
|
if(debug)
|
166
|
print_blobs(Blobs);
|
167
|
return Blobs;
|
168
|
}
|
169
|
|
170
|
|
171
|
blob_info *blob_detect(Mat img){
|
172
|
CV_Assert(img.channels() == 1);
|
173
|
uchar ctr = 0;
|
174
|
uchar equiv_size = 64;
|
175
|
|
176
|
uchar *equiv;
|
177
|
equiv = new uchar[equiv_size];
|
178
|
for(int i=0; i<equiv_size;i++){
|
179
|
equiv[i] = i;
|
180
|
}
|
181
|
|
182
|
|
183
|
for(int j=0; j<img.rows;j++){
|
184
|
for(int i=0; i<img.cols;i++){
|
185
|
if(img.at<uchar>(j,i)){
|
186
|
uchar Pup = 0;
|
187
|
uchar Pleft = 0;
|
188
|
|
189
|
if(j > 0){
|
190
|
Pup = img.at<uchar>(j-1,i);
|
191
|
}
|
192
|
if(i > 0){
|
193
|
Pleft = img.at<uchar>(j,i-1);
|
194
|
}
|
195
|
if((Pup == 0) && (Pleft == 0)){
|
196
|
|
197
|
ctr++;
|
198
|
img.at<uchar>(j,i) = ctr;
|
199
|
} else if((Pup == 0) && (Pleft != 0)){
|
200
|
|
201
|
CV_Assert(i > 0);
|
202
|
img.at<uchar>(j,i) = Pleft;
|
203
|
} else if((Pup != 0) && (Pleft == 0)){
|
204
|
|
205
|
CV_Assert(j > 0);
|
206
|
img.at<uchar>(j,i) = Pup;
|
207
|
} else {
|
208
|
CV_Assert(Pup > 0);
|
209
|
CV_Assert(Pleft > 0);
|
210
|
if(Pup == Pleft)
|
211
|
img.at<uchar>(j,i) = Pup;
|
212
|
else {
|
213
|
CV_Assert(Pup < equiv_size);
|
214
|
CV_Assert(Pleft < equiv_size);
|
215
|
if(Pleft < Pup){
|
216
|
equiv[Pup] = Pleft;
|
217
|
img.at<uchar>(j,i) = Pleft;
|
218
|
} else {
|
219
|
equiv[Pleft] = Pup;
|
220
|
img.at<uchar>(j,i) = Pup;
|
221
|
}
|
222
|
}
|
223
|
}
|
224
|
}
|
225
|
}
|
226
|
}
|
227
|
|
228
|
int *blobCnt;
|
229
|
blobCnt = new int[equiv_size];
|
230
|
for(int i=0; i<equiv_size;i++){
|
231
|
blobCnt[i] = 0;
|
232
|
}
|
233
|
|
234
|
for(int i = 0; i < equiv_size; i++){
|
235
|
int ptr = i;
|
236
|
while(equiv[ptr] != ptr){
|
237
|
ptr = equiv[ptr];
|
238
|
}
|
239
|
equiv[i] = ptr;
|
240
|
}
|
241
|
|
242
|
for(int j=0; j<img.rows;j++){
|
243
|
for(int i=0; i<img.cols;i++){
|
244
|
uchar Pval = img.at<uchar>(j,i);
|
245
|
if(Pval){
|
246
|
uchar newPval = equiv[Pval];
|
247
|
if(Pval != newPval){
|
248
|
img.at<uchar>(j,i) = newPval;
|
249
|
}
|
250
|
blobCnt[newPval]++;
|
251
|
}
|
252
|
}
|
253
|
}
|
254
|
|
255
|
blob_info *BlobInfo = new blob_info();
|
256
|
BlobInfo->blobCnt = 0;
|
257
|
for(int i=1; i<equiv_size;i++){
|
258
|
if(blobCnt[i] == 0)
|
259
|
continue;
|
260
|
BlobInfo->blobCnt++;
|
261
|
}
|
262
|
uchar *blob_id_list;
|
263
|
blob_id_list = new uchar [BlobInfo->blobCnt];
|
264
|
BlobInfo->blobSize = new uint [BlobInfo->blobCnt];
|
265
|
|
266
|
int blob_list_ptr = 0;
|
267
|
for(int i=1; i<equiv_size;i++){
|
268
|
if(blobCnt[i] == 0)
|
269
|
continue;
|
270
|
blob_id_list[blob_list_ptr] = i;
|
271
|
BlobInfo->blobSize[blob_list_ptr] = blobCnt[i];
|
272
|
blob_list_ptr++;
|
273
|
}
|
274
|
|
275
|
BlobInfo->AvgX = new uint [BlobInfo->blobCnt];
|
276
|
BlobInfo->AvgY = new uint [BlobInfo->blobCnt];
|
277
|
for(int i = 0; i < BlobInfo->blobCnt; i++){
|
278
|
int avg_y = 0;
|
279
|
int avg_x = 0;
|
280
|
int id = blob_id_list[i];
|
281
|
for(int j=0; j<img.rows; j++){
|
282
|
for(int k=0; k<img.cols; k++){
|
283
|
if(img.at<uchar>(j,k) == id){
|
284
|
avg_y += j;
|
285
|
avg_x += k;
|
286
|
}
|
287
|
}
|
288
|
}
|
289
|
BlobInfo->AvgY[i] = avg_y / BlobInfo->blobSize[i];
|
290
|
BlobInfo->AvgX[i] = avg_x / BlobInfo->blobSize[i];
|
291
|
}
|
292
|
return BlobInfo;
|
293
|
}
|
294
|
|
295
|
void print_blobs(blob_info *Blobs){
|
296
|
uchar BlobNum = Blobs->blobCnt;
|
297
|
printf("Found %d Blobs:\n",Blobs->blobCnt);
|
298
|
for(int i = 0; i < BlobNum; i++){
|
299
|
printf("size:%d avgX:%d avgY:%d\n",Blobs->blobSize[i],Blobs->AvgX[i],Blobs->AvgY[i]);
|
300
|
}
|
301
|
return;
|
302
|
}
|
303
|
|
304
|
uchar is_start_condition(blob_info *BlobInfo,uchar debug){
|
305
|
if(BlobInfo->blobCnt != 5){
|
306
|
if(debug)
|
307
|
printf("Found only %d blobs\n",BlobInfo->blobCnt);
|
308
|
return 0;
|
309
|
}
|
310
|
int avg_y = 0;
|
311
|
for(int i=0; i<5; i++){
|
312
|
avg_y += BlobInfo->AvgY[i];
|
313
|
}
|
314
|
avg_y = avg_y / 5;
|
315
|
for(int i=0; i<5; i++){
|
316
|
if(abs((int)(BlobInfo->AvgY[i]) - avg_y) > 25){
|
317
|
if(debug)
|
318
|
printf("Blobs too far away %d\n",abs(BlobInfo->AvgY[i] - avg_y));
|
319
|
return 0;
|
320
|
}
|
321
|
}
|
322
|
return 1;
|
323
|
}
|
324
|
|
325
|
uchar is_first_nibble(blob_info *StartBlob, blob_info *FirstBlob,uchar debug){
|
326
|
|
327
|
if(FirstBlob->blobCnt > 4){
|
328
|
if(debug)
|
329
|
printf("More than four blobs\n");
|
330
|
return 0;
|
331
|
}
|
332
|
|
333
|
uchar n = FirstBlob->blobCnt;
|
334
|
uint clkX = StartBlob->AvgX[2];
|
335
|
uint clkY = StartBlob->AvgY[2];
|
336
|
for(int i=0; i<n; i++){
|
337
|
|
338
|
if(dist1(clkX, FirstBlob->AvgX[i], clkY, FirstBlob->AvgY[i]) < 25){
|
339
|
return 0;
|
340
|
}
|
341
|
}
|
342
|
return 1;
|
343
|
}
|
344
|
|
345
|
uchar is_second_nibble(blob_info *StartBlob, blob_info *SecondBlob){
|
346
|
|
347
|
if(SecondBlob->blobCnt < 1){
|
348
|
return 0;
|
349
|
}
|
350
|
|
351
|
uchar n = SecondBlob->blobCnt;
|
352
|
uint clkX = StartBlob->AvgX[2];
|
353
|
uint clkY = StartBlob->AvgY[2];
|
354
|
for(int i=0; i<n; i++){
|
355
|
|
356
|
if(dist1(clkX, SecondBlob->AvgX[i], clkY, SecondBlob->AvgY[i]) < 25){
|
357
|
return 1;
|
358
|
}
|
359
|
}
|
360
|
return 0;
|
361
|
}
|
362
|
|
363
|
uchar get_nibble(blob_info *StartBlob, blob_info *ImgBlob){
|
364
|
uchar nibble = 0;
|
365
|
uchar n = StartBlob->blobCnt;
|
366
|
uchar m = ImgBlob->blobCnt;
|
367
|
for(int i=0; i < n; i++){
|
368
|
if(i == 2)
|
369
|
continue;
|
370
|
uint ledX = StartBlob->AvgX[i];
|
371
|
uint ledY = StartBlob->AvgY[i];
|
372
|
for(int j = 0; j < m; j++){
|
373
|
int dist = dist1(ledX,ImgBlob->AvgX[j],ledY,ImgBlob->AvgY[j]);
|
374
|
if(dist1(ledX,ImgBlob->AvgX[j],ledY,ImgBlob->AvgY[j]) < 25){
|
375
|
if(i > 2)
|
376
|
nibble |= 1<<(i-1);
|
377
|
else
|
378
|
nibble |= 1<<i;
|
379
|
break;
|
380
|
}
|
381
|
}
|
382
|
}
|
383
|
return nibble;
|
384
|
}
|
385
|
|
386
|
uint dist1(int x1, int x2, int y1, int y2){
|
387
|
return abs(x1 - x2) + abs(y1 - y2);
|
388
|
}
|
389
|
|
390
|
void sort_blobs(blob_info *blobs){
|
391
|
uchar n = blobs->blobCnt;
|
392
|
uchar swapped = 1;
|
393
|
while(swapped){
|
394
|
swapped = 0;
|
395
|
for(int i = 1; i < n; i++){
|
396
|
if(blobs->AvgX[i-1] > blobs->AvgX[i]){
|
397
|
swapped = 1;
|
398
|
|
399
|
uint temp = blobs->AvgY[i-1];
|
400
|
blobs->AvgY[i-1] = blobs->AvgY[i];
|
401
|
blobs->AvgY[i] = temp;
|
402
|
|
403
|
temp = blobs->AvgX[i-1];
|
404
|
blobs->AvgX[i-1] = blobs->AvgX[i];
|
405
|
blobs->AvgX[i] = temp;
|
406
|
|
407
|
temp = blobs->blobSize[i-1];
|
408
|
blobs->blobSize[i-1] = blobs->blobSize[i];
|
409
|
blobs->blobSize[i] = temp;
|
410
|
}
|
411
|
}
|
412
|
}
|
413
|
return;
|
414
|
}
|
415
|
|
416
|
|
417
|
|