00001
00002
00003
00004
00005
00006
00007 #include <vector>
00008 #include <map>
00009 #include <string>
00010 #include <algorithm>
00011
00012 #include <cv.h>
00013 #include <highgui.h>
00014 #include <ml.h>
00015
00016 #include "outlet_pose_estimation/detail/outlet_model.h"
00017 #include "outlet_pose_estimation/detail/learning.h"
00018 #include "outlet_pose_estimation/detail/planar.h"
00019 #include "outlet_pose_estimation/detail/outlet_tuple.h"
00020
00021 using namespace std;
00022
00023 const int xsize = 11;
00024 const int ysize = 11;
00025 const int feature_count = xsize*ysize;
00026
00027 void DrawKeypoints(IplImage* img, std::vector<outlet_feature_t> keypts)
00028 {
00029 for(std::vector<outlet_feature_t>::const_iterator it = keypts.begin(); it != keypts.end(); it++)
00030 {
00031 CvPoint center = cvPoint(it->bbox.x + it->bbox.width/2, it->bbox.y + it->bbox.height/2);
00032 int scale = MAX(it->bbox.width, it->bbox.height);
00033 cvCircle(img, center, scale, CV_RGB(255, 0, 0), 2);
00034 }
00035 }
00036
00037 void find_hole_candidates(IplImage* grey, IplImage* mask, CvSeq* socket, float hole_contrast, vector<CvSeq*>& holes)
00038 {
00039 cvSetZero(mask);
00040
00041 for(CvSeq* seq = socket->v_next; seq != 0; seq = seq->h_next)
00042 {
00043 CvRect rect = cvBoundingRect(seq);
00044 #if defined(_EUROPE)
00045 int min_size = 5*grey->width/2272;
00046 int max_size = min_size*6;
00047 if(rect.width < min_size || rect.height < min_size || rect.width > max_size || rect.height > max_size)
00048 {
00049 continue;
00050 }
00051 #else
00052 const int min_size = 1;
00053 const int max_size = 20;
00054
00055 #if 0
00056 if(abs(rect.x - 187) < 5 && abs(rect.y - 250) < 5)
00057 {
00058 int w = 1;
00059 }
00060 #endif
00061
00062 if(rect.width < min_size || rect.height < min_size || rect.width > max_size || rect.height > max_size)
00063 {
00064 continue;
00065 }
00066 #endif //_EUROPE
00067
00068 #if defined(_EUROPE)
00069 if(fabs(float(rect.width)/rect.height - 1) > 0.5)
00070 {
00071 continue;
00072 }
00073 #endif //_EUROPE
00074
00075 #if 0
00076 CvBox2D box = cvMinAreaRect2(seq);
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086 #if defined(_EUROPE)
00087 if(box.size.height < 0 || fabs(float(box.size.width)/box.size.height - 1) > 0.5)
00088 {
00089 continue;
00090 }
00091 #endif //_EUROPE
00092 #endif
00093
00094 #if 0
00095 cvDrawContours(mask, seq, cvScalar(255), cvScalar(255), 0, CV_FILLED);
00096 float avg_inside = cvAvg(grey, mask).val[0];
00097 CvRect bound = double_rect(rect);
00098 bound = fit_rect(bound, grey);
00099 cvRectangle(mask, cvPoint(bound.x, bound.y), cvPoint(bound.x + bound.width, bound.y + bound.height),
00100 cvScalar(255), CV_FILLED);
00101 cvDrawContours(mask, seq, cvScalar(0), cvScalar(0), 0, CV_FILLED);
00102 CvScalar avg_outside, std_outside;
00103 cvAvgSdv(grey, &avg_outside, &std_outside, mask);
00104 #else
00105
00106 #if 0
00107
00108
00109
00110 CvRect inner_rect = resize_rect(rect, 0.75);
00111 cvSetImageROI(grey, inner_rect);
00112 float avg_inside = cvSum(grey).val[0];
00113 cvResetImageROI(grey);
00114 #endif
00115
00116 #if !defined(_TUNING)
00117 CvRect bound = double_rect(rect);
00118 #else
00119 CvRect bound = resize_rect(rect, 2.0f);
00120 #endif //_TUNING
00121 bound = fit_rect(bound, grey);
00122
00123 #if 0
00124
00125
00126
00127 CvScalar avg_outside, std_outside;
00128 cvSetImageROI(grey, bound);
00129 avg_outside = cvSum(grey);
00130 cvResetImageROI(grey);
00131 avg_outside.val[0] -= avg_inside;
00132 avg_inside /= inner_rect.width*inner_rect.height;
00133 avg_outside.val[0] /= bound.width*bound.height;
00134 #endif
00135 #endif
00136 #if 0
00137 cvCopy(grey, mask);
00138 cvDrawContours(mask, seq, cvScalar(255), cvScalar(255), 0, 1);
00139 cvSetImageROI(mask, bound);
00140 cvNamedWindow("2", 1);
00141 cvShowImage("2", mask);
00142 cvWaitKey(0);
00143 cvResetImageROI(mask);
00144 #endif
00145
00146
00147 cvRectangle(mask, cvPoint(bound.x, bound.y), cvPoint(bound.x + bound.width, bound.y + bound.height),
00148 cvScalar(0), CV_FILLED);
00149 #if defined(_EUROPE)
00150 const float avg_factor = 1.0f;
00151 #else
00152
00153 #endif //_EUROPE
00154
00155 float contrast, variation;
00156 calc_contrast_factor(grey, rect, contrast, variation);
00157 #if !defined(_TUNING)
00158 if( avg_outside.val[0] < avg_inside*avg_factor)
00159 {
00160 continue;
00161 }
00162 #else
00163 if(contrast < hole_contrast)
00164 {
00165 continue;
00166 }
00167 #endif //_TUNING
00168
00169 holes.push_back(seq);
00170 }
00171 }
00172
00173 vector<outlet_feature_t>::const_iterator find_fartherst_hole(const vector<vector<outlet_feature_t>::const_iterator>& candidates,
00174 outlet_feature_t feature)
00175 {
00176 vector<vector<outlet_feature_t>::const_iterator>::const_iterator itmax;
00177 int distmax = 0;
00178 int fx = feature.bbox.x + feature.bbox.width/2;
00179 for(vector<vector<outlet_feature_t>::const_iterator>::const_iterator it = candidates.begin(); it != candidates.end(); it++)
00180 {
00181 int cx = (*it)->bbox.x + (*it)->bbox.width/2;
00182 int dist = abs(fx - cx);
00183 if(dist > distmax)
00184 {
00185 distmax = dist;
00186 itmax = it;
00187 }
00188 }
00189
00190 return(*itmax);
00191 }
00192
00193 void find_holes(const vector<outlet_feature_t>& holes, vector<outlet_t>& outlets, IplImage* grey, IplImage* mask, IplImage* img)
00194 {
00195 #if defined(_EUROPE)
00196 int min_dist = 5*grey->width/2272;
00197 int max_dist = 50*grey->width/2272;
00198 int max_dist_prox = max_dist/2;
00199 #else
00200 const int min_dist = 14;
00201 const int max_dist = 80;
00202 const int max_dist_prox = 2*max_dist;
00203 #endif //_EUROPE
00204
00205
00206
00207 vector<vector<int> > references;
00208 references.resize(holes.size());
00209
00210 for(vector<outlet_feature_t>::const_iterator it1 = holes.begin(); it1 != holes.end(); it1++)
00211 {
00212 vector<vector<outlet_feature_t>::const_iterator> candidates;
00213 int proximity_count = 0;
00214
00215 CvRect rect1 = it1->bbox;
00216 int x1 = rect1.x + rect1.width/2;
00217 int y1 = rect1.y + rect1.height/2;
00218
00219 #if 0
00220 if(abs(x1 - 1056) < 10 && abs(y1 - 500) < 10)
00221 {
00222 int w = 1;
00223 }
00224 #endif
00225
00226 #if defined(_TUNING)
00227 int proximity_thresh = 50;
00228 if(x1 < proximity_thresh || y1 < proximity_thresh ||
00229 x1 > grey->width - proximity_thresh ||
00230 y1 > grey->height - proximity_thresh)
00231 {
00232 continue;
00233 }
00234 #endif
00235 for(vector<outlet_feature_t>::const_iterator it2 = holes.begin(); it2 != holes.end(); it2++)
00236 {
00237
00238
00239 CvRect rect2 = it2->bbox;
00240
00241 int x2 = rect2.x + rect2.width/2;
00242 int y2 = rect2.y + rect2.height/2;
00243
00244 #if 0
00245 if(abs(x2 - 1093) < 10 && abs(y2 - 500) < 10)
00246 {
00247 int w = 1;
00248 }
00249 #endif
00250
00251 #if defined(_TUNING)
00252 int proximity_thresh = 20;
00253 if(x2 < proximity_thresh || y2 < proximity_thresh ||
00254 x2 > grey->width - proximity_thresh ||
00255 y2 > grey->height - proximity_thresh)
00256 {
00257 continue;
00258 }
00259 #endif
00260
00261 if(x2 <= x1)
00262 {
00263 continue;
00264 }
00265
00266 if(__max(abs(x1 - x2), abs(y1 - y2)) < max_dist_prox)
00267 {
00268 proximity_count++;
00269 }
00270
00271 #if defined(_SMALL)
00272 const int min_candidates = 5;
00273 const int min_ydist = 5;
00274 #else
00275 #if !defined(_USE_OUTLET_TUPLE)
00276 const int min_candidates = 3;
00277 #else
00278 const int min_candidates = 100;
00279 #endif //_USE_OUTLET_TUPLE
00280 const int min_ydist = 40;
00281 #endif //_SMALL
00282 if(abs(y1 - y2) > min_ydist)
00283 {
00284 continue;
00285 }
00286
00287 #if !defined(_USE_OUTLET_TUPLE)
00288 if(atan2(fabs(y1 - y2), fabs(x1 - x2)) > 2*pi/3)
00289 {
00290 continue;
00291 }
00292 #endif //_USE_OUTLET_TUPLE
00293
00294 float dist = sqrt(float(x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
00295 if(dist < min_dist || dist > max_dist)
00296 {
00297 continue;
00298 }
00299
00300 if(dist < MAX(it1->bbox.width, it2->bbox.width)*5)
00301 {
00302 continue;
00303 }
00304
00305
00306 candidates.push_back(it2);
00307 int s = candidates.size();
00308 if(s > min_candidates)
00309 {
00310 candidates.clear();
00311 break;
00312 }
00313 }
00314
00315
00316 #if defined(_EUROPE)
00317 if(proximity_count < 4 && candidates.size() > 0)
00318 #else
00319 for(unsigned int i = 0; i < candidates.size(); i++)
00320 #endif
00321 {
00322 #if defined(_EUROPE)
00323 vector<outlet_feature_t>::const_iterator cand_it = find_fartherst_hole(candidates, *it1);
00324 #else
00325 vector<outlet_feature_t>::const_iterator cand_it = candidates[i];
00326 #endif //_EUROPE
00327 outlet_feature_t cand = *cand_it;
00328 CvRect rect2 = cand.bbox;
00329 int x2 = rect2.x + rect2.width/2;
00330 int y2 = rect2.y + rect2.height/2;
00331
00332 CvRect small_rect = cvRect(MIN(rect1.x, rect2.x), MIN(rect1.y, rect2.y),
00333 MAX(rect2.x + rect2.width - rect1.x, rect1.x + rect1.width - rect2.x),
00334 MAX(rect2.y + rect2.height - rect1.y, rect1.y + rect1.height - rect2.y));
00335
00336 cvSetZero(mask);
00337
00338 cvSetImageROI(grey, small_rect);
00339 cvSetImageROI(mask, small_rect);
00340 cvThreshold(grey, mask, 140, 255, CV_THRESH_BINARY_INV);
00341 cvResetImageROI(grey);
00342 cvResetImageROI(mask);
00343 #if 0
00344 int color1 = (int)cvAvg(grey, mask).val[0];
00345 #else
00346 int color1 = (int)cvAvg(grey).val[0];
00347 #endif
00348
00349
00350
00351
00352 small_rect = double_rect(small_rect);
00353 small_rect = fit_rect(small_rect, grey);
00354
00355 CvRect large_rect = double_rect(small_rect);
00356 large_rect = fit_rect(large_rect, grey);
00357
00358 cvRectangle(mask, large_rect, cvScalar(255), CV_FILLED);
00359 cvRectangle(mask, small_rect, cvScalar(0), CV_FILLED);
00360 int color2 = (int)cvAvg(grey, mask).val[0];
00361
00362 const float rect_ratio = 1.0f;
00363 if(color2/color1 < rect_ratio)
00364 {
00365
00366 }
00367
00368 #if 0
00369 cvRectangle(img, large_rect, CV_RGB(color2, color2, color2), CV_FILLED);
00370 cvRectangle(img, small_rect, CV_RGB(color1, color1, color1), CV_FILLED);
00371 #endif
00372
00373 outlet_t outlet;
00374 outlet.hole1 = cvPoint(x1, y1);
00375 outlet.hole2 = cvPoint(x2, y2);
00376 outlet.feature1 = *it1;
00377 outlet.feature2 = cand;
00378 outlets.push_back(outlet);
00379
00380 references[it1 - holes.begin()].push_back(outlets.size() - 1);
00381 references[cand_it - holes.begin()].push_back(outlets.size() - 1);
00382 }
00383
00384 candidates.clear();
00385 }
00386
00387 #if defined(_EUROPE)
00388
00389 vector<outlet_t> filtered_outlets;
00390 vector<int> outlet_flags;
00391 outlet_flags.resize(outlets.size(), 1);
00392 for(vector<vector<int> >::const_iterator it = references.begin(); it != references.end(); it++)
00393 {
00394 float dist_max = 0;
00395 int it_max = -1;
00396 for(vector<int>::const_iterator oit = it->begin(); oit != it->end(); oit++)
00397 {
00398 float dist = outlet_size(outlets[*oit]);
00399 if(dist > dist_max)
00400 {
00401 dist_max = dist;
00402 it_max = *oit;
00403 }
00404 }
00405 for(vector<int>::const_iterator oit = it->begin(); oit != it->end(); oit++)
00406 {
00407 if(*oit != it_max)
00408 {
00409 outlet_flags[*oit] = outlet_flags[*oit] & 0;
00410 }
00411 }
00412 }
00413
00414 for(int i = 0; i < outlet_flags.size(); i++)
00415 {
00416 if(outlet_flags[i])
00417 {
00418 filtered_outlets.push_back(outlets[i]);
00419 }
00420 }
00421
00422 outlets = filtered_outlets;
00423 #endif //_EUROPE
00424 }
00425
00426 int test_adjacency(const vector<outlet_feature_t>& features, outlet_feature_t f)
00427 {
00428 int fx = f.bbox.x + f.bbox.width/2;
00429 int fy = f.bbox.y + f.bbox.height/2;
00430 int fscale = max(f.bbox.width, f.bbox.height);
00431 for(vector<outlet_feature_t>::const_iterator it = features.begin(); it != features.end(); it++)
00432 {
00433 int x = it->bbox.x + it->bbox.width/2;
00434 int y = it->bbox.y + it->bbox.height/2;
00435
00436 if(abs(fx - x) < fscale && abs(fy - y) < fscale)
00437 {
00438 return 1;
00439 }
00440 }
00441
00442 return 0;
00443 }
00444
00445 void find_outlet_features(IplImage* src, vector<outlet_feature_t>& features, const char* filename)
00446 {
00447 const float min_intersect = 0.2;
00448
00449
00450 IplImage* grey = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
00451 cvCvtColor(src, grey, CV_RGB2GRAY);
00452 cvSmooth(grey, grey);
00453 IplImage* mask = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
00454
00455 IplImage* mask_black = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
00456 IplImage* mask_white = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
00457 IplImage* imgfeat = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
00458 cvSetZero(imgfeat);
00459
00460 CvMemStorage* storage = cvCreateMemStorage();
00461
00462 IplImage* imgholes = cvCloneImage(src);
00463 for(int coi = 1; coi < 4; coi++)
00464 {
00465 cvSetImageCOI(imgholes, coi);
00466 cvCopy(grey, imgholes);
00467 }
00468 cvSetImageCOI(imgholes, 0);
00469
00470 for(int thresh = 20; thresh < 170; thresh += 20)
00471 {
00472 cvSet(mask_black, cvScalar(255));
00473 cvSet(mask_white, cvScalar(255));
00474 IplImage* tempgrey = cvCloneImage(mask_white);
00475 IplImage* tempmask = cvCloneImage(mask_white);
00476
00477 #if 0
00478 for(int coi = 1; coi < 4; coi++)
00479 {
00480 cvSetImageCOI(src, coi);
00481 cvCopy(src, tempgrey);
00482 cvThreshold(tempgrey, tempmask, thresh, 255, CV_THRESH_BINARY_INV);
00483 cvAnd(mask_black, tempmask, mask_black);
00484
00485 cvThreshold(tempgrey, tempmask, thresh, 255, CV_THRESH_BINARY);
00486 cvAnd(mask_white, tempmask, mask_white);
00487 }
00488 #else
00489 cvThreshold(grey, mask_white, thresh, 255, CV_THRESH_BINARY);
00490 cvThreshold(grey, mask_black, thresh, 255, CV_THRESH_BINARY_INV);
00491 #endif
00492 cvSetImageCOI(src, 0);
00493 cvNot(mask_black, mask_black);
00494
00495 #if 0
00496 cvAnd(mask_white, mask_black, tempgrey);
00497 printf("Processing thresh = %d\n", thresh);
00498 cvNamedWindow("1", 1);
00499 cvShowImage("1", tempgrey);
00500 cvWaitKey(0);
00501 #endif
00502
00503 cvReleaseImage(&tempgrey);
00504
00505 CvSeq* first = 0;
00506 cvFindContours(mask_white, storage, &first, sizeof(CvContour), CV_RETR_CCOMP);
00507
00508 for(CvSeq* seq = first; seq != 0; seq = seq->h_next)
00509 {
00510 CvRect rect = cvBoundingRect(seq);
00511 if(rect.width < 40 || rect.height < 40)
00512 {
00513 continue;
00514 }
00515
00516 cvSetZero(tempmask);
00517 cvDrawContours(tempmask, seq, cvScalar(255), cvScalar(255), 0, CV_FILLED);
00518 cvAnd(tempmask, mask_black, tempmask);
00519 CvSeq* outlet = 0;
00520 #if 0
00521 int w = 1;
00522 if(w)
00523 {
00524
00525 cvNamedWindow("1", 1);
00526 cvShowImage("1", tempmask);
00527 cvSaveImage("mask.jpg", tempmask);
00528 cvWaitKey(0);
00529 }
00530 #endif
00531 cvFindContours(tempmask, storage, &outlet, sizeof(CvContour), CV_RETR_TREE);
00532
00533 int holes_count = 0;
00534 for(CvSeq* seqhole = outlet->v_next; seqhole != NULL; seqhole = seqhole->h_next, holes_count++);
00535 if(holes_count < 2)
00536 {
00537 continue;
00538 }
00539
00540
00541 vector<CvSeq*> holes;
00542 const float default_hole_contrast = 1.1f;
00543 find_hole_candidates(grey, mask, outlet, default_hole_contrast, holes);
00544
00545 for(vector<CvSeq*>::iterator it = holes.begin(); it != holes.end(); it++)
00546 {
00547 CvRect roi = cvBoundingRect(*it);
00548 cvSetImageROI(imgfeat, roi);
00549 float avg = cvAvg(imgfeat).val[0];
00550 if(avg < min_intersect)
00551 {
00552 outlet_feature_t feature;
00553 feature.bbox = roi;
00554
00555
00556 features.push_back(feature);
00557
00558 }
00559 }
00560 cvResetImageROI(imgfeat);
00561
00562 for(vector<outlet_feature_t>::iterator it = features.begin(); it != features.end(); it++)
00563 {
00564
00565 cvCircle(imgholes, cvPoint(it->bbox.x, it->bbox.y), 2, CV_RGB(255, 0, 0));
00566 }
00567
00568 }
00569
00570 #if 0
00571 cvNamedWindow("1", 1);
00572 cvShowImage("1", imgholes);
00573 cvWaitKey(0);
00574 cvSaveImage("mask.jpg", imgholes);
00575 #endif
00576 cvReleaseImage(&tempmask);
00577 }
00578
00579
00580
00581
00582
00583 char buf[1024];
00584
00585 #if defined(_VERBOSE)
00586 sprintf(buf, "../../holes/%s", filename);
00587 cvSaveImage(buf, imgholes);
00588
00589 sprintf(buf, "../../src/%s", filename);
00590 cvSaveImage(buf, src);
00591 #endif //_VERBOSE
00592
00593 cvReleaseImage(&grey);
00594 cvReleaseImage(&mask);
00595
00596 cvReleaseImage(&mask_black);
00597 cvReleaseImage(&mask_white);
00598 cvReleaseImage(&imgholes);
00599 cvReleaseImage(&imgfeat);
00600 cvReleaseMemStorage(&storage);
00601 }
00602
00603 inline float outlet_size(outlet_t outlet)
00604 {
00605 return fabs((float)(outlet.hole2.x - outlet.hole1.x)*(outlet.hole2.x - outlet.hole1.x) +
00606 (outlet.hole2.y - outlet.hole1.y)*(outlet.hole2.y - outlet.hole1.y));
00607 }
00608
00609 CvRect outlet_rect(outlet_t outlet)
00610 {
00611 float dist = fabs((float)outlet.hole2.x - outlet.hole1.x);
00612 #if defined(_EUROPE)
00613 float width = dist*2.0f;
00614 #else
00615 float width = dist*1.5f;
00616 #endif
00617 float height = width;
00618 return double_rect(cvRect(outlet.hole1.x - width*0.25f, outlet.hole1.y - height*0.5f, width, height));
00619
00620 }
00621
00622 void move_features(vector<outlet_feature_t>& features, CvPoint origin)
00623 {
00624 for(vector<outlet_feature_t>::iterator it = features.begin(); it != features.end(); it++)
00625 {
00626 it->bbox.x += origin.x;
00627 it->bbox.y += origin.y;
00628 }
00629 }
00630
00631 void detect_outlets(IplImage* src, vector<outlet_feature_t>& features, vector<outlet_t>& outlets,
00632 outlet_tuple_t* outlet_tuple, const char* output_path, const char* filename)
00633 {
00634 IplImage* temp = cvCloneImage(src);
00635 IplImage* grey = 0;
00636
00637 CvRect roi = outlet_tuple ? outlet_tuple->roi : cvRect(0, 0, src->width, src->height);
00638 cvSetImageROI(src, roi);
00639 grey = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_8U, 1);
00640 #if 0
00641 cvCvtColor(src, grey, CV_RGB2GRAY);
00642 #else
00643 cvSetImageCOI(src, 3);
00644 cvCopy(src, grey);
00645 cvSetImageCOI(src, 0);
00646 cvConvertScale(grey, grey, 0.5);
00647 #endif
00648
00649 cvErode(grey, grey);
00650 cvDilate(grey, grey);
00651
00652 #if 0
00653 cvNamedWindow("1", 1);
00654 cvShowImage("1", grey);
00655 cvWaitKey(0);
00656 #endif
00657
00658 const float default_hole_contrast = 1.1f;
00659 find_outlet_features_fast(grey, features, default_hole_contrast, output_path, filename);
00660
00661 move_features(features, cvPoint(roi.x, roi.y));
00662
00663 cvReleaseImage(&grey);
00664 cvResetImageROI(src);
00665
00666 #if _PREDICT
00667 printf("Filtering keypoints...");
00668 CvRTrees* rtrees = new CvRTrees();
00669 rtrees->load("../../forest.xml");
00670 FilterPoints(grey, features, rtrees);
00671 printf("done. %d points left\n", features.size());
00672 delete rtrees;
00673 #endif //_PREDICT
00674
00675 #if defined(_USE_OUTLET_TUPLE)
00676
00677
00678 if(outlet_tuple)
00679 {
00680 IplImage* distance_map = calc_tuple_distance_map(outlet_tuple->tuple_mask);
00681 filter_features_distance_mask(features, distance_map);
00682 cvReleaseImage(&distance_map);
00683 }
00684
00685 #endif //_USE_OUTLET_TUPLE
00686
00687 #if defined(_VERBOSE)
00688 char buf[1024];
00689 if(output_path && filename)
00690 {
00691 IplImage* _src = cvCloneImage(src);
00692 DrawKeypoints(_src, features);
00693 sprintf(buf, "%s/keyout/%s", output_path, filename);
00694 cvSaveImage(buf, _src);
00695 cvReleaseImage(&_src);
00696 }
00697 #endif //_VERBOSE
00698
00699 grey = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
00700 IplImage* mask = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
00701 cvCvtColor(src, grey, CV_RGB2GRAY);
00702 find_holes(features, outlets, grey, mask, temp);
00703
00704 #if defined(_PREDICT_OUTLETS)
00705 CvRTrees* outlet_rtrees = new CvRTrees();
00706 outlet_rtrees->load("../../outlet_forest.xml");
00707 filter_outlets(grey, outlets, outlet_rtrees);
00708 delete outlet_rtrees;
00709 #endif //_PREDICT_OUTLETS
00710
00711
00712 #if defined(_VERBOSE)
00713 if(output_path && filename)
00714 {
00715 draw_outlets(temp, outlets);
00716
00717 sprintf(buf, "%s/output/%s", output_path, filename);
00718 strcpy(buf + strlen(buf) - 3, "jpg");
00719 cvSaveImage(buf, temp);
00720 }
00721 #endif //_VERBOSE
00722
00723 #if defined(_TUNING) && 1
00724 cvCopy(src, temp);
00725 cvResetImageROI(src);
00726 cvResetImageROI(grey);
00727
00728 #endif // _TUNING
00729
00730
00731 cvReleaseImage(&temp);
00732 cvReleaseImage(&grey);
00733 cvReleaseImage(&mask);
00734 }
00735
00736 int filter_outlets_templmatch(IplImage* src, vector<outlet_t>& outlets, IplImage* templ, const char* output_path, const char* filename, CvMat** homography,
00737 CvPoint3D32f* origin, CvPoint2D32f* scale)
00738 {
00739
00740
00741 CvMat* map_matrix = cvCreateMat(3, 3, CV_32FC1);
00742 CvSize dst_size;
00743 int ret = calc_image_homography(src, map_matrix, &dst_size, 0, origin, scale, output_path, filename);
00744 if(ret)
00745 {
00746 if(homography)
00747 {
00748 *homography = map_matrix;
00749 }
00750 const int max_image_width = 2048;
00751 const int max_image_height = 2048;
00752 dst_size.width = MIN(dst_size.width, max_image_width);
00753 dst_size.height = MIN(dst_size.height, max_image_height);
00754 printf("warped size: %d %d\n", dst_size.width, dst_size.height);
00755 IplImage* warped = cvCreateImage(dst_size, IPL_DEPTH_8U, 3);
00756 cvWarpPerspective(src, warped, map_matrix);
00757
00758 cvSaveImage("warped.jpg", warped);
00759
00760
00761 IplImage* warped_mask = find_templates(warped, templ);
00762 cvDilate(warped_mask, warped_mask, 0, 4);
00763
00764 #if 0
00765 cvNamedWindow("1", 1);
00766 cvShowImage("1", warped);
00767 cvWaitKey(0);
00768 #endif
00769
00770 filter_outlets_templ_ex(outlets, map_matrix, warped_mask);
00771
00772 cvReleaseImage(&warped);
00773 }
00774
00775 if(!homography)
00776 {
00777 cvReleaseMat(&map_matrix);
00778 }
00779
00780 return ret;
00781 }
00782
00783 void read_outlet_roi(const char* filename, outlet_roi_t& outlet_roi)
00784 {
00785 FILE* fp = fopen(filename, "rt");
00786
00787 int ret = 0;
00788 char buf[1024];
00789 int x1, y1, x2, y2;
00790 while((ret = fscanf(fp, "%s %d %d %d %d\n", buf, &x1, &y1, &x2, &y2)) > 0)
00791 {
00792 string str = string(buf);
00793 outlet_roi[str].push_back(cvRect(x1, y1, x2 - x1, y2 - y1));
00794 }
00795
00796 fclose(fp);
00797 }
00798
00799 int is_point_inside_roi(const vector<CvRect>& rects, CvPoint point)
00800 {
00801 for(vector<CvRect>::const_iterator it = rects.begin(); it != rects.end(); it++)
00802 {
00803 if(is_point_inside_rect(*it, point))
00804 {
00805 return 1;
00806 }
00807 }
00808
00809 return 0;
00810 }
00811
00812 int is_point_incenter_roi(const vector<CvRect>& rects, CvPoint point)
00813 {
00814 for(vector<CvRect>::const_iterator it = rects.begin(); it != rects.end(); it++)
00815 {
00816 CvRect _small = resize_rect(*it, 0.5f);
00817 if(is_point_inside_rect(_small, point))
00818 {
00819 return 1;
00820 }
00821 }
00822
00823 return 0;
00824 }
00825
00826 int is_point_inside_roi(const outlet_roi_t& outlet_roi, CvPoint point, string img_name)
00827 {
00828 map<string, vector<CvRect> >::const_iterator it = outlet_roi.find(img_name);
00829
00830 if(it == outlet_roi.end())
00831 {
00832
00833 return 0;
00834 }
00835
00836 int ret = is_point_inside_roi(it->second, point);
00837 return ret;
00838 }
00839
00840 inline int is_rect_inside_rect(CvRect large, CvRect smaller)
00841 {
00842 if(smaller.x >= large.x && smaller.y >= large.y && smaller.x + smaller.width <= large.x + large.width &&
00843 smaller.y + smaller.height <= large.y + large.height)
00844 {
00845 return 1;
00846 }
00847 else
00848 {
00849 return 0;
00850 }
00851 }
00852
00853 void calc_labels(const vector<CvRect>& rects, const vector<outlet_feature_t>& keypts, vector<int>& labels)
00854 {
00855 for(vector<outlet_feature_t>::const_iterator it = keypts.begin(); it != keypts.end(); it++)
00856 {
00857 CvPoint center = cvPoint(it->bbox.x + it->bbox.width/2, it->bbox.y + it->bbox.height/2);
00858 #if defined(_TRAIN)
00859 int label = is_point_incenter_roi(rects, center);
00860 #else
00861 int label = is_point_inside_roi(rects, center);
00862 #endif //_TRAIN
00863 labels.push_back(label);
00864 }
00865 }
00866
00867 void extract_intensity_features(IplImage* grey, const vector<outlet_feature_t>& keypts, CvMat** mat,
00868 int equalize_hist, const vector<int>& labels, const char* buf)
00869 {
00870 int start_row = 0;
00871
00872 if(!(*mat))
00873 {
00874 *mat = cvCreateMat(keypts.size(), feature_count, CV_32FC1);
00875 }
00876 else
00877 {
00878 start_row = (*mat)->rows;
00879 CvMat* _mat = cvCreateMat((*mat)->rows + keypts.size(), feature_count, CV_32FC1);
00880
00881 for(int r = 0; r < (*mat)->rows; r++)
00882 {
00883 memcpy(_mat->data.ptr + _mat->step*r, (*mat)->data.ptr + (*mat)->step*r, sizeof(float)*_mat->cols);
00884 }
00885
00886
00887 cvReleaseMat(mat);
00888 *mat = _mat;
00889 }
00890
00891
00892 IplImage* features = cvCreateImage(cvSize(xsize, ysize), IPL_DEPTH_8U, 1);
00893
00894
00895 for(unsigned int r = 0; r < keypts.size(); r++)
00896 {
00897
00898
00899
00900 const float scale_factor = 2.0f;
00901
00902
00903 CvRect roi = keypts[r].bbox;
00904 roi = resize_rect(roi, scale_factor);
00905 roi = fit_rect(roi, grey);
00906
00907 cvSetImageROI(grey, roi);
00908 cvResize(grey, features);
00909 if(equalize_hist)
00910 {
00911 cvEqualizeHist(features, features);
00912 }
00913 #if 0
00914 const float norm = 1;
00915 #else
00916 float norm = cvSum(features).val[0];
00917 #endif
00918 for(int y = 0; y < ysize; y++)
00919 {
00920 for(int x = 0; x < xsize; x++)
00921 {
00922 cvmSet(*mat, start_row + r, y*xsize + x, float(features->imageData[features->widthStep*y + x])/norm);
00923 }
00924 }
00925 cvResetImageROI(grey);
00926 if(labels.size() == 0)
00927 {
00928
00929 continue;
00930 }
00931
00932
00933 char filename[1024];
00934 char lab[1024];
00935 if(labels[start_row + r] == 0)
00936 {
00937 continue;
00938 strcpy(lab, "neg");
00939 }
00940 else
00941 {
00942 strcpy(lab, "pos");
00943 }
00944 #if defined(_VERBOSE)
00945 sprintf(filename, "../../rfsamples/%s/%s_%d.jpg", lab, buf, start_row + r);
00946 cvSaveImage(filename, features);
00947 #endif
00948 }
00949
00950 cvReleaseImage(&features);
00951 }
00952
00953 CvMat* vector2mat(const vector<int>& vec)
00954 {
00955 CvMat* mat = cvCreateMat(vec.size(), 1, CV_32SC1);
00956 for(unsigned int i = 0; i < vec.size(); i++)
00957 {
00958 *(int*)(mat->data.ptr + mat->step*i) = vec[i];
00959 }
00960
00961 return mat;
00962 }
00963
00964 void FilterPoints(IplImage* grey, vector<outlet_feature_t>& keypts, const CvRTrees* rtrees)
00965 {
00966
00967
00968
00969
00970
00971
00972
00973
00974 vector<outlet_feature_t> filtered;
00975 for(vector<outlet_feature_t>::const_iterator it = keypts.begin(); it != keypts.end(); it++)
00976 {
00977 vector<outlet_feature_t> temp;
00978 temp.push_back(*it);
00979 CvMat* sample = 0;
00980 extract_intensity_features(grey, temp, &sample);
00981 float prob = rtrees->predict_prob(sample);
00982 if(prob > 350.0f)
00983 {
00984 outlet_feature_t feature;
00985 feature.bbox = it->bbox;
00986 feature.weight = prob;
00987 filtered.push_back(feature);
00988 }
00989 }
00990
00991
00992
00993
00994
00995
00996
00997 keypts = filtered;
00998 }
00999
01000 void filter_outlets(IplImage* grey, vector<outlet_t>& outlets, CvRTrees* rtrees)
01001 {
01002 vector<outlet_t> filtered_outlets;
01003 vector<outlet_t>::const_iterator max_it;
01004 for(vector<outlet_t>::const_iterator it = outlets.begin(); it != outlets.end(); it++)
01005 {
01006 vector<outlet_feature_t> features;
01007 outlet_feature_t feature;
01008 feature.bbox = outlet_rect(*it);
01009 features.push_back(feature);
01010 CvMat* sample = 0;
01011 extract_intensity_features(grey, features, &sample, 1);
01012 float prob = rtrees->predict_prob(sample);
01013 printf("outlet center = %d %d, prob = %f\n", feature.bbox.x + feature.bbox.width/2,
01014 feature.bbox.y + feature.bbox.height/2, prob);
01015 #if 1
01016 if(prob > 0.0f)
01017 {
01018 outlet_t outlet = *it;
01019 outlet.weight = prob;
01020 filtered_outlets.push_back(outlet);
01021 }
01022 }
01023 #else
01024 if(prob > max_prob)
01025 {
01026 max_prob = prob;
01027 max_it = it;
01028 }
01029 }
01030
01031 if(max_prob > 0)
01032 {
01033 filtered_outlets.push_back(*max_it);
01034 }
01035
01036 #endif
01037 outlets = filtered_outlets;
01038 }
01039
01040
01041 void find_outlet_features_fast(IplImage* src, vector<outlet_feature_t>& features, float hole_contrast,
01042 const char* output_path, const char* filename)
01043 {
01044 const float min_intersect = 1;
01045
01046
01047 IplImage* grey = 0;
01048
01049 if(src->nChannels == 3)
01050 {
01051 grey = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
01052
01053 #if _USE_OUTLET_TUPLE
01054 cvSetImageCOI(src, 3);
01055 cvCopy(src, grey);
01056
01057 #else
01058 cvCvtColor(src, grey, CV_RGB2GRAY);
01059 #endif
01060 }
01061 else
01062 {
01063 grey = src;
01064 }
01065
01066 cvSmooth(grey, grey);
01067
01068 IplImage* mask = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
01069
01070 IplImage* mask_black = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
01071 IplImage* mask_white = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
01072 IplImage* imgfeat = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
01073 cvSetZero(imgfeat);
01074
01075 CvMemStorage* storage = cvCreateMemStorage();
01076
01077 IplImage* imgholes = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 3);
01078 for(int coi = 1; coi < 4; coi++)
01079 {
01080 cvSetImageCOI(imgholes, coi);
01081 cvCopy(grey, imgholes);
01082 }
01083 cvSetImageCOI(imgholes, 0);
01084
01085 #if !defined(_TUNING)
01086 for(int thresh = 20; thresh < 170; thresh += 20)
01087 #else
01088 for(int thresh = 30; thresh < 170; thresh += thresh < 20 ? 5: 10)
01089 #endif
01090 {
01091 cvSet(mask_black, cvScalar(255));
01092 cvSet(mask_white, cvScalar(255));
01093 IplImage* tempgrey = cvCloneImage(mask_white);
01094 IplImage* tempmask = cvCloneImage(mask_white);
01095
01096 #if 0
01097 for(int coi = 1; coi < 4; coi++)
01098 {
01099 cvSetImageCOI(src, coi);
01100 cvCopy(src, tempgrey);
01101 cvThreshold(tempgrey, tempmask, thresh, 255, CV_THRESH_BINARY_INV);
01102 cvAnd(mask_black, tempmask, mask_black);
01103
01104 cvThreshold(tempgrey, tempmask, thresh, 255, CV_THRESH_BINARY);
01105 cvAnd(mask_white, tempmask, mask_white);
01106 }
01107 #else
01108 cvThreshold(grey, mask_white, thresh, 255, CV_THRESH_BINARY);
01109 cvThreshold(grey, mask_black, thresh, 255, CV_THRESH_BINARY_INV);
01110 #endif
01111 cvSetImageCOI(src, 0);
01112 cvNot(mask_black, mask_black);
01113
01114 #if 0
01115 cvAnd(mask_white, mask_black, tempgrey);
01116
01117
01118 printf("Processing thresh = %d\n", thresh);
01119 cvNamedWindow("1", 1);
01120 cvShowImage("1", tempgrey);
01121 cvWaitKey(0);
01122 #endif
01123
01124 cvReleaseImage(&tempgrey);
01125
01126 cvAnd(mask_white, mask_black, tempmask);
01127
01128
01129 CvSeq* first = 0;
01130 cvFindContours(tempmask, storage, &first, sizeof(CvContour), CV_RETR_CCOMP);
01131
01132 for(CvSeq* seq = first; seq != 0; seq = seq->h_next)
01133 {
01134
01135
01136
01137
01138 CvRect rect = cvBoundingRect(seq);
01139 if(rect.width < 40 || rect.height < 40)
01140 {
01141 continue;
01142 }
01143
01144 CvSeq* outlet = seq;
01145
01146 int holes_count = 0;
01147 for(CvSeq* seqhole = outlet->v_next; seqhole != NULL; seqhole = seqhole->h_next, holes_count++);
01148 if(holes_count < 2)
01149 {
01150 continue;
01151 }
01152
01153
01154 vector<CvSeq*> holes;
01155 find_hole_candidates(grey, mask, outlet, hole_contrast, holes);
01156
01157 for(vector<CvSeq*>::iterator it = holes.begin(); it != holes.end(); it++)
01158 {
01159 CvRect roi = cvBoundingRect(*it);
01160
01161 #if 0
01162 if(abs(roi.x - 56) < 5 && abs(roi.y - 189) < 5)
01163 {
01164 int w = 1;
01165 }
01166 #endif
01167
01168
01169
01170 cvSetImageROI(imgfeat, roi);
01171 float avg = cvAvg(imgfeat).val[0];
01172 cvResetImageROI(imgfeat);
01173 if(avg < min_intersect)
01174 {
01175 outlet_feature_t feature;
01176 feature.bbox = roi;
01177 features.push_back(feature);
01178
01179 cvDrawContours(imgfeat, *it, cvScalar(255), cvScalar(255), 0, CV_FILLED);
01180 }
01181 }
01182 cvResetImageROI(imgfeat);
01183
01184 for(vector<outlet_feature_t>::iterator it = features.begin(); it != features.end(); it++)
01185 {
01186 cvRectangle(imgholes, it->bbox, CV_RGB(0, 0, 255), 1);
01187
01188 }
01189
01190 }
01191
01192 #if 0
01193 cvNamedWindow("1", 1);
01194 cvShowImage("1", imgholes);
01195 cvWaitKey(0);
01196 cvSaveImage("mask.jpg", imgholes);
01197 #endif
01198 cvReleaseImage(&tempmask);
01199 }
01200
01201
01202
01203
01204
01205
01206
01207 #if 0
01208 filter_canny(grey, features);
01209 #endif
01210
01211 char buf[1024];
01212
01213 #if defined(_VERBOSE)
01214 if(output_path && filename)
01215 {
01216 sprintf(buf, "%s/holes/%s", output_path, filename);
01217 cvSaveImage(buf, imgholes);
01218 }
01219 #endif //_VERBOSE
01220
01221 if(src->nChannels == 3)
01222 {
01223 cvReleaseImage(&grey);
01224 }
01225 cvReleaseImage(&mask);
01226
01227 cvReleaseImage(&mask_black);
01228 cvReleaseImage(&mask_white);
01229 cvReleaseImage(&imgholes);
01230 cvReleaseImage(&imgfeat);
01231 cvReleaseMemStorage(&storage);
01232 }
01233
01234 int is_outlet_inside_roi(const outlet_roi_t& outlet_roi, outlet_t outlet, string img_name)
01235 {
01236 map<string, vector<CvRect> >::const_iterator it = outlet_roi.find(img_name);
01237 if(it == outlet_roi.end())
01238 {
01239
01240 return 0;
01241 }
01242
01243 int ret1 = is_point_inside_roi(it->second, outlet.hole1);
01244 int ret2 = is_point_inside_roi(it->second, outlet.hole2);
01245
01246 return ret1 && ret2;
01247 }
01248
01249 int generate_outlet_samples(IplImage* grey, outlet_t outlet, int count, CvMat** predictors, const char* filename)
01250 {
01251 IplImage** patches = new IplImage*[count];
01252 CvRect roi = outlet_rect(outlet);
01253 cvSetImageROI(grey, roi);
01254 gen_random_homog_patches(grey, count, patches);
01255 cvResetImageROI(grey);
01256 save_image_array("../../patches", filename, count, patches);
01257
01258 int outlet_count = 0;
01259 for(int i = 0; i < count; i++)
01260 {
01261 #if 0
01262 outlet_feature_t feature;
01263 feature.bbox = cvRect(0, 0, patches[i]->width, patches[i]->height);
01264 vector<outlet_feature_t> features;
01265 features.push_back(feature);
01266 extract_intensity_features(patches[i], features, predictors);
01267 #else
01268 vector<outlet_feature_t> features;
01269 vector<outlet_t> outlets;
01270 IplImage* color = cvCreateImage(cvSize(patches[i]->width, patches[i]->height), IPL_DEPTH_8U, 3);
01271 cvCvtColor(patches[i], color, CV_GRAY2RGB);
01272 detect_outlets(color, features, outlets, 0, 0, filename);
01273 if(outlets.size() > 0)
01274 {
01275 outlet_feature_t feature;
01276 feature.bbox = outlet_rect(outlets[0]);
01277 vector<outlet_feature_t> features;
01278 features.push_back(feature);
01279 extract_intensity_features(patches[i], features, predictors);
01280 outlet_count++;
01281 }
01282 else
01283 {
01284 continue;
01285 }
01286 #endif
01287 }
01288
01289
01290
01291 for(int i = 0; i < count; i++)
01292 {
01293 cvReleaseImage(&patches[i]);
01294 }
01295 delete []patches;
01296
01297 return(outlet_count);
01298 }
01299
01300 void train_outlet_model(const char* path, const char* config_filename,
01301 const char* roi_filename, const char* forest_filename)
01302 {
01303 const int samples_per_outlet = 30;
01304 outlet_roi_t outlet_roi;
01305 read_outlet_roi(roi_filename, outlet_roi);
01306
01307 CvMat* predictors = 0;
01308 vector<int> labels;
01309
01310 FILE* fp = fopen(config_filename, "rt");
01311 char buf[1024];
01312 int class_id;
01313 int ret;
01314 while((ret=fscanf(fp, "%d %s\n", &class_id, buf)) > 0)
01315 {
01316 printf("Processing file %s...", buf);
01317
01318 char filename[1024];
01319 sprintf(filename, "%s/%s", path, buf);
01320 IplImage* src = cvLoadImage(filename);
01321 IplImage* grey = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
01322 cvCvtColor(src, grey, CV_RGB2GRAY);
01323
01324 vector<outlet_feature_t> features;
01325 vector<outlet_t> outlets;
01326 detect_outlets(src, features, outlets, 0, 0, buf);
01327
01328
01329 for(vector<outlet_t>::const_iterator it = outlets.begin(); it != outlets.end(); it++)
01330 {
01331 CvRect orect = outlet_rect(*it);
01332 outlet_feature_t feature;
01333 feature.bbox = orect;
01334 vector<outlet_feature_t> f;
01335 f.push_back(feature);
01336 int outlet_count = generate_outlet_samples(grey, *it, samples_per_outlet, &predictors, buf);
01337
01338 int outlet_class = class_id == 1 && is_outlet_inside_roi(outlet_roi, *it, string(buf));
01339 labels.insert(labels.end(), outlet_count, outlet_class);
01340 }
01341
01342 #if defined(_VERBOSE)
01343 DrawKeypoints(src, features);
01344 sprintf(filename, "../../keyout/%s", buf);
01345 cvSaveImage(filename, src);
01346 #endif //_VERBOSE
01347
01348 cvReleaseImage(&grey);
01349 cvReleaseImage(&src);
01350
01351 printf("done.\n");
01352 }
01353
01354 CvMat* labels_mat = vector2mat(labels);
01355
01356 printf("Training RF model...");
01357 CvRTrees* rtrees = train_rf(predictors, labels_mat);
01358 printf("done.\n");
01359
01360 rtrees->save("../../outlet_forest.xml");
01361 }
01362
01363 void write_pr(const char* pr_filename, const char* image_filename, const outlet_roi_t& outlet_roi,
01364 const vector<outlet_t>& outlets)
01365 {
01366 FILE* fp = fopen(pr_filename, "at");
01367
01368 for(vector<outlet_t>::const_iterator it = outlets.begin(); it != outlets.end(); it++)
01369 {
01370
01371 int class_id = is_outlet_inside_roi(outlet_roi, *it, image_filename);
01372 float weight = MIN(it->feature1.weight, it->feature2.weight);
01373 fprintf(fp, "%s,%d,%f,%d,%d\n", image_filename, class_id, weight, (it->hole1.x + it->hole2.x)/2,
01374 (it->hole1.y + it->hole2.y)/2);
01375 }
01376
01377 fclose(fp);
01378 }
01379
01380 void filter_negative_samples(const vector<CvRect>& rects, vector<outlet_feature_t>& keypts, float fraction)
01381 {
01382 vector<int> labels;
01383 calc_labels(rects, keypts, labels);
01384 vector<outlet_feature_t> filtered_keypts;
01385 for(unsigned int i = 0; i < labels.size(); i++)
01386 {
01387 if(labels[i] == 1 || float(rand())/RAND_MAX < fraction)
01388 {
01389 filtered_keypts.push_back(keypts[i]);
01390 continue;
01391 }
01392 }
01393
01394 keypts = filtered_keypts;
01395 }
01396
01397 void calc_contrast_factor(IplImage* grey, CvRect rect, float& contrast, float& variation)
01398 {
01399 CvPoint center = cvPoint(rect.x + rect.width/2,
01400 rect.y + rect.height/2);
01401 int scale = MAX(rect.width/2, rect.height/2);
01402 rect = cvRect(center.x - scale, center.y - scale, 2*scale, 2*scale);
01403 rect = resize_rect(rect, 1.5f);
01404 rect = fit_rect(rect, grey);
01405
01406 int Ic = (unsigned char)grey->imageData[center.y*grey->widthStep + center.x];
01407 int I[4];
01408 I[0] = (unsigned char)grey->imageData[rect.y*grey->widthStep + rect.x];
01409 I[1] = (unsigned char)grey->imageData[(rect.y + rect.height)*grey->widthStep + rect.x];
01410 I[2] = (unsigned char)grey->imageData[(rect.y + rect.height)*grey->widthStep + rect.x + rect.width];
01411 I[3] = (unsigned char)grey->imageData[rect.y*grey->widthStep + rect.x + rect.width];
01412 int minI = 65535;
01413 int maxI = 0;
01414 int avgI = 0;
01415 for(int i = 0; i < 4; i++)
01416 {
01417 minI = MIN(minI, I[i]);
01418 maxI = MAX(maxI, I[i]);
01419 avgI += I[i];
01420 }
01421 avgI /= 4;
01422
01423 contrast = float(avgI)/Ic;
01424 variation = float(maxI - minI)/maxI;
01425 }
01426
01427 bool outlet_orient_pred_greater(outlet_t outlet1, outlet_t outlet2)
01428 {
01429 return outlet1.weight_orient > outlet2.weight_orient;
01430 }
01431
01432 bool outlet_orient_pred_dist_greater(outlet_t outlet1, outlet_t outlet2)
01433 {
01434 return outlet1.feature1.weight + outlet1.feature2.weight >
01435 outlet2.feature1.weight + outlet2.feature2.weight;
01436 }
01437
01438 void select_central_outlets(vector<outlet_t>& outlets, int count)
01439 {
01440 sort(outlets.begin(), outlets.end(), outlet_orient_pred_dist_greater);
01441 count = MIN(count, (int)outlets.size());
01442 outlets = vector<outlet_t>(outlets.begin(), outlets.begin() + count);
01443 }
01444
01445 void select_orient_outlets(CvPoint2D32f orientation, vector<outlet_t>& outlets, int count)
01446 {
01447
01448 float mod = sqrt(orientation.x*orientation.x + orientation.y*orientation.y);
01449 orientation.x /= mod;
01450 orientation.y /= mod;
01451
01452
01453 vector<outlet_t> filtered_outlets;
01454 const float min_product = cos(pi*10/180);
01455 for(vector<outlet_t>::iterator it = outlets.begin(); it != outlets.end(); it++)
01456 {
01457 CvPoint2D32f outlet_dir = cvPoint2D32f(it->hole2.x - it->hole1.x, it->hole2.y - it->hole1.y);
01458 float outlet_mod = sqrt(outlet_dir.x*outlet_dir.x + outlet_dir.y*outlet_dir.y);
01459 outlet_dir.x /= outlet_mod;
01460 outlet_dir.y /= outlet_mod;
01461
01462 float product = outlet_dir.x*orientation.x + outlet_dir.y*orientation.y;
01463 it->weight_orient = product;
01464 if(count == 0 && product > min_product)
01465 {
01466 filtered_outlets.push_back(*it);
01467 }
01468 }
01469
01470 if(count == 0)
01471 {
01472 outlets = filtered_outlets;
01473 return;
01474 }
01475
01476 sort(outlets.begin(), outlets.end(), outlet_orient_pred_greater);
01477 count = MIN(count, (int)outlets.size());
01478 outlets = vector<outlet_t>(outlets.begin(), outlets.begin() + count);
01479 }
01480
01481 int select_orient_outlets_ex(IplImage* grey, vector<outlet_t>& outlets, const char* filename)
01482 {
01483 #if 0
01484 cvNamedWindow("1", 1);
01485 cvShowImage("1", grey);
01486 cvWaitKey(0);
01487 #endif
01488
01489 #if 0
01490 int ret = cvFindChessboardCorners(grey, cvSize(board_width, board_height),
01491 corners, &corners_found);
01492
01493 printf("ret = %d\n", ret);
01494 if(ret == 0)
01495 {
01496 return 0;
01497 }
01498
01499
01500 CvPoint2D32f hor_dir = cvPoint2D32f(corners[4].x - corners[0].x, corners[4].y - corners[0].y);
01501 #else
01502 CvPoint2D32f hor_dir;
01503 char buf[1024];
01504 sprintf(buf, "../../../images/us_outlets_hr/%s", filename);
01505 strcpy(buf + strlen(buf) - 3, "txt");
01506 FILE* fp = fopen(buf, "rt");
01507 if(fp != 0)
01508 {
01509 int xdir = -1, ydir = -1;
01510 fscanf(fp, "%d %d\n", &xdir, &ydir);
01511 fclose(fp);
01512 hor_dir = cvPoint2D32f(xdir, ydir);
01513 }
01514 else
01515 {
01516 printf("File %s not found...\n", buf);
01517 return 0;
01518 }
01519
01520 #endif
01521
01522 select_orient_outlets(hor_dir, outlets);
01523
01524 return 1;
01525 }
01526
01527 void draw_outlets(IplImage* temp, const vector<outlet_t>& outlets)
01528 {
01529 for(vector<outlet_t>::const_iterator it = outlets.begin(); it != outlets.end(); it++)
01530 {
01531 CvPoint hole1 = it->is_subpixel ? cvPoint(floor(it->hole1f.x), floor(it->hole1f.y)) : it->hole1;
01532 CvPoint hole2 = it->is_subpixel ? cvPoint(floor(it->hole2f.x), floor(it->hole2f.y)) : it->hole2;
01533 CvPoint hole_ground = it->is_subpixel ? CvPoint(it->hole_groundf) : it->ground_hole;
01534 CvScalar red_color = CV_RGB(255, 0, 0);
01535 CvScalar blue_color = CV_RGB(0, 0, 255);
01536
01537 cvCircle(temp, hole1, 1, it->hole1_detected ? red_color : blue_color, CV_FILLED);
01538 cvCircle(temp, hole2, 1, it->hole2_detected ? red_color : blue_color, CV_FILLED);
01539 cvCircle(temp, hole_ground, 1, it->ground_hole_detected ? red_color : blue_color, CV_FILLED);
01540
01541
01542
01543 }
01544 }
01545
01546 void filter_canny(IplImage* grey, vector<outlet_feature_t>& features)
01547 {
01548 const int max_size = 100;
01549 const float min_dist = 10;
01550
01551 IplImage* canny = cvCloneImage(grey);
01552 cvCanny(grey, canny, 20, 40);
01553
01554 IplImage* canny1 = cvCloneImage(canny);
01555 CvMemStorage* storage = cvCreateMemStorage();
01556 CvSeq* first = 0;
01557 cvFindContours(canny1, storage, &first, sizeof(CvContour), CV_RETR_CCOMP);
01558 for(CvSeq* seq = first; seq != 0; seq = seq->h_next)
01559 {
01560 CvRect rect = cvBoundingRect(seq);
01561 if(MAX(rect.width, rect.height) < max_size && seq->total < max_size)
01562 {
01563 cvDrawContours(canny, seq, cvScalar(0), cvScalar(0), 0, CV_FILLED);
01564 }
01565 }
01566
01567
01568
01569
01570
01571
01572
01573
01574
01575
01576 for(int i = 0; i < min_dist; i++)
01577 {
01578 cvDilate(canny, canny);
01579 }
01580 vector<outlet_feature_t> filtered;
01581 for(vector<outlet_feature_t>::const_iterator it = features.begin(); it != features.end(); it++)
01582 {
01583 CvPoint center = cvPoint(it->bbox.x + it->bbox.width/2, it->bbox.y + it->bbox.height/2);
01584 if(canny->imageData[center.y*canny->widthStep + center.x] == 0)
01585 {
01586 filtered.push_back(*it);
01587 }
01588 }
01589
01590 features = filtered;
01591 }
01592
01593 IplImage* load_match_template_mask(const char* filename)
01594 {
01595 char buf[1024];
01596 sprintf(buf, "../../../rectify_outlets/mask/%s", filename);
01597 strcpy(buf + strlen(buf) - 3, "jpg");
01598 IplImage* mask = cvLoadImage(buf);
01599 return(mask);
01600 }
01601
01602 int load_homography_map(const char* filename, CvMat** map_matrix)
01603 {
01604 char buf[1024];
01605 sprintf(buf, "../../../rectify_outlets/homography/%s", filename);
01606 strcpy(buf + strlen(buf) - 3, "xml");
01607 *map_matrix = (CvMat*)cvLoad(buf);
01608 if(*map_matrix == 0)
01609 {
01610 return 0;
01611 }
01612 else
01613 {
01614 return 1;
01615 }
01616 }
01617
01618 void filter_outlets_templ_ex(vector<outlet_t>& outlets, CvMat* map_matrix, IplImage* mask)
01619 {
01620 const int outlet_width = 50;
01621 const int outlet_height = 25;
01622
01623 vector<outlet_t> filtered;
01624 CvMat* src = cvCreateMat(1, 1, CV_32FC2);
01625 CvMat* dst = cvCreateMat(1, 1, CV_32FC2);
01626
01627 for(vector<outlet_t>::const_iterator it = outlets.begin(); it != outlets.end(); it++)
01628 {
01629 CvPoint2D32f center = cvPoint2D32f((it->hole1.x + it->hole2.x)*0.5f, (it->hole1.y + it->hole2.y)*0.5f);
01630
01631
01632 src->data.fl[0] = center.x;
01633 src->data.fl[1] = center.y;
01634 cvPerspectiveTransform(src, dst, map_matrix);
01635 CvPoint mapped_center = cvPoint((int)dst->data.fl[0] - outlet_width/2, (int)dst->data.fl[1] - outlet_height/2);
01636 if(is_point_inside_rect(cvRect(0, 0, mask->width, mask->height), mapped_center) &&
01637 mask->imageData[mapped_center.y*mask->widthStep + mapped_center.x])
01638 {
01639 filtered.push_back(*it);
01640 }
01641
01642 }
01643
01644 cvReleaseMat(&src);
01645 cvReleaseMat(&dst);
01646
01647 outlets = filtered;
01648 }
01649
01650
01651 int filter_outlets_templ(vector<outlet_t>& outlets, const char* filename)
01652 {
01653
01654 IplImage* mask = load_match_template_mask(filename);
01655 if(mask == 0)
01656 {
01657 printf("Homography mask not found for image %s\n", filename);
01658 return 0;
01659 }
01660 cvThreshold(mask, mask, 128, 255, CV_THRESH_BINARY);
01661 cvDilate(mask, mask, 0, 2);
01662
01663 CvMat* map_matrix = 0;
01664 int ret = load_homography_map(filename, &map_matrix);
01665 if(ret == 0)
01666 {
01667 printf("Matrix not found for image %s\n", filename);
01668 return 0;
01669 }
01670
01671 filter_outlets_templ_ex(outlets, map_matrix, mask);
01672
01673 cvReleaseImage(&mask);
01674 cvReleaseMat(&map_matrix);
01675
01676 return 1;
01677 }
01678
01679 CvPoint3D32f map_point_rt(CvPoint3D32f point, CvMat* rotation_mat, CvMat* translation_vector)
01680 {
01681 CvMat* _point = cvCreateMat(3, 1, CV_32FC1);
01682 cvmSet(_point, 0, 0, point.x);
01683 cvmSet(_point, 1, 0, point.y);
01684 cvmSet(_point, 2, 0, point.z);
01685
01686 CvMat* _res = cvCreateMat(3, 1, CV_32FC1);
01687
01688 cvMatMulAdd(rotation_mat, _point, translation_vector, _res);
01689
01690 CvPoint3D32f res = cvPoint3D32f(cvmGet(_res, 0, 0), cvmGet(_res, 1, 0), cvmGet(_res, 2, 0));
01691
01692 cvReleaseMat(&_res);
01693 cvReleaseMat(&_point);
01694
01695 return res;
01696 }
01697
01698 void getImagePoints(const std::vector<outlet_t>& outlets, std::vector<cv::Point2f>& image_points, std::vector<bool>& is_detected)
01699 {
01700 const int point_count = outlets.size()*3;
01701 image_points.resize(point_count);
01702 is_detected.resize(point_count);
01703
01704 for(size_t i = 0; i < outlets.size(); i++)
01705 {
01706 image_points[3*i] = outlets[i].is_subpixel ? outlets[i].hole1f : cv::Point2f(outlets[i].hole1);
01707 is_detected[3*i] = outlets[i].hole1_detected;
01708
01709 image_points[3*i + 1] = outlets[i].is_subpixel ? outlets[i].hole2f : cv::Point2f(outlets[i].hole2);
01710 is_detected[3*i + 1] = outlets[i].hole2_detected;
01711
01712 image_points[3*i + 2] = outlets[i].is_subpixel ? outlets[i].hole_groundf : cv::Point2f(outlets[i].ground_hole);
01713 is_detected[3*i + 2] = outlets[i].ground_hole_detected;
01714 }
01715 }
01716
01717 void estimateCameraPosition(const vector<cv::Point2f>& image_points, const vector<cv::Point3f>& object_points,
01718 CvMat* intrinsic_matrix, CvMat* distortion_params, CvMat* rotation_vector, CvMat* translation_vector)
01719 {
01720 int point_count = image_points.size();
01721 CvMat* object_mat = cvCreateMat(point_count, 3, CV_32FC1);
01722 CvMat* image_mat = cvCreateMat(point_count, 2, CV_32FC1);
01723
01724 for(int i = 0; i < point_count; i++)
01725 {
01726 cvmSet(object_mat, i, 0, object_points[i].x);
01727 cvmSet(object_mat, i, 1, object_points[i].y);
01728 cvmSet(object_mat, i, 2, object_points[i].z);
01729
01730 cvmSet(image_mat, i, 0, image_points[i].x);
01731 cvmSet(image_mat, i, 1, image_points[i].y);
01732 }
01733
01734
01735 cvFindExtrinsicCameraParams2(object_mat, image_mat, intrinsic_matrix, distortion_params,
01736 rotation_vector, translation_vector);
01737
01738 cvReleaseMat(&object_mat);
01739 cvReleaseMat(&image_mat);
01740 }
01741
01742 void calc_outlet_coords(CvMat* rotation_vector, CvMat* translation_vector, const vector<cv::Point3f>& object_points, vector<outlet_t>& outlets)
01743 {
01744 CvMat* rotation_mat = cvCreateMat(3, 3, CV_32FC1);
01745 cvRodrigues2(rotation_vector, rotation_mat);
01746
01747 for(size_t j = 0; j < outlets.size(); j++)
01748 {
01749 outlets[j].coord_hole1 = map_point_rt(object_points[3*j], rotation_mat, translation_vector);
01750 outlets[j].coord_hole2 = map_point_rt(object_points[3*j + 1], rotation_mat, translation_vector);
01751 outlets[j].coord_hole_ground = map_point_rt(object_points[3*j + 2], rotation_mat, translation_vector);
01752 }
01753
01754 cvReleaseMat(&rotation_mat);
01755 }
01756
01757 void calc_outlet_coords(vector<outlet_t>& outlets, const outlet_template_t& outlet_template,
01758 CvMat* intrinsic_matrix, CvMat* distortion_params)
01759 {
01760 int point_count = outlet_template.get_count()*3;
01761
01762 std::vector<cv::Point3f> object_points;
01763 outlet_template.get_holes_3d(object_points);
01764
01765 std::vector<cv::Point2f> image_points;
01766 std::vector<bool> is_detected;
01767 getImagePoints(outlets, image_points, is_detected);
01768
01769 cv::Mat object_mat(point_count, 3, CV_32FC1);
01770 cv::Mat image_mat(point_count, 2, CV_32FC1);
01771
01772 int row = 0;
01773 for(int i = 0; i < point_count; i++)
01774 {
01775 if(!is_detected[i]) continue;
01776 object_mat.at<float>(row, 0) = object_points[i].x;
01777 object_mat.at<float>(row, 1) = object_points[i].y;
01778 object_mat.at<float>(row, 2) = object_points[i].z;
01779
01780 image_mat.at<float>(row, 0) = image_points[i].x;
01781 image_mat.at<float>(row, 1) = image_points[i].y;
01782
01783 row++;
01784 }
01785
01786
01787 cv::Mat rotation_vector(3, 1, CV_32FC1);
01788 CvMat _rotation_vector = rotation_vector;
01789 cv::Mat translation_vector(3, 1, CV_32FC1);
01790 CvMat _translation_vector = translation_vector;
01791
01792 CvMat _object_mat = object_mat.rowRange(0, row);
01793 CvMat _image_mat = image_mat.rowRange(0, row);
01794 cvFindExtrinsicCameraParams2(&_object_mat, &_image_mat, intrinsic_matrix, distortion_params,
01795 &_rotation_vector, &_translation_vector);
01796
01797 cv::Mat rotation_mat(3, 3, CV_32FC1);
01798 CvMat _rotation_mat = rotation_mat;
01799 cvRodrigues2(&_rotation_vector, &_rotation_mat);
01800
01801 for(size_t j = 0; j < outlets.size(); j++)
01802 {
01803 outlets[j].coord_hole1 = map_point_rt(object_points[3*j], &_rotation_mat, &_translation_vector);
01804 outlets[j].coord_hole2 = map_point_rt(object_points[3*j + 1], &_rotation_mat, &_translation_vector);
01805 outlets[j].coord_hole_ground = map_point_rt(object_points[3*j + 2], &_rotation_mat, &_translation_vector);
01806
01807 }
01808 }
01809
01810 void calc_outlet_coords_ground(vector<outlet_t>& outlets, const outlet_template_t& outlet_template,
01811 CvMat* intrinsic_matrix, CvMat* distortion_params)
01812 {
01813 int point_count = outlet_template.get_count()*3;
01814 CvPoint3D32f* object_points = new CvPoint3D32f[point_count];
01815 CvMat* object_mat = cvCreateMat(outlets.size(), 3, CV_32FC1);
01816 CvPoint2D32f* image_points = new CvPoint2D32f[point_count];
01817 CvMat* image_mat = cvCreateMat(outlets.size(), 2, CV_32FC1);
01818
01819 outlet_template.get_holes_3d(object_points);
01820 for(int i = 2; i < point_count; i += 3)
01821 {
01822 cvmSet(object_mat, (i-2)/3, 0, object_points[i].x);
01823 cvmSet(object_mat, (i-2)/3, 1, object_points[i].y);
01824 cvmSet(object_mat, (i-2)/3, 2, object_points[i].z);
01825 }
01826
01827 for(size_t j = 0; j < outlets.size(); j++)
01828 {
01829 cvmSet(image_mat, j, 0, outlets[j].ground_hole.x);
01830 cvmSet(image_mat, j, 1, outlets[j].ground_hole.y);
01831 }
01832
01833 CvMat* rotation_vector = cvCreateMat(3, 1, CV_32FC1);
01834 CvMat* translation_vector = cvCreateMat(3, 1, CV_32FC1);
01835
01836 cvFindExtrinsicCameraParams2(object_mat, image_mat, intrinsic_matrix, distortion_params,
01837 rotation_vector, translation_vector);
01838
01839 CvMat* rotation_mat = cvCreateMat(3, 3, CV_32FC1);
01840 cvRodrigues2(rotation_vector, rotation_mat);
01841
01842 for(size_t j = 0; j < outlets.size(); j++)
01843 {
01844 outlets[j].coord_hole1 = map_point_rt(object_points[3*j], rotation_mat, translation_vector);
01845 outlets[j].coord_hole2 = map_point_rt(object_points[3*j + 1], rotation_mat, translation_vector);
01846 outlets[j].coord_hole_ground = map_point_rt(object_points[3*j + 2], rotation_mat, translation_vector);
01847 }
01848
01849 cvReleaseMat(&rotation_mat);
01850 cvReleaseMat(&rotation_vector);
01851 cvReleaseMat(&translation_vector);
01852 cvReleaseMat(&object_mat);
01853 cvReleaseMat(&image_mat);
01854 delete []object_points;
01855 delete []image_points;
01856 }
01857
01858
01859 int calc_outlet_coords(vector<outlet_t>& outlets, CvMat* map_matrix, CvPoint3D32f origin, CvPoint2D32f scale,
01860 CvMat* rotation_vector, CvMat* translation_vector, CvMat* inv_map_matrix)
01861 {
01862
01863 CvMat* rotation_mat = cvCreateMat(3, 3, CV_32FC1);
01864 cvRodrigues2(rotation_vector, rotation_mat);
01865
01866
01867 CvMat* src = cvCreateMat(1, 2, CV_32FC2);
01868 CvMat* dst = cvCreateMat(1, 2, CV_32FC2);
01869
01870 for(vector<outlet_t>::iterator it = outlets.begin(); it != outlets.end(); it++)
01871 {
01872 src->data.fl[0] = it->hole1.x;
01873 src->data.fl[1] = it->hole1.y;
01874 src->data.fl[2] = it->hole2.x;
01875 src->data.fl[3] = it->hole2.y;
01876 cvPerspectiveTransform(src, dst, map_matrix);
01877 it->coord_hole1 = cvPoint3D32f((dst->data.fl[0] - origin.x)*scale.x,
01878 (dst->data.fl[1] - origin.y)*scale.y, -origin.z);
01879 it->coord_hole2 = cvPoint3D32f((dst->data.fl[2] - origin.x)*scale.x,
01880 (dst->data.fl[3] - origin.y)*scale.y, -origin.z);
01881 const float ground_hole_offset = 11.5;
01882 it->coord_hole_ground = cvPoint3D32f((it->coord_hole1.x + it->coord_hole2.x)*0.5f,
01883 (it->coord_hole1.y + it->coord_hole2.y)*0.5f - ground_hole_offset,
01884 0.0f);
01885
01886 #if !defined(_GHT) // TBD somehow the inverse matrix here is corrupt when _GHT is defined
01887 if(inv_map_matrix)
01888 {
01889
01890 src->data.fl[0] = it->coord_hole_ground.x;
01891 src->data.fl[1] = it->coord_hole_ground.y;
01892 cvPerspectiveTransform(src, dst, inv_map_matrix);
01893 it->ground_hole = cvPoint(floor(dst->data.fl[0]), floor(dst->data.fl[1]));
01894 }
01895 #endif //_GHT
01896
01897 it->coord_hole1 = map_point_rt(it->coord_hole1, rotation_mat, translation_vector);
01898 it->coord_hole2 = map_point_rt(it->coord_hole2, rotation_mat, translation_vector);
01899 it->coord_hole_ground = map_point_rt(it->coord_hole_ground, rotation_mat, translation_vector);
01900 }
01901
01902 cvReleaseMat(&src);
01903 cvReleaseMat(&dst);
01904
01905 cvReleaseMat(&rotation_mat);
01906
01907 return 1;
01908 }
01909
01910 void calc_outlet_dist_stat(const vector<outlet_t>& outlets, float& mean, float& stddev)
01911 {
01912 float sum = 0;
01913 float sum2 = 0;
01914
01915 for(vector<outlet_t>::const_iterator it = outlets.begin(); it != outlets.end(); it++)
01916 {
01917 float diff = length(it->coord_hole2 - it->coord_hole1);
01918 sum += diff;
01919 sum2 += diff*diff;
01920 }
01921 mean = sum/outlets.size();
01922 stddev = sqrt(sum2/outlets.size() - mean*mean);
01923 }
01924
01925 void calc_outlet_tuple_dist_stat(const vector<outlet_t>& outlets, float& ground_dist_x1,
01926 float& ground_dist_x2, float& ground_dist_y)
01927 {
01928 if(outlets.size() < 4) return;
01929
01930
01931
01932
01933
01934
01935
01936
01937
01938
01939 ground_dist_x1 = length(outlets[1].coord_hole_ground - outlets[0].coord_hole_ground);
01940 ground_dist_x2 = length(outlets[2].coord_hole_ground - outlets[3].coord_hole_ground);
01941 ground_dist_y = length(outlets[2].coord_hole_ground - outlets[1].coord_hole_ground);
01942 }
01943
01944 int find_origin_chessboard(IplImage* src, CvMat* map_matrix, CvPoint3D32f& origin, float bar_length)
01945 {
01946 const int board_width = 6;
01947 const int board_height = 9;
01948 const int corners_count = 6*9;
01949 CvPoint2D32f corners[corners_count];
01950 int found_corners = 0;
01951 cvFindChessboardCorners(src, cvSize(board_width, board_height), corners, &found_corners);
01952 if(found_corners < 4*board_width)
01953 {
01954 return 0;
01955 }
01956
01957 CvMat* _src = cvCreateMat(1, 2, CV_32FC2);
01958 CvMat* _dst = cvCreateMat(1, 2, CV_32FC2);
01959 _src->data.fl[0] = corners[3*board_width].x;
01960 _src->data.fl[1] = corners[3*board_width].y;
01961 _src->data.fl[2] = corners[3*board_width + board_width - 1].x;
01962 _src->data.fl[3] = corners[3*board_width + board_width - 1].y;
01963 cvPerspectiveTransform(_src, _dst, map_matrix);
01964 origin = cvPoint3D32f(_dst->data.fl[0], _dst->data.fl[1], 0);
01965 bar_length = (_dst->data.fl[2] - _dst->data.fl[0])/(board_width - 1);
01966
01967 return 0;
01968 }
01969
01970 void filter_outlets_mask(vector<outlet_t>& outlets, IplImage* mask)
01971 {
01972 vector<outlet_t> filtered;
01973 for(vector<outlet_t>::const_iterator it = outlets.begin(); it != outlets.end(); it++)
01974 {
01975 const outlet_t outlet = *it;
01976 if(mask->imageData[it->hole1.y*mask->widthStep + it->hole1.x] != 0 &&
01977 mask->imageData[it->hole2.y*mask->widthStep + it->hole2.x] != 0)
01978 {
01979 filtered.push_back(*it);
01980 }
01981 }
01982
01983 outlets = filtered;
01984 }
01985
01986 void filter_features_mask(vector<outlet_feature_t>& features, IplImage* mask)
01987 {
01988 vector<outlet_feature_t> filtered;
01989 for(vector<outlet_feature_t>::const_iterator it = features.begin(); it != features.end(); it++)
01990 {
01991 CvPoint center = cvPoint(it->bbox.x + it->bbox.width/2, it->bbox.y + it->bbox.height/2);
01992 if(mask->imageData[mask->widthStep*center.y + center.x] != 0)
01993 {
01994 filtered.push_back(*it);
01995 }
01996 }
01997
01998 features = filtered;
01999 }
02000
02001 IplImage* calc_tuple_distance_map(IplImage* tuple_mask)
02002 {
02003 IplImage* mask = cvCloneImage(tuple_mask);
02004
02005
02006
02007 cvCopy(tuple_mask, mask);
02008
02009
02010 IplImage* dist = cvCreateImage(cvSize(mask->width, mask->height), IPL_DEPTH_32F, 1);
02011 cvDistTransform(mask, dist);
02012
02013 double minval, maxval;
02014 cvMinMaxLoc(dist, &minval, &maxval);
02015
02016 cvReleaseImage(&mask);
02017
02018 return dist;
02019 }
02020
02021 void filter_features_distance_mask(vector<outlet_feature_t>& features, IplImage* distance_map)
02022 {
02023 vector<outlet_feature_t> filtered;
02024
02025 const double dist_factor = 0.5;
02026 double dist_max = 0;
02027 cvMinMaxLoc(distance_map, 0, &dist_max);
02028 double dist_min = dist_max*dist_factor;
02029
02030 for(vector<outlet_feature_t>::iterator it = features.begin(); it != features.end(); it++)
02031 {
02032 CvPoint center = feature_center(*it);
02033 float dist = *((float*)(distance_map->imageData + distance_map->widthStep*center.y) + center.x);
02034 if(dist > dist_min)
02035 {
02036 it->weight = dist;
02037 filtered.push_back(*it);
02038 }
02039
02040 #if 0
02041 if(abs(center.x - 207) < 5 && abs(center.y - 185) < 5)
02042 {
02043 int w = 1;
02044 }
02045 #endif
02046
02047 }
02048
02049 features = filtered;
02050 }
02051
02052 int find_outlet_position(outlet_t outlet, IplImage* tuple_mask)
02053 {
02054 int idx1 = (unsigned char)tuple_mask->imageData[outlet.hole1.y*tuple_mask->widthStep + outlet.hole1.x];
02055 int idx2 = (unsigned char)tuple_mask->imageData[outlet.hole2.y*tuple_mask->widthStep + outlet.hole2.x];
02056
02057 if(idx1 == idx2)
02058 {
02059 return idx1;
02060 }
02061 else
02062 {
02063 return -1;
02064 }
02065 }
02066
02067 void filter_outlets_tuple(vector<outlet_t>& outlets, IplImage* tuple_mask, CvPoint2D32f hor_dir)
02068 {
02069 vector<outlet_t> filtered;
02070 vector<int> outlets_idx;
02071
02072 for(vector<outlet_t>::const_iterator it = outlets.begin(); it != outlets.end(); it++)
02073 {
02074 int idx = find_outlet_position(*it, tuple_mask);
02075 outlets_idx.push_back(idx);
02076 }
02077
02078 for(int i = 0; i < 4; i++)
02079 {
02080 vector<outlet_t> candidates;
02081 for(unsigned int j = 0; j < outlets.size(); j++)
02082 {
02083 if(outlets_idx[j] == i + 1)
02084 {
02085 candidates.push_back(outlets[j]);
02086 }
02087 }
02088
02089 if(candidates.size() > 0)
02090 {
02091 #if 0
02092 select_orient_outlets(hor_dir, candidates, 2);
02093 int idx = candidates[0].feature1.weight + candidates[0].feature2.weight >
02094 candidates[1].feature1.weight + candidates[1].feature2.weight ? 0 : 1;
02095 filtered.push_back(candidates[0]);
02096 #else
02097 select_central_outlets(candidates, 1);
02098 filtered.push_back(candidates[0]);
02099 #endif
02100 }
02101 }
02102
02103 outlets = filtered;
02104 }
02105
02106 void get_outlet_coordinates(const outlet_t& outlet, CvPoint3D32f* points)
02107 {
02108 points[1] = outlet.coord_hole1;
02109 points[2] = outlet.coord_hole2;
02110 points[0] = outlet.coord_hole_ground;
02111 }
02112
02113 void filter_outlets_size(vector<outlet_t>& outlets)
02114 {
02115 const float outlet_length = 12.0f;
02116 const float outlet_length_min = outlet_length*0.8f;
02117 const float outlet_length_max = outlet_length/0.8f;
02118
02119 vector<outlet_t> filtered;
02120 for(vector<outlet_t>::const_iterator it = outlets.begin(); it != outlets.end(); it++)
02121 {
02122 float dist = length(it->coord_hole1 - it->coord_hole2);
02123 if(dist > outlet_length_min && dist < outlet_length_max)
02124 {
02125 filtered.push_back(*it);
02126 }
02127 }
02128
02129 outlets = filtered;
02130 }
02131
02132 CvRect getOutletROI(const vector<outlet_t>& outlets)
02133 {
02134 CvPoint pmin = cvPoint(1e10, 1e10);
02135 CvPoint pmax = cvPoint(-1e10, -1e10);
02136
02137 for(size_t i = 0; i < outlets.size(); i++)
02138 {
02139 pmin.x = MIN(pmin.x, MIN(outlets[i].hole1.x, MIN(outlets[i].hole2.x,
02140 outlets[i].ground_hole.x)));
02141 pmin.y = MIN(pmin.y, MIN(outlets[i].hole1.y, MIN(outlets[i].hole2.y,
02142 outlets[i].ground_hole.y)));
02143 pmax.x = MAX(pmax.x, MAX(outlets[i].hole1.x, MAX(outlets[i].hole2.x,
02144 outlets[i].ground_hole.x)));
02145 pmax.y = MAX(pmax.y, MAX(outlets[i].hole1.y, MAX(outlets[i].hole2.y,
02146 outlets[i].ground_hole.y)));
02147 }
02148
02149 CvRect roi = cvRect(pmin.x, pmin.y, pmax.x - pmin.x + 1, pmax.y - pmin.y + 1);
02150 return(roi);
02151 }
02152
02153 void findPreciseOutletLocations(IplImage* grey, const outlet_template_t& outlet_template, vector<outlet_t>& outlets)
02154 {
02155 if(outlets.size() != 2)
02156 {
02157 printf("findPreciseHoleLocations: unsupported number of outlets\n");
02158 return;
02159 }
02160
02161 const float outlet_hole_length_r = 9;
02162 const float outlet_hole_length_l = 7;
02163
02164 std::vector<cv::Point3f> template_holes;
02165 outlet_template.get_holes_3d(template_holes);
02166 float holes_vert_dist_3d = length(template_holes[4] - template_holes[1]);
02167 cv::Point2f dir_l = cv::Point2f(outlets[0].hole1) - cv::Point2f(outlets[1].hole1);
02168 cv::Point2f dir_r = cv::Point2f(outlets[0].hole2) - cv::Point2f(outlets[1].hole2);
02169
02170 dir_l = dir_l*(outlet_hole_length_l/holes_vert_dist_3d);
02171 dir_r = dir_r*(outlet_hole_length_r/holes_vert_dist_3d);
02172
02173 float hole_width = 5;
02174 cv::Point2f dir_l_perp = cv::Point2f(-dir_l.y, dir_l.x)*(hole_width/outlet_hole_length_l);
02175 cv::Point2f dir_r_perp = cv::Point2f(-dir_r.y, dir_r.x)*(hole_width/outlet_hole_length_r);
02176
02177 for(size_t i = 0; i < 2; i++)
02178 {
02179 cv::Point2f hole;
02180
02181 findPrecisePowerHoleLocation(grey, outlets[i].hole1, dir_l, dir_l_perp, outlets[i].hole1f);
02182
02183 findPrecisePowerHoleLocation(grey, outlets[i].hole2, dir_r, dir_r_perp, outlets[i].hole2f);
02184
02185 findPreciseGroundHoleLocation(grey, outlets[i].ground_hole, outlets[i].hole_groundf);
02186
02187 outlets[i].is_subpixel = true;
02188 }
02189 }
02190
02191 float avgLine(IplImage* grey, cv::Point2f* line_ends)
02192 {
02193 CvLineIterator line_it;
02194 int count = cvInitLineIterator(grey, cv::Point(line_ends[0].x, line_ends[0].y), cv::Point(line_ends[1].x, line_ends[1].y), &line_it);
02195
02196 float sum = 0;
02197 for(int i = 0; i < count; i++)
02198 {
02199 sum += line_it.ptr[0];
02200 CV_NEXT_LINE_POINT(line_it);
02201 }
02202
02203 return sum/count;
02204 }
02205
02206 void findPrecisePowerHoleLocation(IplImage* grey, cv::Point2f center, cv::Point2f dir, cv::Point2f dir_perp, cv::Point2f& hole)
02207 {
02208 const float d_step = 0.05f;
02209 const float d_perp_step = 0.2f;
02210 center += cv::Point2f(0.5f, 0.5f);
02211
02212 float min_sum = 1e10;
02213 for(float d = 0; d < 1; d += d_step)
02214 {
02215 for(float d_perp = 0; d_perp < 1; d_perp += d_perp_step)
02216 {
02217 cv::Point2f p = center + dir*(d - 0.5f) + dir_perp*(d_perp - 0.5f);
02218 cv::Point2f line_ends[2];
02219 line_ends[0] = p - dir*0.5f;
02220 line_ends[1] = p + dir*0.5f;
02221
02222 float sum = avgLine(grey, line_ends);
02223 if(sum < min_sum)
02224 {
02225 min_sum = sum;
02226 hole = p;
02227 }
02228 }
02229 }
02230 }
02231
02232 void findPreciseGroundHoleLocation(IplImage* grey, cv::Point2f center, cv::Point2f& hole)
02233 {
02234 #if 0
02235
02236 hole = center + cv::Point2f(0.5f, 0.5f);
02237 #else
02238 cv::Mat mask(grey->height + 2, grey->width + 2, CV_8UC1);
02239 IplImage _mask = mask;
02240 cvSetZero(&_mask);
02241 CvConnectedComp comp;
02242 const int color_offset = 10;
02243 CvRect roi = cvGetImageROI(&_mask);
02244
02245 cvFloodFill(grey, CvPoint(center), cvScalar(255), cvScalar(color_offset), cvScalar(color_offset), &comp, 4 | CV_FLOODFILL_MASK_ONLY, &_mask);
02246 hole = cv::Point2f(comp.rect.x + float(comp.rect.width)/2, comp.rect.y + float(comp.rect.height)/2);
02247 #endif
02248 }
02249
02250 void findPreciseOutletLocationsAvg(IplImage* grey, const outlet_template_t& outlet_template, vector<outlet_t>& outlets)
02251 {
02252 CvRect roi = cvGetImageROI(grey);
02253 cv::Mat mat(roi.height, roi.width, CV_8UC1);
02254 IplImage _mat = mat;
02255 CvRNG rng = cvRNG(0xffffffff);
02256
02257 for(size_t i = 0; i < outlets.size(); i++)
02258 {
02259 outlets[i].hole1f = cv::Point2f(0.0f, 0.0f);
02260 outlets[i].hole2f = cv::Point2f(0.0f, 0.0f);
02261 outlets[i].hole_groundf = cv::Point2f(0.0f, 0.0f);
02262 }
02263
02264 const int count = 50;
02265 vector<outlet_t> _outlets = outlets;
02266 for(int k = 0; k < count; k++)
02267 {
02268 cvRandArr(&rng, &_mat, CV_RAND_UNI, cvScalar(0), cvScalar(100));
02269
02270
02271
02272
02273
02274 cvAdd(&_mat, grey, &_mat);
02275
02276 findPreciseOutletLocations(&_mat, outlet_template, _outlets);
02277
02278 for(size_t i = 0; i < outlets.size(); i++)
02279 {
02280 outlets[i].hole1f += _outlets[i].hole1f;
02281 outlets[i].hole2f += _outlets[i].hole2f;
02282 outlets[i].hole_groundf += _outlets[i].hole_groundf;
02283 }
02284 }
02285
02286 for(size_t i = 0; i < outlets.size(); i++)
02287 {
02288 outlets[i].hole1f *= 1.0f/count;
02289 outlets[i].hole2f *= 1.0f/count;
02290 outlets[i].hole_groundf *= 1.0f/count;
02291
02292 outlets[i].is_subpixel = true;
02293 }
02294 }
02295
02296 cv::Point3f flipVector(cv::Point3f vec, cv::Point3f center)
02297 {
02298 double center_length = length(center);
02299 cv::Point3f flipped = vec - center*float((center.dot(vec - center)/(center_length*center_length)));
02300 return flipped;
02301 }
02302
02303 void flipOutlet(std::vector<outlet_t>& outlets)
02304 {
02305 if(outlets.size() != 2) return;
02306 if(length(outlets[0].coord_hole_ground) < length(outlets[1].coord_hole_ground)) return;
02307
02308
02309
02310 cv::Point3f center(0.0, 0.0, 0.0);
02311 #if 0
02312 for(size_t i = 0; i < outlets.size(); i++)
02313 {
02314 center = center + outlets[i].coord_hole1 + outlets[i].coord_hole2 + outlets[i].coord_hole_ground;
02315 }
02316 center = center*(1.0f/3/outlets.size());
02317 #else
02318 center = outlets[0].coord_hole_ground;
02319 #endif
02320
02321 for(size_t i = 0; i < outlets.size(); i++)
02322 {
02323
02324 outlets[i].coord_hole_ground = flipVector(outlets[i].coord_hole_ground, center);
02325 outlets[i].coord_hole1 = flipVector(outlets[i].coord_hole1, center);
02326 outlets[i].coord_hole2 = flipVector(outlets[i].coord_hole2, center);
02327 }
02328 }