/home/fwph/code/wurde/modules/visionModule/Operators/face/haar.cpp

Go to the documentation of this file.
00001 #define LINEAR_INTERPOLATION
00002 
00003 /* Haar features calculation */
00004 #include "MyHaar.H"
00005 #include "cv.h"
00006 #include <stdio.h>
00007 #include <Logger.H>
00008 using namespace RobotObjects;
00009 using namespace std;
00010 
00011 /* IPP functions for object detection */
00012 icvHaarClassifierInitAlloc_32f_t icvHaarClassifierInitAlloc_32f_p = 0;
00013 icvHaarClassifierFree_32f_t icvHaarClassifierFree_32f_p = 0;
00014 icvApplyHaarClassifier_32s32f_C1R_t icvApplyHaarClassifier_32s32f_C1R_p = 0;
00015 icvRectStdDev_32s32f_C1R_t icvRectStdDev_32s32f_C1R_p = 0;
00016 
00017 const float icv_stage_threshold_bias = 0.0001f;
00018 
00019 static CvHaarClassifierCascade*
00020 icvCreateHaarClassifierCascade( int stage_count )
00021 {
00022     CvHaarClassifierCascade* cascade = 0;
00023     
00024     CV_FUNCNAME( "icvCreateHaarClassifierCascade" );
00025 
00026     __BEGIN__;
00027 
00028     int block_size = sizeof(*cascade) + stage_count*sizeof(*cascade->stage_classifier);
00029 
00030     if( stage_count <= 0 )
00031         CV_ERROR( CV_StsOutOfRange, "Number of stages should be positive" );
00032 
00033     CV_CALL( cascade = (CvHaarClassifierCascade*)cvAlloc( block_size ));
00034     memset( cascade, 0, block_size );
00035 
00036     cascade->stage_classifier = (CvHaarStageClassifier*)(cascade + 1);
00037     cascade->flags = CV_HAAR_MAGIC_VAL;
00038     cascade->count = stage_count;
00039 
00040     __END__;
00041 
00042     return cascade;
00043 }
00044 
00045 static void
00046 icvReleaseHidHaarClassifierCascade( CvHidHaarClassifierCascade** _cascade )
00047 {
00048     if( _cascade && *_cascade )
00049     {
00050         CvHidHaarClassifierCascade* cascade = *_cascade;
00051         if( cascade->ipp_stages && icvHaarClassifierFree_32f_p )
00052         {
00053             int i;
00054             for( i = 0; i < cascade->count; i++ )
00055             {
00056                 if( cascade->ipp_stages[i] )
00057                     icvHaarClassifierFree_32f_p( cascade->ipp_stages[i] );
00058             }
00059         }
00060         cvFree( (void**)&cascade->ipp_stages );
00061         cvFree( (void**)_cascade );
00062     }
00063 }
00064 
00065 /* create more efficient internal representation of haar classifier cascade */
00066 CvHidHaarClassifierCascade*
00067 icvCreateHidHaarClassifierCascade( CvHaarClassifierCascade* cascade )
00068 {
00069     CvRect* ipp_features = 0;
00070     float *ipp_weights = 0, *ipp_thresholds = 0, *ipp_val1 = 0, *ipp_val2 = 0;
00071     int* ipp_counts = 0;
00072 
00073     CvHidHaarClassifierCascade* out = 0;
00074 
00075     CV_FUNCNAME( "icvCreateHidHaarClassifierCascade" );
00076 
00077     __BEGIN__;
00078 
00079     int i, j, k, l;
00080     int datasize;
00081     int total_classifiers = 0;
00082     int total_nodes = 0;
00083     char errorstr[100];
00084     CvHidHaarClassifier* haar_classifier_ptr;
00085     CvHidHaarTreeNode* haar_node_ptr;
00086     CvSize orig_window_size;
00087     int has_tilted_features = 0;
00088     int can_use_ipp = icvHaarClassifierInitAlloc_32f_p != 0 &&
00089                       icvHaarClassifierFree_32f_p != 0 &&
00090                       icvApplyHaarClassifier_32s32f_C1R_p != 0 &&
00091                       icvRectStdDev_32s32f_C1R_p != 0;
00092     int max_count = 0;
00093 
00094     if( !CV_IS_HAAR_CLASSIFIER(cascade) )
00095         CV_ERROR( !cascade ? CV_StsNullPtr : CV_StsBadArg, "Invalid classifier pointer" );
00096 
00097     if( cascade->hid_cascade )
00098         CV_ERROR( CV_StsError, "hid_cascade has been already created" );
00099 
00100     if( !cascade->stage_classifier )
00101         CV_ERROR( CV_StsNullPtr, "" );
00102 
00103     if( cascade->count <= 0 )
00104         CV_ERROR( CV_StsOutOfRange, "Negative number of cascade stages" );
00105 
00106     orig_window_size = cascade->orig_window_size;
00107     
00108     /* check input structure correctness and calculate total memory size needed for
00109        internal representation of the classifier cascade */
00110     for( i = 0; i < cascade->count; i++ )
00111     {
00112         CvHaarStageClassifier* stage_classifier = cascade->stage_classifier + i;
00113 
00114         if( !stage_classifier->classifier ||
00115             stage_classifier->count <= 0 )
00116         {
00117             sprintf( errorstr, "header of the stage classifier #%d is invalid "
00118                      "(has null pointers or non-positive classfier count)", i );
00119             CV_ERROR( CV_StsError, errorstr );
00120         }
00121 
00122         max_count = MAX( max_count, stage_classifier->count );
00123         total_classifiers += stage_classifier->count;
00124 
00125         for( j = 0; j < stage_classifier->count; j++ )
00126         {
00127             CvHaarClassifier* classifier = stage_classifier->classifier + j;
00128 
00129             total_nodes += classifier->count;
00130             for( l = 0; l < classifier->count; l++ )
00131             {
00132                 for( k = 0; k < CV_HAAR_FEATURE_MAX; k++ )
00133                 {
00134                     if( classifier->haar_feature[l].rect[k].r.width )
00135                     {
00136                         CvRect r = classifier->haar_feature[l].rect[k].r;
00137                         int tilted = classifier->haar_feature[l].tilted;
00138                         has_tilted_features |= tilted != 0;
00139                         if( r.width < 0 || r.height < 0 || r.y < 0 ||
00140                             r.x + r.width > orig_window_size.width
00141                             ||
00142                             (!tilted &&
00143                             (r.x < 0 || r.y + r.height > orig_window_size.height))
00144                             ||
00145                             (tilted && (r.x - r.height < 0 ||
00146                             r.y + r.width + r.height > orig_window_size.height)))
00147                         {
00148                             sprintf( errorstr, "rectangle #%d of the classifier #%d of "
00149                                      "the stage classifier #%d is not inside "
00150                                      "the reference (original) cascade window", k, j, i );
00151                             CV_ERROR( CV_StsNullPtr, errorstr );
00152                         }
00153                     }
00154                 }
00155             }
00156         }
00157     }
00158 
00159     // this is an upper boundary for the whole hidden cascade size
00160     datasize = sizeof(CvHidHaarClassifierCascade) +
00161                sizeof(CvHidHaarStageClassifier)*cascade->count +
00162                sizeof(CvHidHaarClassifier) * total_classifiers +
00163                sizeof(CvHidHaarTreeNode) * total_nodes +
00164                sizeof(void*)*(total_nodes + total_classifiers);
00165 
00166     CV_CALL( out = (CvHidHaarClassifierCascade*)cvAlloc( datasize ));
00167     memset( out, 0, sizeof(*out) );
00168 
00169     /* init header */
00170     out->count = cascade->count;
00171     out->stage_classifier = (CvHidHaarStageClassifier*)(out + 1);
00172     haar_classifier_ptr = (CvHidHaarClassifier*)(out->stage_classifier + cascade->count);
00173     haar_node_ptr = (CvHidHaarTreeNode*)(haar_classifier_ptr + total_classifiers);
00174 
00175     out->is_stump_based = 1;
00176     out->has_tilted_features = has_tilted_features;
00177     out->is_tree = 0;
00178 
00179     /* initialize internal representation */
00180     for( i = 0; i < cascade->count; i++ )
00181     {
00182         CvHaarStageClassifier* stage_classifier = cascade->stage_classifier + i;
00183         CvHidHaarStageClassifier* hid_stage_classifier = out->stage_classifier + i;
00184 
00185         hid_stage_classifier->count = stage_classifier->count;
00186         hid_stage_classifier->threshold = stage_classifier->threshold - icv_stage_threshold_bias;
00187         hid_stage_classifier->classifier = haar_classifier_ptr;
00188         hid_stage_classifier->two_rects = 1;
00189         haar_classifier_ptr += stage_classifier->count;
00190 
00191         hid_stage_classifier->parent = (stage_classifier->parent == -1)
00192             ? NULL : out->stage_classifier + stage_classifier->parent;
00193         hid_stage_classifier->next = (stage_classifier->next == -1)
00194             ? NULL : out->stage_classifier + stage_classifier->next;
00195         hid_stage_classifier->child = (stage_classifier->child == -1)
00196             ? NULL : out->stage_classifier + stage_classifier->child;
00197         
00198         out->is_tree |= hid_stage_classifier->next != NULL;
00199 
00200         for( j = 0; j < stage_classifier->count; j++ )
00201         {
00202             CvHaarClassifier* classifier = stage_classifier->classifier + j;
00203             CvHidHaarClassifier* hid_classifier = hid_stage_classifier->classifier + j;
00204             int node_count = classifier->count;
00205             float* alpha_ptr = (float*)(haar_node_ptr + node_count);
00206 
00207             hid_classifier->count = node_count;
00208             hid_classifier->node = haar_node_ptr;
00209             hid_classifier->alpha = alpha_ptr;
00210             
00211             for( l = 0; l < node_count; l++ )
00212             {
00213                 CvHidHaarTreeNode* node = hid_classifier->node + l;
00214                 CvHaarFeature* feature = classifier->haar_feature + l;
00215                 memset( node, -1, sizeof(*node) );
00216                 node->threshold = classifier->threshold[l];
00217                 node->left = classifier->left[l];
00218                 node->right = classifier->right[l];
00219 
00220                 if( fabs(feature->rect[2].weight) < DBL_EPSILON ||
00221                     feature->rect[2].r.width == 0 ||
00222                     feature->rect[2].r.height == 0 )
00223                     memset( &(node->feature.rect[2]), 0, sizeof(node->feature.rect[2]) );
00224                 else
00225                     hid_stage_classifier->two_rects = 0;
00226             }
00227 
00228             memcpy( alpha_ptr, classifier->alpha, (node_count+1)*sizeof(alpha_ptr[0]));
00229             haar_node_ptr =
00230                 (CvHidHaarTreeNode*)cvAlignPtr(alpha_ptr+node_count+1, sizeof(void*));
00231 
00232             out->is_stump_based &= node_count == 1;
00233         }
00234     }
00235 
00236     can_use_ipp &= !out->has_tilted_features && !out->is_tree && out->is_stump_based;
00237 
00238     if( can_use_ipp )
00239     {
00240         int ipp_datasize = cascade->count*sizeof(out->ipp_stages[0]);
00241         float ipp_weight_scale=(float)(1./((orig_window_size.width-icv_object_win_border*2)*
00242             (orig_window_size.height-icv_object_win_border*2)));
00243 
00244         CV_CALL( out->ipp_stages = (void**)cvAlloc( ipp_datasize ));
00245         memset( out->ipp_stages, 0, ipp_datasize );
00246 
00247         CV_CALL( ipp_features = (CvRect*)cvAlloc( max_count*3*sizeof(ipp_features[0]) ));
00248         CV_CALL( ipp_weights = (float*)cvAlloc( max_count*3*sizeof(ipp_weights[0]) ));
00249         CV_CALL( ipp_thresholds = (float*)cvAlloc( max_count*sizeof(ipp_thresholds[0]) ));
00250         CV_CALL( ipp_val1 = (float*)cvAlloc( max_count*sizeof(ipp_val1[0]) ));
00251         CV_CALL( ipp_val2 = (float*)cvAlloc( max_count*sizeof(ipp_val2[0]) ));
00252         CV_CALL( ipp_counts = (int*)cvAlloc( max_count*sizeof(ipp_counts[0]) ));
00253 
00254         for( i = 0; i < cascade->count; i++ )
00255         {
00256             CvHaarStageClassifier* stage_classifier = cascade->stage_classifier + i;
00257             for( j = 0, k = 0; j < stage_classifier->count; j++ )
00258             {
00259                 CvHaarClassifier* classifier = stage_classifier->classifier + j;
00260                 int rect_count = 2 + (classifier->haar_feature->rect[2].r.width != 0);
00261 
00262                 ipp_thresholds[j] = classifier->threshold[0];
00263                 ipp_val1[j] = classifier->alpha[0];
00264                 ipp_val2[j] = classifier->alpha[1];
00265                 ipp_counts[j] = rect_count;
00266                 
00267                 for( l = 0; l < rect_count; l++, k++ )
00268                 {
00269                     ipp_features[k] = classifier->haar_feature->rect[l].r;
00270                     //ipp_features[k].y = orig_window_size.height - ipp_features[k].y - ipp_features[k].height;
00271                     ipp_weights[k] = classifier->haar_feature->rect[l].weight*ipp_weight_scale;
00272                 }
00273             }
00274             
00275             if( icvHaarClassifierInitAlloc_32f_p( &out->ipp_stages[i],
00276                 ipp_features, ipp_weights, ipp_thresholds,
00277                 ipp_val1, ipp_val2, ipp_counts, stage_classifier->count ) < 0 )
00278                 break;
00279         }
00280 
00281         if( i < cascade->count )
00282         {
00283             for( j = 0; j < i; j++ )
00284                 if( icvHaarClassifierFree_32f_p && out->ipp_stages[i] )
00285                     icvHaarClassifierFree_32f_p( out->ipp_stages[i] );
00286             cvFree( (void**)&out->ipp_stages );
00287         }
00288     }
00289 
00290     cascade->hid_cascade = out;
00291     assert( (char*)haar_node_ptr - (char*)out <= datasize );
00292 
00293     __END__;
00294 
00295     if( cvGetErrStatus() < 0 )
00296         icvReleaseHidHaarClassifierCascade( &out );
00297 
00298     cvFree( (void**)&ipp_features );
00299     cvFree( (void**)&ipp_weights );
00300     cvFree( (void**)&ipp_thresholds );
00301     cvFree( (void**)&ipp_val1 );
00302     cvFree( (void**)&ipp_val2 );
00303     cvFree( (void**)&ipp_counts );
00304 
00305     return out;
00306 }
00307 
00308 
00309 #define sum_elem_ptr(sum,row,col)  \
00310     ((sumtype*)CV_MAT_ELEM_PTR_FAST((sum),(row),(col),sizeof(sumtype)))
00311 
00312 #define sqsum_elem_ptr(sqsum,row,col)  \
00313     ((sqsumtype*)CV_MAT_ELEM_PTR_FAST((sqsum),(row),(col),sizeof(sqsumtype)))
00314 
00315 #define calc_sum(rect,offset) \
00316     ((rect).p0[offset] - (rect).p1[offset] - (rect).p2[offset] + (rect).p3[offset])
00317 
00318 
00319 CV_IMPL void
00320 cvSetImagesForHaarClassifierCascade( CvHaarClassifierCascade* _cascade,
00321                                      const CvArr* _sum,
00322                                      const CvArr* _sqsum,
00323                                      const CvArr* _tilted_sum,
00324                                      double scale )
00325 {
00326     CV_FUNCNAME("cvSetImagesForHaarClassifierCascade");
00327 
00328     __BEGIN__;
00329 
00330     CvMat sum_stub, *sum = (CvMat*)_sum;
00331     CvMat sqsum_stub, *sqsum = (CvMat*)_sqsum;
00332     CvMat tilted_stub, *tilted = (CvMat*)_tilted_sum;
00333     CvHidHaarClassifierCascade* cascade;
00334     int coi0 = 0, coi1 = 0;
00335     int i, j, k, l;
00336     CvRect equ_rect;
00337     double weight_scale;
00338 
00339     if( !CV_IS_HAAR_CLASSIFIER(_cascade) )
00340         CV_ERROR( !_cascade ? CV_StsNullPtr : CV_StsBadArg, "Invalid classifier pointer" );
00341 
00342     if( scale <= 0 )
00343         CV_ERROR( CV_StsOutOfRange, "Scale must be positive" );
00344 
00345     CV_CALL( sum = cvGetMat( sum, &sum_stub, &coi0 ));
00346     CV_CALL( sqsum = cvGetMat( sqsum, &sqsum_stub, &coi1 ));
00347 
00348     if( coi0 || coi1 )
00349         CV_ERROR( CV_BadCOI, "COI is not supported" );
00350 
00351     if( !CV_ARE_SIZES_EQ( sum, sqsum ))
00352         CV_ERROR( CV_StsUnmatchedSizes, "All integral images must have the same size" );
00353 
00354     if( CV_MAT_TYPE(sqsum->type) != CV_64FC1 ||
00355         CV_MAT_TYPE(sum->type) != CV_32SC1 )
00356         CV_ERROR( CV_StsUnsupportedFormat,
00357         "Only (32s, 64f, 32s) combination of (sum,sqsum,tilted_sum) formats is allowed" );
00358 
00359     if( !_cascade->hid_cascade )
00360         CV_CALL( icvCreateHidHaarClassifierCascade(_cascade) );
00361 
00362     cascade = _cascade->hid_cascade;
00363 
00364     if( cascade->has_tilted_features )
00365     {
00366         CV_CALL( tilted = cvGetMat( tilted, &tilted_stub, &coi1 ));
00367 
00368         if( CV_MAT_TYPE(tilted->type) != CV_32SC1 )
00369             CV_ERROR( CV_StsUnsupportedFormat,
00370             "Only (32s, 64f, 32s) combination of (sum,sqsum,tilted_sum) formats is allowed" );
00371 
00372         if( sum->step != tilted->step )
00373             CV_ERROR( CV_StsUnmatchedSizes,
00374             "Sum and tilted_sum must have the same stride (step, widthStep)" );
00375 
00376         if( !CV_ARE_SIZES_EQ( sum, tilted ))
00377             CV_ERROR( CV_StsUnmatchedSizes, "All integral images must have the same size" );
00378         cascade->tilted = *tilted;
00379     }
00380     
00381     _cascade->scale = scale;
00382     _cascade->real_window_size.width = cvRound( _cascade->orig_window_size.width * scale );
00383     _cascade->real_window_size.height = cvRound( _cascade->orig_window_size.height * scale );
00384 
00385     cascade->sum = *sum;
00386     cascade->sqsum = *sqsum;
00387     
00388     equ_rect.x = equ_rect.y = cvRound(scale);
00389     equ_rect.width = cvRound((_cascade->orig_window_size.width-2)*scale);
00390     equ_rect.height = cvRound((_cascade->orig_window_size.height-2)*scale);
00391     weight_scale = 1./(equ_rect.width*equ_rect.height);
00392     cascade->inv_window_area = weight_scale;
00393 
00394     cascade->p0 = sum_elem_ptr(*sum, equ_rect.y, equ_rect.x);
00395     cascade->p1 = sum_elem_ptr(*sum, equ_rect.y, equ_rect.x + equ_rect.width );
00396     cascade->p2 = sum_elem_ptr(*sum, equ_rect.y + equ_rect.height, equ_rect.x );
00397     cascade->p3 = sum_elem_ptr(*sum, equ_rect.y + equ_rect.height,
00398                                      equ_rect.x + equ_rect.width );
00399 
00400     cascade->pq0 = sqsum_elem_ptr(*sqsum, equ_rect.y, equ_rect.x);
00401     cascade->pq1 = sqsum_elem_ptr(*sqsum, equ_rect.y, equ_rect.x + equ_rect.width );
00402     cascade->pq2 = sqsum_elem_ptr(*sqsum, equ_rect.y + equ_rect.height, equ_rect.x );
00403     cascade->pq3 = sqsum_elem_ptr(*sqsum, equ_rect.y + equ_rect.height,
00404                                           equ_rect.x + equ_rect.width );
00405 
00406     /* init pointers in haar features according to real window size and
00407        given image pointers */
00408     for( i = 0; i < _cascade->count; i++ )
00409     {
00410         for( j = 0; j < cascade->stage_classifier[i].count; j++ )
00411         {
00412             for( l = 0; l < cascade->stage_classifier[i].classifier[j].count; l++ )
00413             {
00414                 CvHaarFeature* feature = 
00415                     &_cascade->stage_classifier[i].classifier[j].haar_feature[l];
00416                 /* CvHidHaarClassifier* classifier =
00417                     cascade->stage_classifier[i].classifier + j; */
00418                 CvHidHaarFeature* hidfeature = 
00419                     &cascade->stage_classifier[i].classifier[j].node[l].feature;
00420                 double sum0 = 0, area0 = 0;
00421                 CvRect r[3];
00422 #if CV_ADJUST_FEATURES
00423                 int base_w = -1, base_h = -1;
00424                 int new_base_w = 0, new_base_h = 0;
00425                 int kx, ky;
00426                 int flagx = 0, flagy = 0;
00427                 int x0 = 0, y0 = 0;
00428 #endif
00429                 int nr;
00430 
00431                 /* align blocks */
00432                 for( k = 0; k < CV_HAAR_FEATURE_MAX; k++ )
00433                 {
00434                     if( !hidfeature->rect[k].p0 )
00435                         break;
00436 #if CV_ADJUST_FEATURES
00437                     r[k] = feature->rect[k].r;
00438                     base_w = (int)CV_IMIN( (unsigned)base_w, (unsigned)(r[k].width-1) );
00439                     base_w = (int)CV_IMIN( (unsigned)base_w, (unsigned)(r[k].x - r[0].x-1) );
00440                     base_h = (int)CV_IMIN( (unsigned)base_h, (unsigned)(r[k].height-1) );
00441                     base_h = (int)CV_IMIN( (unsigned)base_h, (unsigned)(r[k].y - r[0].y-1) );
00442 #endif
00443                 }
00444 
00445                 nr = k;
00446 
00447 #if CV_ADJUST_FEATURES
00448                 base_w += 1;
00449                 base_h += 1;
00450                 kx = r[0].width / base_w;
00451                 ky = r[0].height / base_h;
00452 
00453                 if( kx <= 0 )
00454                 {
00455                     flagx = 1;
00456                     new_base_w = cvRound( r[0].width * scale ) / kx;
00457                     x0 = cvRound( r[0].x * scale );
00458                 }
00459 
00460                 if( ky <= 0 )
00461                 {
00462                     flagy = 1;
00463                     new_base_h = cvRound( r[0].height * scale ) / ky;
00464                     y0 = cvRound( r[0].y * scale );
00465                 }
00466 #endif
00467         
00468                 for( k = 0; k < nr; k++ )
00469                 {
00470                     CvRect tr;
00471                     double correction_ratio;
00472             
00473 #if CV_ADJUST_FEATURES
00474                     if( flagx )
00475                     {
00476                         tr.x = (r[k].x - r[0].x) * new_base_w / base_w + x0;
00477                         tr.width = r[k].width * new_base_w / base_w;
00478                     }
00479                     else
00480 #endif
00481                     {
00482                         tr.x = cvRound( r[k].x * scale );
00483                         tr.width = cvRound( r[k].width * scale );
00484                     }
00485 
00486 #if CV_ADJUST_FEATURES
00487                     if( flagy )
00488                     {
00489                         tr.y = (r[k].y - r[0].y) * new_base_h / base_h + y0;
00490                         tr.height = r[k].height * new_base_h / base_h;
00491                     }
00492                     else
00493 #endif
00494                     {
00495                         tr.y = cvRound( r[k].y * scale );
00496                         tr.height = cvRound( r[k].height * scale );
00497                     }
00498 
00499 #if CV_ADJUST_WEIGHTS
00500                     {
00501                     // RAINER START
00502                     const float orig_feature_size =  (float)(feature->rect[k].r.width)*feature->rect[k].r.height; 
00503                     const float orig_norm_size = (float)(_cascade->orig_window_size.width)*(_cascade->orig_window_size.height);
00504                     const float feature_size = float(tr.width*tr.height);
00505                     //const float normSize    = float(equ_rect.width*equ_rect.height);
00506                     float target_ratio = orig_feature_size / orig_norm_size;
00507                     //float isRatio = featureSize / normSize;
00508                     //correctionRatio = targetRatio / isRatio / normSize;
00509                     correction_ratio = target_ratio / feature_size;
00510                     // RAINER END
00511                     }
00512 #else
00513                     correction_ratio = weight_scale * (!feature->tilted ? 1 : 0.5);
00514 #endif
00515 
00516                     if( !feature->tilted )
00517                     {
00518                         hidfeature->rect[k].p0 = sum_elem_ptr(*sum, tr.y, tr.x);
00519                         hidfeature->rect[k].p1 = sum_elem_ptr(*sum, tr.y, tr.x + tr.width);
00520                         hidfeature->rect[k].p2 = sum_elem_ptr(*sum, tr.y + tr.height, tr.x);
00521                         hidfeature->rect[k].p3 = sum_elem_ptr(*sum, tr.y + tr.height, tr.x + tr.width);
00522                     }
00523                     else
00524                     {
00525                         hidfeature->rect[k].p2 = sum_elem_ptr(*tilted, tr.y + tr.width, tr.x + tr.width);
00526                         hidfeature->rect[k].p3 = sum_elem_ptr(*tilted, tr.y + tr.width + tr.height,
00527                                                               tr.x + tr.width - tr.height);
00528                         hidfeature->rect[k].p0 = sum_elem_ptr(*tilted, tr.y, tr.x);
00529                         hidfeature->rect[k].p1 = sum_elem_ptr(*tilted, tr.y + tr.height, tr.x - tr.height);
00530                     }
00531 
00532                     hidfeature->rect[k].weight = (float)(feature->rect[k].weight * correction_ratio);
00533 
00534                     if( k == 0 )
00535                         area0 = tr.width * tr.height;
00536                     else
00537                         sum0 += hidfeature->rect[k].weight * tr.width * tr.height;
00538                 }
00539 
00540                 hidfeature->rect[0].weight = (float)(-sum0/area0);
00541             } /* l */
00542         } /* j */
00543     }
00544 
00545     __END__;
00546 }
00547 
00548 
00549 CV_INLINE
00550 double icvEvalHidHaarClassifier( CvHidHaarClassifier* classifier,
00551                                  double variance_norm_factor,
00552                                  size_t p_offset )
00553 {
00554     int idx = 0;
00555     do 
00556     {
00557         CvHidHaarTreeNode* node = classifier->node + idx;
00558         double t = node->threshold * variance_norm_factor;
00559 
00560         double sum = calc_sum(node->feature.rect[0],p_offset) * node->feature.rect[0].weight;
00561         sum += calc_sum(node->feature.rect[1],p_offset) * node->feature.rect[1].weight;
00562 
00563         if( node->feature.rect[2].p0 )
00564             sum += calc_sum(node->feature.rect[2],p_offset) * node->feature.rect[2].weight;
00565 
00566         idx = sum < t ? node->left : node->right;
00567     }
00568     while( idx > 0 );
00569     return classifier->alpha[-idx];
00570 }
00571 
00572 
00573 CV_IMPL int
00574 cvRunHaarClassifierCascade( CvHaarClassifierCascade* _cascade,
00575                             CvPoint pt, int start_stage )
00576 {
00577     int result = -1;
00578     CV_FUNCNAME("cvRunHaarClassifierCascade");
00579 
00580     __BEGIN__;
00581 
00582     int p_offset, pq_offset;
00583     int i, j;
00584     double mean, variance_norm_factor;
00585     CvHidHaarClassifierCascade* cascade;
00586 
00587     if( !CV_IS_HAAR_CLASSIFIER(_cascade) )
00588         CV_ERROR( !_cascade ? CV_StsNullPtr : CV_StsBadArg, "Invalid cascade pointer" );
00589 
00590     cascade = _cascade->hid_cascade;
00591     if( !cascade )
00592         CV_ERROR( CV_StsNullPtr, "Hidden cascade has not been created.\n"
00593             "Use cvSetImagesForHaarClassifierCascade" );
00594 
00595     if( pt.x < 0 || pt.y < 0 ||
00596         pt.x + _cascade->real_window_size.width >= cascade->sum.width-2 ||
00597         pt.y + _cascade->real_window_size.height >= cascade->sum.height-2 )
00598         EXIT;
00599 
00600     p_offset = pt.y * (cascade->sum.step/sizeof(sumtype)) + pt.x;
00601     pq_offset = pt.y * (cascade->sqsum.step/sizeof(sqsumtype)) + pt.x;
00602     mean = calc_sum(*cascade,p_offset)*cascade->inv_window_area;
00603     variance_norm_factor = cascade->pq0[pq_offset] - cascade->pq1[pq_offset] -
00604                            cascade->pq2[pq_offset] + cascade->pq3[pq_offset];
00605     variance_norm_factor = variance_norm_factor*cascade->inv_window_area - mean*mean;
00606     if( variance_norm_factor >= 0. )
00607         variance_norm_factor = sqrt(variance_norm_factor);
00608     else
00609         variance_norm_factor = 1.;
00610 
00611     if( cascade->is_tree )
00612     {
00613         CvHidHaarStageClassifier* ptr;
00614         assert( start_stage == 0 );
00615 
00616         result = 1;
00617         ptr = cascade->stage_classifier;
00618 
00619         while( ptr )
00620         {
00621             double stage_sum = 0;
00622 
00623             for( j = 0; j < ptr->count; j++ )
00624             {
00625                 stage_sum += icvEvalHidHaarClassifier( ptr->classifier + j,
00626                     variance_norm_factor, p_offset );
00627             }
00628 
00629             if( stage_sum >= ptr->threshold )
00630             {
00631                 ptr = ptr->child;
00632             }
00633             else
00634             {
00635                 while( ptr && ptr->next == NULL ) ptr = ptr->parent;
00636                 if( ptr == NULL )
00637                 {
00638                     result = 0;
00639                     EXIT;
00640                 }
00641                 ptr = ptr->next;
00642             }
00643         }
00644     }
00645     else if( cascade->is_stump_based )
00646     {
00647         for( i = start_stage; i < cascade->count; i++ )
00648         {
00649             double stage_sum = 0;
00650 
00651             if( cascade->stage_classifier[i].two_rects )
00652             {
00653                 for( j = 0; j < cascade->stage_classifier[i].count; j++ )
00654                 {
00655                     CvHidHaarClassifier* classifier = cascade->stage_classifier[i].classifier + j;
00656                     CvHidHaarTreeNode* node = classifier->node;
00657                     double sum, t = node->threshold*variance_norm_factor, a, b;
00658 
00659                     sum = calc_sum(node->feature.rect[0],p_offset) * node->feature.rect[0].weight;
00660                     sum += calc_sum(node->feature.rect[1],p_offset) * node->feature.rect[1].weight;
00661 
00662                     a = classifier->alpha[0];
00663                     b = classifier->alpha[1];
00664                     stage_sum += sum < t ? a : b;
00665                 }
00666             }
00667             else
00668             {
00669                 for( j = 0; j < cascade->stage_classifier[i].count; j++ )
00670                 {
00671                     CvHidHaarClassifier* classifier = cascade->stage_classifier[i].classifier + j;
00672                     CvHidHaarTreeNode* node = classifier->node;
00673                     double sum, t = node->threshold*variance_norm_factor, a, b;
00674 
00675                     sum = calc_sum(node->feature.rect[0],p_offset) * node->feature.rect[0].weight;
00676                     sum += calc_sum(node->feature.rect[1],p_offset) * node->feature.rect[1].weight;
00677 
00678                     if( node->feature.rect[2].p0 )
00679                         sum += calc_sum(node->feature.rect[2],p_offset) * node->feature.rect[2].weight;
00680 
00681                     a = classifier->alpha[0];
00682                     b = classifier->alpha[1];
00683                     stage_sum += sum < t ? a : b;
00684                 }
00685             }
00686 
00687             if( stage_sum < cascade->stage_classifier[i].threshold )
00688             {
00689                 result = -i;
00690                 EXIT;
00691             }
00692         }
00693     }
00694     else
00695     {
00696         for( i = start_stage; i < cascade->count; i++ )
00697         {
00698             double stage_sum = 0;
00699 
00700             for( j = 0; j < cascade->stage_classifier[i].count; j++ )
00701             {
00702                 stage_sum += icvEvalHidHaarClassifier(
00703                     cascade->stage_classifier[i].classifier + j,
00704                     variance_norm_factor, p_offset );
00705             }
00706 
00707             if( stage_sum < cascade->stage_classifier[i].threshold )
00708             {
00709                 result = -i;
00710                 EXIT;
00711             }
00712         }
00713     }
00714 
00715     result = 1;
00716 
00717     __END__;
00718 
00719     return result;
00720 }
00721 
00722 
00723 CV_IMPL CvSeq*
00724 cvHaarDetectObjects( const CvArr* _img,
00725                      CvHaarClassifierCascade* cascade,
00726                      CvMemStorage* storage, double scale_factor,
00727                      int min_neighbors, int flags, CvSize min_size )
00728 {
00729     int split_stage = 2;
00730     CvMat stub, *img = (CvMat*)_img;
00731     CvMat *temp = 0, *sum = 0, *tilted = 0, *sqsum = 0, *norm_img = 0, *sumcanny = 0, *img_small = 0;
00732     CvSeq* seq = 0;
00733     CvSeq* seq2 = 0;
00734     CvSeq* idx_seq = 0;
00735     CvSeq* result_seq = 0;
00736     CvMemStorage* temp_storage = 0;
00737     CvAvgComp* comps = 0;
00738     
00739     CV_FUNCNAME( "cvHaarDetectObjects" );
00740 
00741     __BEGIN__;
00742 
00743     double factor;
00744     int i, npass = 2, coi;
00745     int do_canny_pruning = flags & CV_HAAR_DO_CANNY_PRUNING;
00746 
00747     if( !CV_IS_HAAR_CLASSIFIER(cascade) )
00748         CV_ERROR( !cascade ? CV_StsNullPtr : CV_StsBadArg, "Invalid classifier cascade" );
00749 
00750     if( !storage )
00751         CV_ERROR( CV_StsNullPtr, "Null storage pointer" );
00752 
00753     CV_CALL( img = cvGetMat( img, &stub, &coi ));
00754     if( coi )
00755         CV_ERROR( CV_BadCOI, "COI is not supported" );
00756 
00757     if( CV_MAT_DEPTH(img->type) != CV_8U )
00758         CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit images are supported" );
00759 
00760     CV_CALL( temp = cvCreateMat( img->rows, img->cols, CV_8UC1 ));
00761     CV_CALL( sum = cvCreateMat( img->rows + 1, img->cols + 1, CV_32SC1 ));
00762     CV_CALL( sqsum = cvCreateMat( img->rows + 1, img->cols + 1, CV_64FC1 ));
00763     CV_CALL( temp_storage = cvCreateChildMemStorage( storage ));
00764 
00765     if( !cascade->hid_cascade )
00766         CV_CALL( icvCreateHidHaarClassifierCascade(cascade) );
00767 
00768     if( cascade->hid_cascade->has_tilted_features )
00769         tilted = cvCreateMat( img->rows + 1, img->cols + 1, CV_32SC1 );
00770 
00771     seq = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvRect), temp_storage );
00772     seq2 = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvAvgComp), temp_storage );
00773     result_seq = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvAvgComp), storage );
00774 
00775     if( min_neighbors == 0 )
00776         seq = result_seq;
00777 
00778     if( CV_MAT_CN(img->type) > 1 )
00779     {
00780         cvCvtColor( img, temp, CV_BGR2GRAY );
00781         img = temp;
00782     }
00783     
00784     if( flags & CV_HAAR_SCALE_IMAGE )
00785     {
00786         CvSize win_size0 = cascade->orig_window_size;
00787         int use_ipp = cascade->hid_cascade->ipp_stages != 0 &&
00788                     icvApplyHaarClassifier_32s32f_C1R_p != 0;
00789 
00790         if( use_ipp )
00791             CV_CALL( norm_img = cvCreateMat( img->rows, img->cols, CV_32FC1 ));
00792         CV_CALL( img_small = cvCreateMat( img->rows + 1, img->cols + 1, CV_8UC1 ));
00793 
00794         for( factor = 1; ; factor *= scale_factor )
00795         {
00796             int positive = 0;
00797             int x, y;
00798             CvSize win_size = { cvRound(win_size0.width*factor),
00799                                 cvRound(win_size0.height*factor) };
00800             CvSize sz = { cvRound( img->cols/factor ), cvRound( img->rows/factor ) };
00801             CvSize sz1 = { sz.width - win_size0.width, sz.height - win_size0.height };
00802             CvRect rect1 = { icv_object_win_border, icv_object_win_border,
00803                 win_size0.width - icv_object_win_border*2,
00804                 win_size0.height - icv_object_win_border*2 };
00805             CvMat img1, sum1, sqsum1, norm1, tilted1, mask1;
00806             CvMat* _tilted = 0;
00807 
00808             if( sz1.width <= 0 || sz1.height <= 0 )
00809                 break;
00810             if( win_size.width < min_size.width || win_size.height < min_size.height )
00811                 continue;
00812 
00813             img1 = cvMat( sz.height, sz.width, CV_8UC1, img_small->data.ptr );
00814             sum1 = cvMat( sz.height+1, sz.width+1, CV_32SC1, sum->data.ptr );
00815             sqsum1 = cvMat( sz.height+1, sz.width+1, CV_64FC1, sqsum->data.ptr );
00816             if( tilted )
00817             {
00818                 tilted1 = cvMat( sz.height+1, sz.width+1, CV_32SC1, tilted->data.ptr );
00819                 _tilted = &tilted1;
00820             }
00821             norm1 = cvMat( sz1.height, sz1.width, CV_32FC1, norm_img ? norm_img->data.ptr : 0 );
00822             mask1 = cvMat( sz1.height, sz1.width, CV_8UC1, temp->data.ptr );
00823 
00824             cvResize( img, &img1, CV_INTER_LINEAR );
00825             cvIntegral( &img1, &sum1, &sqsum1, _tilted );
00826 
00827             if( use_ipp && icvRectStdDev_32s32f_C1R_p( sum1.data.i, sum1.step,
00828                 sqsum1.data.db, sqsum1.step, norm1.data.fl, norm1.step, sz1, rect1 ) < 0 )
00829                 use_ipp = 0;
00830 
00831             if( use_ipp )
00832             {
00833                 positive = mask1.cols*mask1.rows;
00834                 cvSet( &mask1, cvScalarAll(255) );
00835                 for( i = 0; i < cascade->count; i++ )
00836                 {
00837                     if( icvApplyHaarClassifier_32s32f_C1R_p(sum1.data.i, sum1.step,
00838                         norm1.data.fl, norm1.step, mask1.data.ptr, mask1.step,
00839                         sz1, &positive, cascade->hid_cascade->stage_classifier[i].threshold,
00840                         cascade->hid_cascade->ipp_stages[i]) < 0 )
00841                     {
00842                         use_ipp = 0;
00843                         break;
00844                     }
00845                     if( positive <= 0 )
00846                         break;
00847                 }
00848             }
00849             
00850             if( !use_ipp )
00851             {
00852                 cvSetImagesForHaarClassifierCascade( cascade, &sum1, &sqsum1, 0, 1. );
00853                 for( y = 0, positive = 0; y < sz1.height; y++ )
00854                     for( x = 0; x < sz1.width; x++ )
00855                     {
00856                         mask1.data.ptr[mask1.step*y + x] =
00857                             cvRunHaarClassifierCascade( cascade, cvPoint(x,y), 0 ) > 0;
00858                         positive += mask1.data.ptr[mask1.step*y + x];
00859                     }
00860             }
00861 
00862             if( positive > 0 )
00863             {
00864                 for( y = 0; y < sz1.height; y++ )
00865                     for( x = 0; x < sz1.width; x++ )
00866                         if( mask1.data.ptr[mask1.step*y + x] != 0 )
00867                         {
00868                             CvRect obj_rect = { cvRound(y*factor), cvRound(x*factor),
00869                                                 win_size.width, win_size.height };
00870                             cvSeqPush( seq, &obj_rect );
00871                         }
00872             }
00873         }
00874     }
00875     else
00876     {
00877         cvIntegral( img, sum, sqsum, tilted );
00878     
00879         if( do_canny_pruning )
00880         {
00881             sumcanny = cvCreateMat( img->rows + 1, img->cols + 1, CV_32SC1 );
00882             cvCanny( img, temp, 0, 50, 3 );
00883             cvIntegral( temp, sumcanny );
00884         }
00885     
00886         if( (unsigned)split_stage >= (unsigned)cascade->count ||
00887             cascade->hid_cascade->is_tree )
00888         {
00889             split_stage = cascade->count;
00890             npass = 1;
00891         }
00892 
00893         for( factor = 1; factor*cascade->orig_window_size.width < img->cols - 10 &&
00894                          factor*cascade->orig_window_size.height < img->rows - 10;
00895              factor *= scale_factor )
00896         {
00897             const double ystep = MAX( 2, factor );
00898             CvSize win_size = { cvRound( cascade->orig_window_size.width * factor ),
00899                                 cvRound( cascade->orig_window_size.height * factor )};
00900             CvRect equ_rect = { 0, 0, 0, 0 };
00901             int *p0 = 0, *p1 = 0, *p2 = 0, *p3 = 0;
00902             int *pq0 = 0, *pq1 = 0, *pq2 = 0, *pq3 = 0;
00903             int pass, stage_offset = 0;
00904             int stop_height = cvRound((img->rows - win_size.height) / ystep);
00905 
00906             if( win_size.width < min_size.width || win_size.height < min_size.height )
00907                 continue;
00908 
00909             cvSetImagesForHaarClassifierCascade( cascade, sum, sqsum, tilted, factor );
00910             cvZero( temp );
00911 
00912             if( do_canny_pruning )
00913             {
00914                 equ_rect.x = cvRound(win_size.width*0.15);
00915                 equ_rect.y = cvRound(win_size.height*0.15);
00916                 equ_rect.width = cvRound(win_size.width*0.7);
00917                 equ_rect.height = cvRound(win_size.height*0.7);
00918 
00919                 p0 = (int*)(sumcanny->data.ptr + equ_rect.y*sumcanny->step) + equ_rect.x;
00920                 p1 = (int*)(sumcanny->data.ptr + equ_rect.y*sumcanny->step)
00921                             + equ_rect.x + equ_rect.width;
00922                 p2 = (int*)(sumcanny->data.ptr + (equ_rect.y + equ_rect.height)*sumcanny->step) + equ_rect.x;
00923                 p3 = (int*)(sumcanny->data.ptr + (equ_rect.y + equ_rect.height)*sumcanny->step)
00924                             + equ_rect.x + equ_rect.width;
00925 
00926                 pq0 = (int*)(sum->data.ptr + equ_rect.y*sum->step) + equ_rect.x;
00927                 pq1 = (int*)(sum->data.ptr + equ_rect.y*sum->step)
00928                             + equ_rect.x + equ_rect.width;
00929                 pq2 = (int*)(sum->data.ptr + (equ_rect.y + equ_rect.height)*sum->step) + equ_rect.x;
00930                 pq3 = (int*)(sum->data.ptr + (equ_rect.y + equ_rect.height)*sum->step)
00931                             + equ_rect.x + equ_rect.width;
00932             }
00933 
00934             cascade->hid_cascade->count = split_stage;
00935 
00936             for( pass = 0; pass < npass; pass++ )
00937             {
00938     #ifdef _OPENMP
00939     #pragma omp parallel for shared(cascade, stop_height, seq, ystep, temp, \
00940         win_size, pass, npass, sum, p0, p1, p2, p3, pq0, pq1, pq2, pq3, stage_offset)
00941     #endif // _OPENMP
00942 
00943                 for( int _iy = 0; _iy < stop_height; _iy++ )
00944                 {
00945                     int iy = cvRound(_iy*ystep);
00946                     int _ix, _xstep = 1;
00947                     int stop_width = cvRound((img->cols - win_size.width) / ystep);
00948                     uchar* mask_row = temp->data.ptr + temp->step * iy;
00949 
00950                     for( _ix = 0; _ix < stop_width; _ix += _xstep )
00951                     {
00952                         int ix = cvRound(_ix*ystep); // it really should be ystep
00953                     
00954                         if( pass == 0 )
00955                         {
00956                             int result;
00957                             _xstep = 2;
00958 
00959                             if( do_canny_pruning )
00960                             {
00961                                 int offset;
00962                                 int s, sq;
00963                         
00964                                 offset = iy*(sum->step/sizeof(p0[0])) + ix;
00965                                 s = p0[offset] - p1[offset] - p2[offset] + p3[offset];
00966                                 sq = pq0[offset] - pq1[offset] - pq2[offset] + pq3[offset];
00967                                 if( s < 100 || sq < 20 )
00968                                     continue;
00969                             }
00970 
00971                             result = cvRunHaarClassifierCascade( cascade, cvPoint(ix,iy), 0 );
00972     #ifdef _OPENMP
00973     #pragma omp critical
00974     #endif
00975                             if( result > 0 )
00976                             {
00977                                 if( pass < npass - 1 )
00978                                     mask_row[ix] = 1;
00979                                 else
00980                                 {
00981                                     CvRect rect = cvRect(ix,iy,win_size.width,win_size.height);
00982                                     cvSeqPush( seq, &rect );
00983                                 }
00984                             }
00985                             if( result < 0 )
00986                                 _xstep = 1;
00987                         }
00988                         else if( mask_row[ix] )
00989                         {
00990                             int result = cvRunHaarClassifierCascade( cascade, cvPoint(ix,iy),
00991                                                                      stage_offset );
00992     #ifdef _OPENMP
00993     #pragma omp critical
00994     #endif
00995                             if( result > 0 )
00996                             {
00997                                 if( pass == npass - 1 )
00998                                 {
00999                                     CvRect rect = cvRect(ix,iy,win_size.width,win_size.height);
01000                                     cvSeqPush( seq, &rect );
01001                                 }
01002                             }
01003                             else
01004                                 mask_row[ix] = 0;
01005                         }
01006                     }
01007                 }
01008                 stage_offset = cascade->hid_cascade->count;
01009                 cascade->hid_cascade->count = cascade->count;
01010             }
01011         }
01012     }
01013 
01014     if( min_neighbors != 0 )
01015     {
01016         // group retrieved rectangles in order to filter out noise 
01017         int ncomp = cvSeqPartition( seq, 0, &idx_seq, is_equal, 0 );
01018         CV_CALL( comps = (CvAvgComp*)cvAlloc( (ncomp+1)*sizeof(comps[0])));
01019         memset( comps, 0, (ncomp+1)*sizeof(comps[0]));
01020 
01021         // count number of neighbors
01022         for( i = 0; i < seq->total; i++ )
01023         {
01024             CvRect r1 = *(CvRect*)cvGetSeqElem( seq, i );
01025             int idx = *(int*)cvGetSeqElem( idx_seq, i );
01026             assert( (unsigned)idx < (unsigned)ncomp );
01027 
01028             comps[idx].neighbors++;
01029              
01030             comps[idx].rect.x += r1.x;
01031             comps[idx].rect.y += r1.y;
01032             comps[idx].rect.width += r1.width;
01033             comps[idx].rect.height += r1.height;
01034         }
01035 
01036         // calculate average bounding box
01037         for( i = 0; i < ncomp; i++ )
01038         {
01039             int n = comps[i].neighbors;
01040             if( n >= min_neighbors )
01041             {
01042                 CvAvgComp comp;
01043                 comp.rect.x = (comps[i].rect.x*2 + n)/(2*n);
01044                 comp.rect.y = (comps[i].rect.y*2 + n)/(2*n);
01045                 comp.rect.width = (comps[i].rect.width*2 + n)/(2*n);
01046                 comp.rect.height = (comps[i].rect.height*2 + n)/(2*n);
01047                 comp.neighbors = comps[i].neighbors;
01048 
01049                 cvSeqPush( seq2, &comp );
01050             }
01051         }
01052 
01053         // filter out small face rectangles inside large face rectangles
01054         for( i = 0; i < seq2->total; i++ )
01055         {
01056             CvAvgComp r1 = *(CvAvgComp*)cvGetSeqElem( seq2, i );
01057             int j, flag = 1;
01058 
01059             for( j = 0; j < seq2->total; j++ )
01060             {
01061                 CvAvgComp r2 = *(CvAvgComp*)cvGetSeqElem( seq2, j );
01062                 int distance = cvRound( r2.rect.width * 0.2 );
01063             
01064                 if( i != j &&
01065                     r1.rect.x >= r2.rect.x - distance &&
01066                     r1.rect.y >= r2.rect.y - distance &&
01067                     r1.rect.x + r1.rect.width <= r2.rect.x + r2.rect.width + distance &&
01068                     r1.rect.y + r1.rect.height <= r2.rect.y + r2.rect.height + distance &&
01069                     (r2.neighbors > MAX( 3, r1.neighbors ) || r1.neighbors < 3) )
01070                 {
01071                     flag = 0;
01072                     break;
01073                 }
01074             }
01075 
01076             if( flag )
01077             {
01078                 cvSeqPush( result_seq, &r1 );
01079                 /* cvSeqPush( result_seq, &r1.rect ); */
01080             }
01081         }
01082     }
01083 
01084     __END__;
01085 
01086     cvReleaseMemStorage( &temp_storage );
01087     cvReleaseMat( &sum );
01088     cvReleaseMat( &sqsum );
01089     cvReleaseMat( &tilted );
01090     cvReleaseMat( &temp );
01091     cvReleaseMat( &sumcanny );
01092     cvReleaseMat( &norm_img );
01093     cvReleaseMat( &img_small );
01094     cvFree( (void**)&comps );
01095 
01096     return result_seq;
01097 }
01098 
01099 
01100 static CvHaarClassifierCascade*
01101 icvLoadCascadeCART( const char** input_cascade, int n, CvSize orig_window_size )
01102 {
01103     int i;
01104     CvHaarClassifierCascade* cascade = icvCreateHaarClassifierCascade(n);
01105     cascade->orig_window_size = orig_window_size;
01106 
01107     for( i = 0; i < n; i++ )
01108     {
01109         int j, count, l;
01110         float threshold = 0;
01111         const char* stage = input_cascade[i];
01112         int dl = 0;
01113 
01114         /* tree links */
01115         int parent = -1;
01116         int next = -1;
01117 
01118         sscanf( stage, "%d%n", &count, &dl );
01119         stage += dl;
01120         
01121         assert( count > 0 );
01122         cascade->stage_classifier[i].count = count;
01123         cascade->stage_classifier[i].classifier =
01124             (CvHaarClassifier*)cvAlloc( count*sizeof(cascade->stage_classifier[i].classifier[0]));
01125 
01126         for( j = 0; j < count; j++ )
01127         {
01128             CvHaarClassifier* classifier = cascade->stage_classifier[i].classifier + j;
01129             int k, rects = 0;
01130             char str[100];
01131             
01132             sscanf( stage, "%d%n", &classifier->count, &dl );
01133             stage += dl;
01134 
01135             classifier->haar_feature = (CvHaarFeature*) cvAlloc( 
01136                 classifier->count * ( sizeof( *classifier->haar_feature ) +
01137                                       sizeof( *classifier->threshold ) +
01138                                       sizeof( *classifier->left ) +
01139                                       sizeof( *classifier->right ) ) +
01140                 (classifier->count + 1) * sizeof( *classifier->alpha ) );
01141             classifier->threshold = (float*) (classifier->haar_feature+classifier->count);
01142             classifier->left = (int*) (classifier->threshold + classifier->count);
01143             classifier->right = (int*) (classifier->left + classifier->count);
01144             classifier->alpha = (float*) (classifier->right + classifier->count);
01145             
01146             for( l = 0; l < classifier->count; l++ )
01147             {
01148                 sscanf( stage, "%d%n", &rects, &dl );
01149                 stage += dl;
01150 
01151                 assert( rects >= 2 && rects <= CV_HAAR_FEATURE_MAX );
01152 
01153                 for( k = 0; k < rects; k++ )
01154                 {
01155                     CvRect r;
01156                     int band = 0;
01157                     sscanf( stage, "%d%d%d%d%d%f%n",
01158                             &r.x, &r.y, &r.width, &r.height, &band,
01159                             &(classifier->haar_feature[l].rect[k].weight), &dl );
01160                     stage += dl;
01161                     classifier->haar_feature[l].rect[k].r = r;
01162                 }
01163                 sscanf( stage, "%s%n", str, &dl );
01164                 stage += dl;
01165             
01166                 classifier->haar_feature[l].tilted = strncmp( str, "tilted", 6 ) == 0;
01167             
01168                 for( k = rects; k < CV_HAAR_FEATURE_MAX; k++ )
01169                 {
01170                     memset( classifier->haar_feature[l].rect + k, 0,
01171                             sizeof(classifier->haar_feature[l].rect[k]) );
01172                 }
01173                 
01174                 sscanf( stage, "%f%d%d%n", &(classifier->threshold[l]), 
01175                                        &(classifier->left[l]),
01176                                        &(classifier->right[l]), &dl );
01177                 stage += dl;
01178             }
01179             for( l = 0; l <= classifier->count; l++ )
01180             {
01181                 sscanf( stage, "%f%n", &(classifier->alpha[l]), &dl );
01182                 stage += dl;
01183             }
01184         }
01185        
01186         sscanf( stage, "%f%n", &threshold, &dl );
01187         stage += dl;
01188 
01189         cascade->stage_classifier[i].threshold = threshold;
01190 
01191         /* load tree links */
01192         if( sscanf( stage, "%d%d%n", &parent, &next, &dl ) != 2 )
01193         {
01194             parent = i - 1;
01195             next = -1;
01196         }
01197         stage += dl;
01198 
01199         cascade->stage_classifier[i].parent = parent;
01200         cascade->stage_classifier[i].next = next;
01201         cascade->stage_classifier[i].child = -1;
01202 
01203         if( parent != -1 && cascade->stage_classifier[parent].child == -1 )
01204         {
01205             cascade->stage_classifier[parent].child = i;
01206         }
01207     }
01208 
01209     return cascade;
01210 }
01211 
01212 #ifndef _MAX_PATH
01213 #define _MAX_PATH 1024
01214 #endif
01215 
01216 CV_IMPL CvHaarClassifierCascade*
01217 cvLoadHaarClassifierCascade( const char* directory, CvSize orig_window_size )
01218 {
01219     const char** input_cascade = 0; 
01220     CvHaarClassifierCascade *cascade = 0;
01221 
01222     CV_FUNCNAME( "cvLoadHaarClassifierCascade" );
01223 
01224     __BEGIN__;
01225 
01226     int i, n;
01227     const char* slash;
01228     char name[_MAX_PATH];
01229     int size = 0;
01230     char* ptr = 0;
01231 
01232     if( !directory )
01233         CV_ERROR( CV_StsNullPtr, "Null path is passed" );
01234 
01235     n = (int)strlen(directory)-1;
01236     slash = directory[n] == '\\' || directory[n] == '/' ? "" : "/";
01237 
01238     /* try to read the classifier from directory */
01239     for( n = 0; ; n++ )
01240     {
01241         sprintf( name, "%s%s%d/AdaBoostCARTHaarClassifier.txt", directory, slash, n );
01242         FILE* f = fopen( name, "rb" );
01243         if( !f )
01244             break;
01245         fseek( f, 0, SEEK_END );
01246         size += ftell( f ) + 1;
01247         fclose(f);
01248     }
01249 
01250     if( n == 0 && slash[0] )
01251     {
01252         CV_CALL( cascade = (CvHaarClassifierCascade*)cvLoad( directory ));
01253         EXIT;
01254     }
01255     else if( n == 0 )
01256         CV_ERROR( CV_StsBadArg, "Invalid path" );
01257     
01258     size += (n+1)*sizeof(char*);
01259     CV_CALL( input_cascade = (const char**)cvAlloc( size ));
01260     ptr = (char*)(input_cascade + n + 1);
01261 
01262     for( i = 0; i < n; i++ )
01263     {
01264         sprintf( name, "%s/%d/AdaBoostCARTHaarClassifier.txt", directory, i );
01265         FILE* f = fopen( name, "rb" );
01266         if( !f )
01267             CV_ERROR( CV_StsError, "" );
01268         fseek( f, 0, SEEK_END );
01269         size = ftell( f );
01270         fseek( f, 0, SEEK_SET );
01271         fread( ptr, 1, size, f );
01272         fclose(f);
01273         input_cascade[i] = ptr;
01274         ptr += size;
01275         *ptr++ = '\0';
01276     }
01277 
01278     input_cascade[n] = 0;
01279     cascade = icvLoadCascadeCART( input_cascade, n, orig_window_size );
01280 
01281     __END__;
01282 
01283     if( input_cascade )
01284         cvFree( (void**)&input_cascade );
01285 
01286     if( cvGetErrStatus() < 0 )
01287         cvReleaseHaarClassifierCascade( &cascade );
01288 
01289     return cascade;
01290 }
01291 
01292 
01293 CV_IMPL void
01294 cvReleaseHaarClassifierCascade( CvHaarClassifierCascade** _cascade )
01295 {
01296     if( _cascade && *_cascade )
01297     {
01298         int i, j;
01299         CvHaarClassifierCascade* cascade = *_cascade;
01300 
01301         for( i = 0; i < cascade->count; i++ )
01302         {
01303             for( j = 0; j < cascade->stage_classifier[i].count; j++ )
01304                 cvFree( (void**)
01305                     &(cascade->stage_classifier[i].classifier[j].haar_feature) );
01306             cvFree( (void**) &(cascade->stage_classifier[i].classifier) );
01307         }
01308         icvReleaseHidHaarClassifierCascade( &cascade->hid_cascade );
01309         cvFree( (void**)_cascade );
01310     }
01311 }
01312 
01313 
01314 /****************************************************************************************\
01315 *                                  Persistence functions                                 *
01316 \****************************************************************************************/
01317 
01318 /* field names */
01319 
01320 #define ICV_HAAR_SIZE_NAME            "size"
01321 #define ICV_HAAR_STAGES_NAME          "stages"
01322 #define ICV_HAAR_TREES_NAME             "trees"
01323 #define ICV_HAAR_FEATURE_NAME             "feature"
01324 #define ICV_HAAR_RECTS_NAME                 "rects"
01325 #define ICV_HAAR_TILTED_NAME                "tilted"
01326 #define ICV_HAAR_THRESHOLD_NAME           "threshold"
01327 #define ICV_HAAR_LEFT_NODE_NAME           "left_node"
01328 #define ICV_HAAR_LEFT_VAL_NAME            "left_val"
01329 #define ICV_HAAR_RIGHT_NODE_NAME          "right_node"
01330 #define ICV_HAAR_RIGHT_VAL_NAME           "right_val"
01331 #define ICV_HAAR_STAGE_THRESHOLD_NAME   "stage_threshold"
01332 #define ICV_HAAR_PARENT_NAME            "parent"
01333 #define ICV_HAAR_NEXT_NAME              "next"
01334 
01335 static int
01336 icvIsHaarClassifier( const void* struct_ptr )
01337 {
01338     return CV_IS_HAAR_CLASSIFIER( struct_ptr );
01339 }
01340 
01341 static void*
01342 icvReadHaarClassifier( CvFileStorage* fs, CvFileNode* node )
01343 {
01344     CvHaarClassifierCascade* cascade = NULL;
01345 
01346     CV_FUNCNAME( "cvReadHaarClassifier" );
01347 
01348     __BEGIN__;
01349 
01350     char buf[256];
01351     CvFileNode* seq_fn = NULL; /* sequence */
01352     CvFileNode* fn = NULL;
01353     CvFileNode* stages_fn = NULL;
01354     CvSeqReader stages_reader;
01355     int n;
01356     int i, j, k, l;
01357     int parent, next;
01358 
01359     CV_CALL( stages_fn = cvGetFileNodeByName( fs, node, ICV_HAAR_STAGES_NAME ) );
01360     if( !stages_fn || !CV_NODE_IS_SEQ( stages_fn->tag) )
01361         CV_ERROR( CV_StsError, "Invalid stages node" );
01362 
01363     n = stages_fn->data.seq->total;
01364     CV_CALL( cascade = icvCreateHaarClassifierCascade(n) );
01365 
01366     /* read size */
01367     CV_CALL( seq_fn = cvGetFileNodeByName( fs, node, ICV_HAAR_SIZE_NAME ) );
01368     if( !seq_fn || !CV_NODE_IS_SEQ( seq_fn->tag ) || seq_fn->data.seq->total != 2 )
01369         CV_ERROR( CV_StsError, "size node is not a valid sequence." );
01370     CV_CALL( fn = (CvFileNode*) cvGetSeqElem( seq_fn->data.seq, 0 ) );
01371     if( !CV_NODE_IS_INT( fn->tag ) || fn->data.i <= 0 )
01372         CV_ERROR( CV_StsError, "Invalid size node: width must be positive integer" );
01373     cascade->orig_window_size.width = fn->data.i;
01374     CV_CALL( fn = (CvFileNode*) cvGetSeqElem( seq_fn->data.seq, 1 ) );
01375     if( !CV_NODE_IS_INT( fn->tag ) || fn->data.i <= 0 )
01376         CV_ERROR( CV_StsError, "Invalid size node: height must be positive integer" );
01377     cascade->orig_window_size.height = fn->data.i;
01378 
01379     CV_CALL( cvStartReadSeq( stages_fn->data.seq, &stages_reader ) );
01380     for( i = 0; i < n; ++i )
01381     {
01382         CvFileNode* stage_fn;
01383         CvFileNode* trees_fn;
01384         CvSeqReader trees_reader;
01385 
01386         stage_fn = (CvFileNode*) stages_reader.ptr;
01387         if( !CV_NODE_IS_MAP( stage_fn->tag ) )
01388         {
01389             sprintf( buf, "Invalid stage %d", i );
01390             CV_ERROR( CV_StsError, buf );
01391         }
01392 
01393         CV_CALL( trees_fn = cvGetFileNodeByName( fs, stage_fn, ICV_HAAR_TREES_NAME ) );
01394         if( !trees_fn || !CV_NODE_IS_SEQ( trees_fn->tag )
01395             || trees_fn->data.seq->total <= 0 )
01396         {
01397             sprintf( buf, "Trees node is not a valid sequence. (stage %d)", i );
01398             CV_ERROR( CV_StsError, buf );
01399         }
01400 
01401         CV_CALL( cascade->stage_classifier[i].classifier =
01402             (CvHaarClassifier*) cvAlloc( trees_fn->data.seq->total
01403                 * sizeof( cascade->stage_classifier[i].classifier[0] ) ) );
01404         for( j = 0; j < trees_fn->data.seq->total; ++j )
01405         {
01406             cascade->stage_classifier[i].classifier[j].haar_feature = NULL;
01407         }
01408         cascade->stage_classifier[i].count = trees_fn->data.seq->total;
01409 
01410         CV_CALL( cvStartReadSeq( trees_fn->data.seq, &trees_reader ) );
01411         for( j = 0; j < trees_fn->data.seq->total; ++j )
01412         {
01413             CvFileNode* tree_fn;
01414             CvSeqReader tree_reader;
01415             CvHaarClassifier* classifier;
01416             int last_idx;
01417 
01418             classifier = &cascade->stage_classifier[i].classifier[j];
01419             tree_fn = (CvFileNode*) trees_reader.ptr;
01420             if( !CV_NODE_IS_SEQ( tree_fn->tag ) || tree_fn->data.seq->total <= 0 )
01421             {
01422                 sprintf( buf, "Tree node is not a valid sequence."
01423                          " (stage %d, tree %d)", i, j );
01424                 CV_ERROR( CV_StsError, buf );
01425             }
01426 
01427             classifier->count = tree_fn->data.seq->total;
01428             CV_CALL( classifier->haar_feature = (CvHaarFeature*) cvAlloc( 
01429                 classifier->count * ( sizeof( *classifier->haar_feature ) +
01430                                       sizeof( *classifier->threshold ) +
01431                                       sizeof( *classifier->left ) +
01432                                       sizeof( *classifier->right ) ) +
01433                 (classifier->count + 1) * sizeof( *classifier->alpha ) ) );
01434             classifier->threshold = (float*) (classifier->haar_feature+classifier->count);
01435             classifier->left = (int*) (classifier->threshold + classifier->count);
01436             classifier->right = (int*) (classifier->left + classifier->count);
01437             classifier->alpha = (float*) (classifier->right + classifier->count);
01438 
01439             CV_CALL( cvStartReadSeq( tree_fn->data.seq, &tree_reader ) );
01440             for( k = 0, last_idx = 0; k < tree_fn->data.seq->total; ++k )
01441             {
01442                 CvFileNode* node_fn;
01443                 CvFileNode* feature_fn;
01444                 CvFileNode* rects_fn;
01445                 CvSeqReader rects_reader;
01446 
01447                 node_fn = (CvFileNode*) tree_reader.ptr;
01448                 if( !CV_NODE_IS_MAP( node_fn->tag ) )
01449                 {
01450                     sprintf( buf, "Tree node %d is not a valid map. (stage %d, tree %d)",
01451                              k, i, j );
01452                     CV_ERROR( CV_StsError, buf );
01453                 }
01454                 CV_CALL( feature_fn = cvGetFileNodeByName( fs, node_fn,
01455                     ICV_HAAR_FEATURE_NAME ) );
01456                 if( !feature_fn || !CV_NODE_IS_MAP( feature_fn->tag ) )
01457                 {
01458                     sprintf( buf, "Feature node is not a valid map. "
01459                              "(stage %d, tree %d, node %d)", i, j, k );
01460                     CV_ERROR( CV_StsError, buf );
01461                 }
01462                 CV_CALL( rects_fn = cvGetFileNodeByName( fs, feature_fn,
01463                     ICV_HAAR_RECTS_NAME ) );
01464                 if( !rects_fn || !CV_NODE_IS_SEQ( rects_fn->tag )
01465                     || rects_fn->data.seq->total < 1
01466                     || rects_fn->data.seq->total > CV_HAAR_FEATURE_MAX )
01467                 {
01468                     sprintf( buf, "Rects node is not a valid sequence. "
01469                              "(stage %d, tree %d, node %d)", i, j, k );
01470                     CV_ERROR( CV_StsError, buf );
01471                 }
01472                 CV_CALL( cvStartReadSeq( rects_fn->data.seq, &rects_reader ) );
01473                 for( l = 0; l < rects_fn->data.seq->total; ++l )
01474                 {
01475                     CvFileNode* rect_fn;
01476                     CvRect r;
01477 
01478                     rect_fn = (CvFileNode*) rects_reader.ptr;
01479                     if( !CV_NODE_IS_SEQ( rect_fn->tag ) || rect_fn->data.seq->total != 5 )
01480                     {
01481                         sprintf( buf, "Rect %d is not a valid sequence. "
01482                                  "(stage %d, tree %d, node %d)", l, i, j, k );
01483                         CV_ERROR( CV_StsError, buf );
01484                     }
01485                     
01486                     fn = CV_SEQ_ELEM( rect_fn->data.seq, CvFileNode, 0 );
01487                     if( !CV_NODE_IS_INT( fn->tag ) || fn->data.i < 0 )
01488                     {
01489                         sprintf( buf, "x coordinate must be non-negative integer. "
01490                                  "(stage %d, tree %d, node %d, rect %d)", i, j, k, l );
01491                         CV_ERROR( CV_StsError, buf );
01492                     }
01493                     r.x = fn->data.i;
01494                     fn = CV_SEQ_ELEM( rect_fn->data.seq, CvFileNode, 1 );
01495                     if( !CV_NODE_IS_INT( fn->tag ) || fn->data.i < 0 )
01496                     {
01497                         sprintf( buf, "y coordinate must be non-negative integer. "
01498                                  "(stage %d, tree %d, node %d, rect %d)", i, j, k, l );
01499                         CV_ERROR( CV_StsError, buf );
01500                     }
01501                     r.y = fn->data.i;
01502                     fn = CV_SEQ_ELEM( rect_fn->data.seq, CvFileNode, 2 );
01503                     if( !CV_NODE_IS_INT( fn->tag ) || fn->data.i <= 0
01504                         || r.x + fn->data.i > cascade->orig_window_size.width )
01505                     {
01506                         sprintf( buf, "width must be positive integer and "
01507                                  "(x + width) must not exceed window width. "
01508                                  "(stage %d, tree %d, node %d, rect %d)", i, j, k, l );
01509                         CV_ERROR( CV_StsError, buf );
01510                     }
01511                     r.width = fn->data.i;
01512                     fn = CV_SEQ_ELEM( rect_fn->data.seq, CvFileNode, 3 );
01513                     if( !CV_NODE_IS_INT( fn->tag ) || fn->data.i <= 0
01514                         || r.y + fn->data.i > cascade->orig_window_size.height )
01515                     {
01516                         sprintf( buf, "height must be positive integer and "
01517                                  "(y + height) must not exceed window height. "
01518                                  "(stage %d, tree %d, node %d, rect %d)", i, j, k, l );
01519                         CV_ERROR( CV_StsError, buf );
01520                     }
01521                     r.height = fn->data.i;
01522                     fn = CV_SEQ_ELEM( rect_fn->data.seq, CvFileNode, 4 );
01523                     if( !CV_NODE_IS_REAL( fn->tag ) )
01524                     {
01525                         sprintf( buf, "weight must be real number. "
01526                                  "(stage %d, tree %d, node %d, rect %d)", i, j, k, l );
01527                         CV_ERROR( CV_StsError, buf );
01528                     }
01529 
01530                     classifier->haar_feature[k].rect[l].weight = (float) fn->data.f;
01531                     classifier->haar_feature[k].rect[l].r = r;
01532 
01533                     CV_NEXT_SEQ_ELEM( sizeof( *rect_fn ), rects_reader );
01534                 } /* for each rect */
01535                 for( l = rects_fn->data.seq->total; l < CV_HAAR_FEATURE_MAX; ++l )
01536                 {
01537                     classifier->haar_feature[k].rect[l].weight = 0;
01538                     classifier->haar_feature[k].rect[l].r = cvRect( 0, 0, 0, 0 );
01539                 }
01540 
01541                 CV_CALL( fn = cvGetFileNodeByName( fs, feature_fn, ICV_HAAR_TILTED_NAME));
01542                 if( !fn || !CV_NODE_IS_INT( fn->tag ) )
01543                 {
01544                     sprintf( buf, "tilted must be 0 or 1. "
01545                              "(stage %d, tree %d, node %d)", i, j, k );
01546                     CV_ERROR( CV_StsError, buf );
01547                 }
01548                 classifier->haar_feature[k].tilted = ( fn->data.i != 0 );
01549                 CV_CALL( fn = cvGetFileNodeByName( fs, node_fn, ICV_HAAR_THRESHOLD_NAME));
01550                 if( !fn || !CV_NODE_IS_REAL( fn->tag ) )
01551                 {
01552                     sprintf( buf, "threshold must be real number. "
01553                              "(stage %d, tree %d, node %d)", i, j, k );
01554                     CV_ERROR( CV_StsError, buf );
01555                 }
01556                 classifier->threshold[k] = (float) fn->data.f;
01557                 CV_CALL( fn = cvGetFileNodeByName( fs, node_fn, ICV_HAAR_LEFT_NODE_NAME));
01558                 if( fn )
01559                 {
01560                     if( !CV_NODE_IS_INT( fn->tag ) || fn->data.i <= k
01561                         || fn->data.i >= tree_fn->data.seq->total )
01562                     {
01563                         sprintf( buf, "left node must be valid node number. "
01564                                  "(stage %d, tree %d, node %d)", i, j, k );
01565                         CV_ERROR( CV_StsError, buf );
01566                     }
01567                     /* left node */
01568                     classifier->left[k] = fn->data.i;
01569                 }
01570                 else
01571                 {
01572                     CV_CALL( fn = cvGetFileNodeByName( fs, node_fn,
01573                         ICV_HAAR_LEFT_VAL_NAME ) );
01574                     if( !fn )
01575                     {
01576                         sprintf( buf, "left node or left value must be specified. "
01577                                  "(stage %d, tree %d, node %d)", i, j, k );
01578                         CV_ERROR( CV_StsError, buf );
01579                     }
01580                     if( !CV_NODE_IS_REAL( fn->tag ) )
01581                     {
01582                         sprintf( buf, "left value must be real number. "
01583                                  "(stage %d, tree %d, node %d)", i, j, k );
01584                         CV_ERROR( CV_StsError, buf );
01585                     }
01586                     /* left value */
01587                     if( last_idx >= classifier->count + 1 )
01588                     {
01589                         sprintf( buf, "Tree structure is broken: too many values. "
01590                                  "(stage %d, tree %d, node %d)", i, j, k );
01591                         CV_ERROR( CV_StsError, buf );
01592                     }
01593                     classifier->left[k] = -last_idx;
01594                     classifier->alpha[last_idx++] = (float) fn->data.f;
01595                 }
01596                 CV_CALL( fn = cvGetFileNodeByName( fs, node_fn,ICV_HAAR_RIGHT_NODE_NAME));
01597                 if( fn )
01598                 {
01599                     if( !CV_NODE_IS_INT( fn->tag ) || fn->data.i <= k
01600                         || fn->data.i >= tree_fn->data.seq->total )
01601                     {
01602                         sprintf( buf, "right node must be valid node number. "
01603                                  "(stage %d, tree %d, node %d)", i, j, k );
01604                         CV_ERROR( CV_StsError, buf );
01605                     }
01606                     /* right node */
01607                     classifier->right[k] = fn->data.i;
01608                 }
01609                 else
01610                 {
01611                     CV_CALL( fn = cvGetFileNodeByName( fs, node_fn,
01612                         ICV_HAAR_RIGHT_VAL_NAME ) );
01613                     if( !fn )
01614                     {
01615                         sprintf( buf, "right node or right value must be specified. "
01616                                  "(stage %d, tree %d, node %d)", i, j, k );
01617                         CV_ERROR( CV_StsError, buf );
01618                     }
01619                     if( !CV_NODE_IS_REAL( fn->tag ) )
01620                     {
01621                         sprintf( buf, "right value must be real number. "
01622                                  "(stage %d, tree %d, node %d)", i, j, k );
01623                         CV_ERROR( CV_StsError, buf );
01624                     }
01625                     /* right value */
01626                     if( last_idx >= classifier->count + 1 )
01627                     {
01628                         sprintf( buf, "Tree structure is broken: too many values. "
01629                                  "(stage %d, tree %d, node %d)", i, j, k );
01630                         CV_ERROR( CV_StsError, buf );
01631                     }
01632                     classifier->right[k] = -last_idx;
01633                     classifier->alpha[last_idx++] = (float) fn->data.f;
01634                 }
01635 
01636                 CV_NEXT_SEQ_ELEM( sizeof( *node_fn ), tree_reader );
01637             } /* for each node */
01638             if( last_idx != classifier->count + 1 )
01639             {
01640                 sprintf( buf, "Tree structure is broken: too few values. "
01641                          "(stage %d, tree %d)", i, j );
01642                 CV_ERROR( CV_StsError, buf );
01643             }
01644 
01645             CV_NEXT_SEQ_ELEM( sizeof( *tree_fn ), trees_reader );
01646         } /* for each tree */
01647 
01648         CV_CALL( fn = cvGetFileNodeByName( fs, stage_fn, ICV_HAAR_STAGE_THRESHOLD_NAME));
01649         if( !fn || !CV_NODE_IS_REAL( fn->tag ) )
01650         {
01651             sprintf( buf, "stage threshold must be real number. (stage %d)", i );
01652             CV_ERROR( CV_StsError, buf );
01653         }
01654         cascade->stage_classifier[i].threshold = (float) fn->data.f;
01655 
01656         parent = i - 1;
01657         next = -1;
01658 
01659         CV_CALL( fn = cvGetFileNodeByName( fs, stage_fn, ICV_HAAR_PARENT_NAME ) );
01660         if( !fn || !CV_NODE_IS_INT( fn->tag )
01661             || fn->data.i < -1 || fn->data.i >= cascade->count )
01662         {
01663             sprintf( buf, "parent must be integer number. (stage %d)", i );
01664             CV_ERROR( CV_StsError, buf );
01665         }
01666         parent = fn->data.i;
01667         CV_CALL( fn = cvGetFileNodeByName( fs, stage_fn, ICV_HAAR_NEXT_NAME ) );
01668         if( !fn || !CV_NODE_IS_INT( fn->tag )
01669             || fn->data.i < -1 || fn->data.i >= cascade->count )
01670         {
01671             sprintf( buf, "next must be integer number. (stage %d)", i );
01672             CV_ERROR( CV_StsError, buf );
01673         }
01674         next = fn->data.i;
01675 
01676         cascade->stage_classifier[i].parent = parent;
01677         cascade->stage_classifier[i].next = next;
01678         cascade->stage_classifier[i].child = -1;
01679 
01680         if( parent != -1 && cascade->stage_classifier[parent].child == -1 )
01681         {
01682             cascade->stage_classifier[parent].child = i;
01683         }
01684         
01685         CV_NEXT_SEQ_ELEM( sizeof( *stage_fn ), stages_reader );
01686     } /* for each stage */
01687 
01688     __END__;
01689 
01690     if( cvGetErrStatus() < 0 )
01691     {
01692         cvReleaseHaarClassifierCascade( &cascade );
01693         cascade = NULL;
01694     }
01695 
01696     return cascade;
01697 }
01698 
01699 static void
01700 icvWriteHaarClassifier( CvFileStorage* fs, const char* name, const void* struct_ptr,
01701                         CvAttrList attributes )
01702 {
01703     CV_FUNCNAME( "cvWriteHaarClassifier" );
01704 
01705     __BEGIN__;
01706 
01707     int i, j, k, l;
01708     char buf[256];
01709     const CvHaarClassifierCascade* cascade = (const CvHaarClassifierCascade*) struct_ptr;
01710 
01711     /* TODO: parameters check */
01712 
01713     CV_CALL( cvStartWriteStruct( fs, name, CV_NODE_MAP, CV_TYPE_NAME_HAAR, attributes ) );
01714     
01715     CV_CALL( cvStartWriteStruct( fs, ICV_HAAR_SIZE_NAME, CV_NODE_SEQ | CV_NODE_FLOW ) );
01716     CV_CALL( cvWriteInt( fs, NULL, cascade->orig_window_size.width ) );
01717     CV_CALL( cvWriteInt( fs, NULL, cascade->orig_window_size.height ) );
01718     CV_CALL( cvEndWriteStruct( fs ) ); /* size */
01719     
01720     CV_CALL( cvStartWriteStruct( fs, ICV_HAAR_STAGES_NAME, CV_NODE_SEQ ) );    
01721     for( i = 0; i < cascade->count; ++i )
01722     {
01723         CV_CALL( cvStartWriteStruct( fs, NULL, CV_NODE_MAP ) );
01724         sprintf( buf, "stage %d", i );
01725         CV_CALL( cvWriteComment( fs, buf, 1 ) );
01726         
01727         CV_CALL( cvStartWriteStruct( fs, ICV_HAAR_TREES_NAME, CV_NODE_SEQ ) );
01728 
01729         for( j = 0; j < cascade->stage_classifier[i].count; ++j )
01730         {
01731             CvHaarClassifier* tree = &cascade->stage_classifier[i].classifier[j];
01732 
01733             CV_CALL( cvStartWriteStruct( fs, NULL, CV_NODE_SEQ ) );
01734             sprintf( buf, "tree %d", j );
01735             CV_CALL( cvWriteComment( fs, buf, 1 ) );
01736 
01737             for( k = 0; k < tree->count; ++k )
01738             {
01739                 CvHaarFeature* feature = &tree->haar_feature[k];
01740 
01741                 CV_CALL( cvStartWriteStruct( fs, NULL, CV_NODE_MAP ) );
01742                 if( k )
01743                 {
01744                     sprintf( buf, "node %d", k );
01745                 }
01746                 else
01747                 {
01748                     sprintf( buf, "root node" );
01749                 }
01750                 CV_CALL( cvWriteComment( fs, buf, 1 ) );
01751 
01752                 CV_CALL( cvStartWriteStruct( fs, ICV_HAAR_FEATURE_NAME, CV_NODE_MAP ) );
01753                 
01754                 CV_CALL( cvStartWriteStruct( fs, ICV_HAAR_RECTS_NAME, CV_NODE_SEQ ) );
01755                 for( l = 0; l < CV_HAAR_FEATURE_MAX && feature->rect[l].r.width != 0; ++l )
01756                 {
01757                     CV_CALL( cvStartWriteStruct( fs, NULL, CV_NODE_SEQ | CV_NODE_FLOW ) );
01758                     CV_CALL( cvWriteInt(  fs, NULL, feature->rect[l].r.x ) );
01759                     CV_CALL( cvWriteInt(  fs, NULL, feature->rect[l].r.y ) );
01760                     CV_CALL( cvWriteInt(  fs, NULL, feature->rect[l].r.width ) );
01761                     CV_CALL( cvWriteInt(  fs, NULL, feature->rect[l].r.height ) );
01762                     CV_CALL( cvWriteReal( fs, NULL, feature->rect[l].weight ) );
01763                     CV_CALL( cvEndWriteStruct( fs ) ); /* rect */
01764                 }
01765                 CV_CALL( cvEndWriteStruct( fs ) ); /* rects */
01766                 CV_CALL( cvWriteInt( fs, ICV_HAAR_TILTED_NAME, feature->tilted ) );
01767                 CV_CALL( cvEndWriteStruct( fs ) ); /* feature */
01768                 
01769                 CV_CALL( cvWriteReal( fs, ICV_HAAR_THRESHOLD_NAME, tree->threshold[k]) );
01770 
01771                 if( tree->left[k] > 0 )
01772                 {
01773                     CV_CALL( cvWriteInt( fs, ICV_HAAR_LEFT_NODE_NAME, tree->left[k] ) );
01774                 }
01775                 else
01776                 {
01777                     CV_CALL( cvWriteReal( fs, ICV_HAAR_LEFT_VAL_NAME,
01778                         tree->alpha[-tree->left[k]] ) );
01779                 }
01780 
01781                 if( tree->right[k] > 0 )
01782                 {
01783                     CV_CALL( cvWriteInt( fs, ICV_HAAR_RIGHT_NODE_NAME, tree->right[k] ) );
01784                 }
01785                 else
01786                 {
01787                     CV_CALL( cvWriteReal( fs, ICV_HAAR_RIGHT_VAL_NAME,
01788                         tree->alpha[-tree->right[k]] ) );
01789                 }
01790 
01791                 CV_CALL( cvEndWriteStruct( fs ) ); /* split */
01792             }
01793 
01794             CV_CALL( cvEndWriteStruct( fs ) ); /* tree */
01795         }
01796 
01797         CV_CALL( cvEndWriteStruct( fs ) ); /* trees */
01798 
01799         CV_CALL( cvWriteReal( fs, ICV_HAAR_STAGE_THRESHOLD_NAME,
01800                               cascade->stage_classifier[i].threshold) );
01801 
01802         CV_CALL( cvWriteInt( fs, ICV_HAAR_PARENT_NAME,
01803                               cascade->stage_classifier[i].parent ) );
01804         CV_CALL( cvWriteInt( fs, ICV_HAAR_NEXT_NAME,
01805                               cascade->stage_classifier[i].next ) );
01806 
01807         CV_CALL( cvEndWriteStruct( fs ) ); /* stage */
01808     } /* for each stage */
01809     
01810     CV_CALL( cvEndWriteStruct( fs ) ); /* stages */
01811     CV_CALL( cvEndWriteStruct( fs ) ); /* root */
01812 
01813     __END__;
01814 }
01815 
01816 static void*
01817 icvCloneHaarClassifier( const void* struct_ptr )
01818 {
01819     CvHaarClassifierCascade* cascade = NULL;
01820 
01821     CV_FUNCNAME( "cvCloneHaarClassifier" );
01822 
01823     __BEGIN__;
01824 
01825     int i, j, k, n;
01826     const CvHaarClassifierCascade* cascade_src =
01827         (const CvHaarClassifierCascade*) struct_ptr;
01828 
01829     n = cascade_src->count;
01830     CV_CALL( cascade = icvCreateHaarClassifierCascade(n) );
01831     cascade->orig_window_size = cascade_src->orig_window_size;
01832 
01833     for( i = 0; i < n; ++i )
01834     {
01835         cascade->stage_classifier[i].parent = cascade_src->stage_classifier[i].parent;
01836         cascade->stage_classifier[i].next = cascade_src->stage_classifier[i].next;
01837         cascade->stage_classifier[i].child = cascade_src->stage_classifier[i].child;
01838         cascade->stage_classifier[i].threshold = cascade_src->stage_classifier[i].threshold;
01839 
01840         cascade->stage_classifier[i].count = 0;
01841         CV_CALL( cascade->stage_classifier[i].classifier =
01842             (CvHaarClassifier*) cvAlloc( cascade_src->stage_classifier[i].count
01843                 * sizeof( cascade->stage_classifier[i].classifier[0] ) ) );
01844         
01845         cascade->stage_classifier[i].count = cascade_src->stage_classifier[i].count;
01846 
01847         for( j = 0; j < cascade->stage_classifier[i].count; ++j )
01848         {
01849             cascade->stage_classifier[i].classifier[j].haar_feature = NULL;
01850         }
01851 
01852         for( j = 0; j < cascade->stage_classifier[i].count; ++j )
01853         {
01854             const CvHaarClassifier* classifier_src = 
01855                 &cascade_src->stage_classifier[i].classifier[j];
01856             CvHaarClassifier* classifier = 
01857                 &cascade->stage_classifier[i].classifier[j];
01858 
01859             classifier->count = classifier_src->count;
01860             CV_CALL( classifier->haar_feature = (CvHaarFeature*) cvAlloc( 
01861                 classifier->count * ( sizeof( *classifier->haar_feature ) +
01862                                       sizeof( *classifier->threshold ) +
01863                                       sizeof( *classifier->left ) +
01864                                       sizeof( *classifier->right ) ) +
01865                 (classifier->count + 1) * sizeof( *classifier->alpha ) ) );
01866             classifier->threshold = (float*) (classifier->haar_feature+classifier->count);
01867             classifier->left = (int*) (classifier->threshold + classifier->count);
01868             classifier->right = (int*) (classifier->left + classifier->count);
01869             classifier->alpha = (float*) (classifier->right + classifier->count);
01870             for( k = 0; k < classifier->count; ++k )
01871             {
01872                 classifier->haar_feature[k] = classifier_src->haar_feature[k];
01873                 classifier->threshold[k] = classifier_src->threshold[k];
01874                 classifier->left[k] = classifier_src->left[k];
01875                 classifier->right[k] = classifier_src->right[k];
01876                 classifier->alpha[k] = classifier_src->alpha[k];
01877             }
01878             classifier->alpha[classifier->count] = 
01879                 classifier_src->alpha[classifier->count];
01880         }
01881     }
01882 
01883     __END__;
01884 
01885     return cascade;
01886 }
01887 
01888 static int
01889 icvRegisterHaarClassifierType()
01890 {
01891     CV_FUNCNAME( "icvRegisterHaarClassifierType" );
01892 
01893     __BEGIN__;
01894 
01895     CvTypeInfo info;
01896 
01897     info.header_size = sizeof( info );
01898     info.is_instance = icvIsHaarClassifier;
01899     info.release = (CvReleaseFunc) cvReleaseHaarClassifierCascade;
01900     info.read = icvReadHaarClassifier;
01901     info.write = icvWriteHaarClassifier;
01902     info.clone = icvCloneHaarClassifier;
01903     info.type_name = CV_TYPE_NAME_HAAR;
01904     CV_CALL( cvRegisterType( &info ) );
01905 
01906     __END__;
01907 
01908     return 1;
01909 }
01910 
01911 static int icv_register_haar_classifier_type = icvRegisterHaarClassifierType();
01912 
01913 
01914 struct PixelInfo {
01915   double dist;
01916   int minHeight;
01917   int maxHeight;
01918   int minSize;
01919   int maxSize;
01920 };
01921 
01922 /*  Just making backups
01923 inline static int GetXPixel(double Wx, double Wy, double Wz, double pan, double tilt) {
01924   double cp = cos(pan); 
01925   double sp = sin(pan);
01926   double ct = cos(tilt);
01927   double st = sin(tilt);
01928   double camx =  cp * Wx + sp * Wy - cp * 0.13 ;
01929   double camy = -sp * Wx + cp * Wy + sp * 0.13 - 0.09;
01930 
01931   return (320 - int(387.5 * tan(atan2(camy, camx)) + 160));
01932 }
01933 
01934 inline static int GetYPixel(double Wx, double Wy, double Wz, double pan, double tilt) {
01935   double cp = cos(pan);  
01936   double sp = sin(pan);
01937   double ct = cos(tilt);
01938   double st = sin(tilt);
01939   double camx =  cp * Wx + sp * Wy - cp * 0.13 ;
01940   double camz = Wz - 1.42;
01941 
01942   return (240 - int(387.5 * tan(atan2(camz, camx)) + 120));
01943 } */
01944 
01945 inline static int GetXPixel(double Wx, double Wy, double Wz, double pan, double tilt) {
01946   double cp = cos(pan); 
01947   double sp = sin(pan);
01948   double ct = cos(tilt);
01949   double st = sin(tilt);
01950   double camx =  ct * cp * Wx + ct * sp  * Wy + st * Wz - .13 * cp * ct ;
01951   double camy = -sp * Wx + cp * Wy + sp * 0.13 - 0.09;
01952 
01953   return (320 - int(387.5 * tan(atan2(camy, camx)) + 160));
01954 }
01955 
01956 inline static int GetYPixel(double Wx, double Wy, double Wz, double pan, double tilt) {
01957   double cp = cos(pan);  
01958   double sp = sin(pan);
01959   double ct = cos(tilt);
01960   double st = sin(tilt);
01961   
01962   double camx =  ct * cp * Wx +  ct * sp * Wy + st * Wz - 0.13 * cp * ct;
01963   double camz = -st * cp * Wx + -st * sp * Wy + ct * Wz + 0.13 * cp * st - 1.48 * ct;
01964 
01965   return (240 - int(387.5 * tan(atan2(camz, camx)) + 120));
01966 }
01967 
01968 inline static void GetDistances(PixelInfo *pPixelMap, double *pData, double pan, double tilt, double pMinHeight, double pMaxHeight,
01969                                 double pMinSize, double pMaxSize) {
01970     memset(pPixelMap, 0, sizeof(PixelInfo) * 320);
01971     
01972     // Load the useful values in... might be able to be sped up, but it's only a constant 180 iterations, it's not bad
01973     for(int i = 0; i < 180; i++) {
01974       double radians = i / 180.0 * 3.141592 - (3.141592 / 2.0);
01975                 
01976                   double wx = cos(radians) * pData[i];
01977                   double wy = sin(radians) * pData[i];
01978                   double wz = 0.38;
01979                 
01980                   int xpix = GetXPixel(wx, wy, wz, pan, tilt);
01981                 //printf("pData[%i] = %f - cam(%f,%f,%f) - xpix=%i\n", i, pData[i], camx, camy, camz, xpix);
01982                 if(xpix >= 0 && xpix < 320) {
01983                   pPixelMap[xpix].dist = pData[i];
01984                   pPixelMap[xpix].minHeight = GetYPixel(wx,wy,pMinHeight, pan, tilt);
01985                   pPixelMap[xpix].maxHeight = GetYPixel(wx,wy,pMaxHeight, pan, tilt);
01986                   pPixelMap[xpix].minSize = int(387.5 * pMinSize / pData[i]);
01987                   pPixelMap[xpix].maxSize = int(387.5 * pMaxSize / pData[i]);
01988                   //printf("MIN/MAX: (%i,%i)\n", pPixelMap[xpix].minHeight, pPixelMap[xpix].maxHeight);
01989                 }
01990     }
01991     
01992 
01993 #ifdef LINEAR_INTERPOLATION     
01994         //Do a linear interpolation
01995         PixelInfo *from = &pPixelMap[0];
01996         PixelInfo *to = NULL;
01997         int dist = 0;
01998         //printf("-------------------------\nEntering GetDistances loop!\n");
01999         for(int i = 0; i < 320; i++) {
02000       to = &(pPixelMap[i]);
02001       //printf("At pixel: %i, to=%f\n", i, *to);
02002       if(to->dist > 0 || i == 319) {
02003         //printf("Got a valid range!\n");
02004         if(dist > 0) {
02005           //printf("Distance greater than zero: %i\n", dist);
02006           double perStepDist;
02007           double perStepMaxSize;
02008           double perStepMinSize;
02009           if(from->dist > 0) {
02010             //printf("from is > 0: %f\n", *from);
02011             perStepDist = (to->dist - from->dist) / dist;
02012             perStepMaxSize = (to->maxHeight - from->maxHeight) / dist;
02013             perStepMaxSize = (to->minHeight - from->minHeight) / dist;
02014           }
02015           else {
02016             //printf("from is == 0: %f\n", *from);
02017             perStepDist = (to->dist) / dist;
02018             perStepMaxSize = (to->maxHeight) / dist;
02019             perStepMaxSize = (to->minHeight) / dist;
02020           }
02021           //printf("to: %f\n", *to);
02022           //printf("Perstep is: %f\n", perStep);
02023           
02024           
02025           if(from->dist == 0 || i == 319) {
02026             //printf("We're at an edge\n");
02027             if(i == 319) {
02028               to->dist = from->dist;  
02029               to->maxHeight = from->maxHeight;
02030               to->minHeight = from->minHeight;
02031               while(from != to) {
02032                 to--;
02033                 to->dist = from->dist;
02034                 to->maxHeight = from->maxHeight;
02035                 to->minHeight = from->minHeight;
02036               }
02037             }
02038             else {
02039               from->dist = to->dist;  
02040               to->maxHeight = from->maxHeight;
02041               to->minHeight = from->minHeight;
02042               while(from != to) {
02043                 from++;
02044                 from->dist = to->dist;
02045                 to->maxHeight = from->maxHeight;
02046                 to->minHeight = from->minHeight;
02047               } 
02048             }
02049           }
02050           else {
02051             //printf("Need to interpolate\n");
02052             //double *start = from;
02053             while(from != to) {
02054               from++;
02055               from->dist = (from - 1)->dist + perStepDist;
02056               from->maxSize = int((from - 1)->maxSize + perStepMaxSize);
02057               from->minSize = int((from - 1)->minSize + perStepMinSize);
02058             }
02059           }
02060         }
02061         dist =0;
02062         from = to;
02063       }
02064       else {
02065         dist++;
02066       }
02067         } 
02068 #else
02069     PixelInfo *op = &pPixelMap[0];
02070     PixelInfo *np = NULL;
02071     for(int i = 0; i < 320; i++) {
02072       np = &pPixelMap[i];
02073       if(op->dist != np->dist) {
02074         op = np;
02075       }
02076       *np = *op;
02077     }
02078 #endif
02079         
02080         //for(int i = 0; i < 320; i++) {
02081           // printf("([%i]=%f)\n", i, pPixelMap[i].dist);
02082         //}
02083         
02084                 
02085                 /*double worldx = cos(radians - pan) * pData[i];
02086                 double worldy = sin(radians - pan) * pData[i];
02087                 double worldz = 0;
02088                 
02089                 double camx = cos(radians - pan) * pData[i] * cos(-tilt) + 0.13 * cos(pan) - 0.09 * sin(pan);// - 0.05;
02090                 double camy = sin(radians - pan) * pData[i] - 0.09 * cos(pan) + 0.13 * sin(pan);
02091                 double camz = cos(radians - pan) * pData[i] * sin(-tilt) - 1.04;
02092                 
02093                 int xpix = 320 - int(387.5 * tan(atan2(sin(radians - pan) * pData[i] - 0.09 * cos(pan) + 0.13 * sin(pan), cos(radians - pan) * pData[i] * cos(-tilt) + 0.13 * cos(pan) - 0.09 * sin(pan))) + 160);
02094                 int ypix = 240 - int(387.5 * tan(atan2(cos(radians - pan) * pData[i] * sin(-tilt) - 1.04, cos(radians - pan) * pData[i] * cos(-tilt) + 0.13 * cos(pan) - 0.09 * sin(pan))) + 120);*/
02095                 
02096                 
02097 }
02098  
02099 inline bool CouldBeFace(int x, int y, PixelInfo &data, int width) {  
02100        //return (y < data.minHeight && y > data.maxHeight);
02101 
02102        //return (y < data.minHeight && y > data.maxHeight) && (width < data.maxSize && width > data.minSize);
02103        return true;
02104 }
02105 
02106 /*struct pixel {
02107   char b, g, r;
02108 };
02109 
02110 void ColorPixel(void * pImg, int pX, int pY, char pR, char pG, char pB) {
02111   pixel *img = (pixel *)pImg;
02112   if(pX >= 0 && pX < 320 && pY >= 0 && pY < 240) {
02113                 img[pY * 320 + pX].r = pR;
02114                 img[pY * 320 + pX].g = pG;
02115                 img[pY * 320 + pX].b = pB;              
02116         }
02117 }*/
02118 
02119 CvSeq*
02120 myHaarDetectObjects( const CvArr* _img,
02121                      CvHaarClassifierCascade* cascade,
02122                      CvMemStorage* storage, double scale_factor,
02123                      int min_neighbors, int flags, CvSize min_size, double *pRanges, double pPan, double pTilt, double pMinHeight, double pMaxHeight, IplImage *pImg,
02124                      double pMinSize, double pMaxSize)
02125 {
02126        g_logdebug << " minheight: " << pMinHeight << " maxheight: " << pMaxHeight << " minsize: " << pMinSize << " maxsize: " << pMaxSize << endl;
02127     int split_stage = 2;
02128     CvMat stub, *img = (CvMat*)_img;
02129     CvMat *temp = 0, *sum = 0, *tilted = 0, *sqsum = 0, *norm_img = 0, *sumcanny = 0, *img_small = 0;
02130     CvSeq* seq = 0;
02131     CvSeq* seq2 = 0;
02132     CvSeq* idx_seq = 0;
02133     CvSeq* result_seq = 0;
02134     CvMemStorage* temp_storage = 0;
02135     CvAvgComp* comps = 0;
02136     PixelInfo pixelDistanceMap[320];// hardcoded!
02137     
02138     CV_FUNCNAME( "cvHaarDetectObjects" );
02139 
02140     __BEGIN__;
02141 
02142     double factor;
02143     int i, npass = 2, coi;
02144     int do_canny_pruning = flags & CV_HAAR_DO_CANNY_PRUNING;
02145 
02146     if( !CV_IS_HAAR_CLASSIFIER(cascade) )
02147         CV_ERROR( !cascade ? CV_StsNullPtr : CV_StsBadArg, "Invalid classifier cascade" );
02148 
02149     if( !storage )
02150         CV_ERROR( CV_StsNullPtr, "Null storage pointer" );
02151 
02152     CV_CALL( img = cvGetMat( img, &stub, &coi ));
02153     if( coi )
02154         CV_ERROR( CV_BadCOI, "COI is not supported" );
02155 
02156     if( CV_MAT_DEPTH(img->type) != CV_8U )
02157         CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit images are supported" );
02158 
02159     CV_CALL( temp = cvCreateMat( img->rows, img->cols, CV_8UC1 ));
02160     CV_CALL( sum = cvCreateMat( img->rows + 1, img->cols + 1, CV_32SC1 ));
02161     CV_CALL( sqsum = cvCreateMat( img->rows + 1, img->cols + 1, CV_64FC1 ));
02162     CV_CALL( temp_storage = cvCreateChildMemStorage( storage ));
02163 
02164     if( !cascade->hid_cascade )
02165         CV_CALL( icvCreateHidHaarClassifierCascade(cascade) );
02166 
02167     if( cascade->hid_cascade->has_tilted_features )
02168         tilted = cvCreateMat( img->rows + 1, img->cols + 1, CV_32SC1 );
02169 
02170     seq = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvRect), temp_storage );
02171     seq2 = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvAvgComp), temp_storage );
02172     result_seq = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvAvgComp), storage );
02173 
02174     if( min_neighbors == 0 )
02175         seq = result_seq;
02176 
02177     if( CV_MAT_CN(img->type) > 1 )
02178     {
02179         cvCvtColor( img, temp, CV_BGR2GRAY );
02180         img = temp;
02181     }
02182     
02183     if( flags & CV_HAAR_SCALE_IMAGE )
02184     {
02185         CvSize win_size0 = cascade->orig_window_size;
02186         int use_ipp = cascade->hid_cascade->ipp_stages != 0 &&
02187                     icvApplyHaarClassifier_32s32f_C1R_p != 0;
02188 
02189         if( use_ipp )
02190             CV_CALL( norm_img = cvCreateMat( img->rows, img->cols, CV_32FC1 ));
02191         CV_CALL( img_small = cvCreateMat( img->rows + 1, img->cols + 1, CV_8UC1 ));
02192 
02193         for( factor = 1; ; factor *= scale_factor )
02194         {
02195             int positive = 0;
02196             int x, y;
02197             CvSize win_size = { cvRound(win_size0.width*factor),
02198                                 cvRound(win_size0.height*factor) };
02199             CvSize sz = { cvRound( img->cols/factor ), cvRound( img->rows/factor ) };
02200             CvSize sz1 = { sz.width - win_size0.width, sz.height - win_size0.height };
02201             CvRect rect1 = { icv_object_win_border, icv_object_win_border,
02202                 win_size0.width - icv_object_win_border*2,
02203                 win_size0.height - icv_object_win_border*2 };
02204             CvMat img1, sum1, sqsum1, norm1, tilted1, mask1;
02205             CvMat* _tilted = 0;
02206 
02207             if( sz1.width <= 0 || sz1.height <= 0 )
02208                 break;
02209             if( win_size.width < min_size.width || win_size.height < min_size.height )
02210                 continue;
02211 
02212             img1 = cvMat( sz.height, sz.width, CV_8UC1, img_small->data.ptr );
02213             sum1 = cvMat( sz.height+1, sz.width+1, CV_32SC1, sum->data.ptr );
02214             sqsum1 = cvMat( sz.height+1, sz.width+1, CV_64FC1, sqsum->data.ptr );
02215             if( tilted )
02216             {
02217                 tilted1 = cvMat( sz.height+1, sz.width+1, CV_32SC1, tilted->data.ptr );
02218                 _tilted = &tilted1;
02219             }
02220             norm1 = cvMat( sz1.height, sz1.width, CV_32FC1, norm_img ? norm_img->data.ptr : 0 );
02221             mask1 = cvMat( sz1.height, sz1.width, CV_8UC1, temp->data.ptr );
02222 
02223             cvResize( img, &img1, CV_INTER_LINEAR );
02224             cvIntegral( &img1, &sum1, &sqsum1, _tilted );
02225 
02226             if( use_ipp && icvRectStdDev_32s32f_C1R_p( sum1.data.i, sum1.step,
02227                 sqsum1.data.db, sqsum1.step, norm1.data.fl, norm1.step, sz1, rect1 ) < 0 )
02228                 use_ipp = 0;
02229 
02230             if( use_ipp )
02231             {
02232                 positive = mask1.cols*mask1.rows;
02233                 cvSet( &mask1, cvScalarAll(255) );
02234                 for( i = 0; i < cascade->count; i++ )
02235                 {
02236                     if( icvApplyHaarClassifier_32s32f_C1R_p(sum1.data.i, sum1.step,
02237                         norm1.data.fl, norm1.step, mask1.data.ptr, mask1.step,
02238                         sz1, &positive, cascade->hid_cascade->stage_classifier[i].threshold,
02239                         cascade->hid_cascade->ipp_stages[i]) < 0 )
02240                     {
02241                         use_ipp = 0;
02242                         break;
02243                     }
02244                     if( positive <= 0 )
02245                         break;
02246                 }
02247             }
02248             
02249             if( !use_ipp )
02250             {
02251                 cvSetImagesForHaarClassifierCascade( cascade, &sum1, &sqsum1, 0, 1. );
02252                 for( y = 0, positive = 0; y < sz1.height; y++ )
02253                     for( x = 0; x < sz1.width; x++ )
02254                     {
02255                         mask1.data.ptr[mask1.step*y + x] =
02256                             cvRunHaarClassifierCascade( cascade, cvPoint(x,y), 0 ) > 0;
02257                         positive += mask1.data.ptr[mask1.step*y + x];
02258                     }
02259             }
02260 
02261             if( positive > 0 )
02262             {
02263                 for( y = 0; y < sz1.height; y++ )
02264                     for( x = 0; x < sz1.width; x++ )
02265                         if( mask1.data.ptr[mask1.step*y + x] != 0 )
02266                         {
02267                             CvRect obj_rect = { cvRound(y*factor), cvRound(x*factor),
02268                                                 win_size.width, win_size.height };
02269                             cvSeqPush( seq, &obj_rect );
02270                         }
02271             }
02272         }
02273     }
02274     else
02275     {
02276        // printf("BEGIN!\n\n\n");
02277         GetDistances(pixelDistanceMap, pRanges, pPan, pTilt, pMinHeight, pMaxHeight,pMinSize,pMaxSize);
02278         cvIntegral( img, sum, sqsum, tilted );
02279     
02280         if( do_canny_pruning )
02281         {
02282             sumcanny = cvCreateMat( img->rows + 1, img->cols + 1, CV_32SC1 );
02283             cvCanny( img, temp, 0, 50, 3 );
02284             cvIntegral( temp, sumcanny );
02285         }
02286     
02287         if( (unsigned)split_stage >= (unsigned)cascade->count ||
02288             cascade->hid_cascade->is_tree )
02289         {
02290             split_stage = cascade->count;
02291             npass = 1;
02292         }
02293 
02294         for( factor = 1; factor*cascade->orig_window_size.width < img->cols - 10 &&
02295                          factor*cascade->orig_window_size.height < img->rows - 10;
02296              factor *= scale_factor )
02297         {
02298             const double ystep = MAX( 2, factor );
02299             CvSize win_size = { cvRound( cascade->orig_window_size.width * factor ),
02300                                 cvRound( cascade->orig_window_size.height * factor )};
02301             CvRect equ_rect = { 0, 0, 0, 0 };
02302             int *p0 = 0, *p1 = 0, *p2 = 0, *p3 = 0;
02303             int *pq0 = 0, *pq1 = 0, *pq2 = 0, *pq3 = 0;
02304             int pass, stage_offset = 0;
02305             int stop_height = cvRound((img->rows - win_size.height) / ystep);
02306 
02307             if( win_size.width < min_size.width || win_size.height < min_size.height )
02308                 continue;
02309 
02310             cvSetImagesForHaarClassifierCascade( cascade, sum, sqsum, tilted, factor );
02311             cvZero( temp );
02312 
02313             if( do_canny_pruning )
02314             {
02315                 equ_rect.x = cvRound(win_size.width*0.15);
02316                 equ_rect.y = cvRound(win_size.height*0.15);
02317                 equ_rect.width = cvRound(win_size.width*0.7);
02318                 equ_rect.height = cvRound(win_size.height*0.7);
02319 
02320                 p0 = (int*)(sumcanny->data.ptr + equ_rect.y*sumcanny->step) + equ_rect.x;
02321                 p1 = (int*)(sumcanny->data.ptr + equ_rect.y*sumcanny->step)
02322                             + equ_rect.x + equ_rect.width;
02323                 p2 = (int*)(sumcanny->data.ptr + (equ_rect.y + equ_rect.height)*sumcanny->step) + equ_rect.x;
02324                 p3 = (int*)(sumcanny->data.ptr + (equ_rect.y + equ_rect.height)*sumcanny->step)
02325                             + equ_rect.x + equ_rect.width;
02326 
02327                 pq0 = (int*)(sum->data.ptr + equ_rect.y*sum->step) + equ_rect.x;
02328                 pq1 = (int*)(sum->data.ptr + equ_rect.y*sum->step)
02329                             + equ_rect.x + equ_rect.width;
02330                 pq2 = (int*)(sum->data.ptr + (equ_rect.y + equ_rect.height)*sum->step) + equ_rect.x;
02331                 pq3 = (int*)(sum->data.ptr + (equ_rect.y + equ_rect.height)*sum->step)
02332                             + equ_rect.x + equ_rect.width;
02333             }
02334 
02335             cascade->hid_cascade->count = split_stage;
02336 
02337             for( pass = 0; pass < npass; pass++ )
02338             {
02339     #ifdef _OPENMP
02340     #pragma omp parallel for shared(cascade, stop_height, seq, ystep, temp, \
02341         win_size, pass, npass, sum, p0, p1, p2, p3, pq0, pq1, pq2, pq3, stage_offset)
02342     #endif // _OPENMP
02343                 //printf("PASS: %i, stopheight: %i\n--------------------------------------------------------\n---------------------------------------------\n", pass, stop_height);
02344 
02345                 for( int _iy = 0; _iy < stop_height; _iy ++) 
02346                 {
02347                     int iy = cvRound(_iy*ystep);
02348                     int _ix, _xstep = 1;
02349                     int stop_width = cvRound((img->cols - win_size.width) / ystep);
02350                     uchar* mask_row = temp->data.ptr + temp->step * iy;
02351                     
02352                     for( _ix = 0; _ix < stop_width; _ix += _xstep )
02353                     {
02354                         int ix = cvRound(_ix*ystep); // it really should be ystep
02355                         
02356                         //MY STUFF
02357                         int width = cvRound(cascade->orig_window_size.width * factor);
02358                         int height = cvRound(cascade->orig_window_size.height * factor);
02359                         if(!CouldBeFace(ix + int(width / 2), iy + int(width / 2), pixelDistanceMap[ix],width)) {
02360                           continue;
02361                         }
02362                         
02363                         //printf("Face Testing...\n");
02364                        
02365                     
02366                         if( pass == 0 )
02367                         {
02368                             int result;
02369                             _xstep = 2;
02370 
02371                             if( do_canny_pruning )
02372                             {
02373                                 int offset;
02374                                 int s, sq;
02375                         
02376                                 offset = iy*(sum->step/sizeof(p0[0])) + ix;
02377                                 s = p0[offset] - p1[offset] - p2[offset] + p3[offset];
02378                                 sq = pq0[offset] - pq1[offset] - pq2[offset] + pq3[offset];
02379                                 if( s < 100 || sq < 20 )
02380                                     continue;
02381                             }
02382                             result = cvRunHaarClassifierCascade( cascade, cvPoint(ix,iy), 0 );
02383     #ifdef _OPENMP
02384     #pragma omp critical
02385     #endif
02386                             if( result > 0 )
02387                             {
02388                                 if( pass < npass - 1 )
02389                                     mask_row[ix] = 1;
02390                                 else
02391                                 {
02392                                     CvRect rect = cvRect(ix,iy,win_size.width,win_size.height);
02393                                     cvSeqPush( seq, &rect );
02394                                 }
02395                             }
02396                             if( result < 0 )
02397                                 _xstep = 1;
02398                         }
02399                         else if( mask_row[ix] )
02400                         {
02401                             int result = cvRunHaarClassifierCascade( cascade, cvPoint(ix,iy),
02402                                                                      stage_offset );
02403     #ifdef _OPENMP
02404     #pragma omp critical
02405     #endif
02406                             if( result > 0 )
02407                             {
02408                                 if( pass == npass - 1 )
02409                                 {
02410                                     CvRect rect = cvRect(ix,iy,win_size.width,win_size.height);
02411                                     cvSeqPush( seq, &rect );
02412                                 }
02413                             }
02414                             else
02415                                 mask_row[ix] = 0;
02416                         }
02417                     }
02418                 }
02419                 stage_offset = cascade->hid_cascade->count;
02420                 cascade->hid_cascade->count = cascade->count;
02421             }
02422         }
02423     }
02424 
02425     if( min_neighbors != 0 )
02426     {
02427         // group retrieved rectangles in order to filter out noise 
02428         int ncomp = cvSeqPartition( seq, 0, &idx_seq, is_equal, 0 );
02429         CV_CALL( comps = (CvAvgComp*)cvAlloc( (ncomp+1)*sizeof(comps[0])));
02430         memset( comps, 0, (ncomp+1)*sizeof(comps[0]));
02431 
02432         // count number of neighbors
02433         for( i = 0; i < seq->total; i++ )
02434         {
02435             CvRect r1 = *(CvRect*)cvGetSeqElem( seq, i );
02436             int idx = *(int*)cvGetSeqElem( idx_seq, i );
02437             assert( (unsigned)idx < (unsigned)ncomp );
02438 
02439             comps[idx].neighbors++;
02440              
02441             comps[idx].rect.x += r1.x;
02442             comps[idx].rect.y += r1.y;
02443             comps[idx].rect.width += r1.width;
02444             comps[idx].rect.height += r1.height;
02445         }
02446 
02447         // calculate average bounding box
02448         for( i = 0; i < ncomp; i++ )
02449         {
02450             int n = comps[i].neighbors;
02451             if( n >= min_neighbors )
02452             {
02453                 CvAvgComp comp;
02454                 comp.rect.x = (comps[i].rect.x*2 + n)/(2*n);
02455                 comp.rect.y = (comps[i].rect.y*2 + n)/(2*n);
02456                 comp.rect.width = (comps[i].rect.width*2 + n)/(2*n);
02457                 comp.rect.height = (comps[i].rect.height*2 + n)/(2*n);
02458                 comp.neighbors = comps[i].neighbors;
02459 
02460                 cvSeqPush( seq2, &comp );
02461             }
02462         }
02463 
02464         // filter out small face rectangles inside large face rectangles
02465         for( i = 0; i < seq2->total; i++ )
02466         {
02467             CvAvgComp r1 = *(CvAvgComp*)cvGetSeqElem( seq2, i );
02468             int j, flag = 1;
02469 
02470             for( j = 0; j < seq2->total; j++ )
02471             {
02472                 CvAvgComp r2 = *(CvAvgComp*)cvGetSeqElem( seq2, j );
02473                 int distance = cvRound( r2.rect.width * 0.2 );
02474             
02475                 if( i != j &&
02476                     r1.rect.x >= r2.rect.x - distance &&
02477                     r1.rect.y >= r2.rect.y - distance &&
02478                     r1.rect.x + r1.rect.width <= r2.rect.x + r2.rect.width + distance &&
02479                     r1.rect.y + r1.rect.height <= r2.rect.y + r2.rect.height + distance &&
02480                     (r2.neighbors > MAX( 3, r1.neighbors ) || r1.neighbors < 3) )
02481                 {
02482                     flag = 0;
02483                     break;
02484                 }
02485             }
02486 
02487             if( flag )
02488             {
02489                 cvSeqPush( result_seq, &r1 );
02490                 /* cvSeqPush( result_seq, &r1.rect ); */
02491             }
02492         }
02493     }
02494 
02495     __END__;
02496 
02497     cvReleaseMemStorage( &temp_storage );
02498     cvReleaseMat( &sum );
02499     cvReleaseMat( &sqsum );
02500     cvReleaseMat( &tilted );
02501     cvReleaseMat( &temp );
02502     cvReleaseMat( &sumcanny );
02503     cvReleaseMat( &norm_img );
02504     cvReleaseMat( &img_small );
02505     cvFree( (void**)&comps );
02506 
02507     return result_seq;
02508 }
02509 /* End of file. */

Generated on Thu Feb 1 15:31:52 2007 for WURDE by  doxygen 1.5.1