one_way_outlets.cpp
Go to the documentation of this file.
00001 /*
00002  *  one_way_outlets.cpp
00003  *  online_patch
00004  *
00005  *  Created by Victor  Eruhimov on 5/16/09.
00006  *  Copyright 2009 Argus Corp. All rights reserved.
00007  *
00008  */
00009 
00010 #include "outlet_pose_estimation/detail/one_way_outlets.h"
00011 #include "outlet_pose_estimation/detail/outlet_model.h"
00012 #include "outlet_pose_estimation/detail/one_way_descriptor.h"
00013 #include "outlet_pose_estimation/detail/one_way_descriptor_base.h"
00014 #include "outlet_pose_estimation/detail/constellation.h"
00015 #include "outlet_pose_estimation/detail/generalized_hough.h"
00016 #include "outlet_pose_estimation/detail/outlet_tuple.h"
00017 //#include "outlet_pose_estimation/detail/cvcalibinit_lowres.h"
00018 
00019 #include <highgui.h>
00020 #include <stdio.h>
00021 
00022 using namespace std;
00023 
00024 void drawLine(IplImage* image, CvPoint p1, CvPoint p2, CvScalar color, int thickness)
00025 {
00026     if(p1.x < 0 || p1.y < 0 || p2.y < 0 || p2.y < 0) return;
00027 
00028     cvLine(image, p1, p2, color, thickness);
00029 }
00030 
00031 void detect_outlets_2x1_one_way(IplImage* test_image, const CvOneWayDescriptorObject* descriptors,
00032                                 vector<feature_t>& holes, IplImage* color_image,
00033                                 const char* output_path, const char* output_filename)
00034 {
00035 
00036     IplImage* image = cvCreateImage(cvSize(test_image->width, test_image->height), IPL_DEPTH_8U, 3);
00037     cvCvtColor(test_image, image, CV_GRAY2RGB);
00038     IplImage* image1 = cvCloneImage(color_image);
00039 
00040     int64 time1 = cvGetTickCount();
00041 
00042     vector<feature_t> features;
00043     const float default_hole_contrast = 1.5f;
00044     GetHoleFeatures(test_image, features);
00045 
00046     int64 time2 = cvGetTickCount();
00047 #if defined(_VERBOSE)
00048     printf("Found %d test features, time elapsed: %f\n", (int)features.size(), float(time2 - time1)/cvGetTickFrequency()*1e-6);
00049 #endif //_VERBOSE
00050 
00051     IplImage* test_image_features = cvCreateImage(cvSize(test_image->width, test_image->height), IPL_DEPTH_8U, 3);
00052     cvCvtColor(test_image, test_image_features, CV_GRAY2RGB);
00053     DrawFeatures(test_image_features, features);
00054 
00055     vector<feature_t> hole_candidates;
00056     int patch_width = descriptors->GetPatchSize().width/2;
00057     int patch_height = descriptors->GetPatchSize().height/2;
00058     for(int i = 0; i < (int)features.size(); i++)
00059     {
00060         CvPoint center = features[i].pt;
00061         float scale = features[i].size;
00062 
00063         CvRect roi = cvRect(center.x - patch_width/2, center.y - patch_height/2, patch_width, patch_height);
00064         cvSetImageROI(test_image, roi);
00065         roi = cvGetImageROI(test_image);
00066         if(roi.width != patch_width || roi.height != patch_height)
00067         {
00068             continue;
00069         }
00070 
00071         if(abs(center.x - 988/2) < 10 && abs(center.y - 1203/2) < 10)
00072         {
00073             int w = 1;
00074         }
00075 /*        else
00076         {
00077             continue;
00078         }
00079 */
00080         int desc_idx = -1;
00081         int pose_idx = -1;
00082         float distance = 0;
00083 //        printf("i = %d\n", i);
00084         if(i == 331)
00085         {
00086             int w = 1;
00087         }
00088 
00089 #if 0
00090         cvNamedWindow("1", 1);
00091         cvShowImage("1", test_image);
00092         cvWaitKey(0);
00093 #endif
00094         descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance);
00095 
00096         CvPoint center_new = descriptors->GetDescriptor(desc_idx)->GetCenter();
00097         CvScalar color = descriptors->IsDescriptorObject(desc_idx) ? CV_RGB(0, 255, 0) : CV_RGB(255, 0, 0);
00098         int part_idx = descriptors->GetDescriptorPart(desc_idx);
00099         if(part_idx >= 0 && part_idx < 4)
00100         {
00101             color = CV_RGB(255, 255, 0);
00102         }
00103 
00104         if(part_idx == 5 || part_idx == 6)
00105         {
00106             color = CV_RGB(0, 255, 255);
00107         }
00108 
00109         if(part_idx >= 0)
00110         {
00111             feature_t candidate = features[i];
00112             if(part_idx < 4) candidate.class_id = 0;
00113                 else candidate.class_id = 1;
00114             hole_candidates.push_back(candidate);
00115         }
00116 
00117         cvCircle(image, center, scale, color, 2);
00118 
00119         cvResetImageROI(test_image);
00120 
00121 #if 0
00122         IplImage* image1 = cvCreateImage(cvSize(train_image->width, train_image->height), IPL_DEPTH_8U, 3);
00123         cvCvtColor(train_image, image1, CV_GRAY2RGB);
00124         IplImage* image2 = cvCreateImage(cvSize(test_image->width, test_image->height), IPL_DEPTH_8U, 3);
00125         cvCvtColor(test_image, image2, CV_GRAY2RGB);
00126 
00127         cvCircle(image1, center_new, 20, cvScalar(255, 0, 0), 2);
00128         cvCircle(image2, center, 20, cvScalar(255, 0, 0), 2);
00129 #endif
00130 
00131         //printf("Old center: %d,%d; new center: %d,%d\n", center_new.x, center_new.y, center.x, center.y);
00132         CvAffinePose pose = descriptors->GetDescriptor(desc_idx)->GetPose(pose_idx);
00133         //            printf("i = %d, pose: %f,%f,%f,%f\n", i, pose.phi, pose.theta, pose.lambda1, pose.lambda2);
00134         //            printf("Distance = %f\n\n", distance);
00135 
00136 #if 0
00137         cvNamedWindow("1", 1);
00138         cvShowImage("1", image1);
00139 
00140         cvNamedWindow("2", 1);
00141         cvShowImage("2", image2);
00142         cvWaitKey(0);
00143         cvReleaseImage(&image1);
00144         cvReleaseImage(&image2);
00145 #endif
00146     }
00147 
00148     int64 time3 = cvGetTickCount();
00149 
00150 #if defined(_VERBOSE)
00151     printf("Features matched. Time elapsed: %f\n", float(time3 - time2)/cvGetTickFrequency()*1e-6);
00152 #endif //_VERBOSE
00153 
00154     //        printf("%d features before filtering\n", (int)hole_candidates.size());
00155     vector<feature_t> hole_candidates_filtered;
00156     float dist = calc_set_std(descriptors->_GetLabeledFeatures());
00157     FilterOutletFeatures(hole_candidates, hole_candidates_filtered, dist*4);
00158     hole_candidates = hole_candidates_filtered;
00159     //        printf("Set size is %f\n", dist);
00160     //        printf("%d features after filtering\n", (int)hole_candidates.size());
00161 
00162     // clustering
00163     vector<feature_t> clusters;
00164     ClusterOutletFeatures(hole_candidates, clusters, dist*4);
00165     //        float min_error = 0;
00166     //        vector<feature_t> min_features;
00167 
00168     vector<int> indices;
00169 
00170 #if defined(_HOMOGRAPHY)
00171     CvMat* homography = cvCreateMat(3, 3, CV_32FC1);
00172 #else
00173     CvMat* homography = cvCreateMat(2, 3, CV_32FC1);
00174 #endif //_HOMOGRAPHY
00175 
00176     for(int k = 0; k < (int)clusters.size(); k++)
00177     {
00178         vector<feature_t> clustered_features;
00179         SelectNeighborFeatures(hole_candidates, clusters[k].pt, clustered_features, dist*4);
00180 
00181         DetectObjectConstellation(descriptors->_GetLabeledFeatures(), clustered_features, homography, indices);
00182 
00183         // print statistics
00184         int parts = 0;
00185         for(int i = 0; i < (int)indices.size(); i++) parts += (indices[i] >= 0);
00186 #if 0
00187         printf("Found %d parts: ", parts);
00188         vector<int> indices_sort = indices;
00189         sort(indices_sort.begin(), indices_sort.end());
00190         for(int i = 0; i < (int)indices_sort.size(); i++) if(indices_sort[i] >= 0) printf("%d", indices_sort[i]);
00191         printf("\n");
00192 #endif
00193 
00194         // infer missing objects
00195         if(parts > 0)
00196         {
00197             holes.clear();
00198             InferMissingObjects(descriptors->_GetLabeledFeatures(), clustered_features, homography, indices, holes);
00199         }
00200     }
00201 
00202     cvReleaseMat(&homography);
00203 
00204 #if 0
00205     holes.resize(6);
00206     for(int i = 0; i < indices.size(); i++)
00207     {
00208         if(indices[i] == -1) continue;
00209         holes[indices[i]] = hole_candidates[i];
00210     }
00211 #endif
00212 
00213     int64 time4 = cvGetTickCount();
00214 
00215 #if defined(_VERBOSE)
00216     printf("Object detection completed. Time elapsed: %f\n", float(time4 - time3)/cvGetTickFrequency()*1e-6);
00217     printf("Total time elapsed: %f\n", float(time4 - time1)/cvGetTickFrequency()*1e-6);
00218 #endif //_VERBOSE
00219 
00220     IplImage* image2 = cvCloneImage(image1);
00221     CvScalar color_parts[] = {CV_RGB(255, 255, 0), CV_RGB(0, 255, 255)};
00222     for(int i = 0; i < (int)hole_candidates.size(); i++)
00223     {
00224         cvCircle(image2, hole_candidates[i].pt, hole_candidates[i].size, color_parts[hole_candidates[i].class_id], 2);
00225     }
00226 
00227     CvScalar color[] = {CV_RGB(255, 255, 0), CV_RGB(255, 255, 0), CV_RGB(128, 128, 0),
00228     CV_RGB(128, 128, 0), CV_RGB(0, 255, 255), CV_RGB(0, 128, 128)};
00229     for(int i = 0; i < (int)holes.size(); i++)
00230     {
00231         //CvScalar color = i < 4 ? CV_RGB(255, 255, 0) : CV_RGB(0, 255, 255);
00232         cvCircle(image1, holes[i].pt, holes[i].size, color[i], 2);
00233     }
00234 
00235     if(holes.size() >= 6)
00236     {
00237         drawLine(image1, holes[0].pt, holes[1].pt, CV_RGB(255, 0, 0), 3);
00238         drawLine(image1, holes[1].pt, holes[4].pt, CV_RGB(255, 0, 0), 3);
00239         drawLine(image1, holes[4].pt, holes[0].pt, CV_RGB(255, 0, 0), 3);
00240 
00241         drawLine(image1, holes[2].pt, holes[3].pt, CV_RGB(255, 0, 0), 3);
00242         drawLine(image1, holes[3].pt, holes[5].pt, CV_RGB(255, 0, 0), 3);
00243         drawLine(image1, holes[5].pt, holes[2].pt, CV_RGB(255, 0, 0), 3);
00244     }
00245 
00246 #if defined(_VERBOSE)
00247     char test_image_filename[1024];
00248     sprintf(test_image_filename, "%s/features/%s", output_path, output_filename);
00249     cvSaveImage(test_image_filename, image);
00250 
00251     sprintf(test_image_filename, "%s/outlets/%s", output_path, output_filename);
00252     cvSaveImage(test_image_filename, image1);
00253 
00254     sprintf(test_image_filename, "%s/features_filtered/%s", output_path, output_filename);
00255     cvSaveImage(test_image_filename, image2);
00256 #endif //_VERBOSE
00257 
00258     cvReleaseImage(&image);
00259     cvReleaseImage(&image1);
00260     cvReleaseImage(&image2);
00261 }
00262 
00263 void detect_outlets_one_way(IplImage* test_image, const outlet_template_t& outlet_template,
00264                             vector<outlet_t>& holes, IplImage* color_image,
00265                             const char* output_path, const char* output_filename, float* scale_ranges)
00266 {
00267         holes.clear();
00268     ApplyGamma(test_image, 1.0f);
00269 
00270     IplImage* image = cvCloneImage(test_image);
00271         IplImage* image_ = cvCreateImage(cvSize(test_image->width, test_image->height), IPL_DEPTH_8U, 3);
00272     IplImage* image1 = cvCloneImage(color_image);
00273     IplImage* image2 = cvCloneImage(image1);
00274 
00275     int64 time1 = cvGetTickCount();
00276 
00277     vector<feature_t> _features, features;
00278     GetHoleFeatures(test_image, features, outlet_template.GetHoleContrast());
00279 //    FilterFeaturesOnEdges(test_image, _features, features, 0, 20);
00280 
00281     int64 time2 = cvGetTickCount();
00282 
00283 #if defined(_VERBOSE)
00284     printf("Found %d test features, time elapsed: %f\n", (int)features.size(), float(time2 - time1)/cvGetTickFrequency()*1e-6);
00285 #endif
00286 
00287 #if 0
00288     IplImage* test_image_features = cvCreateImage(cvSize(test_image->width, test_image->height), IPL_DEPTH_8U, 3);
00289     cvCvtColor(test_image, test_image_features, CV_GRAY2RGB);
00290     DrawFeatures(test_image_features, features);
00291 
00292     cvNamedWindow("1", 1);
00293     cvShowImage("1", test_image_features);
00294     cvWaitKey(0);
00295 
00296     cvReleaseImage(&test_image_features);
00297 #endif
00298 
00299     CvOneWayDescriptorObject* descriptors = const_cast<CvOneWayDescriptorObject*>(outlet_template.get_one_way_descriptor_base());
00300     vector<feature_t> hole_candidates;
00301     int patch_width = descriptors->GetPatchSize().width/2;
00302     int patch_height = descriptors->GetPatchSize().height/2;
00303         float modelErrorMin = 1e10;
00304     float max_votes = 0;
00305         float min_feature_scale = 1.0;//0.4;
00306         float max_feature_scale = 3.0;//1.2;
00307         float feature_scale_step = 1.15;
00308 
00309     // PJM: temporarily disabling this so I can get separate packages compiling
00310 #if 0
00311         //Remove the chessboard
00312         CvSize size;
00313         size.width = 4;
00314         size.height = 5;
00315         CvPoint2D32f* corners = new CvPoint2D32f[size.width * size.height];
00316         if (cvFindChessboardCornersLowres(test_image, size, corners))
00317         {
00318         cv::Mat chessboard_corners(1, size.width*size.height, CV_32FC2);
00319                 for (int i = 0; i < size.width*size.height; i++)
00320                 {
00321             chessboard_corners.at<CvPoint2D32f>(0, i) = corners[i];
00322                 }
00323         CvMat _chessboard_corners = chessboard_corners;
00324         cv::Mat hull(1, size.width*size.height, CV_32FC2);
00325         CvMat _hull = hull;
00326         cvConvexHull2(&_chessboard_corners, &_hull, CV_CLOCKWISE);
00327         std::vector<feature_t> features_filtered;
00328         for(size_t i = 0; i < features.size(); i++)
00329                 {
00330             const float min_dist = 10.0f;
00331             if(length(features[i].pt - cv::Point2f(124, 228)) < 10)
00332             {
00333                 int w = 1;
00334             }
00335             int ret = cvPointPolygonTest(&_hull, features[i].pt, 0);
00336             if(ret >= 0) continue;
00337             double dist = cvPointPolygonTest(&_hull, features[i].pt, 1);
00338             if(fabs(dist) < min_dist) continue;
00339             features_filtered.push_back(features[i]);
00340                 }
00341 
00342         features = features_filtered;
00343         }
00344         delete[] corners;
00345         //End of
00346 #endif
00347 
00348         for (float _scale = min_feature_scale; _scale <= max_feature_scale; _scale*=feature_scale_step)
00349         {
00350 #if defined(_VERBOSE)
00351                 printf("Scale: %f\n",_scale);
00352 //        printf("Size1 %d,%d, size 2 %d,%d\n", test_image->width, test_image->height, image_->width, image_->height);
00353         cvResetImageROI(test_image);
00354         cvCvtColor(test_image, image_, CV_GRAY2RGB);
00355 #endif
00356                 hole_candidates.clear();
00357                 //int q = 0;
00358 
00359 
00360                 for(int i = 0; i < (int)features.size(); i++)
00361                 {
00362                         CvPoint center = features[i].pt;
00363                         float scale = features[i].size;
00364 
00365                         //if (_scale < scale*1.7/descriptors->GetPatchSize().width*2)
00366                         //      continue;
00367                         //q++;
00368 
00369                         CvRect roi = cvRect(center.x - patch_width/2, center.y - patch_height/2, patch_width, patch_height);
00370                         cvSetImageROI(test_image, roi);
00371                         roi = cvGetImageROI(test_image);
00372                         if(roi.width != patch_width || roi.height != patch_height)
00373                         {
00374                                 continue;
00375                         }
00376 
00377                         roi = resize_rect(roi, 1.0f);
00378                         cvSetImageROI(test_image, roi);
00379 
00380                         int desc_idx = -1;
00381                         int pose_idx = -1;
00382                         float distance = 0;
00383 
00384                         CvMat* avg = 0;
00385                         CvMat* eigenvectors = 0;
00386                         descriptors->GetLowPCA(&avg, &eigenvectors);
00387 
00388         #if 0
00389                         if(abs(center.x - 252) < 10 && abs(center.y - 153) < 10)
00390                         {
00391                                 for(int k = 0; k < descriptors->GetDescriptorCount(); k++)
00392                                 {
00393                                         int part_id = descriptors->GetDescriptorPart(k);
00394                                         if(part_id < 0) continue;
00395                                         CvPoint center = descriptors->GetDescriptor(k)->GetCenter();
00396                                         descriptors->GetDescriptor(k)->EstimatePosePCA(test_image, pose_idx, distance, avg, eigenvectors);
00397                                         printf("k = %d, part_id = %d, center = (%d, %d), distance = %f\n", k, part_id, center.x, center.y, distance);
00398                                 }
00399                                 printf("\n");
00400                         }
00401         #endif
00402 
00403 
00404 
00405                         vector<int> desc_idxs;
00406                         vector<int> pose_idxs;
00407                         vector<float> distances;
00408                         vector<float> scales;
00409                         //int n=3;
00410 
00411 
00412                         scale = _scale;
00413                         float cur_scale;
00414                         float _scale_ranges[2];
00415                         if(scale_ranges)
00416                         {
00417                                 _scale_ranges[0] = scale_ranges[0];
00418                                 _scale_ranges[1] = scale_ranges[1];
00419                         }
00420                         else
00421                         {
00422                                 //_scale_ranges[0] = MAX(0.7f, scale*1.7/descriptors->GetPatchSize().width*2);
00423                                 //_scale_ranges[0] = 0.7f;
00424                                 //_scale_ranges[1] = 2.0f;
00425                                 //_scale_ranges[0] = _scale;
00426                                 _scale_ranges[0] = _scale < scale*1.7/descriptors->GetPatchSize().width*2 ? scale*1.7/descriptors->GetPatchSize().width*2 : _scale;
00427                                 _scale_ranges[1] = _scale_ranges[0]*1.01;
00428                         }
00429                         descriptors->FindDescriptor(test_image, desc_idx, pose_idx, distance,&cur_scale,_scale_ranges);
00430 
00431 
00432 
00433 
00434         #if 0
00435                         if(abs(center.x - 252) < 10 && abs(center.y - 153) < 10)
00436                         {
00437                                 const CvOneWayDescriptor* descriptor = descriptors->GetDescriptor(desc_idx);
00438                 descriptor->EstimatePosePCA(test_image, pose_idx, distance, avg, eigenvectors);
00439 
00440                                 CvPoint center = descriptor->GetCenter();
00441                 printf("center: %d,%d, distance = %f\n", center.x, center.y, distance);
00442 
00443                                 IplImage* img_match = const_cast<CvOneWayDescriptor*>(descriptors->GetDescriptor(desc_idx))->GetPatch(pose_idx);
00444                                 IplImage* img_match_8u = cvCreateImage(cvSize(img_match->width, img_match->height), IPL_DEPTH_8U, 1);
00445                                 double maxval1;
00446                                 cvMinMaxLoc(img_match, 0, &maxval1);
00447 
00448                                 int _pose_idx;
00449                                 float _distance;
00450                                 descriptors->GetDescriptor(0)->EstimatePosePCA(test_image, _pose_idx, _distance, avg, eigenvectors);
00451                                 IplImage* img_correct_match = const_cast<CvOneWayDescriptor*>(descriptors->GetDescriptor(0))->GetPatch(_pose_idx);
00452                                 CvPoint center0 = descriptors->GetDescriptor(0)->GetCenter();
00453                                 printf("Center0: %d,%d\n", center0.x, center0.y);
00454                                 IplImage* img_correct_match_8u = cvCreateImage(cvSize(img_match->width, img_match->height), IPL_DEPTH_8U, 1);
00455                                 double maxval2;
00456                                 cvMinMaxLoc(img_correct_match, 0, &maxval2);
00457                                 double maxval = MAX(maxval1, maxval2);
00458 
00459                                 cvConvertScale(img_match, img_match_8u, 255.0/maxval);
00460                                 cvConvertScale(img_correct_match, img_correct_match_8u, 255.0/maxval);
00461 
00462                                 cvMinMaxLoc(test_image, 0, &maxval);
00463                                 cvConvertScale(test_image, test_image, 255.0/maxval);
00464 
00465                                 cvNamedWindow("match", 1);
00466                                 cvShowImage("match", img_match_8u);
00467                                 cvSaveImage("match.jpg",img_match_8u);
00468                                 cvNamedWindow("correct", 1);
00469                                 cvShowImage("correct", img_correct_match_8u);
00470                                 cvSaveImage("correct.jpg",img_correct_match_8u);
00471                                 cvNamedWindow("src", 1);
00472                                 cvShowImage("src", test_image);
00473                                 cvSaveImage("src.jpg", test_image);
00474                                 cvWaitKey(0);
00475 
00476                         }
00477         #endif
00478 #if defined(_VERBOSE)
00479                         if(desc_idx < 0)
00480                         {
00481                                 printf("Descriptor not found for feature %i, skipping...\n", i);
00482                                 continue;
00483                         }
00484 #endif //_VERBOSE
00485 
00486                         CvPoint center_new = descriptors->GetDescriptor(desc_idx)->GetCenter();
00487                         CvScalar color = descriptors->IsDescriptorObject(desc_idx) ? CV_RGB(0, 255, 0) : CV_RGB(255, 0, 0);
00488                         int part_idx = descriptors->GetDescriptorPart(desc_idx);
00489 
00490                         int min_ground_idx = (int)(descriptors->GetLabeledFeatures().size()) / 3 * 2; // 3 there is number of holes in the outlet (including ground hole)
00491                         if(part_idx >= 0 && part_idx < min_ground_idx)
00492                         {
00493                                 color = CV_RGB(255, 255, 0);
00494                         }
00495 
00496                         if((part_idx >= min_ground_idx) && (part_idx <  (int)(descriptors->GetLabeledFeatures().size())))
00497                         {
00498                                 color = CV_RGB(0, 255, 255);
00499                                 if (scale_ranges)
00500                                         scale_ranges[2] = cur_scale;
00501                         }
00502 
00503 
00504                         if(part_idx >= 0)
00505                         {
00506                                 feature_t candidate = features[i];
00507                                 if(part_idx < min_ground_idx) candidate.class_id = 0;
00508                                 else candidate.class_id = 1;
00509                                 hole_candidates.push_back(candidate);
00510                         }
00511             else if(descriptors->IsDescriptorObject(desc_idx))
00512             {
00513                 feature_t candidate = features[i];
00514                 candidate.class_id = 0;
00515                 hole_candidates.push_back(candidate);
00516             }
00517 
00518                         cvCircle(image_, center, scale, color, 2);
00519 
00520 #if 0// Show descriptor for each feature
00521                         printf("Part idx: %d\n",part_idx);
00522                         IplImage* img_match = const_cast<CvOneWayDescriptor*>(descriptors->GetDescriptor(desc_idx))->GetPatch(pose_idx);
00523                         IplImage* img_match_8u = cvCreateImage(cvSize(img_match->width, img_match->height), IPL_DEPTH_8U, 1);
00524                         double maxval1;
00525                         cvMinMaxLoc(img_match, 0, &maxval1);
00526                         cvConvertScale(img_match, img_match_8u, 255.0/maxval1);
00527 
00528                         IplImage* tmp = cvCloneImage(color_image);
00529                         cvCircle(tmp,center,patch_width/2,color);
00530                         cvNamedWindow("Descriptor",0);
00531                         cvNamedWindow("Patch",0);
00532                         cvNamedWindow("image_");
00533                         cvNamedWindow("orig");
00534                         cvShowImage("Descriptor",img_match_8u);
00535                         cvShowImage("Patch",test_image);
00536                         cvShowImage("image_",tmp);
00537                         cvShowImage("orig",image_);
00538                         cvWaitKey();
00539                         cvReleaseImage(&tmp);
00540                         //cvReleaseImage(&img_match);
00541                         cvReleaseImage(&img_match_8u);
00542 #endif
00543 
00544                         cvResetImageROI(test_image);
00545 
00546                 }
00547 
00548         cvResetImageROI(test_image);
00549 
00550                 int64 time3 = cvGetTickCount();
00551 #if defined(_VERBOSE)
00552                 printf("Features matched. Time elapsed: %f\n", float(time3 - time2)/cvGetTickFrequency()*1e-6);
00553 #endif //_VERBOSE
00554 
00555                 //        printf("%d features before filtering\n", (int)hole_candidates.size());
00556                 vector<feature_t> hole_candidates_filtered;
00557                 float dist = calc_set_std(descriptors->_GetLabeledFeatures());
00558                 FilterOutletFeatures(hole_candidates, hole_candidates_filtered, dist*4);
00559                 hole_candidates = hole_candidates_filtered;
00560                 //        printf("Set size is %f\n", dist);
00561                 //        printf("%d features after filtering\n", (int)hole_candidates.size());
00562 
00563                 // clustering
00564                 vector<feature_t> clusters;
00565                 ClusterOutletFeatures(hole_candidates, clusters, dist*4);
00566 
00567                 vector<int> indices;
00568 
00569 #if defined(_GHT) // Test histogram calculating
00570         float modelError;
00571                 for(int i = 0; i < descriptors->GetPyrLevels(); i++)
00572                 {
00573 
00574                         vector<feature_t> train_features;
00575                         float scale = 1.0f/(1<<i);
00576                         ScaleFeatures(descriptors->_GetLabeledFeatures(), train_features, scale);
00577 
00578 
00579                         vector<outlet_t> _holes;
00580 
00581             int64 _time1 = cvGetTickCount();
00582             float votes = matchOutlets(hole_candidates, outlet_template, train_features, _holes);
00583             int64 _time2 = cvGetTickCount();
00584 
00585 #if defined(_VERBOSE)
00586             printf("GH time elapsed: %f\n", double(_time2 - _time1)/cvGetTickFrequency()*1e-6);
00587 #endif //_VERBOSE
00588 
00589             if(votes > max_votes)
00590             {
00591                 max_votes = votes;
00592                 holes = _holes;
00593             }
00594 
00595 #if 0
00596             cvNamedWindow("1", 1);
00597             IplImage* _test = cvCloneImage(color_image);
00598             draw_outlets(_test, _holes);
00599             cvShowImage("1", _test);
00600             cvWaitKey(0);
00601             cvReleaseImage(&_test);
00602 #endif
00603                 }
00604 #endif //_GHT
00605 
00606 
00607                 int64 time4 = cvGetTickCount();
00608 #if defined(_VERBOSE)
00609                 printf("Object detection completed, max_votes = %d. Time elapsed: %f\n", (int)max_votes,
00610                float(time4 - time3)/cvGetTickFrequency()*1e-6);
00611 #endif //_VERBOSE
00612 
00613 #if defined(_SAVE_VERBOSE)
00614         char test_image_filename[1024];
00615         std::string output_name = std::string(output_filename).substr(0, strlen(output_filename) - 4);
00616         sprintf(test_image_filename, "%s/features/%s_%f.jpg", output_path, output_name.c_str(), _scale);
00617         cvSaveImage(test_image_filename, image_);
00618 #endif //_SAVE_VERBOSE
00619         }
00620         int64 time4 = cvGetTickCount();
00621 #if defined(_VERBOSE)
00622     printf("Total time elapsed: %f\n", float(time4 - time1)/cvGetTickFrequency()*1e-6);
00623 #endif
00624 
00625     //CvScalar color_parts[] = {CV_RGB(255, 255, 0), CV_RGB(0, 255, 255)};
00626     //for(int i = 0; i < (int)hole_candidates.size(); i++)
00627     //{
00628     //    cvCircle(image2, hole_candidates[i].pt, hole_candidates[i].size, color_parts[hole_candidates[i].class_id], 2);
00629     //}
00630 
00631 
00632 #if 1
00633     if(holes.size() == 2)
00634     {
00635         findPreciseOutletLocationsAvg(test_image, outlet_template, holes);
00636 //        printf("hole1: %f,%f, hole2: %f,%f\n", holes[0].hole1f.x, holes[0].hole1f.y, holes[0].hole2f.x, holes[0].hole2f.y);
00637     }
00638 #endif
00639 
00640 
00641 #if defined(_SAVE_VERBOSE)
00642     draw_outlets(image1, holes);
00643     char test_image_filename[1024];
00644     sprintf(test_image_filename, "%s/outlets/%s", output_path, output_filename);
00645     cvSaveImage(test_image_filename, image1);
00646 
00647     sprintf(test_image_filename, "%s/features_filtered/%s", output_path, output_filename);
00648     cvSaveImage(test_image_filename, image2);
00649 #endif //_SAVE_VERBOSE
00650 
00651     cvReleaseImage(&image);
00652         cvReleaseImage(&image_);
00653     cvReleaseImage(&image1);
00654     cvReleaseImage(&image2);
00655 }
00656 //------------------------------
00657 float calc_outlet_position(const vector<feature_t>& hole_candidates, const outlet_template_t& outlet_template,
00658                                                    vector<int>& desc_idx_vec, vector<int>& pose_idx_vec, vector<int>& idx_filtered,
00659                                                    vector<outlet_t>& outlet/*, IplImage* tmp = 0*/)
00660 {
00661         vector<feature_t> _hole_candidates = hole_candidates;
00662         CvOneWayDescriptorObject* descriptors = const_cast<CvOneWayDescriptorObject*>(outlet_template.get_one_way_descriptor_base());
00663     float modelErrorMin = 1e30;
00664         float modelError = modelErrorMin;
00665         outlet.clear();
00666         vector<feature_t> outlet_features;
00667 
00668 
00669         CvMat* transform = cvCreateMat(2, 3, CV_32FC1);
00670     for(int i = 0; i < descriptors->GetPyrLevels(); i++)
00671     {
00672         vector<feature_t> train_features;
00673         ScaleFeatures(descriptors->_GetLabeledFeatures(), train_features, 1.0f/(1<<i));
00674                 float accuracy = sqrt((float)((train_features[1].pt.x -train_features[0].pt.x)*(train_features[1].pt.x -train_features[0].pt.x)+
00675                         (train_features[1].pt.y -train_features[0].pt.y)*(train_features[1].pt.y -train_features[0].pt.y)));
00676 
00677                 for (int j=0;j<(int)hole_candidates.size();j++)
00678                 {
00679                         //if (hole_candidates[j].class_id < 1)
00680                         //      continue;
00681 
00682                         vector<feature_t> t_features = train_features;
00683                         CvAffinePose pose = descriptors->GetDescriptor(desc_idx_vec[idx_filtered[j]])->GetPose(pose_idx_vec[idx_filtered[j]]);
00684 
00685                         GenerateAffineTransformFromPose(cvSize(descriptors->GetPatchSize().width*2, descriptors->GetPatchSize().height*2),pose,transform);
00686                         for (int k=0;k<(int)train_features.size();k++)
00687                         {
00688                                 t_features[k].pt.x = cvmGet(transform,0,0)*train_features[k].pt.x+cvmGet(transform,0,1)*train_features[k].pt.y+cvmGet(transform,0,2);
00689                                 t_features[k].pt.y = cvmGet(transform,1,0)*train_features[k].pt.x+cvmGet(transform,1,1)*train_features[k].pt.y+cvmGet(transform,1,2);
00690                         }
00691 
00692 
00693                         for (int k=0;k<(int)(t_features.size());k++)
00694                         {
00695                                 if (t_features[k].class_id != hole_candidates[j].class_id)
00696                                         continue;
00697                                 vector<feature_t> test_outlet = t_features;
00698                                 vector<feature_t> res_outlet;
00699 
00700                                 for (int l=0;l<(int)t_features.size();l++)
00701                                 {
00702                                         test_outlet[l].pt.x += (hole_candidates[j].pt.x - t_features[k].pt.x);
00703                                         test_outlet[l].pt.y += (hole_candidates[j].pt.y - t_features[k].pt.y);
00704                                 }
00705 
00706                                 float error;
00707                                 calcExactLocation(_hole_candidates,train_features,test_outlet,res_outlet,error,accuracy,false);
00708 
00709 
00710 
00711                                 if (error < modelError)
00712                                 {
00713                                         modelError = error;
00714                                         outlet_features = res_outlet;
00715                                 }
00716 
00717                         }
00718 
00719                 }
00720         }
00721 
00722         cvReleaseMat(&transform);
00723         if ((int)outlet_features.size() == 0)
00724         {
00725                 return modelErrorMin;
00726         }
00727         else
00728         {
00729 
00730                 outlet_t curr_outlet;
00731 
00732                 for (int i=0;i<(int)outlet_features.size()/3;i++)
00733                 {
00734                         curr_outlet.hole1 = outlet_features[2*i].pt;
00735                         curr_outlet.hole2 = outlet_features[2*i+1].pt;
00736                         curr_outlet.ground_hole = outlet_features[2*(int)outlet_features.size()/3+i].pt;
00737                         outlet.push_back(curr_outlet);
00738                 }
00739 
00740                 return modelError;
00741         }
00742 
00743 }
00744 
00745 
00746 //-----------------
00747 
00748 void filterPoints(const std::vector<KeyPointEx>& src, const std::vector<bool>& is_detected, std::vector<KeyPointEx>& dst)
00749 {
00750     dst.clear();
00751     for(size_t i = 0; i < src.size(); i++)
00752     {
00753         if(is_detected[i]) dst.push_back(src[i]);
00754     }
00755 }
00756 
00757 float matchOutlets(const std::vector<KeyPointEx>& test_points, const outlet_template_t& outlet_template,
00758                   const std::vector<KeyPointEx>& template_points, std::vector<outlet_t>& outlets)
00759 {
00760     const PointMatcher& matcher = outlet_template.getGeometricMatcher();
00761     std::vector<float> votes;
00762     std::vector<std::pair<AffineBasis, AffineBasis> > matched_bases;
00763 
00764     matcher.match(test_points, votes, matched_bases);
00765     if(votes.size() == 0)
00766     {
00767         return 0;
00768     }
00769     std::vector<float>::const_iterator max_vote = std::max_element(votes.begin(), votes.end());
00770     int max_idx = max_vote - votes.begin();
00771     std::vector<KeyPointEx> mapped_points;
00772 
00773     mapPoints(template_points, matched_bases[max_idx].first, matched_bases[max_idx].second, mapped_points);
00774 
00775     std::vector<KeyPointEx> matched_test_points;
00776     std::vector<bool> is_detected;
00777     const float max_dist = 7.0f;
00778     findClosestPoint(mapped_points, test_points, matched_test_points, is_detected, max_dist);
00779 
00780     convertFeaturesToOutlet(matched_test_points, is_detected, outlets);
00781 
00782     // special case of 2x2 outlets
00783     if(outlets.size() == 4)
00784     {
00785         // test for mirror position
00786         cv::Point2f vec1 = outlets[1].ground_hole - outlets[0].ground_hole;
00787         cv::Point2f vec2 = outlets[2].ground_hole - outlets[0].ground_hole;
00788         float cross_prod = vec1.x*vec2.y - vec1.y*vec2.x;
00789         if(cross_prod < 0)
00790         {
00791             outlet_t outlet;
00792             // switch outlets
00793             outlet = outlets[0];outlets[0] = outlets[1];outlets[1] = outlet;
00794             outlet = outlets[2];outlets[2] = outlets[3];outlets[3] = outlet;
00795             for(size_t i = 0; i < 4; i++)
00796             {
00797                 cv::Point p = outlets[i].hole1;
00798                 outlets[i].hole1 = outlets[i].hole2;
00799                 outlets[i].hole2 = p;
00800             }
00801         }
00802     }
00803     else if(outlets.size() == 2)
00804     {
00805         if(cv::Point(outlets[0].hole1) == cv::Point(outlets[1].hole1) || cv::Point(outlets[0].hole2) == cv::Point(outlets[1].hole2))
00806         {
00807             // this is a workaround, need to find the root cause and fix this
00808             outlets.clear();
00809             return 0;
00810         }
00811         cv::Point2f vec1 = outlets[1].ground_hole - outlets[0].ground_hole;
00812         cv::Point2f vec2 = outlets[0].hole2 - outlets[0].hole1;
00813         float cross_prod = vec1.x*vec2.y - vec1.y*vec2.x;
00814         if(cross_prod > 0)
00815         {
00816             // switch hole1 and hole2
00817             for(size_t i = 0; i < outlets.size(); i++)
00818             {
00819                 CvPoint p = outlets[i].hole1;
00820                 outlets[i].hole1 = outlets[i].hole2;
00821                 outlets[i].hole2 = p;
00822             }
00823         }
00824     }
00825 
00826     return *max_vote;
00827 }


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