generalized_hough.cpp
Go to the documentation of this file.
00001 //Created by Alexey Latyshev
00002 // Set of functions for Generic Hough Transform (GHT)
00003 #include <cv.h>
00004 #include <highgui.h>
00005 #include "outlet_pose_estimation/detail/generalized_hough.h"
00006 #include "outlet_pose_estimation/detail/features.h"
00007 //using namespace cv;
00008 
00009 using namespace std;
00010 
00011 CV_IMPL CvSparseMat*
00012 cvCreateOutletSparseMat( int dims, const int* sizes, int type )
00013 {
00014         type = CV_MAT_TYPE( type );
00015         int pix_size1 = CV_ELEM_SIZE1(type);
00016         int pix_size = pix_size1*CV_MAT_CN(type);
00017         int i, size;
00018         CvMemStorage* storage;
00019 
00020         if( pix_size == 0 )
00021                 CV_Error( CV_StsUnsupportedFormat, "invalid array data type" );
00022 
00023         if( dims <= 0 || dims > CV_MAX_DIM_HEAP )
00024                 CV_Error( CV_StsOutOfRange, "bad number of dimensions" );
00025 
00026         if( !sizes )
00027                 CV_Error( CV_StsNullPtr, "NULL <sizes> pointer" );
00028 
00029         for( i = 0; i < dims; i++ )
00030         {
00031                 if( sizes[i] <= 0 )
00032                         CV_Error( CV_StsBadSize, "one of dimesion sizes is non-positive" );
00033         }
00034 
00035         CvSparseMat* arr = (CvSparseMat*)cvAlloc(sizeof(*arr)+MAX(0,dims-CV_MAX_DIM)*sizeof(arr->size[0]));
00036 
00037         arr->type = CV_SPARSE_MAT_MAGIC_VAL | type;
00038         arr->dims = dims;
00039         arr->refcount = 0;
00040         arr->hdr_refcount = 1;
00041         memcpy( arr->size, sizes, dims*sizeof(sizes[0]));
00042 
00043         arr->valoffset = (int)cvAlign(sizeof(CvSparseNode), pix_size1);
00044         arr->idxoffset = (int)cvAlign(arr->valoffset + pix_size, sizeof(int));
00045         size = (int)cvAlign(arr->idxoffset + dims*sizeof(int), sizeof(CvSetElem));
00046 
00047 
00048         storage = cvCreateMemStorage( 10000000 );
00049 
00050         arr->heap = cvCreateSet( 0, sizeof(CvSet), size, storage );
00051 
00052         arr->hashsize = CV_SPARSE_HASH_SIZE0;
00053         size = arr->hashsize*sizeof(arr->hashtable[0]);
00054 
00055         arr->hashtable = (void**)cvAlloc( size );
00056         memset( arr->hashtable, 0, size );
00057 
00058         return arr;
00059 }
00060 
00061 
00062 //Filtering outlets out of frame
00063 void filterOutletOutliers(vector<feature_t>& features, vector<feature_t>& dst_outlet, int accuracy)
00064 {
00065 
00066         //Filtering outlets outside of the window
00067         bool first = 0;
00068         bool second = 0;
00069         bool third = 0;
00070         bool forth = 0;
00071         if ((int)dst_outlet.size()/3 < 3)
00072         {
00073                 third = 1;
00074                 forth = 1;
00075         }
00076 
00077         int nOutlets = (int)dst_outlet.size()/3;
00078         for (int i=0;i<(int)dst_outlet.size();i++)
00079         {
00080                 float distance = -1;
00081                 float min_distance = (float)1e38;
00082                 for (int j=0;j<(int)features.size();j++)
00083                 {
00084                         if (dst_outlet[i].class_id == features[j].class_id)
00085                         {
00086                                 distance = (features[j].pt.x - dst_outlet[i].pt.x)*(features[j].pt.x - dst_outlet[i].pt.x)+
00087                                         (features[j].pt.y - dst_outlet[i].pt.y)*(features[j].pt.y - dst_outlet[i].pt.y);
00088                                 if (distance < min_distance)
00089                                         min_distance = distance;
00090                         }
00091                 }
00092 
00093                 distance = min_distance;
00094 
00095                 if ((distance < 1e38)&&(distance < accuracy*accuracy))
00096                 {
00097                         if (nOutlets == 4)
00098                         {
00099                                 if ((i==0)||(i==1)||(i==8))
00100                                 {
00101                                         first = 1;
00102                                 }
00103                                 if ((i==2)||(i==3)||(i==9))
00104                                 {
00105                                         second = 1;
00106                                 }
00107                                 if ((i==4)||(i==5)||(i==10))
00108                                 {
00109                                         third = 1;
00110                                 }
00111                                 if ((i==6)||(i==7)||(i==11))
00112                                 {
00113                                         forth = 1;
00114                                 }
00115                         }
00116                         if (nOutlets == 2)
00117                         {
00118                                 if ((i==0)||(i==1)||(i==4))
00119                                 {
00120                                         first = 1;
00121                                 }
00122                                 if ((i==2)||(i==3)||(i==5))
00123                                 {
00124                                         second = 1;
00125                                 }
00126 
00127                         }
00128 
00129                 }
00130 
00131         }
00132         if (!(first && second && third && forth))
00133         {
00134                 dst_outlet.clear();
00135                 return;
00136         }
00137         //if ((!first && !second)|| (!third && !forth)||(!first && !third)|| (!first && !forth)||(!second && !forth)||(!second && !third))
00138         //{
00139         //      dst_outlet.clear();
00140         //}
00141         //Filtering by outlet angle
00142         else
00143         {
00144                 float alpha = (float)CV_PI;
00145                 float beta = (float)CV_PI;
00146                 float gamma = (float)CV_PI;
00147                 float a,b,c;
00148 
00149                 int overSize = (int)dst_outlet.size()%3;
00150                 for (int i=0;i<nOutlets;i++)
00151                 {
00152                         a = (dst_outlet[2*i].pt.x - dst_outlet[(int)dst_outlet.size() - overSize - nOutlets+i].pt.x)*(dst_outlet[2*i].pt.x - dst_outlet[(int)dst_outlet.size() - overSize - nOutlets+i].pt.x)+
00153                                 (dst_outlet[2*i].pt.y - dst_outlet[(int)dst_outlet.size() - overSize - nOutlets+i].pt.y)*(dst_outlet[2*i].pt.y - dst_outlet[(int)dst_outlet.size() - overSize - nOutlets+i].pt.y);
00154                         b = (dst_outlet[2*i+1].pt.x - dst_outlet[(int)dst_outlet.size() - overSize - nOutlets+i].pt.x)*(dst_outlet[2*i+1].pt.x - dst_outlet[(int)dst_outlet.size() - overSize - nOutlets+i].pt.x)+
00155                                 (dst_outlet[2*i+1].pt.y - dst_outlet[(int)dst_outlet.size() - overSize - nOutlets+i].pt.y)*(dst_outlet[2*i+1].pt.y - dst_outlet[(int)dst_outlet.size() - overSize - nOutlets+i].pt.y);
00156                         c = (dst_outlet[2*i].pt.x - dst_outlet[2*i+1].pt.x)*(dst_outlet[2*i].pt.x - dst_outlet[2*i+1].pt.x)+
00157                                 (dst_outlet[2*i].pt.y - dst_outlet[2*i+1].pt.y)*(dst_outlet[2*i].pt.y - dst_outlet[2*i+1].pt.y);
00158 
00159                         if ((a > 0)&&( b> 0))
00160                                 alpha = acos((a + b - c)/2/sqrt(a*b));
00161                         if ((a > 0)&&( b> 0))
00162                                 beta = acos((a + c - b)/2/sqrt(a*c));
00163                         gamma = (float)CV_PI - alpha - beta;
00164 
00165 
00166                         if ((alpha > CV_PI/2)||(beta > CV_PI/2)||(gamma > CV_PI/2))
00167                         {
00168                                 dst_outlet.clear();
00169                                 return;
00170                         }
00171                 }
00172 
00173         }
00174         //End of filtering by angles
00175 
00176         // Filtering regions "full-of-features"
00177 #if 1
00178         CvPoint min, max;
00179         float coeff = 2.0f;
00180         min.x = (int)dst_outlet[0].pt.x;
00181         min.y = (int)dst_outlet[0].pt.y;
00182         max.x = min.x;
00183         max.y = min.y;
00184         for (int i=1; i<(int)dst_outlet.size(); i++)
00185         {
00186                 if ((int)dst_outlet[i].pt.x > max.x)
00187                         max.x = (int)dst_outlet[i].pt.x ;
00188                 if ((int)dst_outlet[i].pt.y > max.y)
00189                         max.y = (int)dst_outlet[i].pt.y ;
00190                 if ((int)dst_outlet[i].pt.x < min.x)
00191                         min.x = (int)dst_outlet[i].pt.x ;
00192                 if ((int)dst_outlet[i].pt.y < min.y)
00193                         min.y = (int)dst_outlet[i].pt.y ;
00194         }
00195         int width = max.x - min.x;
00196         int height = max.y - min.y;
00197         min.x -=(int)(width*0.2);
00198         max.x +=(int)(width*0.2);
00199         min.y -=(int)(height*0.2);
00200         max.y +=(int)(height*0.2);
00201         //Calculating number of features inside region "min x max"
00202         int count = 0;
00203         for (int i=0;i<(int)features.size(); i++)
00204         {
00205                 if (((int)features[i].pt.x >= min.x)&&((int)features[i].pt.y >= min.y)&&
00206                         ((int)features[i].pt.x <= max.x)&&((int)features[i].pt.y <= max.y))
00207                 {
00208                         count++;
00209                 }
00210 
00211         }
00212         //printf("Features: %d\nCount: %d\n\n",(int)features.size(),count);
00213         if (count > (int)(coeff*dst_outlet.size()))
00214         {
00215                 dst_outlet.clear();
00216                 return;
00217         }
00218 #endif
00219 
00220         //End of filtering
00221 
00222         //End of filtering
00223 }
00224 // Calculates outlet's center from given feature and affine transform
00225 CvPoint* getOutletCenter(feature_t feature, const vector<feature_t>& train_features, int feature_id, float angle1, float x_scale, float y_scale, float angle2)
00226 {
00227         CvPoint* result;
00228         int train_length = (int)train_features.size();
00229         if ((feature_id < 0)||(feature_id > (train_length-1)))
00230                 return NULL;
00231         CvPoint outlet_center;
00232         outlet_center.x = 0;
00233         outlet_center.y = 0;
00234         for (int i=0;i<train_length;i++)
00235         {
00236                 outlet_center.x += (int)train_features[i].pt.x;
00237                 outlet_center.y += (int)train_features[i].pt.y;
00238         }
00239         outlet_center.x /= train_length;
00240         outlet_center.y /= train_length;
00241 
00242         float rel_center_x = - outlet_center.x + train_features[feature_id].pt.x;
00243         float rel_center_y = - outlet_center.y + train_features[feature_id].pt.y;
00244         float t1 = rel_center_x * cos(angle1) + rel_center_y*sin(angle1);
00245         float t2 = - rel_center_x * sin(angle1) + rel_center_y * cos(angle1);
00246         rel_center_x = t1*x_scale;
00247         rel_center_y = t2*y_scale;
00248         t1 = rel_center_x * cos(angle2) + rel_center_y*sin(angle2);
00249         t2 = - rel_center_x * sin(angle2) + rel_center_y * cos(angle2);
00250 
00251         result = new CvPoint();
00252         result->x = (int)(feature.pt.x-t1);
00253         result->y = (int)(feature.pt.y-t2);
00254 
00255         return result;
00256 
00257 }
00258 // Builds 6-dimension histogram [center x, center y, rotation angle1, x scale, y scale, rotation angle 2]
00259 CvSparseMat* buildHoughHist(vector<feature_t>& input, const vector<feature_t>& train_features, int* hist_size, float** ranges)
00260 {
00261         CvSparseMat* hist;
00262 
00263         hist = cvCreateOutletSparseMat(6,hist_size,CV_32FC1);
00264 
00265         int* hist_size2 = new int[7];
00266         for (int i=0;i<6;i++)
00267                 hist_size2[i] = hist_size[i];
00268         hist_size2[6] = (int)train_features.size();
00269 
00270         CvSparseMat* feature_votes = cvCreateOutletSparseMat(7,hist_size2,CV_8UC1);
00271 
00272 
00273         int* idx = new int[6];
00274         int* votes_idx = new int[7];
00275 
00276 
00277         float blur_coeff = 0.17f;
00278 
00279         for (int n = 0; n < (int)input.size();n++)
00280         {
00281                 for (int feature_id = 0; feature_id < (int)train_features.size(); feature_id++)
00282                 {
00283                         if (input[n].class_id != train_features[feature_id].class_id)
00284                                 continue;
00285 
00286                         for (float angle1 = ranges[2][0]+(ranges[2][1]-ranges[2][0])/(hist_size[2]*2); angle1 <= ranges[2][1]; angle1 += ((ranges[2][1]-ranges[2][0] > 0) ? (ranges[2][1]-ranges[2][0])/hist_size[2] : 1))
00287                         {
00288                                 for (float x_scale = ranges[3][0]+(ranges[3][1]-ranges[3][0])/(hist_size[3]*2); x_scale <= ranges[3][1]; x_scale += ((ranges[3][1]-ranges[3][0] > 0) ? (ranges[3][1]-ranges[3][0])/hist_size[3] : 1))
00289                                 {
00290                                         for (float y_scale = ranges[4][0]+(ranges[4][1]-ranges[4][0])/(hist_size[4]*2); y_scale <= ranges[4][1]; y_scale += ((ranges[4][1]-ranges[4][0] > 0) ? (ranges[4][1]-ranges[4][0])/hist_size[4] : 1))
00291                                         {
00292                                                 for (float angle2 = ranges[5][0]+(ranges[5][1]-ranges[5][0])/(hist_size[5]*2); angle2 <= ranges[5][1]; angle2 += ((ranges[5][1]-ranges[5][0] > 0) ? (ranges[5][1]-ranges[5][0])/hist_size[5] : 1))
00293                                                 {
00294 
00295                             if(abs(input[n].pt.x - 379) < 5 && abs(input[n].pt.y - 312) < 5 &&
00296                                fabs(fabs(angle1)) < 0.2 && fabs(fabs(angle2) - 3.1415/3) < 0.2)
00297                             {
00298                                 int w = 1;
00299                             }
00300 
00301                                                         CvPoint* center = getOutletCenter(input[n],train_features,feature_id,angle1,x_scale,y_scale,angle2);
00302 
00303                                                         if (center && (center->x >= ranges[0][0]) && (center->x < ranges[0][1]) && (center->y >= ranges[1][0]) && (center->y < ranges[1][1]))
00304                                                         {
00305                                                                 //Incrementing histogram
00306                                                                 float idx0 = ((center->x - ranges[0][0])/(ranges[0][1]-ranges[0][0]) * hist_size[0]);
00307                                                                 float idx1 = ((center->y - ranges[1][0])/(ranges[1][1]-ranges[1][0]) * hist_size[1]);
00308                                                                 idx[0] = (int)idx0;
00309                                                                 idx[1] = (int)idx1;
00310                                                                 idx[2] = (ranges[2][1] != ranges[2][0]) ? (int)((angle1 - ranges[2][0])/(ranges[2][1]-ranges[2][0]) * hist_size[2]) : 0;
00311                                                                 idx[3] = (ranges[3][1] != ranges[3][0]) ?(int)((x_scale - ranges[3][0])/(ranges[3][1]-ranges[3][0]) * hist_size[3]) : 0;
00312                                                                 idx[4] = (ranges[4][1] != ranges[4][0]) ?(int)((y_scale - ranges[4][0])/(ranges[4][1]-ranges[4][0]) * hist_size[4]) : 0;
00313                                                                 idx[5] = (ranges[5][1] != ranges[5][0]) ?(int)((angle2 - ranges[5][0])/(ranges[5][1]-ranges[5][0]) * hist_size[5]) : 0;
00314 
00315 
00316                                                                 bool isOK = true;
00317                                                                 for (int i=0;i<2; i++)
00318                                                                 {
00319                                                                         if (idx[i] >= hist_size[i])
00320                                                                         {
00321                                                                                 idx[i] = hist_size[i]-1;
00322                                                                                 isOK = false;
00323                                                                         }
00324                                                                         if (idx[i] < 0)
00325                                                                         {
00326                                                                                 idx[i] = 0;
00327                                                                                 isOK = false;
00328                                                                         }
00329                                                                 }
00330 
00331                                                                 if (isOK)
00332                                                                 {
00333                                                                         for (int i=0;i<6;i++)
00334                                                                                 votes_idx[i] = idx[i];
00335                                                                         votes_idx[6] = feature_id;
00336 
00337                                                                         int val = (int)cvGetND(feature_votes,votes_idx).val[0];
00338 
00339                                                                         if (val)
00340                                                                         {
00341                                                                                 isOK = false;
00342                                                                         }
00343                                                                         else
00344                                                                         {
00345                                                                                 cvSetND(feature_votes,votes_idx,cvScalar(1));
00346                                                                         }
00347                                                                 }
00348 
00349                                                                 if (isOK)
00350                                                                 {
00351 
00352                                                                         float value = (float)cvGetRealND(hist,idx);
00353                                                                         cvSetRealND(hist,idx,++value);
00354 
00356                                                                         //int idx2[6];
00357                                                                         //for (int i=0;i<6;i++)
00358                                                                         //      idx2[i]=idx[i];
00359 
00360                                                                         //int move_x = 0;
00361                                                                         //int move_y = 0;
00362 
00363 
00364                                                                         //if (((idx0 - idx[0]) < blur_coeff)&&(idx[0] > 0))
00365                                                                         //      move_x=-1;
00366                                                                         //else
00367                                                                         //      if (((-idx0 + idx[0]+1) < blur_coeff)&&(idx[0] <(hist_size[0]-1)))
00368                                                                         //              move_x=1;
00369                                                                         //if (((idx1 - idx[1]) < blur_coeff)&&(idx[1] > 0))
00370                                                                         //      move_y=-1;
00371                                                                         //else
00372                                                                         //      if (((-idx1 + idx[1]+1) < blur_coeff)&&(idx[1] < (hist_size[1]-1)))
00373                                                                         //              move_y=1;
00374 
00375                                                                         //idx2[0] = idx[0]+move_x;
00376                                                                         //idx2[1] = idx[1]+move_y;
00377 
00378                                                                         //if ((move_x != 0) || (move_y !=0))
00379                                                                         //{
00380                                                                         //      float value2 = (float)cvGetRealND(hist,idx2);
00381                                                                         //      //if (value2 > value)
00382                                                                         //              cvSetRealND(hist,idx2,++value2);
00383                                                                         //      //else
00384                                                                         //      //      cvSetRealND(hist,idx,++value);
00385                                                                         //}
00386 
00388 
00389                                                                         //Bins Blur (x & y)
00390                                                                         //int idx2[6];
00391                                                                         //for (int i=0;i<6;i++)
00392                                                                         //      idx2[i]=idx[i];
00393                                                                         //for (int x = -1;x<=1;x++)
00394                                                                         //      for (int y= -1;y<=1;y++)
00395                                                                         //      {
00396                                                                         //              if (((idx[0]+x) >=0)&&((idx[1]+y) >=0)&&((idx[0]+x) < hist_size[0])&&((idx[1]+y) < hist_size[1]))
00397                                                                         //              {
00398                                                                         //                      idx2[0] = idx[0]+x;
00399                                                                         //                      idx2[1] = idx[1]+y;
00400                                                                         //                      float value = cvGetRealND(hist,idx2);
00401                                                                         //                      cvSetRealND(hist,idx2,++value);
00402                                                                         //              }
00403                                                                         //      }
00404                                                                         //End
00405 
00406 
00407                                                                 }
00408 
00409 
00410                                                                 delete center;
00411                                                                 center = 0;
00412                                                         }
00413                                                         if (center)
00414                                                                 delete center;
00415                                                 }
00416                                         }
00417                                 }
00418                         }
00419                 }
00420         }
00421         delete[] idx;
00422         delete[] votes_idx;
00423         cvReleaseSparseMat(&feature_votes);
00424 
00425         return hist;
00426 }
00427 //---------
00428 // Calculates maximums of histogram.
00429 float** getMaxHistValues(const CvHistogram* hist, int* hist_size)
00430 {
00431         float** values = new float*[1];
00432         *values = new float[6];
00433 
00434         float max_val, min_val;
00435         int* idx = new int[6];
00436         cvGetMinMaxHistValue(hist,&min_val,&max_val,0,idx);
00437         printf("\nVotes: %f\n ",max_val);
00438 
00439         (*values)[0] = hist->thresh[0][0]+(hist->thresh[0][1]-hist->thresh[0][0])/hist_size[0]*idx[0];
00440         (*values)[1] = hist->thresh[1][0]+(hist->thresh[1][1]-hist->thresh[1][0])/hist_size[1]*idx[1];
00441         (*values)[2] = hist->thresh[2][0]+(hist->thresh[2][1]-hist->thresh[2][0])/hist_size[2]*idx[2];
00442         (*values)[3] = hist->thresh[3][0]+(hist->thresh[3][1]-hist->thresh[3][0])/hist_size[3]*idx[3];
00443         (*values)[4] = hist->thresh[4][0]+(hist->thresh[4][1]-hist->thresh[4][0])/hist_size[4]*idx[4];
00444         (*values)[5] = hist->thresh[5][0]+(hist->thresh[5][1]-hist->thresh[5][0])/hist_size[5]*idx[5];
00445 
00446         delete[] idx;
00447         return values;
00448 }
00449 //---------
00450 //---------
00451 // Calculates maximums of histogram.
00452 void getMaxHistValues(const CvSparseMat* hist, int* hist_size, float** ranges, float** &maxs, int& count, int MIN_VOTES)
00453 {
00454         count = 0;
00455         //int MIN_VOTES = 4;
00456         CvSparseMatIterator mat_iterator;
00457         CvSparseNode* node = cvInitSparseMatIterator( hist, &mat_iterator );
00458 
00459         for( ; node != 0; node = cvGetNextSparseNode( &mat_iterator ))
00460         {
00461                 //  const int* idx = CV_NODE_IDX( (CvSparseMat*)hist->bins, node ); /* get pointer to the element indices */
00462                 float val = *(float*)CV_NODE_VAL( hist, node ); /* get value of the element
00463                                                                                                                 (assume that the type is CV_32FC1) */
00464                 if (val >= MIN_VOTES)
00465                         (count) ++;
00466         }
00467         //Endof
00468 
00469         if (count > 0)
00470         {
00471                 maxs = new float*[count];
00472                 for (int i=0; i<(count); i++)
00473                         maxs[i] = new float[6];
00474 
00475                 int i=0;
00476                 node = cvInitSparseMatIterator( hist, &mat_iterator );
00477                 for( ; node != 0; node = cvGetNextSparseNode( &mat_iterator ))
00478                 {
00479                         int* idx = CV_NODE_IDX( hist, node ); /* get pointer to the element indices */
00480                         float val = *(float*)CV_NODE_VAL( (CvSparseMat*)hist, node ); /* get value of the element
00481                                                                                                                                                   (assume that the type is CV_32FC1) */
00482                         if (val >= MIN_VOTES)
00483                         {
00484                                 maxs[i][0] = (float)(ranges[0][0]+(ranges[0][1]-ranges[0][0])/hist_size[0]*(idx[0]+0.5));
00485                                 maxs[i][1] = (float)(ranges[1][0]+(ranges[1][1]-ranges[1][0])/hist_size[1]*(idx[1]+0.5));
00486                                 maxs[i][2] = (float)(ranges[2][0]+(ranges[2][1]-ranges[2][0])/hist_size[2]*(idx[2]+0.5));
00487                                 maxs[i][3] = (float)(ranges[3][0]+(ranges[3][1]-ranges[3][0])/hist_size[3]*(idx[3]+0.5));
00488                                 maxs[i][4] = (float)(ranges[4][0]+(ranges[4][1]-ranges[4][0])/hist_size[4]*(idx[4]+0.5));
00489                                 maxs[i][5] = (float)(ranges[5][0]+(ranges[5][1]-ranges[5][0])/hist_size[5]*(idx[5]+0.5));
00490                                 i++;
00491                         }
00492 
00493                         //delete[] idx;
00494                 }
00495 
00496         }
00497         else
00498         {
00499                 maxs = NULL;
00500                 count = 0;
00501         }
00502 
00503 }
00504 //---------
00505 int getMaxHistValues(const CvSparseMat* hist, int* hist_size, float** ranges, float** &maxs, int& count)
00506 {
00507         //IplImage* img = cvCreateImage(cvSize(640,480),8,3);
00508         count = 0;
00509         float MIN_VOTES = 0;
00510         CvSparseMatIterator mat_iterator;
00511         CvSparseNode* node = cvInitSparseMatIterator( hist, &mat_iterator );
00512 
00513         for( ; node != 0; node = cvGetNextSparseNode( &mat_iterator ))
00514         {
00515                 //  const int* idx = CV_NODE_IDX( hist, node ); /* get pointer to the element indices */
00516                 float val = *(float*)CV_NODE_VAL( hist, node ); /* get value of the element
00517                                                                                                                 (assume that the type is CV_32FC1) */
00518                 //if (val > 0)
00519                 //{
00520                 //      //CvPoint pt;
00521                 //      int* idx = CV_NODE_IDX( hist, node );
00522                 //      int x = ranges[0][0]+(ranges[0][1]-ranges[0][0])/hist_size[0]*(idx[0]+0.5);
00523                 //      int y = ranges[1][0]+(ranges[1][1]-ranges[1][0])/hist_size[1]*(idx[1]+0.5);
00524                 //      int blue = ((uchar*)(img->imageData + img->widthStep*y))[x*3];
00525                 //      int green = ((uchar*)(img->imageData + img->widthStep*y))[x*3+1];
00526                 //      int red = ((uchar*)(img->imageData + img->widthStep*y))[x*3+2];
00527                 //      int v = blue+green+red;
00528 
00529                 //      v+=val;
00530                 //      blue = ((v / 255 > 0) ? 255 : v % 255);
00531 
00532                 //      green = ((v / 510 > 0) ? 255 : ((blue == 255) ? v % 510 - 255 : 0));
00533 
00534                 //      red = ((green == 255) ? v % 765 - 510 : 0);
00535 
00536                 //      //((uchar*)(img->imageData + img->widthStep*y))[x*3] = blue;
00537                 //      //((uchar*)(img->imageData + img->widthStep*y))[x*3+1] = 100;
00538                 //      //((uchar*)(img->imageData + img->widthStep*y))[x*3+2] = red;
00539                 //
00540 
00541                 //      //CvScalar color; color.val[0] = img->imageData[pt.y*img->width + pt.x];
00542                 //      //color.val[0] +=5;
00543                 //      // img->imageData[pt.y*img->width + pt.x]+=5;
00544                 //      cvDrawRect(img,cvPoint(x-2,y-2),cvPoint(x+2,y+2),cvScalar(blue,green,red),CV_FILLED);
00545 
00546                 //}
00547                 if (val > MIN_VOTES)
00548                         MIN_VOTES = val-1;
00549         }
00550         //cvSaveImage("1.bmp",img);
00551 
00552         node = cvInitSparseMatIterator( hist, &mat_iterator );
00553 
00554         for( ; node != 0; node = cvGetNextSparseNode( &mat_iterator ))
00555         {
00556                 //  const int* idx = CV_NODE_IDX( hist, node ); /* get pointer to the element indices */
00557                 float val = *(float*)CV_NODE_VAL( hist, node ); /* get value of the element
00558                                                                                                                 (assume that the type is CV_32FC1) */
00559                 if (val >= MIN_VOTES)
00560                         (count) ++;
00561         }
00562         //Endof
00563 
00564         if (count > 0)
00565         {
00566                 maxs = new float*[count];
00567                 for (int i=0; i<(count); i++)
00568                         maxs[i] = new float[6];
00569 
00570                 int i=0;
00571                 node = cvInitSparseMatIterator( hist, &mat_iterator );
00572                 for( ; node != 0; node = cvGetNextSparseNode( &mat_iterator ))
00573                 {
00574                         int* idx = CV_NODE_IDX( hist, node ); /* get pointer to the element indices */
00575                         float val = *(float*)CV_NODE_VAL( hist, node ); /* get value of the element
00576                                                                                                                         (assume that the type is CV_32FC1) */
00577                         if (val >= MIN_VOTES)
00578                         {
00579                                 maxs[i][0] = (float)(ranges[0][0]+(ranges[0][1]-ranges[0][0])/hist_size[0]*(idx[0]+0.5));
00580                                 maxs[i][1] = (float)(ranges[1][0]+(ranges[1][1]-ranges[1][0])/hist_size[1]*(idx[1]+0.5));
00581                                 maxs[i][2] = (float)(ranges[2][0]+(ranges[2][1]-ranges[2][0])/hist_size[2]*(idx[2]+0.5));
00582                                 maxs[i][3] = (float)(ranges[3][0]+(ranges[3][1]-ranges[3][0])/hist_size[3]*(idx[3]+0.5));
00583                                 maxs[i][4] = (float)(ranges[4][0]+(ranges[4][1]-ranges[4][0])/hist_size[4]*(idx[4]+0.5));
00584                                 maxs[i][5] = (float)(ranges[5][0]+(ranges[5][1]-ranges[5][0])/hist_size[5]*(idx[5]+0.5));
00585                                 i++;
00586                         }
00587 
00588                         //delete[] idx;
00589                 }
00590 
00591         }
00592         else
00593         {
00594                 maxs = NULL;
00595                 count = 0;
00596         }
00597 
00598         int res = (int)MIN_VOTES;
00599         return res;
00600 
00601 }
00602 
00603 
00604 // Calculates outlet features from given train outlet and affine transform
00605 // Affine transform is array [center x, center y, rotation angle1, x scale, y scale, rotation angle 2]
00606 void calcOutletPosition(const vector<feature_t>& train_features, float* affine_transform, vector<feature_t>& features)
00607 {
00608         CvPoint center = cvPoint((int)(affine_transform[0]),(int)(affine_transform[1]));
00609         float angle1 = affine_transform[2];
00610         float x_scale = affine_transform[3];
00611         float y_scale = affine_transform[4];
00612         float angle2 = affine_transform[5];
00613         int train_length = (int)train_features.size();
00614         //CvPoint* result = new CvPoint[train_length];
00615 
00616         CvPoint outlet_center;
00617         outlet_center.x = 0;
00618         outlet_center.y = 0;
00619         for (int i=0;i<train_length;i++)
00620         {
00621                 outlet_center.x += (int)train_features[i].pt.x;
00622                 outlet_center.y += (int)train_features[i].pt.y;
00623         }
00624         outlet_center.x /= train_length;
00625         outlet_center.y /= train_length;
00626 
00627         for (int i=0; i< train_length; i++)
00628         {
00629                 float rel_center_x = - outlet_center.x + train_features[i].pt.x;
00630                 float rel_center_y = - outlet_center.y + train_features[i].pt.y;
00631                 float t1 = rel_center_x * cos(angle1) + rel_center_y*sin(angle1);
00632                 float t2 = - rel_center_x * sin(angle1) + rel_center_y * cos(angle1);
00633                 rel_center_x = t1*x_scale;
00634                 rel_center_y = t2*y_scale;
00635                 t1 = rel_center_x * cos(angle2) + rel_center_y*sin(angle2);
00636                 t2 = - rel_center_x * sin(angle2) + rel_center_y * cos(angle2);
00637 
00638                 CvPoint result_point;
00639                 result_point.x = (int)(center.x+t1);
00640                 result_point.y = (int)(center.y+t2);
00641                 feature_t feature;
00642                 feature.size = train_features[i].size;
00643                 feature.pt = result_point;
00644                 feature.class_id = train_features[i].class_id;
00645 
00646                 features.push_back(feature);
00647         }
00648 
00649 
00650 }
00651 //------
00652 //Use false movements filter
00653 // dst_outlet is outlet after moving some outlet holes to the new places
00654 // projected_outlet is outlet after affine transform applying to the template outlet
00655 //The algorithm checks should we apply the movement or return to the original position
00656 void filterFalseMovements(const vector<feature_t>& projected_outlet, vector<feature_t>& dst_outlet)
00657 {
00658         // Filtering false movements
00659         vector <float> orig_right;
00660         vector <float> orig_left;
00661         vector <float> new_right;
00662         vector <float> new_left;
00663         float dist = 0.0f;
00664         float diff_coeff = 1.6f;
00665         float diff_coeff_1 = 1/diff_coeff;
00666         int nPower = (int)dst_outlet.size()/3*2;
00667         int nGround = nPower/2;
00668         orig_right.resize(nPower+nGround);
00669         orig_left.resize(nPower+nGround);
00670         new_right.resize(nPower+nGround);
00671         new_left.resize(nPower+nGround);
00672         for (int i=0;i<nGround;i++)
00673         {
00674                 dist = (dst_outlet[nPower+i].pt.x-dst_outlet[2*i].pt.x)*(dst_outlet[nPower+i].pt.x-dst_outlet[2*i].pt.x)+
00675                         (dst_outlet[nPower+i].pt.y-dst_outlet[2*i].pt.y)*(dst_outlet[nPower+i].pt.y-dst_outlet[2*i].pt.y);
00676                 new_right[nPower+i] = dist;
00677                 new_left[2*i] = dist;
00678 
00679                 dist = (dst_outlet[nPower+i].pt.x-dst_outlet[2*i+1].pt.x)*(dst_outlet[nPower+i].pt.x-dst_outlet[2*i+1].pt.x)+
00680                         (dst_outlet[nPower+i].pt.y-dst_outlet[2*i+1].pt.y)*(dst_outlet[nPower+i].pt.y-dst_outlet[2*i+1].pt.y);
00681                 new_left[nPower+i] = dist;
00682                 new_right[2*i+1] = dist;
00683 
00684                 dist = (dst_outlet[2*i+1].pt.x-dst_outlet[2*i].pt.x)*(dst_outlet[2*i+1].pt.x-dst_outlet[2*i].pt.x)+
00685                         (dst_outlet[2*i+1].pt.y-dst_outlet[2*i].pt.y)*(dst_outlet[2*i+1].pt.y-dst_outlet[2*i].pt.y);
00686                 new_right[2*i] = dist;
00687                 new_left[2*i+1] = dist;
00688 
00689                 dist = (projected_outlet[nPower+i].pt.x-projected_outlet[2*i].pt.x)*(projected_outlet[nPower+i].pt.x-projected_outlet[2*i].pt.x)+
00690                         (projected_outlet[nPower+i].pt.y-projected_outlet[2*i].pt.y)*(projected_outlet[nPower+i].pt.y-projected_outlet[2*i].pt.y);
00691                 orig_right[nPower+i] = dist;
00692                 orig_left[2*i] = dist;
00693 
00694                 dist = (projected_outlet[nPower+i].pt.x-projected_outlet[2*i+1].pt.x)*(projected_outlet[nPower+i].pt.x-projected_outlet[2*i+1].pt.x)+
00695                         (projected_outlet[nPower+i].pt.y-projected_outlet[2*i+1].pt.y)*(projected_outlet[nPower+i].pt.y-projected_outlet[2*i+1].pt.y);
00696                 orig_left[nPower+i] = dist;
00697                 orig_right[2*i+1] = dist;
00698 
00699                 dist = (projected_outlet[2*i+1].pt.x-projected_outlet[2*i].pt.x)*(projected_outlet[2*i+1].pt.x-projected_outlet[2*i].pt.x)+
00700                         (projected_outlet[2*i+1].pt.y-projected_outlet[2*i].pt.y)*(projected_outlet[2*i+1].pt.y-projected_outlet[2*i].pt.y);
00701                 orig_right[2*i] = dist;
00702                 orig_left[2*i+1] = dist;
00703         }
00704 
00705         for (int i=0;i<nPower+nGround;i++)
00706         {
00707                 if (new_left[i] > 0)
00708                 {
00709                         float rel = orig_left[i]/new_left[i];
00710                         if ((rel > diff_coeff)||(rel  <diff_coeff_1))
00711                         {
00712                                 dst_outlet[i].pt = projected_outlet[i].pt;
00713                                 continue;
00714 
00715                         }
00716                 }
00717 
00718                 if (new_right[i] > 0)
00719                 {
00720                         float rel = orig_right[i]/new_right[i];
00721                         if ((rel > diff_coeff)||(rel  <diff_coeff_1))
00722                         {
00723                                 dst_outlet[i].pt = projected_outlet[i].pt;
00724                                 continue;
00725                         }
00726                 }
00727         }
00728 
00730         //{
00731         //      bool useProjected = false;
00732         //      if (new_left[i] > 0)
00733         //      {
00734         //              float rel = orig_left[i]/new_left[i];
00735         //              if ((rel > diff_coeff)||(rel  <diff_coeff_1))
00736         //              {
00737         //                      useProjected = true;
00738         //              }
00739         //      }
00740         //      if (new_right[i] > 0)
00741         //      {
00742         //              float rel = orig_right[i]/new_right[i];
00743         //              if ((rel > diff_coeff)||(rel  <diff_coeff_1))
00744         //              {
00745         //                      useProjected = true;
00746         //              }
00747         //      }
00748         //      if (new_right[2*(i-nPower)] > 0)
00749         //      {
00750         //              float rel = orig_right[2*(i-nPower)]/new_right[2*(i-nPower)];
00751         //              if ((rel > diff_coeff)||(rel  <diff_coeff_1))
00752         //              {
00753         //                      useProjected = true;
00754         //              }
00755         //      }
00756 
00757         //      if (useProjected)
00758         //      {
00759         //              dst_outlet[i] = projected_outlet[i];
00760         //              dst_outlet[2*(i-nPower)] = projected_outlet[2*(i-nPower)];
00761         //              dst_outlet[2*(i-nPower)+1] = projected_outlet[2*(i-nPower)+1];
00762         //      }
00763         //}*/
00764 
00765 }
00766 //------
00767 //for each point of src_outlet the closest point of features vector founds (not far than accuracy distznce)
00768 //index of the closest features puts into index array (its size MUST BE equal to src_outlet.size())
00769 //index[i] == -1 means unable to choose the closest feature
00770 //max_diff_coeff - coefficient is used for two distances comparision. If relation betweeen distances to the two nearest features less than sqrt(max_diff_coeff) we unable to choose one of them
00771 void getNearestFeaturesIndexes(const vector<feature_t>& src_outlet, const vector<feature_t>& features, int* indexes, int accuracy, float max_diff_coeff)
00772 {
00773                 //float max_diff_coeff = 2.0f;
00774         // Trying to find the nearest features for src_outlet holes
00775         for (int i=0;i<(int)src_outlet.size();i++)
00776         {
00777                 int min_index = -1;
00778                 float min_distance = (float)1e30;
00779                 float last_min_distance;
00780                 for (int j=0;j<(int)features.size();j++)
00781                 {
00782                         if (features[j].class_id == src_outlet[i].class_id)
00783                         {
00784                                 float distance = (features[j].pt.x - src_outlet[i].pt.x)*(features[j].pt.x - src_outlet[i].pt.x)+
00785                                         (features[j].pt.y - src_outlet[i].pt.y)*(features[j].pt.y - src_outlet[i].pt.y);
00786                                 if ((distance < min_distance)/* && (distance < accuracy*accuracy)*/)
00787                                 {
00788                                         last_min_distance = min_distance;
00789                                         min_distance = distance;
00790                                         min_index = j;
00791                                 }
00792                                 else
00793                                 {
00794                                         //Add points comparing
00795                                         if ((distance < last_min_distance)&&((features[j].pt.x != features[min_index].pt.x)||(features[j].pt.y != features[min_index].pt.y)))
00796                                         {
00797                                                 last_min_distance = distance;
00798                                         }
00799                                 }
00800                         }
00801 
00802                 }
00803                 if (min_distance < accuracy*accuracy)
00804                         indexes[i] = min_index;
00805                 else
00806                         min_index = -1;
00807                 if (min_index > -1)
00808                 {
00809                         if ((min_distance > 0) && (last_min_distance/min_distance <= max_diff_coeff))
00810                         {
00811                                 indexes[i] = -1;
00812                                 //printf("---->Unable to choose feature\n");
00813                         }
00814                 }
00815         }
00816         // Removing the same indexes
00817         for (int i=0;i< (int)(src_outlet.size());i++)
00818         {
00819                 if (indexes[i] >=0)
00820                 {
00821                         bool wasRepeat = false;
00822                         for (int j=i+1;j< (int)(src_outlet.size());j++)
00823                         {
00824                                 if (indexes[i]==indexes[j])
00825                                 {
00826                                         indexes[j] = -1;
00827                                         wasRepeat = true;
00828                                 }
00829                         }
00830                         if (wasRepeat)
00831                                 indexes[i] = -1;
00832                 }
00833         }
00834 }
00835 //------
00836 //for each point of dst_outlet the closest point of features vector founds (not far than distance between two power holes / 3)
00837 //index of the closest feature is in the index array (its size MUST BE equal to dst_outlet.size())
00838 //index[i] == -1 means unable to choose the closest feature
00839 //max_diff_coeff - coefficient is used for two distances comparision. If relation betweeen distances to the two nearest features less than sqrt(max_diff_coeff) we unable to choose one of them
00840 // if we are able to choose the closest feature we believe it is real outlet feature
00841 void attractOutletToFeatures(const vector<feature_t>& train_features, const vector<feature_t>& features,vector<feature_t>& dst_outlet, const int* indexes, float max_diff_coeff)
00842 {
00843                 for (int i=0;i< (int)(dst_outlet.size());i++)
00844                 {
00845                         // The second attraction
00846                         //Temp
00847                         int min_index = -1;
00848                         float min_distance = (float)1e38;
00849                         float last_min_distance;
00850                         for (int j=0;j<(int)features.size();j++)
00851                         {
00852                                 if (features[j].class_id == dst_outlet[i].class_id)
00853                                 {
00854                                         float distance = (features[j].pt.x - dst_outlet[i].pt.x)*(features[j].pt.x - dst_outlet[i].pt.x)+
00855                                                 (features[j].pt.y - dst_outlet[i].pt.y)*(features[j].pt.y - dst_outlet[i].pt.y);
00856                                         if (distance < min_distance)
00857                                         {
00858                                                 last_min_distance = distance;
00859                                                 //setting min distance to power holes distance / 3
00860                                                 float acc = (float)((train_features[1].pt.x - train_features[0].pt.x)*(train_features[1].pt.x - train_features[0].pt.x)+
00861                                                         (train_features[1].pt.y - train_features[0].pt.y)*(train_features[1].pt.y - train_features[0].pt.y))/9;
00862                                                 if (distance < acc)
00863                                                 {
00864                                                         min_distance = distance;
00865                                                         min_index = j;
00866                                                 }
00867                                         }
00868                                         else
00869                                         {
00870                                                 if (distance < last_min_distance)
00871                                                 {
00872                                                         last_min_distance = distance;
00873                                                 }
00874                                         }
00875                                 }
00876                         }
00877                         if (min_index >= 0)
00878                         {
00879                                 if (((min_distance > 0) && (last_min_distance/min_distance <= max_diff_coeff))||(min_distance == 0))
00880                                         dst_outlet[i] = features[min_index];
00881 
00882                         }
00883                         else
00884 
00885                                 //End
00886 
00887                                 if (indexes[i] >=0)
00888                                 {
00889                                         dst_outlet[i] = features[indexes[i]];
00890                                 }
00891                 }
00892 }
00893 
00894 //------
00895 void calcExactLocation(vector<feature_t>& features,const vector<feature_t>& train_features, vector<feature_t>& src_outlet,
00896                        vector<feature_t>& dst_outlet, float& reprojectionError, int accuracy, bool useSecondAttraction)
00897 {
00898         float max_diff_coeff = 2.0f;
00899         if (((int)train_features.size()) == ((int)src_outlet.size()))
00900         {
00901                 vector<CvPoint> train_points;
00902                 train_points.clear();
00903 
00904                 //vector<CvPoint> src_outlet_points;
00905                 vector<CvPoint> features_points;
00906                 features_points.clear();
00907                 int* indexes = new int[(int)train_features.size()];
00908 
00909 
00910                 for (int i=0;i<(int)train_features.size();i++)
00911                 {
00912                         indexes[i] = -1;
00913                 }
00914                 //for (int i=0;i<(int)src_outlet.size();i++)
00915                 //{
00916                 //      src_outlet_points.push_back(src_outlet[i].center);
00917                 //}
00918 
00919 
00920                 getNearestFeaturesIndexes(src_outlet,features, indexes, accuracy, max_diff_coeff);
00921 
00922 
00923                 for (int i=0;i< (int)(src_outlet.size());i++)
00924                 {
00925                         if (indexes[i] >=0)
00926                         {
00927                                 train_points.push_back(train_features[i].pt);
00928                                 features_points.push_back(features[indexes[i]].pt);
00929                         }
00930                 }
00931                 int nPoints = (int)train_points.size();
00932                 //The nearest features were found
00933 
00934                 if (((int)train_points.size() > 3) /*&& ((int)train_points.size() > ((int)train_features.size()/2))*/)
00935                 {
00936                         //Projecting the template outlet to the image
00937                         CvMat* homography = cvCreateMat(2, 3, CV_32FC1);
00938                         FindAffineTransform(train_points, features_points, homography);
00939                         reprojectionError =     CalcAffineReprojectionError(train_points, features_points, homography) + 1000000 - 10000*(int)train_points.size();
00940                         dst_outlet.clear();
00941                         MapFeaturesAffine(train_features, dst_outlet, homography);
00942 
00943                         vector<feature_t> projected_outlet = dst_outlet;
00944 
00945                         // Trying to find the nearest feaatures for dst_outlet holes and change holes position to features positions
00946                         if (useSecondAttraction)
00947                         {
00948                                 attractOutletToFeatures(train_features,features,dst_outlet,indexes,max_diff_coeff);
00949                         }
00950 
00951 
00953                         //train_points.clear();
00954                         //features_points.clear();
00955                         //for (int i=0;i< (int)(src_outlet.size());i++)
00956                         //{
00957                         //      if (indexes[i] >=0)
00958                         //      {
00959                         //              train_points.push_back(temp[i].pt);
00960                         //              features_points.push_back(dst_outlet[i].pt);
00961                         //      }
00962 
00963                         //}
00964                         //float reprojectionError_new = CalcAffineReprojectionError(train_points, features_points, homography) + 1000000 - 10000*(int)train_points.size();
00967                         //if (reprojectionError_new > reprojectionError)
00968                         //{
00969                         //      dst_outlet = temp;
00970                         //      printf("old\n");
00971                         //}
00972                         //else
00973                         //{
00974                         //      reprojectionError = reprojectionError_new;
00975                         //      printf("new\n");
00976                         //}
00977 
00978 
00979 #if 1
00980                         filterFalseMovements(projected_outlet, dst_outlet);
00981 #endif
00982 
00983                         cvReleaseMat(&homography);
00984 
00985                 }
00986                 else
00987                 {
00988                         dst_outlet.clear();
00989                         reprojectionError = (float)1e38;
00990                 }
00991 
00992 
00993                 delete[] indexes;
00994         }
00995         else
00996         {
00997                 dst_outlet.clear();
00998                 reprojectionError = (float)1e38;
00999         }
01000 
01001 
01002 }
01003 //----------
01004 
01005 void convertFeaturesToOutlet(const vector<feature_t>& res_features, vector<outlet_t>& holes, IplImage* resImage)
01006 {
01007         holes.clear();
01008         outlet_t outlet;
01009 
01010         for (int i=0;i<(int)res_features.size()/3;i++)
01011         {
01012                 outlet.hole1 = res_features[2*i].pt;
01013                 outlet.hole2 = res_features[2*i+1].pt;
01014                 outlet.ground_hole = res_features[(int)res_features.size()/3*2+i].pt;
01015                 holes.push_back(outlet);
01016                 if (resImage)
01017                 {
01018                         CvScalar pointColor = cvScalar(255,0,50);
01019                         cvLine(resImage, cvPoint((int)(outlet.ground_hole.x+7), (int)(outlet.ground_hole.y)), cvPoint((int)(outlet.ground_hole.x-7),(int)(outlet.ground_hole.y)),pointColor,2);
01020                         cvLine(resImage, cvPoint((int)(outlet.ground_hole.x), (int)(outlet.ground_hole.y+7)), cvPoint((int)(outlet.ground_hole.x), (int)(outlet.ground_hole.y-7)),pointColor,2);
01021                         pointColor = cvScalar(0,255,50);
01022                         cvLine(resImage, cvPoint((int)(outlet.hole1.x+7), (int)(outlet.hole1.y)), cvPoint((int)(outlet.hole1.x-7),(int)(outlet.hole1.y)),pointColor,2);
01023                         cvLine(resImage, cvPoint((int)(outlet.hole1.x), (int)(outlet.hole1.y+7)), cvPoint((int)(outlet.hole1.x), (int)(outlet.hole1.y-7)),pointColor,2);
01024                         cvLine(resImage, cvPoint((int)(outlet.hole2.x+7), (int)(outlet.hole2.y)), cvPoint((int)(outlet.hole2.x-7),(int)(outlet.hole2.y)),pointColor,2);
01025                         cvLine(resImage, cvPoint((int)(outlet.hole2.x), (int)(outlet.hole2.y+7)), cvPoint((int)(outlet.hole2.x), (int)(outlet.hole2.y-7)),pointColor,2);
01026                 }
01027         }
01028 }
01029 
01030 void convertFeaturesToOutlet(const vector<feature_t>& res_features, const vector<bool>& is_detected, vector<outlet_t>& holes)
01031 {
01032         holes.clear();
01033         outlet_t outlet;
01034 
01035         for (int i=0;i<(int)res_features.size()/3;i++)
01036         {
01037                 outlet.hole1 = res_features[2*i].pt;
01038                 outlet.hole1_detected = is_detected[2*i];
01039 
01040                 outlet.hole2 = res_features[2*i+1].pt;
01041                 outlet.hole2_detected = is_detected[2*i+1];
01042 
01043                 outlet.ground_hole = res_features[(size_t)res_features.size()/3*2+i].pt;
01044                 outlet.ground_hole_detected = is_detected[(size_t)res_features.size()/3*2+i];
01045 
01046                 holes.push_back(outlet);
01047         }
01048 }
01049 //----------
01050 //repainting features (hole candidates)
01051 //features are closed to the ground hole will be ground hole candidates
01052 //the same is for power holes
01053 void repaintFeatures(const vector<feature_t> hole_candidates, const vector<feature_t>& hole_features, vector<feature_t>& hole_candidates_repainted, int accuracy = 10)
01054 {
01055         float dist_coeff = (float)0.015;
01056         for(int i = 0; i < (int)hole_candidates.size(); i++)
01057         {
01058                 float min_dist = 1e10;
01059                 int min_idx = -1;
01060                 for (int j=0; j<(int)hole_features.size();j++)
01061                 {
01062                         float d = (hole_features[j].pt.x - hole_candidates[i].pt.x)*(hole_features[j].pt.x - hole_candidates[i].pt.x) +
01063                                 (hole_features[j].pt.y - hole_candidates[i].pt.y)*(hole_features[j].pt.y - hole_candidates[i].pt.y);
01064                         if ((d < accuracy*accuracy*dist_coeff)&&(d<min_dist))
01065                         {
01066                                 min_idx = j;
01067                                 min_dist = d;
01068                         }
01069                 }
01070                 hole_candidates_repainted.push_back(hole_candidates[i]);
01071                 if (min_idx>=0)
01072                         hole_candidates_repainted[i].class_id = hole_features[min_idx].class_id;
01073         }
01074 }
01075 //----------
01076 float generalizedHoughTransform(vector<feature_t>& hole_candidates, const vector<feature_t>& train_features, int* hist_size, float** ranges,vector<outlet_t>& holes, IplImage* ghtImage, IplImage* resImage, char* output_path, char* base_filename)
01077 {
01078     const float max_error = (float)1e38;
01079     const int min_votes = 3;
01080 
01081         IplImage* ght = 0;
01082         if (ghtImage)
01083         {
01084                 ght = cvCloneImage(ghtImage);
01085         }
01086         CvSparseMat* hist = buildHoughHist(hole_candidates,train_features,hist_size,ranges);
01087         float** values = new float*[1];// = getMaxHistValues(hist,hist_size);
01088         int count = 1;
01089         int votes = 0;
01090         votes = getMaxHistValues(hist,hist_size,ranges,values,count);
01091 #if defined(_VERBOSE)
01092         printf("Votes: %d\n",votes);
01093 #endif
01094         cvReleaseSparseMat(&hist);
01095 
01096     if(votes < min_votes)
01097     {
01098         cvReleaseImage(&ght);
01099         return max_error;
01100     }
01101 
01103         //      x_size = test_image->width/11;
01104         //      y_size = test_image->height/11;
01105         //      int hist_size2[] = {x_size, y_size, angle1_size, x_scale_size, y_scale_size, angle2_size};
01106         //      hist = buildHoughHist(hole_candidates,train_features,hist_size2,ranges);
01107         //      float** values2 = new float*[1];// = getMaxHistValues(hist,hist_size);
01108         //      int count2 = 1;
01109         //      int votes2 = 0;
01110         //      votes2 = getMaxHistValues(hist,hist_size2,ranges,values2,count2);
01111         //#if defined(_VERBOSE)
01112         //              printf("Votes2: %d\n",votes2);
01113         //#endif
01114         //      cvReleaseSparseMat(&hist);
01115         //
01116         //      if (votes2 > votes)
01117         //      {
01118         //              for (int i=0;i<count;i++)
01119         //                      delete[] values[i];
01120         //              delete[] values;
01121         //              values = new float*[count2];
01122         //              for (int i=0;i<count2;i++)
01123         //              {
01124         //                      values[i] = new float[6];
01125         //                      for (int j=0;j<6;j++)
01126         //                      {
01127         //                              values[i][j] = values2[i][j];
01128         //                      }
01129         //              }
01130         //              count = count2;
01131         //              votes = votes2;
01132         //      }
01133         //      for (int i=0;i<count2;i++)
01134         //              delete[] values2[i];
01135         //      delete[] values2;
01136         //
01138 
01139         vector<feature_t> hole_features;
01140         vector<feature_t> hole_features_corrected;
01141         vector<feature_t> res_features;
01142 
01143         float accuracy = sqrt((float)((train_features[1].pt.x -train_features[0].pt.x)*(train_features[1].pt.x -train_features[0].pt.x)+
01144                 (train_features[1].pt.y -train_features[0].pt.y)*(train_features[1].pt.y -train_features[0].pt.y)));
01145         float error = (float)1e38;
01146         int index = -1;
01147         //IplImage* t = cvCloneImage(resImage);
01148 
01149         //We have count candidates to the outlet position
01150         //Chooseng the best one
01151         for (int j=0;j<count;j++)
01152         {
01153                 float currError;
01154                 hole_features.clear();
01155                 calcOutletPosition(train_features, values[j],hole_features);
01156 
01157                 //drawing current candidate if we have to do it
01158                 if (ghtImage && (hole_features.size() > 0) && output_path && base_filename)
01159                 {
01160                         char path[1024];
01161                         sprintf(path,"%s/%s_%d.jpg",output_path,base_filename,j);
01162                         IplImage* tmp = cvCloneImage(ght);
01163                         //cvNamedWindow(path);
01164                         //cvShowImage(path,tmp);
01165                         //cvWaitKey();
01166                         for(int l = 0; l < (int)hole_features.size(); l++)
01167                         {
01168 
01169                                 CvScalar pointColor = hole_features[l].class_id == 0 ? cvScalar(0,255,50) : cvScalar(255,0,50);
01170                                 cvLine(tmp, cvPoint((int)(hole_features[l].pt.x+7), (int)(hole_features[l].pt.y)), cvPoint((int)(hole_features[l].pt.x-7),(int)(hole_features[l].pt.y)),pointColor,2);
01171                                 cvLine(tmp, cvPoint((int)(hole_features[l].pt.x), (int)(hole_features[l].pt.y+7)), cvPoint((int)(hole_features[l].pt.x), (int)(hole_features[l].pt.y-7)),pointColor,2);
01172                         }
01173                         //cvShowImage(path,tmp);
01174                         //cvWaitKey();
01175                         cvSaveImage(path,tmp);
01176                         cvReleaseImage(&tmp);
01177                 }
01178 
01179                 if (ghtImage)
01180                 {
01181                         for(int i = 0; i < (int)hole_features.size(); i++)
01182                         {
01183                                 CvScalar pointColor = hole_features[i].class_id == 0 ? cvScalar(0,255,50) : cvScalar(255,0,50);
01184                                 cvLine(ghtImage, cvPoint((int)(hole_features[i].pt.x+7), (int)(hole_features[i].pt.y)), cvPoint((int)(hole_features[i].pt.x-7),(int)(hole_features[i].pt.y)),pointColor,2);
01185                                 cvLine(ghtImage, cvPoint((int)(hole_features[i].pt.x), (int)(hole_features[i].pt.y+7)), cvPoint((int)(hole_features[i].pt.x), (int)(hole_features[i].pt.y-7)),pointColor,2);
01186 
01187                         }
01188                 }
01189                 //Repaint features
01190                 vector<feature_t> hole_candidates2;
01191                 if (hole_features.size() >0)
01192                 {
01193                         repaintFeatures(hole_candidates, hole_features, hole_candidates2,(int)accuracy);
01194                 }
01195                 else
01196                 {
01197                         hole_candidates2 = hole_candidates;
01198                 }
01199                 //  CvScalar color_parts[] = {CV_RGB(255, 255, 0), CV_RGB(0, 255, 255)};
01200                 //  for(int i = 0; i < (int)hole_candidates.size(); i++)
01201                 //  {
01202                 //      cvCircle(ght, hole_candidates2[i].pt, hole_candidates2[i].size, color_parts[hole_candidates2[i].class_id], 2);
01203 
01204                 //  }
01205                 //cvNamedWindow("1");
01206                 //cvShowImage("1",ght);
01207                 //cvWaitKey();
01208                 //End of
01209 #if 1
01210                 calcExactLocation(hole_candidates2,train_features,hole_features,hole_features_corrected,currError,(int)accuracy);
01211 #else
01212                 calcExactLocation(hole_candidates2,train_features,hole_features,hole_features_corrected,currError,(int)accuracy,false);
01213 #endif
01214 
01215                 if (currError < error)
01216                 {
01217                         index = j;
01218                         error = currError;
01219                         res_features.clear();
01220                         for (int i =0; i< (int)hole_features_corrected.size(); i++)
01221                                 res_features.push_back(hole_features_corrected[i]);
01222                 }
01223 
01224 //        printf("candidate %d, error %f\n", j, currError);
01225 
01226         }
01227 
01228 #if 1
01229         if (res_features.size() > 0)
01230         {
01231                 filterOutletOutliers(hole_candidates,res_features,(int)accuracy);
01232         }
01233 #endif
01234 
01235         // int outlet_holes_count = 3;
01236         if ((int)(res_features.size()) > 0)
01237         {
01238                 convertFeaturesToOutlet(res_features, holes, resImage);
01239         }
01240         else
01241         {
01242                 error = max_error;
01243         }
01244 
01245 
01246         for (int i=0;i<count;i++)
01247                 delete[] values[i];
01248         delete[] values;
01249         if (ghtImage)
01250                 cvReleaseImage(&ght);
01251 
01252         return error;
01253 }


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