outlet_detector.cpp
Go to the documentation of this file.
00001 /*
00002  *  outlet_detector.cpp
00003  *  outlet_sample
00004  *
00005  *  Created by Victor  Eruhimov on 2/19/09.
00006  *  Copyright 2009 Argus Corp. All rights reserved.
00007  *
00008  */
00009 
00010 #include <stdio.h>
00011 #include <stdarg.h>
00012 
00013 #include "outlet_pose_estimation/detail/one_way_outlets.h"
00014 
00015 #if defined(_VERBOSE)
00016 static int PRINTF( const char* fmt, ... )
00017 {
00018     va_list args;
00019     va_start(args, fmt);
00020     int ret = vprintf(fmt, args);
00021         return ret;
00022 }
00023 #else
00024 static int PRINTF( const char*, ... )
00025 {
00026     return 0;
00027 }
00028 #endif // _VERBOSE
00029 
00030 
00031 #include "outlet_pose_estimation/detail/outlet_detector.h"
00032 #include "outlet_pose_estimation/detail/planar.h"
00033 #include "outlet_pose_estimation/detail/outlet_tuple.h"
00034 #include "outlet_pose_estimation/detail/gh_outlets.h"
00035 
00036 #include "highgui.h"
00037 
00038 using namespace std;
00039 
00040 int detect_outlet_tuple(IplImage* src, CvMat* intrinsic_matrix, CvMat* distortion_params,
00041         vector<outlet_t>& outlets, const outlet_template_t& outlet_templ,
00042         const char* output_path, const char* filename, float* scale_ranges)
00043 {
00044 #if 1
00045     if (distortion_params) {
00046         // correcting for distortion
00047         IplImage* _img = cvCloneImage(src);
00048         //              int64 _t1 = cvGetTickCount();
00049         cvUndistort2(_img, src, intrinsic_matrix, distortion_params);
00050         //              int64 _t2 = cvGetTickCount();
00051         //              printf("Undistort time elapsed: %f", double(_t2 - _t1)/cvGetTickFrequency()*1e-6);
00052         cvReleaseImage(&_img);
00053     }
00054 #endif
00055 
00056     int ret = 1;
00057 
00058 #if !defined(_GHT)
00059     if(outlet_templ.get_color() == outletOrange && outlet_templ.get_count() == 4)
00060     {
00061         ret = detect_outlet_tuple_2x2_orange(src, intrinsic_matrix, distortion_params, outlets, outlet_templ, output_path, filename);
00062     }
00063     else if(outlet_templ.get_count() == 2)
00064     {
00065         ret = detect_outlet_tuple_2x1(src, intrinsic_matrix, distortion_params, outlets, outlet_templ, output_path, filename);
00066     }
00067 #else
00068     CvRect roi = cvGetImageROI(src);
00069     IplImage* red = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_8U, 1);
00070 #if defined(_RED)
00071     cvSetImageCOI(src, 3);
00072     cvCopy(src, red);
00073     cvSetImageCOI(src, 0);
00074 #else
00075     cvSetZero(red);
00076     cvSetImageCOI(src, 3);
00077     cvCopy(src, red);
00078     cvConvertScale(red, red, 0.5);
00079     IplImage* red1 = cvCloneImage(red);
00080     cvSetImageCOI(src, 2);
00081     cvCopy(src, red1);
00082     cvConvertScale(red1, red1, 0.5);
00083     cvAdd(red, red1, red);
00084     cvReleaseImage(&red1);
00085     cvSetImageCOI(src, 0);
00086 #endif //_RED
00087 
00088 #if 1
00089     detect_outlets_one_way(red, outlet_templ, outlets, src,output_path, filename, scale_ranges);
00090 #else
00091 //    detect_outlets_gh(red, outlet_templ, outlets, src, output_path, filename);
00092 #endif
00093     
00094     //printf("Returned %d outlets\n", outlets.size());
00095 
00096     // take into account image roi
00097     for(size_t i = 0; i < outlets.size(); i++)
00098     {
00099         outlets[i].hole1 = cv::Point2d(outlets[i].hole1) + cv::Point2d(roi.x, roi.y);
00100         outlets[i].hole2 = cv::Point2d(outlets[i].hole2) + cv::Point2d(roi.x, roi.y);
00101         outlets[i].ground_hole = cv::Point2d(outlets[i].ground_hole) + cv::Point2d(roi.x, roi.y);
00102     }
00103 
00104 
00105 #endif
00106     cvReleaseImage(&red);
00107 
00108     if(outlets.size() != (size_t)outlet_templ.get_count())
00109     {
00110         // template not found
00111         return 0;
00112     }
00113     else
00114     {
00115         ret = 1;
00116     }
00117 
00118 #if 0
00119     calc_outlet_3d_coord_2x2(intrinsic_matrix, outlet_templ, outlets);
00120 #else
00121     // now find 3d coordinates of the outlet in the camera reference frame
00122     // first, calculate homography
00123 #ifdef _DO_POSE_ESTIMATE_IN_DETECTOR
00124         if (intrinsic_matrix)
00125         {
00126 #if 0
00127                 CvPoint3D32f* object_holes_3d = new CvPoint3D32f[outlet_templ.get_count()*3];
00128                 CvPoint2D32f* object_holes_2d = new CvPoint2D32f[outlet_templ.get_count()*3];
00129                 CvPoint2D32f* image_holes = new CvPoint2D32f[outlet_templ.get_count()*3];
00130 
00131                 outlet_templ.get_holes_2d(object_holes_2d);
00132                 outlet_templ.get_holes_3d(object_holes_3d);
00133                 for(size_t i = 0; i < outlets.size(); i++)
00134                 {
00135                         image_holes[3*i] = cvPoint2D32f(outlets[i].hole1.x, outlets[i].hole1.y); // power left
00136                         image_holes[3*i + 1] = cvPoint2D32f(outlets[i].hole2.x, outlets[i].hole2.y); // power right
00137                         image_holes[3*i + 2] = cvPoint2D32f(outlets[i].ground_hole.x, outlets[i].ground_hole.y); // ground hole
00138                 }
00139 
00140                 CvMat* homography = cvCreateMat(3, 3, CV_32FC1);
00141                 CvMat* inv_homography = cvCreateMat(3, 3, CV_32FC1);
00142 
00143                 CvMat* image_holes_mat = cvCreateMat(outlet_templ.get_count()*3, 2, CV_32FC1);
00144                 CvMat* object_holes_mat = cvCreateMat(outlet_templ.get_count()*3, 2, CV_32FC1);
00145                 for(int i = 0; i < outlet_templ.get_count()*3; i++)
00146                 {
00147                         cvmSet(image_holes_mat, i, 0, image_holes[i].x);
00148                         cvmSet(image_holes_mat, i, 1, image_holes[i].y);
00149                         cvmSet(object_holes_mat, i, 0, object_holes_2d[i].x);
00150                         cvmSet(object_holes_mat, i, 1, object_holes_2d[i].y);
00151                 }
00152                 cvFindHomography(image_holes_mat, object_holes_mat, homography);
00153                 cvFindHomography(object_holes_mat, image_holes_mat, inv_homography);
00154                 cvReleaseMat(&image_holes_mat);
00155                 cvReleaseMat(&object_holes_mat);
00156 
00157                 CvPoint3D32f origin;
00158                 CvPoint2D32f origin_2d;
00159                 map_point_homography(image_holes[0], homography, origin_2d);
00160                 origin = cvPoint3D32f(origin_2d.x, origin_2d.y, 0.0);
00161                 CvPoint2D32f scale = cvPoint2D32f(1.0, 1.0);
00162 
00163                 CvMat* rotation_vector = cvCreateMat(3, 1, CV_32FC1);
00164                 CvMat* translation_vector = cvCreateMat(3, 1, CV_32FC1);
00165                 calc_camera_pose(intrinsic_matrix, 0, outlet_templ.get_count()*3, object_holes_3d, image_holes,
00166                                                  rotation_vector, translation_vector);
00167                 calc_outlet_coords(outlets, homography, origin, scale, rotation_vector, translation_vector, inv_homography);
00168 
00169                 delete object_holes_2d;
00170                 delete object_holes_3d;
00171                 delete image_holes;
00172                 cvReleaseMat(&rotation_vector);
00173                 cvReleaseMat(&translation_vector);
00174                 cvReleaseMat(&homography);
00175                 cvReleaseMat(&inv_homography);
00176 #else
00177         if(outlets.size() >= 4)
00178         {
00179             calc_outlet_coords_ground(outlets, outlet_templ, intrinsic_matrix, 0);
00180         }
00181         else
00182         {
00183             calc_outlet_coords(outlets, outlet_templ, intrinsic_matrix, 0);
00184 //            printf("Outlet 0 ground hole: %f,%f,%f\n", outlets[0].coord_hole_ground.x, outlets[0].coord_hole_ground.y, outlets[0].coord_hole_ground.z);
00185         }
00186 
00187 #endif
00188         }
00189 #endif // Whether to do pose estimation at all
00190 #endif
00191 
00192     return ret;
00193 }
00194 
00195 void calc_outlet_3d_coord_2x2(CvMat* intrinsic_matrix, const outlet_template_t& outlet_templ, vector<outlet_t>& outlets)
00196 {
00197         // filter outlets using template match
00198         CvMat* homography = 0;
00199         CvPoint3D32f origin;
00200         CvPoint2D32f scale;
00201 
00202         homography = cvCreateMat(3, 3, CV_32FC1);
00203         CvMat* inv_homography = cvCreateMat(3, 3, CV_32FC1);
00204 
00205     CvPoint2D32f centers[4];
00206     for(int i = 0; i < 4; i++)
00207     {
00208         centers[i] = cvPoint2D32f((outlets[i].hole1.x + outlets[i].hole2.x)*0.5,
00209                                   (outlets[i].hole1.y + outlets[i].hole2.y)*0.5);
00210     }
00211 
00212     calc_outlet_homography(centers, homography,
00213                            outlet_templ, inv_homography);
00214 
00215 
00216         calc_origin_scale(centers, homography, &origin, &scale);
00217 
00218         CvMat* rotation_vector = cvCreateMat(3, 1, CV_32FC1);
00219         CvMat* translation_vector = cvCreateMat(3, 1, CV_32FC1);
00220         calc_camera_outlet_pose(intrinsic_matrix, 0, outlet_templ, centers, rotation_vector, translation_vector);
00221         calc_outlet_coords(outlets, homography, origin, scale, rotation_vector, translation_vector, inv_homography);
00222         cvReleaseMat(&rotation_vector);
00223         cvReleaseMat(&translation_vector);
00224         cvReleaseMat(&inv_homography);
00225 }
00226 
00227 int detect_outlet_tuple_2x2_orange(IplImage* src, CvMat* intrinsic_matrix, CvMat* distortion_params,
00228                             vector<outlet_t>& outlets, const outlet_template_t& outlet_templ,
00229                             const char* output_path, const char* filename)
00230 {
00231         outlet_tuple_t outlet_tuple;
00232 
00233         outlet_tuple.tuple_mask = cvCreateImage(cvSize(src->width, src->height), IPL_DEPTH_8U, 1);
00234         int ret = find_outlet_centroids(src, outlet_tuple, output_path, filename);
00235         if(!ret)
00236         {
00237                 PRINTF("find_outlet_centroids did not find a tuple\n");
00238                 return 0;
00239         }
00240 
00241         vector<outlet_feature_t> features;
00242         detect_outlets(src, features, outlets, &outlet_tuple, output_path, filename);
00243 
00244         CvPoint2D32f hor_dir = outlet_tuple.centers[1] - outlet_tuple.centers[0];
00245         //      select_orient_outlets(hor_dir, outlets, 4);
00246 
00247         // filter outlets using template match
00248         CvMat* homography = 0;
00249         CvPoint3D32f origin;
00250         CvPoint2D32f scale;
00251 
00252         homography = cvCreateMat(3, 3, CV_32FC1);
00253         CvMat* inv_homography = cvCreateMat(3, 3, CV_32FC1);
00254 
00255         // test the distance
00256         const int iter_count = 1;
00257         for(int j = 0; j < iter_count; j++)
00258         {
00259                 calc_outlet_homography(outlet_tuple.centers, homography,
00260                                                            outlet_templ, inv_homography);
00261 
00262 /*
00263                 float sum_dist = 0;
00264                 for(int i = 0; i < 4; i++)
00265                 {
00266                         vector<CvPoint2D32f> borders;
00267                         map_vector(outlet_tuple.borders[i], homography, borders);
00268                         CvPoint2D32f center = calc_center(borders);
00269                         vector<CvPoint2D32f> temp;
00270                         temp.push_back(outlet_tuple.centers[i]);
00271                         map_vector(temp, homography, temp);
00272                         float dist = length(temp[0] - center);
00273                         sum_dist += dist;
00274 
00275                         temp.clear();
00276                         temp.push_back(center);
00277                         map_vector(temp, inv_homography, temp);
00278                         outlet_tuple.centers[i] = temp[0];
00279                 }
00280 
00281 
00282 #if defined(_VERBOSE)
00283                 printf("Iteration %d: error %f pixels\n", j, sum_dist/4);
00284 #endif //_VERBOSE
00285 */
00286         }
00287 
00288         calc_origin_scale(outlet_tuple.centers, homography, &origin, &scale);
00289 
00290         CvMat* rotation_vector = cvCreateMat(3, 1, CV_32FC1);
00291         CvMat* translation_vector = cvCreateMat(3, 1, CV_32FC1);
00292         calc_camera_outlet_pose(intrinsic_matrix, 0, outlet_templ, outlet_tuple.centers, rotation_vector, translation_vector);
00293         calc_outlet_coords(outlets, homography, origin, scale, rotation_vector, translation_vector, inv_homography);
00294         cvReleaseMat(&rotation_vector);
00295         cvReleaseMat(&translation_vector);
00296         cvReleaseMat(&inv_homography);
00297 
00298         filter_outlets_size(outlets);
00299 
00300         filter_outlets_tuple(outlets, outlet_tuple.tuple_mask, hor_dir);
00301 
00302 #if defined(_VERBOSE)
00303         if(output_path && filename)
00304         {
00305                 IplImage* temp = cvCloneImage(src);
00306                 draw_outlets(temp, outlets);
00307 
00308                 char buf[1024];
00309                 sprintf(buf, "%s/output_filt/%s", output_path, filename);
00310                 strcpy(buf + strlen(buf) - 3, "jpg");
00311                 cvSaveImage(buf, temp);
00312 
00313                 cvReleaseImage(&temp);
00314         }
00315 #endif //_VERBOSE
00316 
00317         PRINTF(" found %d holes, %d outlets\n", features.size(), outlets.size());
00318 
00319         if(homography == 0)
00320         {
00321                 PRINTF("Homography mask not found.\n");
00322                 return 0;
00323         }
00324         else if(outlets.size() != 4)
00325         {
00326         cvReleaseMat(&homography);
00327                 PRINTF("Outlet tuple not found!\n");
00328                 return 0;
00329         }
00330 
00331     cvReleaseMat(&homography);
00332 
00333         return 1;
00334 }
00335 
00336 int detect_outlet_tuple_2x1(IplImage* src, CvMat* intrinsic_matrix, CvMat* distortion_params,
00337                                    vector<outlet_t>& outlets, const outlet_template_t& outlet_templ,
00338                                    const char* output_path, const char* filename)
00339 {
00340     vector<feature_t> holes;
00341 
00342     IplImage* img_small = cvCreateImage(cvSize(src->width/2, src->height/2), IPL_DEPTH_8U, 3);
00343     cvResize(src, img_small);
00344 
00345     IplImage* red = cvCreateImage(cvSize(img_small->width, img_small->height), IPL_DEPTH_8U, 1);
00346     cvSetImageCOI(img_small, 3);
00347     cvCopy(img_small, red);
00348     cvSetImageCOI(img_small, 0);
00349 
00350     detect_outlets_2x1_one_way(red, outlet_templ.get_one_way_descriptor_base(), holes, img_small, output_path, filename);
00351     cvReleaseImage(&red);
00352     cvReleaseImage(&img_small);
00353 
00354     if(holes.size() == 6)
00355     {
00356         features2outlets_2x1(holes, outlets);
00357 
00358         CvPoint2D32f centers[6];
00359         for(int i = 0; i < 6; i++)
00360         {
00361             centers[i] = cvPoint2D32f(holes[i].pt.x*2, holes[i].pt.y*2);
00362         }
00363 
00364         CvPoint2D32f object_points[6];
00365         generate_object_points_2x1(object_points);
00366 
00367         CvMat* homography = cvCreateMat(3, 3, CV_32FC1);
00368         cvGetPerspectiveTransform(centers, object_points, homography);
00369 
00370         CvMat* rotation_vector = cvCreateMat(3, 1, CV_32FC1);
00371         CvMat* translation_vector = cvCreateMat(3, 1, CV_32FC1);
00372 
00373         calc_camera_outlet_pose(intrinsic_matrix, distortion_params, outlet_templ, centers, rotation_vector, translation_vector);
00374 
00375         calc_outlet_coords(outlets, homography, cvPoint3D32f(0.0f, 0.0f, 0.0f), cvPoint2D32f(1.0f, 1.0f),
00376             rotation_vector, translation_vector);
00377 
00378         return 1;
00379     }
00380     else
00381     {
00382         return 0;
00383     }
00384 }
00385 
00386 void features2outlets_2x1(const vector<feature_t>& features, vector<outlet_t>& outlets)
00387 {
00388     outlet_t outlet;
00389     outlet.hole1 = features[0].pt*2;
00390     outlet.hole2 = features[1].pt*2;
00391     outlet.ground_hole = features[4].pt*2;
00392     outlets.push_back(outlet);
00393 
00394     outlet.hole1 = features[2].pt*2;
00395     outlet.hole2 = features[3].pt*2;
00396     outlet.ground_hole = features[5].pt*2;
00397     outlets.push_back(outlet);
00398 }


outlet_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Mon Dec 2 2013 13:21:49