outlet_model.cpp
Go to the documentation of this file.
00001 // outlet_model.cpp :
00002 //
00003 //*****************************************************************************************
00004 // Warning: this is research code with poor architecture, performance and no documentation!
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 //              float area = fabs(cvContourArea(seq));
00078 //              float length = cvArcLength(seq);
00079 //              float circle_coeff = fabs(area*4*pi)/(length*length);
00080 /*              if(circle_coeff < 0.8)
00081                 {
00082                         continue;
00083                 }
00084                 assert(circle_coeff <= 1.5);
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                 // !! this is old contrast calculation that I found unnecessary on
00108                 // the latest data (16mm and 5mp)
00109                 // so this is disabled to save computational time
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                 // !! this is old contrast calculation that I found unnecessary on
00125                 // the latest data (16mm and 5mp)
00126                 // so this is disabled to save computational time
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                 // return the mask in its previous state
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 //              const float avg_factor = 1.3f;
00153 #endif //_EUROPE
00154 
00155                 float contrast, variation;
00156                 calc_contrast_factor(grey, rect, contrast, variation);
00157 #if !defined(_TUNING)
00158                 if(/*std_outside.val[0]/avg_outside.val[0] > 0.3f ||*/ avg_outside.val[0] < avg_inside*avg_factor)
00159                 {
00160                         continue;
00161                 }
00162 #else
00163                 if(contrast < hole_contrast)// /*|| variation > 0.7f*/ || avg_outside.val[0] < avg_inside*1.0f)
00164                 {
00165                         continue;
00166                 }
00167 #endif //_TUNING
00168                 //printf("Std outside = %f\n", std_outside.val[0]/avg_outside.val[0]);
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;//7;//20;
00201         const int max_dist = 80;//30;//100;
00202         const int max_dist_prox = 2*max_dist;
00203 #endif //_EUROPE
00204 
00205         // we will keep a list of references to outlets for each feature
00206         // we will use this to filter out overlapping outlets
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 //                      if(it1 >= it2) continue;
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                         // check if there are other outlets in the region
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;//candidates[0].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                         //cvRectangle(mask, small_rect, cvScalar(255), CV_FILLED);
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 //                      cvSaveImage("grey.jpg", grey);
00349 //                      cvSaveImage("mask.jpg", mask);
00350 
00351                         // double the size of the rectangle
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 //                              continue;
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         // filter overlapping outlets
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         //cvErode(src, src);
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                 //cvCopy(src, temp);
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 //                                      if(test_adjacency(features, feature) == 0)
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                                 //cvRectangle(imgholes, it->bbox, CV_RGB(255, 0, 0), 1);
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          cvNamedWindow("1", 0);
00580          cvSImage("1", temp);
00581          cvWaitKey(0);
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 //      cvErode(tuple_mask, tuple_mask, 0, 10); // remove feature points on the edges
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         // using template matching for further filtering of fps
00740         // first calculate homography
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                 // now load template and calculate the mask
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         //vector<CvRect> vrect = it->second;
00830         if(it == outlet_roi.end())
00831         {
00832                 // no element with such a name
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                 // copy mat to the beginning of _mat
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                 // now free *mat
00887                 cvReleaseMat(mat);
00888                 *mat = _mat;
00889         }
00890 
00891         // prepare a temporary image
00892         IplImage* features = cvCreateImage(cvSize(xsize, ysize), IPL_DEPTH_8U, 1);
00893 
00894         // now extract intensity values and copy them to the matrix
00895         for(unsigned int r = 0; r < keypts.size(); r++)
00896         {
00897 //              CvPoint center = cvPoint(keypts[r].bbox.x + keypts[r].bbox.width/2,
00898 //                                                               keypts[r].bbox.y + keypts[r].bbox.height/2);
00899 //              int scale = keypts[r].bbox.width;
00900                 const float scale_factor = 2.0f;
00901 //              CvRect roi = cvRect(center.x - scale_factor*scale, center.y - scale_factor*scale,
00902 //                                                      2*scale_factor*scale, 2*scale_factor*scale);
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                         // do not save image patch
00929                         continue;
00930                 }
00931 
00932                 // save the feature to an image
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 /*      IplImage* _temp = cvCreateImage(cvSize(grey->width, grey->height), IPL_DEPTH_8U, 3);
00967         for(int coi = 1; coi < 4; coi++)
00968         {
00969                 cvSetImageCOI(_temp, coi);
00970                 cvCopyImage(grey, _temp);
00971         }
00972         cvSetImageCOI(_temp, 0);
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 //      cvNamedWindow("1", 1);
00992 //      cvShowImage("1", _temp);
00993 //      cvWaitKey();
00994 
00995 //      cvReleaseImage(&_temp);
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;//0.2;
01045 
01046         //cvErode(src, src);
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 //              cvConvertScale(grey, grey, 0.4);
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 //_TUNING
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 //              cvRectangle(tempgrey, cvPoint(0, 0), cvPoint(tempmask->width-1, tempmask->height-1),
01117 //                                      cvScalar(0), 50);
01118                 printf("Processing thresh = %d\n", thresh);
01119                 cvNamedWindow("1", 1);
01120                 cvShowImage("1", tempgrey);
01121                 cvWaitKey(0);
01122 #endif
01123                 //cvCopy(src, temp);
01124                 cvReleaseImage(&tempgrey);
01125 
01126                 cvAnd(mask_white, mask_black, tempmask);
01127 //              cvRectangle(tempmask, cvPoint(0, 0), cvPoint(tempmask->width-1, tempmask->height-1),
01128 //                                      cvScalar(0), 50);
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 //                      if(seq->total < 20)
01135 //                      {
01136 //                              continue;
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 //                              float scale_factor = MAX(2.0f, 2*min_pixel_area/roi.width);
01169 //                              roi = resize_rect(roi, scale_factor);
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                                 //cvCircle(imgholes, cvPoint(it->bbox.x, it->bbox.y), 2, CV_RGB(0, 0, 255));
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          cvNamedWindow("1", 0);
01202          cvSImage("1", temp);
01203          cvWaitKey(0);
01204          */
01205 
01206         // filtering with canny
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                 // no element with such a name
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         // releasing image patches
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                 // now transform outlets into features and calculate labels
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                 // determine outlet class
01371                 int class_id = is_outlet_inside_roi(outlet_roi, *it, image_filename);
01372                 float weight = MIN(it->feature1.weight, it->feature2.weight);//it->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) // leaving fraction negative samples
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         // normalize the vector of horizontal direction
01448         float mod = sqrt(orientation.x*orientation.x + orientation.y*orientation.y);
01449         orientation.x /= mod;
01450         orientation.y /= mod;
01451 
01452         // filter out non-horizontal outlets
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         // find horizonal direction
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 //              cvLine(temp, hole1, hole2, CV_RGB(0, 0, 255), 2);
01541                 //CvRect orect = outlet_rect(*it);
01542                 //cvRectangle(temp, orect, CV_RGB(0, 255, 0), 2);
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 /*      cvCopy(canny, canny1);
01568         for(vector<outlet_feature_t>::const_iterator it = features.begin(); it != features.end(); it++)
01569         {
01570                 cvRectangle(canny1, it->bbox, cvScalar(255), 1);
01571         }
01572         cvNamedWindow("1", 1);
01573         cvShowImage("1", canny1);
01574         cvWaitKey(0);
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; // hack, should be updated each time a template size is changed
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                 // map center point to the mask
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 // uses template matching, currently done offline
01651 int filter_outlets_templ(vector<outlet_t>& outlets, const char* filename)
01652 {
01653         // load mask and homography
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 //        printf("image_points[%d]: %f,%f\n", i, image_points[i].x, image_points[i].y);
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         // create rotation matrix
01863         CvMat* rotation_mat = cvCreateMat(3, 3, CV_32FC1);
01864         cvRodrigues2(rotation_vector, rotation_mat);
01865 
01866         // map outlets to rectified plane
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; // mm
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             // update ground hole image coordinates
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         // calculate the average distance between the holes
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 //      CvPoint2D32f ground_holes[4];
01931 //      for(int i = 0; i < 4; i++)
01932 //      {
01933 //              CvPoint3D32f point = outlets[i].coord_hole_ground;
01934 //              ground_holes[i] = cvPoint2D32f(point.x, point.y);
01935 //      }
01936 
01937 //      order_tuple2(ground_holes); outlets are ordered before, no need to do it here!
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         // create an image with tuple boundaries
02006 
02007         cvCopy(tuple_mask, mask);
02008 
02009         // calculate distance map
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     // for now, just return the original hole -- I didn't see any jitter in ground hole positions
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 //        CvRect roi1 = cvGetImageROI(grey);
02270 //        CvRect roi2 = cvGetImageROI(&_mat);
02271 //        printf("roi input: %d,%d,%d,%d, roi rand: %d,%d,%d,%d\n",
02272 //            roi1.x, roi1.y, roi1.width, roi1.height,
02273 //            roi2.x, roi2.y, roi2.width, roi2.height);
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; // implemented for 2x1 outlet tuple only
02306   if(length(outlets[0].coord_hole_ground) < length(outlets[1].coord_hole_ground)) return; // the pose is correct
02307 
02308   //printf("FLIPPING OUTLET POSE\n");
02309   // otherwise, flip outlet pose
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 //    int sign = i == 0 ? -1 : 1;
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 }


outlet_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Thu Aug 27 2015 14:29:52