outlet_tuple.cpp
Go to the documentation of this file.
00001 /*
00002  *  outlet_tuple.cpp
00003  *  rectify_outlets
00004  *
00005  *  Created by Victor  Eruhimov on 1/25/09.
00006  *  Copyright 2009 Argus Corp. All rights reserved.
00007  *
00008  */
00009 
00010 //*****************************************************************************************
00011 // Warning: this is research code with poor architecture, performance and no documentation!
00012 //*****************************************************************************************
00013 
00014 #include <limits.h>
00015 #include <math.h>
00016 #include <stdio.h>
00017 
00018 #include <vector>
00019 #include <algorithm>
00020 using namespace std;
00021 
00022 #include "outlet_pose_estimation/detail/outlet_tuple.h"
00023 #include <highgui.h>
00024 #include <cvwimage.h>
00025 
00026 #include "outlet_pose_estimation/detail/one_way_descriptor.h"
00027 
00028 const float pi = 3.1415926f;
00029 static const char template_filename[] = "outlet_template.yml";
00030 
00031 
00032 CvPoint2D32f calc_center(CvSeq* seq)
00033 {
00034         CvMoments moments;
00035         cvMoments(seq, &moments);
00036         CvPoint2D32f center;
00037         center.x = cvGetSpatialMoment(&moments, 1, 0);
00038         center.y = cvGetSpatialMoment(&moments, 0, 1);
00039         float area = cvGetSpatialMoment(&moments, 0, 0);
00040         center.x /= area;
00041         center.y /= area;
00042 
00043         return center;
00044 }
00045 
00046 CvPoint2D32f calc_center(const vector<CvPoint2D32f>& points)
00047 {
00048         CvPoint2D32f center = cvPoint2D32f(0, 0);
00049         for(unsigned int i = 0; i < points.size(); i++)
00050         {
00051                 center.x += points[i].x;
00052                 center.y += points[i].y;
00053         }
00054 
00055         center.x /= points.size();
00056         center.y /= points.size();
00057 
00058         return(center);
00059 }
00060 
00061 int find_dir(const CvPoint2D32f* dir, int xsign, int ysign)
00062 {
00063         for(int i = 0; i < 4; i++)
00064         {
00065                 if(dir[i].x*xsign > 0 && dir[i].y*ysign > 0)
00066                 {
00067                         return i;
00068                 }
00069         }
00070         return -1;
00071 }
00072 
00073 int order_tuple(CvPoint2D32f* centers)
00074 {
00075         CvPoint2D32f ordered[4];
00076         int idx[4];
00077 
00078         CvPoint2D32f center = cvPoint2D32f(0.0f, 0.0f);
00079         for(int i = 0; i < 4; i++)
00080         {
00081                 center.x += centers[i].x;
00082                 center.y += centers[i].y;
00083         }
00084         center.x *= 0.25f;
00085         center.y *= 0.25f;
00086 
00087         CvPoint2D32f dir[4];
00088         for(int i = 0; i < 4; i++)
00089         {
00090                 dir[i].x = centers[i].x - center.x;
00091                 dir[i].y = centers[i].y - center.y;
00092         }
00093 
00094         idx[0] = find_dir(dir, -1, -1);
00095         idx[1] = find_dir(dir, 1, -1);
00096         idx[2] = find_dir(dir, 1, 1);
00097         idx[3] = find_dir(dir, -1, 1);
00098 
00099         // check if any of the tuples are not found
00100         int found[4] = {-1,-1,-1,-1};
00101         int count_lost = 0;
00102         int idx_lost = 0;
00103         for(int i = 0; i < 4; i++)
00104         {
00105                 if(idx[i] != -1)
00106                 {
00107                         found[idx[i]] = 1;
00108                 }
00109                 else
00110                 {
00111                         idx_lost = i;
00112                         count_lost++;
00113                 }
00114         }
00115 
00116         if(count_lost > 1)
00117         {
00118                 printf("%d outlets cannot be ordered, not enough for a tuple\n", count_lost);
00119                 return 0;
00120         }
00121 
00122         for(int i = 0; i < 4; i++)
00123         {
00124                 if(found[i] == -1)
00125                 {
00126                         idx[idx_lost] = i;
00127                 }
00128         }
00129 
00130         for(int i = 0; i < 4; i++)
00131         {
00132                 ordered[i] = centers[idx[i]];
00133         }
00134 
00135         for(int i = 0; i < 4; i++)
00136         {
00137                 centers[i] = ordered[i];
00138         }
00139 
00140         return 1;
00141 }
00142 
00143 bool helper_pred_greater(outlet_elem_t h1, outlet_elem_t h2)
00144 {
00145         return h1.angle < h2.angle;
00146 }
00147 
00148 int find_start_idx(const vector<outlet_elem_t>& helper_vec)
00149 {
00150         int start_idx = -1;
00151         float min_angle_diff = 1e10;
00152         for(int i = 0; i < 4; i++)
00153         {
00154                 float angle_diff = fabs(helper_vec[i].angle + 4*pi/5);
00155                 if(angle_diff < min_angle_diff)
00156                 {
00157                         min_angle_diff = angle_diff;
00158                         start_idx = i;
00159                 }
00160         }
00161 
00162         return start_idx;
00163 
00164 }
00165 
00166 
00167 int find_start_idx2(const vector<outlet_elem_t>& helper_vec)
00168 {
00169         for(int i = 0; i < 4; i++)
00170         {
00171                 // find the fartherst point
00172                 float max_dist = 0;
00173                 int max_j = -1;
00174                 for(int j = 0; j < 4; j++)
00175                 {
00176                         if(j == i) continue;
00177 
00178                         float dist = length(helper_vec[i].center - helper_vec[j].center);
00179                         if(dist > max_dist)
00180                         {
00181                                 max_dist = dist;
00182                                 max_j = j;
00183                         }
00184                 }
00185 
00186                 if(helper_vec[i].center.x < helper_vec[max_j].center.x)
00187                 {
00188                         return i;
00189                 }
00190         }
00191 
00192         assert(0);
00193         return -1; // should never reach this point
00194 }
00195 
00196 int find_start_idx3(const vector<outlet_elem_t>& helper_vec)
00197 {
00198         const float mean_angle = -3*pi/4;
00199         float max_angle = pi/2*0.8f;;
00200         for(int i = 0; i < 4; i++)
00201         {
00202                 float angle = helper_vec[i].angle - mean_angle;
00203                 if(angle > pi) angle -= 2*pi;
00204                 if(fabs(angle) > max_angle)
00205                 {
00206                         continue;
00207                 }
00208 
00209                 int prev_idx = (i + 3)%4;
00210                 int post_idx = (i + 1)%4;
00211                 CvPoint2D32f prev_vec = helper_vec[prev_idx].center - helper_vec[i].center;
00212                 CvPoint2D32f post_vec = helper_vec[post_idx].center - helper_vec[i].center;
00213                 float l1 = length(prev_vec);
00214                 float l2 = length(post_vec);
00215                 if(l2 > l1 && post_vec.x > 0)
00216                 {
00217                         return i;
00218                 }
00219         }
00220 
00221         // this is not a tuple
00222         return -1;
00223 
00224 }
00225 
00226 int order_tuple2(vector<outlet_elem_t>& tuple)
00227 {
00228         vector<outlet_elem_t> ordered;
00229 
00230         CvPoint2D32f center = cvPoint2D32f(0.0f, 0.0f);
00231         for(int i = 0; i < 4; i++)
00232         {
00233                 center.x += tuple[i].center.x;
00234                 center.y += tuple[i].center.y;
00235         }
00236         center.x *= 0.25f;
00237         center.y *= 0.25f;
00238 
00239         CvPoint2D32f dir[4];
00240         for(int i = 0; i < 4; i++)
00241         {
00242                 dir[i].x = tuple[i].center.x - center.x;
00243                 dir[i].y = tuple[i].center.y - center.y;
00244 
00245                 tuple[i].angle = atan2(dir[i].y, dir[i].x);
00246                 tuple[i].idx = i;
00247         }
00248         sort(tuple.begin(), tuple.end(), helper_pred_greater);
00249 
00250 #if 0
00251         int start_idx = find_start_idx(helper_vec);
00252 #else
00253         int start_idx = find_start_idx3(tuple);
00254     if(start_idx < 0)
00255     {
00256         return 0;
00257     }
00258 #endif
00259 
00260         ordered = tuple;
00261         for(int i = 0; i < 4; i++)
00262         {
00263                 int _i = (i + start_idx)%4;
00264                 ordered[i] = tuple[_i];
00265         }
00266 
00267         tuple = ordered;
00268         return 1;
00269 }
00270 
00271 int find_outlet_centroids(IplImage* img, outlet_tuple_t& outlet_tuple, const char* output_path, const char* filename)
00272 {
00273     cv::WImageBuffer1_b grey_buf(img->width, img->height);
00274     cv::WImageBuffer1_b binary_buf(img->width, img->height);
00275     IplImage* grey = grey_buf.Ipl();
00276         IplImage* binary = binary_buf.Ipl();
00277         cvCvtColor(img, grey, CV_RGB2GRAY);
00278         cvSmooth(grey, grey);
00279         cvAdaptiveThreshold(grey, binary, 255, CV_ADAPTIVE_THRESH_MEAN_C,
00280                                                 CV_THRESH_BINARY_INV, 13, -1);
00281     IplImage* _binary = cvCloneImage(binary);
00282     CvMemStorage* storage = cvCreateMemStorage();
00283         int found_tuple = 0;
00284 
00285     for(int dstep = 1; dstep < 10; dstep++)
00286     {
00287         cvErode(_binary, binary, 0, dstep);
00288         cvDilate(binary, binary, 0, dstep);
00289 
00290 #if defined(_VERBOSE_TUPLE)
00291         cvNamedWindow("1", 1);
00292         cvShowImage("1", binary);
00293         cvWaitKey(0);
00294         cvSaveImage("mask.jpg", binary);
00295 #endif
00296 
00297         CvSeq* first = 0;
00298         cvFindContours(binary, storage, &first, sizeof(CvContour), CV_RETR_CCOMP);
00299         vector<CvSeq*> candidates;
00300 
00301 #if defined(_VERBOSE_TUPLE)
00302         IplImage* img1 = cvCloneImage(img);
00303 #endif //_VERBOSE_TUPLE
00304 
00305         cv::WImageBuffer1_b mask_buf( cvCloneImage(grey) );
00306         IplImage* mask = mask_buf.Ipl();
00307         for(CvSeq* seq = first; seq != NULL; seq = seq->h_next)
00308         {
00309             CvRect rect = cvBoundingRect(seq);
00310 #if !defined(_OUTLET_HR)
00311             const int xmin = 30;
00312             const int xmax = 150;
00313             const int ymin = 15;
00314             const int ymax = 50;
00315             const int min_width = 5;
00316 #else
00317             const int xmin = 50;
00318             const int xmax = 400;
00319             const int ymin = 30;
00320             const int ymax = 400;
00321             const int min_width = 5;
00322 #endif //OUTLET_HR
00323 
00324 
00325             if(rect.width < xmin || rect.width > xmax || rect.height < ymin || rect.height > ymax)
00326             {
00327                 continue;
00328             }
00329 
00330 #if 0
00331             if(abs(rect.x - 1312) < 50 && abs(rect.y - 1576) < 50)
00332             {
00333                 int w = 1;
00334             }
00335 #endif
00336 
00337             float area = fabs(cvContourArea(seq));
00338             float perimeter = fabs(cvArcLength(seq));
00339 
00340             if(area/perimeter*2 < min_width)
00341             {
00342                 continue;
00343             }
00344 
00345             //cvSetImageROI(img, rect);
00346             cvSetZero(mask);
00347             cvDrawContours(mask, seq, cvScalar(255), cvScalar(255), 0, CV_FILLED);
00348 
00349             CvScalar mean = cvAvg(img, mask);
00350             //cvResetImageROI(img);
00351             if(mean.val[2]/mean.val[1] < 1.8f)
00352             {
00353                 continue;
00354             }
00355 
00356             candidates.push_back(seq);
00357 
00358 #if defined(_VERBOSE_TUPLE)
00359             cvDrawContours(img1, seq, CV_RGB(255, 0, 0), CV_RGB(255, 0, 0), 0, 2);
00360 #endif //_VERBOSE_TUPLE
00361         }
00362 
00363 #if defined(_VERBOSE_TUPLE)
00364         cvNamedWindow("1", 1);
00365         cvShowImage("1", img1);
00366         cvWaitKey(0);
00367         cvReleaseImage(&img1);
00368 #endif //_VERBOSE_TUPLE
00369 
00370         vector<outlet_elem_t> tuple;
00371         for(unsigned int i = 0; i < candidates.size(); i++)
00372         {
00373             vector<outlet_elem_t> tuple_candidates;
00374             outlet_elem_t outlet_elem;
00375             outlet_elem.seq = candidates[i];
00376             outlet_elem.center = calc_center(candidates[i]);
00377             tuple_candidates.push_back(outlet_elem);
00378 
00379             CvRect rect1 = cvBoundingRect(candidates[i]);
00380             for(unsigned int j = 0; j < candidates.size(); j++)
00381             {
00382                 if(j <= i) continue;
00383                 CvRect rect2 = cvBoundingRect(candidates[j]);
00384 
00385                 // test the pair
00386                 CvPoint center1 = rect_center(rect1);
00387                 CvPoint center2 = rect_center(rect2);
00388 
00389                 if(2.0f*(rect1.width - rect2.width)/(rect1.width + rect2.width) > 0.3f ||
00390                    2.0f*(rect1.height - rect2.height)/(rect1.height + rect2.height) > 0.3f)
00391                 {
00392                     continue;
00393                 }
00394 
00395                 float dist = sqrt(float(center1.x - center2.x)*(center1.x - center2.x) +
00396                                   (center1.y - center2.y)*(center1.y - center2.y));
00397                 if(dist > 4*rect1.width)
00398                 {
00399                     continue;
00400                 }
00401 
00402                 // found pair, add rect2 to the tuple
00403                 outlet_elem_t outlet_elem;
00404                 outlet_elem.seq = candidates[j];
00405                 outlet_elem.center = calc_center(candidates[j]);
00406                 tuple_candidates.push_back(outlet_elem);
00407             }
00408 
00409             // find the tuple
00410             found_tuple = find_tuple(tuple_candidates, outlet_tuple.centers);
00411             if(found_tuple == 1)
00412             {
00413                 // found the tuple!
00414                 tuple = tuple_candidates;
00415                 break;
00416             }
00417         }
00418 
00419 #if defined(_VERBOSE_TUPLE) || defined(_VERBOSE)
00420         IplImage* img2 = cvCloneImage(img);
00421 #endif //_VERBOSE_TUPLE
00422 
00423         if(found_tuple == 1)
00424         {
00425             for(int i = 0; i < 4; i++)
00426             {
00427                 tuple[i].seq = close_seq(tuple[i].seq, storage, 10, binary);
00428                 if(tuple[i].seq == 0)
00429                 {
00430                     found_tuple = 0;
00431                     break;
00432                 }
00433             }
00434 
00435             if(found_tuple == 0) break;
00436             // draw the mask
00437             if(outlet_tuple.tuple_mask)
00438             {
00439                 cvSetZero(outlet_tuple.tuple_mask);
00440                 for(int i = 0; i < 4; i++)
00441                 {
00442                     cvDrawContours(outlet_tuple.tuple_mask, tuple[i].seq, cvScalar(i + 1), cvScalar(i + 1), 0, CV_FILLED);
00443                 }
00444             }
00445 
00446             // calculate the tuple roi
00447             CvRect tuple_roi[4];
00448             for(int i = 0; i < 4; i++)
00449             {
00450                 tuple_roi[i] = cvBoundingRect(tuple[i].seq);
00451             }
00452             calc_bounding_rect(4, tuple_roi, outlet_tuple.roi);
00453 
00454 #if defined(_VERBOSE_TUPLE) || defined(_VERBOSE)
00455             cvCircle(img2, cvPoint(outlet_tuple.centers[0]), 10, CV_RGB(0, 255, 0));
00456             cvCircle(img2, cvPoint(outlet_tuple.centers[1]), 10, CV_RGB(0, 0, 255));
00457             cvCircle(img2, cvPoint(outlet_tuple.centers[2]), 10, CV_RGB(255, 255, 255));
00458             cvCircle(img2, cvPoint(outlet_tuple.centers[3]), 10, CV_RGB(0, 255, 255));
00459 #endif //VERBOSE_TUPLE
00460 
00461             // save outlet borders
00462             for(int i = 0; i < 4; i++)
00463             {
00464                 for(int j = 0; j < tuple[i].seq->total; j++)
00465                 {
00466                     CvPoint* p = (CvPoint*)cvGetSeqElem(tuple[i].seq, j);
00467                     outlet_tuple.borders[i].push_back(cvPoint2D32f(p->x, p->y));
00468                 }
00469             }
00470         }
00471 
00472 #if defined(_VERBOSE_TUPLE)
00473         cvNamedWindow("1", 1);
00474         cvShowImage("1", img2);
00475         cvWaitKey(0);
00476 
00477         cvThreshold(outlet_tuple.tuple_mask, binary, 0, 255, CV_THRESH_BINARY);
00478         cvShowImage("1", binary);
00479         cvWaitKey(0);
00480 
00481 #endif //_VERBOSE_TUPLE
00482 
00483 
00484 #if defined(_VERBOSE)
00485         if(output_path && filename)
00486         {
00487             char buf[1024];
00488             sprintf(buf, "%s/warped/%s", output_path, filename);
00489             cvSaveImage(buf, img2);
00490         }
00491 #endif //_VERBOSE
00492 
00493 #if defined(_VERBOSE) || defined(_VERBOSE_TUPLE)
00494         cvReleaseImage(&img2);
00495 #endif
00496         if(found_tuple == 1) break;
00497     }
00498 
00499         cvReleaseMemStorage(&storage);
00500     cvReleaseImage(&_binary);
00501 
00502         return found_tuple;
00503 
00504 }
00505 
00506 CvSeq* close_seq(CvSeq* seq, CvMemStorage* storage, int closure_dist, IplImage* workspace)
00507 {
00508     cvSetZero(workspace);
00509     cvDrawContours(workspace, seq, cvScalar(255), cvScalar(255), 0, CV_FILLED);
00510     cvDilate(workspace, workspace, 0, closure_dist);
00511     cvErode(workspace, workspace, 0, closure_dist);
00512 
00513     CvSeq* first = 0;
00514     cvFindContours(workspace, storage, &first, sizeof(CvContour), CV_RETR_LIST);
00515     CvSeq* hull = cvConvexHull2(first, storage, CV_CLOCKWISE, 1);
00516     return(hull);
00517 }
00518 
00519 void calc_bounding_rect(int count, const CvRect* rects, CvRect& bounding_rect)
00520 {
00521         CvPoint tl = cvPoint(INT_MAX, INT_MAX); // top left point
00522         CvPoint br = cvPoint(INT_MIN, INT_MIN); //bottom right point
00523         for(int i = 0; i < count; i++)
00524         {
00525                 tl.x = MIN(tl.x, rects[i].x);
00526                 tl.y = MIN(tl.y, rects[i].y);
00527                 br.x = MAX(br.x, rects[i].x + rects[i].width);
00528                 br.y = MAX(br.y, rects[i].y + rects[i].height);
00529         }
00530 
00531         bounding_rect.x = tl.x;
00532         bounding_rect.y = tl.y;
00533         bounding_rect.width = br.x - tl.x;
00534         bounding_rect.height = br.y - tl.y;
00535 }
00536 
00537 int find_tuple(vector<outlet_elem_t>& candidates, CvPoint2D32f* centers)
00538 {
00539         if(candidates.size() < 4)
00540         {
00541                 // should be at least 4 candidates for a tuple
00542                 return 0;
00543         }
00544 
00545         if(candidates.size() > 15)
00546         {
00547                 // too many candidates -- the algorithm will be slow
00548                 return 0;
00549         }
00550 
00551         if(candidates.size() == 4)
00552         {
00553                 // we've got a tuple!
00554                 int ret = order_tuple2(candidates);
00555         if(ret == 0)
00556         {
00557             return 0;
00558         }
00559 
00560                 for(int i = 0; i < 4; i++)
00561                 {
00562                         centers[i] = candidates[i].center;
00563                 }
00564                 return 1;
00565         }
00566 
00567         printf("find_tuple: The case of more than 4 candidates is not yet supported!\n");
00568         return 0;
00569 
00570 /*
00571         // look for affine-transformed rectangle with outlet centers in its corners
00572         // for doing that iterate through all possible 4-tuples
00573         int idx[4] = {0, 0, 0, -1};
00574         while(1)
00575         {
00576                 // move to the next 4-tuple
00577                 for(int i = 3; i >= 0; i--)
00578                 {
00579                         if(
00580         }
00581  */
00582 }
00583 
00584 const int outlet_width = 50;
00585 const int outlet_height = 25;
00586 
00587 const float xsize = 46.1f;
00588 const float ysize = 38.7f;
00589 const float outlet_xsize = 12.37; // mm, the distance between the holes
00590 const float outlet_ysize = 11.5; // mm, the distance from the ground hole to the power holes
00591 
00592 void calc_outlet_homography(const CvPoint2D32f* centers, CvMat* map_matrix,
00593                                                         const outlet_template_t& templ, CvMat* inverse_map_matrix)
00594 {
00595         CvPoint2D32f rectified[4];
00596 
00597 #if 0
00598         rectified[0] = centers[0];
00599         rectified[1] = cvPoint2D32f(centers[0].x + outlet_width, centers[0].y);
00600         rectified[2] = cvPoint2D32f(centers[0].x + outlet_width, centers[0].y + outlet_height);
00601         rectified[3] = cvPoint2D32f(centers[0].x, centers[0].y + outlet_height);
00602 #else
00603         memcpy(rectified, templ.get_template(), templ.get_count()*sizeof(CvPoint2D32f));
00604 #endif
00605 
00606         cvGetPerspectiveTransform(centers, rectified, map_matrix);
00607 
00608         if(inverse_map_matrix)
00609         {
00610                 cvGetPerspectiveTransform(rectified, centers, inverse_map_matrix);
00611         }
00612 }
00613 
00614 void map_point_homography(CvPoint2D32f point, CvMat* homography, CvPoint2D32f& result)
00615 {
00616         CvMat* src = cvCreateMat(1, 1, CV_32FC2);
00617         CvMat* dst = cvCreateMat(1, 1, CV_32FC2);
00618 
00619     src->data.fl[0] = point.x;
00620     src->data.fl[1] = point.y;
00621 
00622     cvPerspectiveTransform(src, dst, homography);
00623 
00624     result.x = dst->data.fl[0];
00625     result.y = dst->data.fl[1];
00626 
00627     cvReleaseMat(&src);
00628     cvReleaseMat(&dst);
00629 }
00630 
00631 void map_vector_homography(const vector<CvPoint2D32f>& points, CvMat* homography, vector<CvPoint2D32f>& result)
00632 {
00633         int points_count = points.size();
00634         CvMat* src = cvCreateMat(1, points_count, CV_32FC2);
00635         CvMat* dst = cvCreateMat(1, points_count, CV_32FC2);
00636 
00637         for(unsigned int i = 0; i < points.size(); i++)
00638         {
00639                 src->data.fl[2*i] = points[i].x;
00640                 src->data.fl[2*i + 1] = points[i].y;
00641         }
00642 
00643         cvPerspectiveTransform(src, dst, homography);
00644 
00645         result.clear();
00646         for(int i = 0; i < points_count; i++)
00647         {
00648                 result.push_back(cvPoint2D32f(dst->data.fl[2*i], dst->data.fl[2*i + 1]));
00649         }
00650 
00651         cvReleaseMat(&src);
00652         cvReleaseMat(&dst);
00653 }
00654 
00655 void map_image_corners(CvSize src_size, CvMat* map_matrix, CvMat* corners, CvMat* dst)
00656 {
00657         corners->data.fl[0] = 0;
00658         corners->data.fl[1] = 0;
00659         corners->data.fl[2] = src_size.width;
00660         corners->data.fl[3] = 0;
00661         corners->data.fl[4] = src_size.width;
00662         corners->data.fl[5] = src_size.height;
00663         corners->data.fl[6] = 0;
00664         corners->data.fl[7] = src_size.height;
00665         cvPerspectiveTransform(corners, dst, map_matrix);
00666 }
00667 
00668 //void calc_image_homography(const CvPoint2D32f* centers, CvSize src_size, CvMat** xmap, CvMat** ymap, CvSize* dst_size)
00669 void calc_outlet_homography(const CvPoint2D32f* centers, CvSize src_size, CvMat* map_matrix, CvSize* dst_size)
00670 {
00671 //      CvMat* map_matrix = cvCreateMat(3, 3, CV_32FC1);
00672         CvMat* inverse_map_matrix = cvCreateMat(3, 3, CV_32FC1);
00673         calc_outlet_homography(centers, map_matrix, outlet_template_t(), inverse_map_matrix);
00674 
00675         CvMat* corners = cvCreateMat(1, 4, CV_32FC2);
00676         CvMat* dst = cvCreateMat(1, 4, CV_32FC2);
00677         map_image_corners(src_size, map_matrix, corners, dst);
00678 
00679         float xmin = 1e10, ymin = 1e10, xmax = -1e10, ymax = -1e10;
00680         float src_xmin, src_ymin;
00681         for(int i = 0; i < 4; i++)
00682         {
00683                 if(xmin > dst->data.fl[2*i])
00684                 {
00685                         xmin = dst->data.fl[2*i];
00686                         src_ymin = corners->data.fl[2*i + 1];
00687                 }
00688                 if(xmax < dst->data.fl[2*i])
00689                 {
00690                         xmax = dst->data.fl[2*i];
00691 //                      src_xmax = corners->data.fl[2*i];
00692                 }
00693                 if(ymin > dst->data.fl[2*i + 1])
00694                 {
00695                         ymin = dst->data.fl[2*i + 1];
00696                         src_xmin = corners->data.fl[2*i];
00697                 }
00698                 if(ymax < dst->data.fl[2*i + 1])
00699                 {
00700                         ymax = dst->data.fl[2*i + 1];
00701 //                      src_ymax = corners->data.fl[2*i + 1];
00702                 }
00703         }
00704 
00706         if(dst_size)
00707         {
00708                 dst_size->width = xmax;//ceil(xmax - xmin) + 1;
00709                 dst_size->height = ymax;//ceil(ymax - ymin) + 1;
00710         }
00711 /*
00712         CvMat* src_points = cvCreateMat(1, dst_size->width*dst_size->height, CV_32FC2);
00713         CvMat* dst_points = cvCreateMat(1, dst_size->width*dst_size->height, CV_32FC2);
00714         for(int r = 0; r < dst_size->height; r++)
00715         {
00716                 for(int c = 0; c < dst_size->width; c++)
00717                 {
00718                         dst_points->data.fl[r*dst_size->width + c*2] = c;// - xmin;
00719                         dst_points->data.fl[r*dst_size->width + c*2 + 1] = r;// - ymin;
00720                 }
00721         }
00722         cvPerspectiveTransform(dst_points, src_points, inverse_map_matrix);
00723 
00724         *xmap = cvCreateMat(dst_size->height, dst_size->width, CV_32FC1);
00725         *ymap = cvCreateMat(dst_size->height, dst_size->width, CV_32FC1);
00726         for(int r = 0; r < dst_size->height; r++)
00727         {
00728                 for(int c = 0; c < dst_size->width; c++)
00729                 {
00730                         cvmSet(*xmap, r, c, src_points->data.fl[r*dst_size->width + c*2]);
00731                         cvmSet(*ymap, r, c, src_points->data.fl[r*dst_size->width + c*2 + 1]);
00732                 }
00733         }
00734 
00735         cvReleaseMat(&src_points);
00736         cvReleaseMat(&dst_points);
00737  */
00738         cvReleaseMat(&corners);
00739         cvReleaseMat(&dst);
00740 }
00741 
00742 int calc_image_homography(IplImage* src, CvMat* map_matrix, CvSize* dst_size, CvPoint2D32f* hor_dir, CvPoint3D32f* origin,
00743                                                   CvPoint2D32f* scale, const char* output_path, const char* filename, CvPoint2D32f* _centers)
00744 {
00745         outlet_tuple_t outlet_tuple;
00746         outlet_tuple.tuple_mask = 0;
00747         int ret = find_outlet_centroids(src, outlet_tuple, output_path, filename);
00748         if(!ret)
00749         {
00750                 printf("Centroids not found\n");
00751                 return 0;
00752         }
00753 
00754         if(_centers)
00755         {
00756                 memcpy(_centers, outlet_tuple.centers, 4*sizeof(CvPoint2D32f));
00757         }
00758 
00759         if(hor_dir)
00760         {
00761                 hor_dir->x = outlet_tuple.centers[1].x - outlet_tuple.centers[0].x;
00762                 hor_dir->y = outlet_tuple.centers[1].y - outlet_tuple.centers[0].y;
00763         }
00764 
00765         calc_outlet_homography(outlet_tuple.centers, cvSize(src->width, src->height), map_matrix, dst_size);
00766 
00767         calc_origin_scale(outlet_tuple.centers, map_matrix, origin, scale);
00768 
00769         return 1;
00770 }
00771 
00772 void calc_origin_scale(const CvPoint2D32f* centers, CvMat* map_matrix, CvPoint3D32f* origin, CvPoint2D32f* scale)
00773 {
00774         float scalex, scaley;
00775         if(origin)
00776         {
00777                 CvMat* _src = cvCreateMat(1, 3, CV_32FC2);
00778                 CvMat* _dst = cvCreateMat(1, 3, CV_32FC2);
00779 
00780                 _src->data.fl[0] = centers[0].x;
00781                 _src->data.fl[1] = centers[0].y;
00782                 _src->data.fl[2] = centers[1].x;
00783                 _src->data.fl[3] = centers[1].y;
00784                 _src->data.fl[4] = centers[2].x;
00785                 _src->data.fl[5] = centers[2].y;
00786 
00787                 cvPerspectiveTransform(_src, _dst, map_matrix);
00788 
00789                 origin->x = _dst->data.fl[0];
00790                 origin->y = _dst->data.fl[1];
00791                 origin->z = 0;
00792 
00793 #if 0
00794                 scalex = xsize/(_dst->data.fl[2] - _dst->data.fl[0]);
00795                 scaley = ysize/(_dst->data.fl[5] - _dst->data.fl[3]);
00796 #else
00797                 scalex = scaley = 1.0f;
00798 #endif
00799 
00800                 cvReleaseMat(&_src);
00801                 cvReleaseMat(&_dst);
00802         }
00803 
00804         if(scale)
00805         {
00806                 *scale = cvPoint2D32f(scalex, scaley);
00807         }
00808 }
00809 
00810 IplImage* find_templates(IplImage* img, IplImage* templ)
00811 {
00812         IplImage* templr = cvCreateImage(cvSize(outlet_width, outlet_height), IPL_DEPTH_8U, 3);
00813         cvResize(templ, templr);
00814 
00815         IplImage* dist = cvCreateImage(cvSize(img->width - templr->width + 1, img->height - templr->height + 1), IPL_DEPTH_32F, 1);
00816         cvMatchTemplate(img, templr, dist, CV_TM_SQDIFF);
00817 
00818         double max_dist, min_dist;
00819         cvMinMaxLoc(dist, &min_dist, &max_dist);
00820 
00821         IplImage* mask = cvCreateImage(cvSize(dist->width, dist->height), IPL_DEPTH_8U, 1);
00822 
00823         double thresh = min_dist*2.0f;
00824         cvThreshold(dist, mask, thresh, 255, CV_THRESH_BINARY_INV);
00825 
00826         for(int r = 0; r < dist->height; r++)
00827         {
00828                 for(int c = 0; c < dist->width; c++)
00829                 {
00830                         if(mask->imageData[r*mask->widthStep + c] == 0)
00831                         {
00832                                 continue;
00833                         }
00834 
00835 //                      cvCircle(img, cvPoint(c + templr->width/2, r + templr->height/2), 20, CV_RGB(color, color, 0), 3);
00836 //                      cvRectangle(img, cvPoint(c, r), cvPoint(c + templr->width, r + templr->height), CV_RGB(color, color, 0), 2);
00837                         cvRectangle(img, cvPoint(c, r), cvPoint(c + templr->width, r + templr->height), CV_RGB(0, 0, 255), 2);
00838                 }
00839         }
00840 
00841         cvReleaseImage(&templr);
00842         cvReleaseImage(&dist);
00843 
00844         return mask;
00845 }
00846 
00847 void calc_camera_outlet_pose(CvMat* intrinsic_mat, CvMat* distortion_coeffs, const outlet_template_t& outlet_template,
00848                              const CvPoint2D32f* image_points, CvMat* rotation_vector, CvMat* translation_vector)
00849 {
00850     if(outlet_template.get_count() == 4 && outlet_template.get_color() == outletOrange)
00851     {
00852         calc_camera_pose_2x2(intrinsic_mat, distortion_coeffs, image_points, rotation_vector, translation_vector);
00853     }
00854 
00855     if(outlet_template.get_count() == 2)
00856     {
00857         calc_camera_pose_2x1(intrinsic_mat, distortion_coeffs, image_points, rotation_vector, translation_vector);
00858     }
00859 }
00860 
00861 void generate_object_points_2x1(CvPoint3D32f* points)
00862 {
00863     const float outlet_width = 12.37f; //mm
00864     const float ground_hole_offset = 11.5f; //mm
00865     points[0] = cvPoint3D32f(-outlet_width/2, 0.0f, 0.0f);
00866     points[1] = cvPoint3D32f(outlet_width/2, 0.0f, 0.0f);
00867     points[2] = cvPoint3D32f(-outlet_width/2, ysize, 0.0f);
00868     points[3] = cvPoint3D32f(outlet_width/2, ysize, 0.0f);
00869     points[4] = cvPoint3D32f(0.0f, -ground_hole_offset, 0.0f);
00870     points[5] = cvPoint3D32f(0.0f, ysize - ground_hole_offset, 0.0f);
00871 }
00872 
00873 void generate_object_points_2x1(CvPoint2D32f* points)
00874 {
00875     const float outlet_width = 12.37f; //mm
00876     const float ground_hole_offset = 11.5f; //mm
00877     points[0] = cvPoint2D32f(-outlet_width/2, 0.0f);
00878     points[1] = cvPoint2D32f(outlet_width/2, 0.0f);
00879     points[2] = cvPoint2D32f(-outlet_width/2, ysize);
00880     points[3] = cvPoint2D32f(outlet_width/2, ysize);
00881     points[4] = cvPoint2D32f(0.0f, -ground_hole_offset);
00882     points[5] = cvPoint2D32f(0.0f, ysize - ground_hole_offset);
00883 }
00884 
00885 void calc_camera_pose_2x1(CvMat* intrinsic_mat, CvMat* distortion_coeffs, const CvPoint2D32f* centers,
00886                           CvMat* rotation_vector, CvMat* translation_vector)
00887 {
00888     CvPoint3D32f object_points[6];
00889     generate_object_points_2x1(object_points);
00890 
00891     calc_camera_pose(intrinsic_mat, distortion_coeffs, 6, object_points, centers, rotation_vector, translation_vector);
00892 }
00893 
00894 void calc_camera_pose_2x2(CvMat* intrinsic_mat, CvMat* distortion_coeffs, const CvPoint2D32f* centers,
00895                           CvMat* rotation_vector, CvMat* translation_vector)
00896 {
00897     CvPoint3D32f object_points[] = {
00898         cvPoint3D32f(0.0f, 0.0f, 0.0f),
00899         cvPoint3D32f(xsize, 0.0f, 0.0f),
00900         cvPoint3D32f(xsize, ysize, 0.0f),
00901         cvPoint3D32f(0.0f, ysize, 0.0f)
00902         };
00903 
00904     calc_camera_pose(intrinsic_mat, distortion_coeffs, 4, object_points, centers, rotation_vector, translation_vector);
00905 }
00906 
00907 void calc_camera_pose(CvMat* intrinsic_mat, CvMat* distortion_coeffs, int point_count, const CvPoint3D32f* object_points,
00908                       const CvPoint2D32f* image_points, CvMat* rotation_vector, CvMat* translation_vector)
00909 {
00910         CvMat* object_mat = cvCreateMat(point_count, 3, CV_32FC1);
00911         CvMat* image_mat = cvCreateMat(point_count, 2, CV_32FC1);
00912 
00913     for(int i = 0; i < point_count; i++)
00914     {
00915         cvmSet(object_mat, i, 0, object_points[i].x);
00916         cvmSet(object_mat, i, 1, object_points[i].y);
00917         cvmSet(object_mat, i, 2, object_points[i].z);
00918     }
00919 
00920         for(int i = 0; i < point_count; i++)
00921         {
00922                 cvmSet(image_mat, i, 0, image_points[i].x);
00923                 cvmSet(image_mat, i, 1, image_points[i].y);
00924         }
00925 
00926         CvMat* _distortion_coeffs = 0;
00927         if(distortion_coeffs == 0)
00928         {
00929                 _distortion_coeffs = cvCreateMat(1, 5, CV_32FC1);
00930                 for(int i = 0; i < 5; i++) cvmSet(_distortion_coeffs, 0, i, 0.0f);
00931         }
00932         else
00933         {
00934                 _distortion_coeffs = distortion_coeffs;
00935         }
00936 
00937         cvFindExtrinsicCameraParams2(object_mat, image_mat, intrinsic_mat, _distortion_coeffs, rotation_vector,
00938                                                                  translation_vector);
00939 
00940         if(distortion_coeffs == 0)
00941         {
00942                 cvReleaseMat(&_distortion_coeffs);
00943         }
00944 
00945         cvReleaseMat(&object_mat);
00946         cvReleaseMat(&image_mat);
00947 }
00948 
00949 void outlet_template_t::save(const char* filename)
00950 {
00951     CvMemStorage* storage = cvCreateMemStorage();
00952     CvFileStorage* fs = cvOpenFileStorage(filename, storage, CV_STORAGE_WRITE);
00953 
00954     cvWriteInt(fs, "outlet count", outlet_count);
00955     for(int i = 0; i < outlet_count; i++)
00956     {
00957         char buf[1024];
00958 
00959         sprintf(buf, "outlet %d center x", i);
00960         cvWriteReal(fs, buf, centers[i].x);
00961 
00962         sprintf(buf, "outlet %d center y", i);
00963         cvWriteReal(fs, buf, centers[i].y);
00964     }
00965 
00966     cvWriteString(fs, "train path", m_train_path.c_str());
00967     cvWriteString(fs, "train config", m_train_config.c_str());
00968     cvWriteString(fs, "pca config", m_pca_config.c_str());
00969     cvWriteInt(fs, "patch width", m_patch_size.width);
00970     cvWriteInt(fs, "patch height", m_patch_size.height);
00971     cvWriteInt(fs, "pose count", m_pose_count);
00972 
00973     if(m_outlet_color == outletWhite)
00974     {
00975         cvWriteString(fs, "outlet color", "white");
00976     }
00977 
00978     if(m_outlet_color == outletOrange)
00979     {
00980         cvWriteString(fs, "outlet color", "orange");
00981     }
00982 
00983     cvWriteReal(fs, "hole contrast", m_hole_contrast);
00984 
00985     cvReleaseFileStorage(&fs);
00986     cvReleaseMemStorage(&storage);
00987 }
00988 
00989 int outlet_template_t::load(const char* path)
00990 {
00991     m_train_path = string(path);
00992 
00993     char buf[1024];
00994     sprintf(buf, "%s/%s", path, template_filename);
00995 
00996     CvMemStorage* storage = cvCreateMemStorage();
00997     CvFileStorage* fs = cvOpenFileStorage(buf, storage, CV_STORAGE_READ);
00998 
00999     CvFileNode* node = cvGetFileNodeByName(fs, 0, "outlet count");
01000     if(!node)
01001     {
01002         cvReleaseFileStorage(&fs);
01003         cvReleaseMemStorage(&storage);
01004         return 0;
01005     }
01006 
01007     int _outlet_count = cvReadInt(node);
01008 
01009     CvPoint2D32f* _outlet_centers = new CvPoint2D32f[_outlet_count];
01010 
01011     for(int i = 0; i < _outlet_count; i++)
01012     {
01013         char buf[1024];
01014 
01015         sprintf(buf, "outlet %d center x", i);
01016         node = cvGetFileNodeByName(fs, 0, buf);
01017         _outlet_centers[i].x = cvReadReal(node);
01018 
01019         sprintf(buf, "outlet %d center y", i);
01020         node = cvGetFileNodeByName(fs, 0, buf);
01021         _outlet_centers[i].y = cvReadReal(node);
01022     }
01023 
01024     initialize(_outlet_count, _outlet_centers);
01025 
01026     node = cvGetFileNodeByName(fs, 0, "train config");
01027     if(node)
01028     {
01029         const char* train_config = cvReadString(node);
01030         m_train_config = string(train_config);
01031     }
01032 
01033     node = cvGetFileNodeByName(fs, 0, "pca config");
01034     if(node)
01035     {
01036         const char* pca_config = cvReadString(node);
01037         m_pca_config = string(pca_config);
01038     }
01039 
01040     node = cvGetFileNodeByName(fs, 0, "pca hr config");
01041     if(node)
01042     {
01043         const char* pca_hr_config = cvReadString(node);
01044         m_pca_hr_config = string(pca_hr_config);
01045     }
01046 
01047     node = cvGetFileNodeByName(fs, 0, "pca descriptors");
01048     if(node)
01049     {
01050         const char* pca_desc_config = cvReadString(node);
01051         m_pca_desc_config = string(pca_desc_config);
01052     }
01053 
01054     node = cvGetFileNodeByName(fs, 0, "patch width");
01055     if(node)
01056     {
01057         m_patch_size.width = cvReadInt(node);
01058     }
01059 
01060     node = cvGetFileNodeByName(fs, 0, "patch height");
01061     if(node)
01062     {
01063         m_patch_size.height = cvReadInt(node);
01064     }
01065 
01066     node = cvGetFileNodeByName(fs, 0, "pose count");
01067     if(node)
01068     {
01069         m_pose_count = cvReadInt(node);
01070     }
01071 
01072     node =cvGetFileNodeByName(fs, 0, "outlet color");
01073     if(node)
01074     {
01075         const char* outlet_color = cvReadString(node);
01076         if(strcmp(outlet_color, "white") == 0)
01077         {
01078             m_outlet_color = outletWhite;
01079         }
01080         if(strcmp(outlet_color, "orange") == 0)
01081         {
01082             m_outlet_color = outletOrange;
01083         }
01084     }
01085 
01086     node = cvGetFileNodeByName(fs, 0, "hole contrast");
01087     if(node)
01088     {
01089         m_hole_contrast = (float)cvReadReal(node);
01090     }
01091 
01092 
01093     cvReleaseFileStorage(&fs);
01094     cvReleaseMemStorage(&storage);
01095     create_one_way_descriptor_base();
01096 
01097     initialize_geometric_hash();
01098 
01099     delete [] _outlet_centers;
01100 
01101     return 1;
01102 }
01103 
01104 void outlet_template_t::get_holes_3d(CvPoint3D32f* holes) const
01105 {
01106     const CvPoint2D32f* centers = get_template();
01107 
01108     for(int i = 0; i < get_count(); i++)
01109     {
01110         CvPoint2D32f center = centers[i];
01111         float alpha = 1.0f + 1.0f/60;
01112         holes[3*i] = cvPoint3D32f((center.x - outlet_xsize/2)*alpha, center.y*alpha, 0.0f); // power left
01113         holes[3*i + 1] = cvPoint3D32f((center.x + outlet_xsize/2)*alpha, center.y*alpha, 0.0f); // power right
01114         holes[3*i + 2] = cvPoint3D32f(center.x*alpha, (center.y - outlet_ysize)*alpha, 0.0f); // ground
01115     }
01116 }
01117 
01118 void outlet_template_t::get_holes_3d(std::vector<cv::Point3f>& holes) const
01119 {
01120     const CvPoint2D32f* centers = get_template();
01121     holes.resize(get_count()*3);
01122 
01123     for(int i = 0; i < get_count(); i++)
01124     {
01125         CvPoint2D32f center = centers[i];
01126         float alpha = 1.0f + 0.7f/30;
01127         holes[3*i] = cv::Point3f(center.x - outlet_xsize/2, center.y, 0.0f)*alpha; // power left
01128         holes[3*i + 1] = cv::Point3f(center.x + outlet_xsize/2, center.y, 0.0f)*alpha; // power right
01129         holes[3*i + 2] = cv::Point3f(center.x, center.y - outlet_ysize, 0.0f)*alpha; // ground
01130     }
01131 }
01132 
01133 void outlet_template_t::get_holes_2d(CvPoint2D32f* holes) const
01134 {
01135     const CvPoint2D32f* centers = get_template();
01136 
01137     for(int i = 0; i < get_count(); i++)
01138     {
01139         CvPoint2D32f center = centers[i];
01140         holes[3*i] = cvPoint2D32f(center.x - outlet_xsize/2, center.y); // power left
01141         holes[3*i + 1] = cvPoint2D32f(center.x + outlet_xsize/2, center.y); // power right
01142         holes[3*i + 2] = cvPoint2D32f(center.x, center.y - outlet_ysize); // ground
01143     }
01144 }
01145 
01146 void outlet_template_t::initialize_geometric_hash()
01147 {
01148     std::vector<KeyPointEx>& template_points = m_base->GetLabeledFeatures();
01149     geometric_matcher.addModel(template_points);
01150 }
01151 
01152 
01153 void writeCvPoint(CvFileStorage* fs, const char* name, CvPoint pt)
01154 {
01155     cvStartWriteStruct(fs, name, CV_NODE_SEQ);
01156     cvWriteRawData(fs, &pt, 1, "ii");
01157     cvEndWriteStruct(fs);
01158 }
01159 
01160 void readCvPointByName(CvFileStorage* fs, CvFileNode* parent, const char* name, CvPoint& pt)
01161 {
01162     CvFileNode* node = cvGetFileNodeByName(fs, parent, name);
01163         if (node)
01164         {
01165                 cvReadRawData(fs, node, &pt, "ii");
01166 #if defined(_SCALE_IMAGE_2)
01167                 pt.x /= 2;
01168                 pt.y /= 2;
01169 #endif //_SCALE_IMAGE_2
01170         }
01171         else
01172         {
01173                 pt.x = -1;
01174                 pt.y = -1;
01175         }
01176 }
01177 
01178 void readTrainingBase(const char* config_filename, char* outlet_filename,
01179                       char* nonoutlet_filename, vector<feature_t>& train_features)
01180 {
01181     CvMemStorage* storage = cvCreateMemStorage();
01182 
01183     CvFileStorage* fs = cvOpenFileStorage(config_filename, storage, CV_STORAGE_READ);
01184 
01185     CvFileNode* outlet_node = cvGetFileNodeByName(fs, 0, "outlet");
01186     const char* str = cvReadStringByName(fs, outlet_node, "outlet filename");
01187     strcpy(outlet_filename, str);
01188 
01189     CvFileNode* nonoutlet_node = cvGetFileNodeByName(fs, 0, "nonoutlet");
01190     str = cvReadStringByName(fs, nonoutlet_node, "nonoutlet filename");
01191     strcpy(nonoutlet_filename, str);
01192 
01193     CvPoint pt;
01194 
01195         int index = 1;
01196         char feature_name[10];
01197         while (1)
01198         {
01199                 sprintf(feature_name, "power%d", index++);
01200                 readCvPointByName(fs, outlet_node, feature_name, pt);
01201                 if ((pt.x == -1)&&(pt.y==-1))
01202                         break;
01203                 train_features.push_back(feature_t(pt, 1, 0));
01204         }
01205 
01206         index = 1;
01207         while (1)
01208         {
01209                 sprintf(feature_name, "ground%d", index++);
01210                 readCvPointByName(fs, outlet_node, feature_name, pt);
01211                 if ((pt.x == -1)&&(pt.y==-1))
01212                         break;
01213                 train_features.push_back(feature_t(pt, 1, 1));
01214         }
01215     cvReleaseFileStorage(&fs);
01216 
01217     cvReleaseMemStorage(&storage);
01218 }


outlet_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Thu Nov 28 2013 11:46:23