modules/objdetect/src/matching.cpp

   1 #include "precomp.hpp"
   2 #include "opencv2/objdetect/objdetect_c.h"
   3 #include "_lsvm_matching.h"
   4 #include <stdio.h>
   5
   6 #ifndef max
   7 #define max(a,b)            (((a) > (b)) ? (a) : (b))
   8 #endif
   9
  10 #ifndef min
  11 #define min(a,b)            (((a) < (b)) ? (a) : (b))
  12 #endif
  13
  14 /*
  15 // Function for convolution computation
  16 //
  17 // INPUT
  18 // Fi                - filter object
  19 // map               - feature map
  20 // OUTPUT
  21 // f                 - the convolution
  22 // RESULT
  23 // Error status
  24 */
  25 int convolution(const CvLSVMFilterObject *Fi, const CvLSVMFeatureMap *map, float *f)
  26 {
  27     int n1, m1, n2, m2, p, /*size,*/ diff1, diff2;
  28     int i1, i2, j1, j2, k;
  29     float tmp_f1, tmp_f2, tmp_f3, tmp_f4;
  30     float *pMap = NULL;
  31     float *pH = NULL;
  32
  33     n1 = map->sizeY;
  34     m1 = map->sizeX;
  35     n2 = Fi->sizeY;
  36     m2 = Fi->sizeX;
  37     p = map->numFeatures;
  38
  39     diff1 = n1 - n2 + 1;
  40     diff2 = m1 - m2 + 1;
  41     //size = diff1 * diff2;
  42     for (j1 = diff2 - 1; j1 >= 0; j1--)
  43     {
  44
  45         for (i1 = diff1 - 1; i1 >= 0; i1--)
  46         {
  47             tmp_f1 = 0.0f;
  48             tmp_f2 = 0.0f;
  49             tmp_f3 = 0.0f;
  50             tmp_f4 = 0.0f;
  51             for (i2 = 0; i2 < n2; i2++)
  52             {
  53                 for (j2 = 0; j2 < m2; j2++)
  54                 {
  55                     pMap = map->map + (i1 + i2) * m1 * p + (j1 + j2) * p;//sm2
  56                     pH = Fi->H + (i2 * m2 + j2) * p;//sm2
  57                     for (k = 0; k < p/4; k++)
  58                     {
  59
  60                         tmp_f1 += pMap[4*k]*pH[4*k];//sm2
  61                         tmp_f2 += pMap[4*k+1]*pH[4*k+1];
  62                         tmp_f3 += pMap[4*k+2]*pH[4*k+2];
  63                         tmp_f4 += pMap[4*k+3]*pH[4*k+3];
  64                     }
  65
  66                     if (p%4==1)
  67                     {
  68                         tmp_f1 += pH[p-1]*pMap[p-1];
  69                     }
  70                     else
  71                     {
  72                         if (p%4==2)
  73                         {
  74                             tmp_f1 += pH[p-2]*pMap[p-2] + pH[p-1]*pMap[p-1];
  75                         }
  76                         else
  77                         {
  78                             if (p%4==3)
  79                             {
  80                                 tmp_f1 += pH[p-3]*pMap[p-3] + pH[p-2]*pMap[p-2] + pH[p-1]*pMap[p-1];
  81                             }
  82                         }
  83                     }
  84
  85                 }
  86             }
  87             f[i1 * diff2 + j1] = tmp_f1 + tmp_f2 + tmp_f3 + tmp_f4;//sm1
  88         }
  89     }
  90     return LATENT_SVM_OK;
  91 }
  92
  93 /*
  94 // Computation multiplication of FFT images
  95 //
  96 // API
  97 // int fftImagesMulti(float *fftImage1, float *fftImage2, int numRows, int numColls,
  98                       float *multi);
  99 // INPUT
 100 // fftImage1         - first fft image
 101 // fftImage2         - second fft image
 102 // (numRows, numColls) - image dimesions
 103 // OUTPUT
 104 // multi             - multiplication
 105 // RESULT
 106 // Error status
 107 */
 108 int fftImagesMulti(float *fftImage1, float *fftImage2, int numRows, int numColls,
 109                    float *multi)
 110 {
 111     int i, index, size;
 112     size = numRows * numColls;
 113     for (i = 0; i < size; i++)
 114     {
 115         index = 2 * i;
 116         multi[index] = fftImage1[index] * fftImage2[index] -
 117                        fftImage1[index + 1] * fftImage2[index + 1];
 118         multi[index + 1] = fftImage1[index] * fftImage2[index + 1] +
 119                            fftImage1[index + 1] * fftImage2[index];
 120     }
 121     return LATENT_SVM_OK;
 122 }
 123
 124 /*
 125 // Turnover filter matrix for the single feature
 126 //
 127 // API
 128 // int rot2PI(float *filter, int dimX, int dimY, float *rot2PIFilter,
 129               int p, int shift);
 130 // INPUT
 131 // filter            - filter weight matrix
 132 // (dimX, dimY)      - dimension of filter matrix
 133 // p                 - number of features
 134 // shift             - number of feature (or channel)
 135 // OUTPUT
 136 // rot2PIFilter      - rotated matrix
 137 // RESULT
 138 // Error status
 139 */
 140 int rot2PI(float *filter, int dimX, int dimY, float *rot2PIFilter,
 141            int p, int shift)
 142 {
 143     int i, size;
 144     size = dimX * dimY;
 145     for (i = 0; i < size; i++)
 146     {
 147         rot2PIFilter[i] = filter[(size - i - 1) * p + shift];
 148     }
 149     return LATENT_SVM_OK;
 150 }
 151
 152 /*
 153 // Addition nullable bars to the dimension of feature map (single feature)
 154 //
 155 // API
 156 // int addNullableBars(float *rot2PIFilter, int dimX, int dimY,
 157                        float *newFilter, int newDimX, int newDimY);
 158 // INPUT
 159 // rot2PIFilter      - filter matrix for the single feature that was rotated
 160 // (dimX, dimY)      - dimension rot2PIFilter
 161 // (newDimX, newDimY)- dimension of feature map for the single feature
 162 // OUTPUT
 163 // newFilter         - filter matrix with nullable bars
 164 // RESULT
 165 // Error status
 166 */
 167 int addNullableBars(float *rot2PIFilter, int dimX, int dimY,
 168                     float *newFilter, int newDimX, int newDimY)
 169 {
 170     int size, i, j;
 171     size = newDimX * newDimY;
 172     for (i = 0; i < size; i++)
 173     {
 174         newFilter[2 * i] = 0.0;
 175         newFilter[2 * i + 1] = 0.0;
 176     }
 177     for (i = 0; i < dimY; i++)
 178     {
 179         for (j = 0; j < dimX; j++)
 180         {
 181             newFilter[2 * (i * newDimX + j)] = rot2PIFilter[i * dimX + j];
 182         }
 183     }
 184     return LATENT_SVM_OK;
 185 }
 186
 187 /*
 188 // Computation FFT image for filter object
 189 //
 190 // API
 191 // int getFFTImageFilterObject(const CvLSVMFilterObject *filter,
 192                                int mapDimX, int mapDimY,
 193                                fftImage **image);
 194 // INPUT
 195 // filter        - filter object
 196 // (mapDimX, mapDimY)- dimension of feature map
 197 // OUTPUT
 198 // image         - fft image
 199 // RESULT
 200 // Error status
 201 */
 202 int getFFTImageFilterObject(const CvLSVMFilterObject *filter,
 203                             int mapDimX, int mapDimY,
 204                             CvLSVMFftImage **image)
 205 {
 206     int i, mapSize, filterSize, res;
 207     float *newFilter, *rot2PIFilter;
 208
 209     filterSize = filter->sizeX * filter->sizeY;
 210     mapSize = mapDimX * mapDimY;
 211
 212     res = allocFFTImage(image, filter->numFeatures, mapDimX, mapDimY);
 213     if (res != LATENT_SVM_OK)
 214         return res;
 215
 216     newFilter = (float *)malloc(sizeof(float) * (2 * mapSize));
 217     rot2PIFilter = (float *)malloc(sizeof(float) * filterSize);
 218     for (i = 0; i < filter->numFeatures; i++)
 219     {
 220         rot2PI(filter->H, filter->sizeX, filter->sizeY, rot2PIFilter, filter->numFeatures, i);
 221         addNullableBars(rot2PIFilter, filter->sizeX, filter->sizeY,
 222                         newFilter, mapDimX, mapDimY);
 223         fft2d(newFilter, (*image)->channels[i], mapDimY, mapDimX);
 224     }
 225     free(newFilter);
 226     free(rot2PIFilter);
 227     return LATENT_SVM_OK;
 228 }
 229
 230 /*
 231 // Computation FFT image for feature map
 232 //
 233 // API
 234 // int getFFTImageFeatureMap(const featureMap *map, fftImage **image);
 235 // INPUT
 236 // OUTPUT
 237 // RESULT
 238 // Error status
 239 */
 240 int getFFTImageFeatureMap(const CvLSVMFeatureMap *map, CvLSVMFftImage **image)
 241 {
 242     int i, j, size;
 243     float *buf;
 244     allocFFTImage(image, map->numFeatures, map->sizeX, map->sizeY);
 245     size = map->sizeX * map->sizeY;
 246     buf = (float *)malloc(sizeof(float) * (2 * size));
 247     for (i = 0; i < map->numFeatures; i++)
 248     {
 249         for (j = 0; j < size; j++)
 250         {
 251             buf[2 * j] = map->map[j * map->numFeatures + i];
 252             buf[2 * j + 1] = 0.0;
 253         }
 254         fft2d(buf, (*image)->channels[i], map->sizeY, map->sizeX);
 255     }
 256     free(buf);
 257     return LATENT_SVM_OK;
 258 }
 259
 260 /*
 261 // Function for convolution computation using FFT
 262 //
 263 // API
 264 // int convFFTConv2d(const fftImage *featMapImage, const fftImage *filterImage,
 265                      int filterDimX, int filterDimY, float **conv);
 266 // INPUT
 267 // featMapImage      - feature map image
 268 // filterImage       - filter image
 269 // (filterDimX,filterDimY) - filter dimension
 270 // OUTPUT
 271 // conv              - the convolution
 272 // RESULT
 273 // Error status
 274 */
 275 int convFFTConv2d(const CvLSVMFftImage *featMapImage, const CvLSVMFftImage *filterImage,
 276                   int filterDimX, int filterDimY, float **conv)
 277 {
 278     int i, j, size, diffX, diffY, index;
 279     float *imagesMult, *imagesMultRes, *fconv;
 280     size = 2 * featMapImage->dimX * featMapImage->dimY;
 281     imagesMult = (float *)malloc(sizeof(float) * size);
 282     imagesMultRes = (float *)malloc(sizeof(float) * size);
 283     fftImagesMulti(featMapImage->channels[0], filterImage->channels[0],
 284             featMapImage->dimY, featMapImage->dimX, imagesMultRes);
 285     for (i = 1; (i < (int)featMapImage->numFeatures) && (i < (int)filterImage->numFeatures); i++)
 286     {
 287         fftImagesMulti(featMapImage->channels[i],filterImage->channels[i],
 288             featMapImage->dimY, featMapImage->dimX, imagesMult);
 289         for (j = 0; j < size; j++)
 290         {
 291             imagesMultRes[j] += imagesMult[j];
 292         }
 293     }
 294     fconv = (float *)malloc(sizeof(float) * size);
 295     fftInverse2d(imagesMultRes, fconv, featMapImage->dimY, featMapImage->dimX);
 296     diffX = featMapImage->dimX - filterDimX + 1;
 297     diffY = featMapImage->dimY - filterDimY + 1;
 298     *conv = (float *)malloc(sizeof(float) * (diffX * diffY));
 299     for (i = 0; i < diffY; i++)
 300     {
 301         for (j = 0; j < diffX; j++)
 302         {
 303             index = (i + filterDimY - 1) * featMapImage->dimX +
 304                     (j + filterDimX - 1);
 305             (*conv)[i * diffX + j] = fconv[2 * index];
 306         }
 307     }
 308     free(imagesMult);
 309     free(imagesMultRes);
 310     free(fconv);
 311     return LATENT_SVM_OK;
 312 }
 313
 314 /*
 315 // Computation objective function D according the original paper
 316 //
 317 // API
 318 // int filterDispositionLevel(const CvLSVMFilterObject *Fi, const featurePyramid *H,
 319                               int level, float **scoreFi,
 320                               int **pointsX, int **pointsY);
 321 // INPUT
 322 // Fi                - filter object (weights and coefficients of penalty
 323                        function that are used in this routine)
 324 // H                 - feature pyramid
 325 // level             - level number
 326 // OUTPUT
 327 // scoreFi           - values of distance transform on the level at all positions
 328 // (pointsX, pointsY)- positions that correspond to the maximum value
 329                        of distance transform at all grid nodes
 330 // RESULT
 331 // Error status
 332 */
 333 int filterDispositionLevel(const CvLSVMFilterObject *Fi, const CvLSVMFeatureMap *pyramid,
 334                            float **scoreFi,
 335                            int **pointsX, int **pointsY)
 336 {
 337     int n1, m1, n2, m2, /*p,*/ size, diff1, diff2;
 338     float *f;
 339     int i1, j1;
 340     int res;
 341
 342     n1 = pyramid->sizeY;
 343     m1 = pyramid->sizeX;
 344     n2 = Fi->sizeY;
 345     m2 = Fi->sizeX;
 346     //p = pyramid->numFeatures;
 347     (*scoreFi) = NULL;
 348     (*pointsX) = NULL;
 349     (*pointsY) = NULL;
 350
 351     // Processing the situation when part filter goes
 352     // beyond the boundaries of the block set
 353     if (n1 < n2 || m1 < m2)
 354     {
 355         return FILTER_OUT_OF_BOUNDARIES;
 356     } /* if (n1 < n2 || m1 < m2) */
 357
 358     // Computation number of positions for the filter
 359     diff1 = n1 - n2 + 1;
 360     diff2 = m1 - m2 + 1;
 361     size = diff1 * diff2;
 362
 363     // Allocation memory for additional array (must be free in this function)
 364     f = (float *)malloc(sizeof(float) * size);
 365     // Allocation memory for arrays for saving decisions
 366     (*scoreFi) = (float *)malloc(sizeof(float) * size);
 367     (*pointsX) = (int *)malloc(sizeof(int) * size);
 368     (*pointsY) = (int *)malloc(sizeof(int) * size);
 369
 370     // Consruction values of the array f
 371     // (a dot product vectors of feature map and weights of the filter)
 372     res = convolution(Fi, pyramid, f);
 373     if (res != LATENT_SVM_OK)
 374     {
 375         free(f);
 376         free(*scoreFi);
 377         free(*pointsX);
 378         free(*pointsY);
 379         return res;
 380     }
 381
 382     // TODO: necessary to change
 383     for (i1 = 0; i1 < diff1; i1++)
 384     {
 385          for (j1 = 0; j1 < diff2; j1++)
 386          {
 387              f[i1 * diff2 + j1] *= (-1);
 388          }
 389     }
 390
 391     // Decision of the general distance transform task
 392     DistanceTransformTwoDimensionalProblem(f, diff1, diff2, Fi->fineFunction,
 393                                           (*scoreFi), (*pointsX), (*pointsY));
 394
 395     // Release allocated memory
 396     free(f);
 397     return LATENT_SVM_OK;
 398 }
 399
 400 /*
 401 // Computation objective function D according the original paper using FFT
 402 //
 403 // API
 404 // int filterDispositionLevelFFT(const CvLSVMFilterObject *Fi, const fftImage *featMapImage,
 405                                  float **scoreFi,
 406                                  int **pointsX, int **pointsY);
 407 // INPUT
 408 // Fi                - filter object (weights and coefficients of penalty
 409                        function that are used in this routine)
 410 // featMapImage      - FFT image of feature map
 411 // OUTPUT
 412 // scoreFi           - values of distance transform on the level at all positions
 413 // (pointsX, pointsY)- positions that correspond to the maximum value
 414                        of distance transform at all grid nodes
 415 // RESULT
 416 // Error status
 417 */
 418 int filterDispositionLevelFFT(const CvLSVMFilterObject *Fi, const CvLSVMFftImage *featMapImage,
 419                               float **scoreFi,
 420                               int **pointsX, int **pointsY)
 421 {
 422     int n1, m1, n2, m2, /*p,*/ size, diff1, diff2;
 423     float *f;
 424     int i1, j1;
 425     int res;
 426     CvLSVMFftImage *filterImage;
 427
 428     n1 = featMapImage->dimY;
 429     m1 = featMapImage->dimX;
 430     n2 = Fi->sizeY;
 431     m2 = Fi->sizeX;
 432     //p = featMapImage->numFeatures;
 433     (*scoreFi) = NULL;
 434     (*pointsX) = NULL;
 435     (*pointsY) = NULL;
 436
 437     // Processing the situation when part filter goes
 438     // beyond the boundaries of the block set
 439     if (n1 < n2 || m1 < m2)
 440     {
 441         return FILTER_OUT_OF_BOUNDARIES;
 442     } /* if (n1 < n2 || m1 < m2) */
 443
 444     // Computation number of positions for the filter
 445     diff1 = n1 - n2 + 1;
 446     diff2 = m1 - m2 + 1;
 447     size = diff1 * diff2;
 448
 449     // Allocation memory for arrays for saving decisions
 450     (*scoreFi) = (float *)malloc(sizeof(float) * size);
 451     (*pointsX) = (int *)malloc(sizeof(int) * size);
 452     (*pointsY) = (int *)malloc(sizeof(int) * size);
 453
 454     // create filter image
 455     getFFTImageFilterObject(Fi, featMapImage->dimX, featMapImage->dimY, &filterImage);
 456
 457     // Consruction values of the array f
 458     // (a dot product vectors of feature map and weights of the filter)
 459     res = convFFTConv2d(featMapImage, filterImage, Fi->sizeX, Fi->sizeY, &f);
 460     if (res != LATENT_SVM_OK)
 461     {
 462         free(f);
 463         free(*scoreFi);
 464         free(*pointsX);
 465         free(*pointsY);
 466         return res;
 467     }
 468
 469     // TODO: necessary to change
 470     for (i1 = 0; i1 < diff1; i1++)
 471     {
 472          for (j1 = 0; j1 < diff2; j1++)
 473          {
 474              f[i1 * diff2 + j1] *= (-1);
 475          }
 476     }
 477
 478     // Decision of the general distance transform task
 479     DistanceTransformTwoDimensionalProblem(f, diff1, diff2, Fi->fineFunction,
 480                                           (*scoreFi), (*pointsX), (*pointsY));
 481
 482     // Release allocated memory
 483     free(f);
 484     freeFFTImage(&filterImage);
 485     return LATENT_SVM_OK;
 486 }
 487
 488 /*
 489 // Computation border size for feature map
 490 //
 491 // API
 492 // int computeBorderSize(int maxXBorder, int maxYBorder, int *bx, int *by);
 493 // INPUT
 494 // maxXBorder        - the largest root filter size (X-direction)
 495 // maxYBorder        - the largest root filter size (Y-direction)
 496 // OUTPUT
 497 // bx                - border size (X-direction)
 498 // by                - border size (Y-direction)
 499 // RESULT
 500 // Error status
 501 */
 502 int computeBorderSize(int maxXBorder, int maxYBorder, int *bx, int *by)
 503 {
 504     *bx = (int)ceilf(((float) maxXBorder) / 2.0f + 1.0f);
 505     *by = (int)ceilf(((float) maxYBorder) / 2.0f + 1.0f);
 506     return LATENT_SVM_OK;
 507 }
 508
 509 /*
 510 // Addition nullable border to the feature map
 511 //
 512 // API
 513 // int addNullableBorder(featureMap *map, int bx, int by);
 514 // INPUT
 515 // map               - feature map
 516 // bx                - border size (X-direction)
 517 // by                - border size (Y-direction)
 518 // OUTPUT
 519 // RESULT
 520 // Error status
 521 */
 522 int addNullableBorder(CvLSVMFeatureMap *map, int bx, int by)
 523 {
 524     int sizeX, sizeY, i, j, k;
 525     float *new_map;
 526     sizeX = map->sizeX + 2 * bx;
 527     sizeY = map->sizeY + 2 * by;
 528     // fix for Windows Phone 8 ARM compiler
 529     size_t size = sizeof(float) * sizeX * sizeY * map->numFeatures;
 530     new_map = (float *)malloc(size);
 531     for (i = 0; i < sizeX * sizeY * map->numFeatures; i++)
 532     {
 533         new_map[i] = 0.0;
 534     }
 535     for (i = by; i < map->sizeY + by; i++)
 536     {
 537         for (j = bx; j < map->sizeX + bx; j++)
 538         {
 539             for (k = 0; k < map->numFeatures; k++)
 540             {
 541                 new_map[(i * sizeX + j) * map->numFeatures + k] =
 542                     map->map[((i - by) * map->sizeX + j - bx) * map->numFeatures + k];
 543             }
 544         }
 545     }
 546     map->sizeX = sizeX;
 547     map->sizeY = sizeY;
 548     free(map->map);
 549     map->map = new_map;
 550     return LATENT_SVM_OK;
 551 }
 552
 553 static CvLSVMFeatureMap* featureMapBorderPartFilter(CvLSVMFeatureMap *map,
 554                                        int maxXBorder, int maxYBorder)
 555 {
 556     int bx, by;
 557     int sizeX, sizeY, i, j, k;
 558     CvLSVMFeatureMap *new_map;
 559
 560     computeBorderSize(maxXBorder, maxYBorder, &bx, &by);
 561     sizeX = map->sizeX + 2 * bx;
 562     sizeY = map->sizeY + 2 * by;
 563     allocFeatureMapObject(&new_map, sizeX, sizeY, map->numFeatures);
 564     for (i = 0; i < sizeX * sizeY * map->numFeatures; i++)
 565     {
 566         new_map->map[i] = 0.0f;
 567     }
 568     for (i = by; i < map->sizeY + by; i++)
 569     {
 570         for (j = bx; j < map->sizeX + bx; j++)
 571         {
 572             for (k = 0; k < map->numFeatures; k++)
 573             {
 574                 new_map->map[(i * sizeX + j) * map->numFeatures + k] =
 575                     map->map[((i - by) * map->sizeX + j - bx) * map->numFeatures + k];
 576             }
 577         }
 578     }
 579     return new_map;
 580 }
 581
 582 /*
 583 // Computation the maximum of the score function at the level
 584 //
 585 // API
 586 // int maxFunctionalScoreFixedLevel(const CvLSVMFilterObject **all_F, int n,
 587                                     const featurePyramid *H,
 588                                     int level, float b,
 589                                     int maxXBorder, int maxYBorder,
 590                                     float *score, CvPoint **points, int *kPoints,
 591                                     CvPoint ***partsDisplacement);
 592 // INPUT
 593 // all_F             - the set of filters (the first element is root filter,
 594                        the other - part filters)
 595 // n                 - the number of part filters
 596 // H                 - feature pyramid
 597 // level             - feature pyramid level for computation maximum score
 598 // b                 - linear term of the score function
 599 // maxXBorder        - the largest root filter size (X-direction)
 600 // maxYBorder        - the largest root filter size (Y-direction)
 601 // OUTPUT
 602 // score             - the maximum of the score function at the level
 603 // points            - the set of root filter positions (in the block space)
 604 // levels            - the set of levels
 605 // kPoints           - number of root filter positions
 606 // partsDisplacement - displacement of part filters (in the block space)
 607 // RESULT
 608 // Error status
 609 */
 610 int maxFunctionalScoreFixedLevel(const CvLSVMFilterObject **all_F, int n,
 611                                  const CvLSVMFeaturePyramid *H,
 612                                  int level, float b,
 613                                  int maxXBorder, int maxYBorder,
 614                                  float *score, CvPoint **points,
 615                                  int *kPoints, CvPoint ***partsDisplacement)
 616 {
 617     int i, j, k, dimX, dimY, nF0, mF0/*, p*/;
 618     int diff1, diff2, index, last, partsLevel;
 619     CvLSVMFilterDisposition **disposition;
 620     float *f;
 621     float *scores;
 622     float sumScorePartDisposition, maxScore;
 623     int res;
 624     CvLSVMFeatureMap *map;
 625 #ifdef FFT_CONV
 626     CvLSVMFftImage *rootFilterImage, *mapImage;
 627 #else
 628 #endif
 629
 630     /*
 631     // DEBUG variables
 632     FILE *file;
 633     char *tmp;
 634     char buf[40] = "..\\Data\\score\\score", buf1[10] = ".csv";
 635     tmp = (char *)malloc(sizeof(char) * 80);
 636     itoa(level, tmp, 10);
 637     strcat(tmp, buf1);
 638     //*/
 639
 640     // Feature map matrix dimension on the level
 641     dimX = H->pyramid[level]->sizeX;
 642     dimY = H->pyramid[level]->sizeY;
 643
 644     // Number of features
 645     //p = H->pyramid[level]->numFeatures;
 646
 647     // Getting dimension of root filter
 648     nF0 = all_F[0]->sizeY;
 649     mF0 = all_F[0]->sizeX;
 650     // Processing the situation when root filter goes
 651     // beyond the boundaries of the block set
 652     if (nF0 > dimY || mF0 > dimX)
 653     {
 654         return LATENT_SVM_FAILED_SUPERPOSITION;
 655     }
 656
 657     diff1 = dimY - nF0 + 1;
 658     diff2 = dimX - mF0 + 1;
 659
 660     // Allocation memory for saving values of function D
 661     // on the level for each part filter
 662     disposition = (CvLSVMFilterDisposition **)malloc(sizeof(CvLSVMFilterDisposition *) * n);
 663     for (i = 0; i < n; i++)
 664     {
 665         disposition[i] = (CvLSVMFilterDisposition *)malloc(sizeof(CvLSVMFilterDisposition));
 666     }
 667
 668     // Allocation memory for values of score function for each block on the level
 669     scores = (float *)malloc(sizeof(float) * (diff1 * diff2));
 670
 671     // A dot product vectors of feature map and weights of root filter
 672 #ifdef FFT_CONV
 673     getFFTImageFeatureMap(H->pyramid[level], &mapImage);
 674     getFFTImageFilterObject(all_F[0], H->pyramid[level]->sizeX, H->pyramid[level]->sizeY, &rootFilterImage);
 675     res = convFFTConv2d(mapImage, rootFilterImage, all_F[0]->sizeX, all_F[0]->sizeY, &f);
 676     freeFFTImage(&mapImage);
 677     freeFFTImage(&rootFilterImage);
 678 #else
 679     // Allocation memory for saving a dot product vectors of feature map and
 680     // weights of root filter
 681     f = (float *)malloc(sizeof(float) * (diff1 * diff2));
 682     // A dot product vectors of feature map and weights of root filter
 683     res = convolution(all_F[0], H->pyramid[level], f);
 684 #endif
 685     if (res != LATENT_SVM_OK)
 686     {
 687         free(f);
 688         free(scores);
 689         for (i = 0; i < n; i++)
 690         {
 691             free(disposition[i]);
 692         }
 693         free(disposition);
 694         return res;
 695     }
 696
 697     // Computation values of function D for each part filter
 698     // on the level (level - LAMBDA)
 699     partsLevel = level - LAMBDA;
 700     // For feature map at the level 'partsLevel' add nullable border
 701     map = featureMapBorderPartFilter(H->pyramid[partsLevel],
 702                                      maxXBorder, maxYBorder);
 703
 704     // Computation the maximum of score function
 705     sumScorePartDisposition = 0.0;
 706 #ifdef FFT_CONV
 707     getFFTImageFeatureMap(map, &mapImage);
 708     for (k = 1; k <= n; k++)
 709     {
 710         filterDispositionLevelFFT(all_F[k], mapImage,
 711                                &(disposition[k - 1]->score),
 712                                &(disposition[k - 1]->x),
 713                                &(disposition[k - 1]->y));
 714     }
 715     freeFFTImage(&mapImage);
 716 #else
 717     for (k = 1; k <= n; k++)
 718     {
 719         filterDispositionLevel(all_F[k], map,
 720                                &(disposition[k - 1]->score),
 721                                &(disposition[k - 1]->x),
 722                                &(disposition[k - 1]->y));
 723     }
 724 #endif
 725     scores[0] = f[0] - sumScorePartDisposition + b;
 726     maxScore = scores[0];
 727     (*kPoints) = 0;
 728     for (i = 0; i < diff1; i++)
 729     {
 730         for (j = 0; j < diff2; j++)
 731         {
 732             sumScorePartDisposition = 0.0;
 733             for (k = 1; k <= n; k++)
 734             {
 735                 // This condition takes on a value true
 736                 // when filter goes beyond the boundaries of block set
 737                 if ((2 * i + all_F[k]->V.y <
 738                             map->sizeY - all_F[k]->sizeY + 1) &&
 739                     (2 * j + all_F[k]->V.x <
 740                             map->sizeX - all_F[k]->sizeX + 1))
 741                 {
 742                     index = (2 * i + all_F[k]->V.y) *
 743                                 (map->sizeX - all_F[k]->sizeX + 1) +
 744                             (2 * j + all_F[k]->V.x);
 745                     sumScorePartDisposition += disposition[k - 1]->score[index];
 746                 }
 747             }
 748             scores[i * diff2 + j] = f[i * diff2 + j] - sumScorePartDisposition + b;
 749             if (maxScore < scores[i * diff2 + j])
 750             {
 751                 maxScore = scores[i * diff2 + j];
 752                 (*kPoints) = 1;
 753             }
 754             else if ((scores[i * diff2 + j] - maxScore) *
 755                      (scores[i * diff2 + j] - maxScore) <= EPS)
 756             {
 757                 (*kPoints)++;
 758             } /* if (maxScore < scores[i * diff2 + j]) */
 759         }
 760     }
 761
 762     // Allocation memory for saving positions of root filter and part filters
 763     (*points) = (CvPoint *)malloc(sizeof(CvPoint) * (*kPoints));
 764     (*partsDisplacement) = (CvPoint **)malloc(sizeof(CvPoint *) * (*kPoints));
 765     for (i = 0; i < (*kPoints); i++)
 766     {
 767         (*partsDisplacement)[i] = (CvPoint *)malloc(sizeof(CvPoint) * n);
 768     }
 769
 770     /*// DEBUG
 771     strcat(buf, tmp);
 772     file = fopen(buf, "w+");
 773     //*/
 774     // Construction of the set of positions for root filter
 775     // that correspond the maximum of score function on the level
 776     (*score) = maxScore;
 777     last = 0;
 778     for (i = 0; i < diff1; i++)
 779     {
 780         for (j = 0; j < diff2; j++)
 781         {
 782             if ((scores[i * diff2 + j] - maxScore) *
 783                 (scores[i * diff2 + j] - maxScore) <= EPS)
 784             {
 785                 (*points)[last].y = i;
 786                 (*points)[last].x = j;
 787                 for (k = 1; k <= n; k++)
 788                 {
 789                     if ((2 * i + all_F[k]->V.y <
 790                             map->sizeY - all_F[k]->sizeY + 1) &&
 791                         (2 * j + all_F[k]->V.x <
 792                             map->sizeX - all_F[k]->sizeX + 1))
 793                     {
 794                         index = (2 * i + all_F[k]->V.y) *
 795                                    (map->sizeX - all_F[k]->sizeX + 1) +
 796                                 (2 * j + all_F[k]->V.x);
 797                         (*partsDisplacement)[last][k - 1].x =
 798                                               disposition[k - 1]->x[index];
 799                         (*partsDisplacement)[last][k - 1].y =
 800                                               disposition[k - 1]->y[index];
 801                     }
 802                 }
 803                 last++;
 804             } /* if ((scores[i * diff2 + j] - maxScore) *
 805                      (scores[i * diff2 + j] - maxScore) <= EPS) */
 806             //fprintf(file, "%lf;", scores[i * diff2 + j]);
 807         }
 808         //fprintf(file, "\n");
 809     }
 810     //fclose(file);
 811     //free(tmp);
 812
 813     // Release allocated memory
 814     for (i = 0; i < n ; i++)
 815     {
 816         free(disposition[i]->score);
 817         free(disposition[i]->x);
 818         free(disposition[i]->y);
 819         free(disposition[i]);
 820     }
 821     free(disposition);
 822     free(f);
 823     free(scores);
 824     freeFeatureMapObject(&map);
 825     return LATENT_SVM_OK;
 826 }
 827
 828 /*
 829 // Computation score function at the level that exceed threshold
 830 //
 831 // API
 832 // int thresholdFunctionalScoreFixedLevel(const CvLSVMFilterObject **all_F, int n,
 833                                           const featurePyramid *H,
 834                                           int level, float b,
 835                                           int maxXBorder, int maxYBorder,
 836                                           float scoreThreshold,
 837                                           float **score, CvPoint **points, int *kPoints,
 838                                           CvPoint ***partsDisplacement);
 839 // INPUT
 840 // all_F             - the set of filters (the first element is root filter,
 841                        the other - part filters)
 842 // n                 - the number of part filters
 843 // H                 - feature pyramid
 844 // level             - feature pyramid level for computation maximum score
 845 // b                 - linear term of the score function
 846 // maxXBorder        - the largest root filter size (X-direction)
 847 // maxYBorder        - the largest root filter size (Y-direction)
 848 // scoreThreshold    - score threshold
 849 // OUTPUT
 850 // score             - score function at the level that exceed threshold
 851 // points            - the set of root filter positions (in the block space)
 852 // levels            - the set of levels
 853 // kPoints           - number of root filter positions
 854 // partsDisplacement - displacement of part filters (in the block space)
 855 // RESULT
 856 // Error status
 857 */
 858 int thresholdFunctionalScoreFixedLevel(const CvLSVMFilterObject **all_F, int n,
 859                                        const CvLSVMFeaturePyramid *H,
 860                                        int level, float b,
 861                                        int maxXBorder, int maxYBorder,
 862                                        float scoreThreshold,
 863                                        float **score, CvPoint **points, int *kPoints,
 864                                        CvPoint ***partsDisplacement)
 865 {
 866     int i, j, k, dimX, dimY, nF0, mF0/*, p*/;
 867     int diff1, diff2, index, last, partsLevel;
 868     CvLSVMFilterDisposition **disposition;
 869     float *f;
 870     float *scores;
 871     float sumScorePartDisposition;
 872     int res;
 873     CvLSVMFeatureMap *map;
 874 #ifdef FFT_CONV
 875     CvLSVMFftImage *rootFilterImage, *mapImage;
 876 #else
 877 #endif
 878     /*
 879     // DEBUG variables
 880     FILE *file;
 881     char *tmp;
 882     char buf[40] = "..\\Data\\score\\score", buf1[10] = ".csv";
 883     tmp = (char *)malloc(sizeof(char) * 80);
 884     itoa(level, tmp, 10);
 885     strcat(tmp, buf1);
 886     //*/
 887
 888     // Feature map matrix dimension on the level
 889     dimX = H->pyramid[level]->sizeX;
 890     dimY = H->pyramid[level]->sizeY;
 891
 892     // Number of features
 893     //p = H->pyramid[level]->numFeatures;
 894
 895     // Getting dimension of root filter
 896     nF0 = all_F[0]->sizeY;
 897     mF0 = all_F[0]->sizeX;
 898     // Processing the situation when root filter goes
 899     // beyond the boundaries of the block set
 900     if (nF0 > dimY || mF0 > dimX)
 901     {
 902         return LATENT_SVM_FAILED_SUPERPOSITION;
 903     }
 904
 905     diff1 = dimY - nF0 + 1;
 906     diff2 = dimX - mF0 + 1;
 907
 908     // Allocation memory for saving values of function D
 909     // on the level for each part filter
 910     disposition = (CvLSVMFilterDisposition **)malloc(sizeof(CvLSVMFilterDisposition *) * n);
 911     for (i = 0; i < n; i++)
 912     {
 913         disposition[i] = (CvLSVMFilterDisposition *)malloc(sizeof(CvLSVMFilterDisposition));
 914     }
 915
 916     // Allocation memory for values of score function for each block on the level
 917     scores = (float *)malloc(sizeof(float) * (diff1 * diff2));
 918     // A dot product vectors of feature map and weights of root filter
 919 #ifdef FFT_CONV
 920     getFFTImageFeatureMap(H->pyramid[level], &mapImage);
 921     getFFTImageFilterObject(all_F[0], H->pyramid[level]->sizeX, H->pyramid[level]->sizeY, &rootFilterImage);
 922     res = convFFTConv2d(mapImage, rootFilterImage, all_F[0]->sizeX, all_F[0]->sizeY, &f);
 923     freeFFTImage(&mapImage);
 924     freeFFTImage(&rootFilterImage);
 925 #else
 926     // Allocation memory for saving a dot product vectors of feature map and
 927     // weights of root filter
 928     f = (float *)malloc(sizeof(float) * (diff1 * diff2));
 929     res = convolution(all_F[0], H->pyramid[level], f);
 930 #endif
 931     if (res != LATENT_SVM_OK)
 932     {
 933         free(f);
 934         free(scores);
 935         for (i = 0; i < n; i++)
 936         {
 937             free(disposition[i]);
 938         }
 939         free(disposition);
 940         return res;
 941     }
 942
 943     // Computation values of function D for each part filter
 944     // on the level (level - LAMBDA)
 945     partsLevel = level - LAMBDA;
 946     // For feature map at the level 'partsLevel' add nullable border
 947     map = featureMapBorderPartFilter(H->pyramid[partsLevel],
 948                                      maxXBorder, maxYBorder);
 949
 950     // Computation the maximum of score function
 951     sumScorePartDisposition = 0.0;
 952 #ifdef FFT_CONV
 953     getFFTImageFeatureMap(map, &mapImage);
 954     for (k = 1; k <= n; k++)
 955     {
 956         filterDispositionLevelFFT(all_F[k], mapImage,
 957                                &(disposition[k - 1]->score),
 958                                &(disposition[k - 1]->x),
 959                                &(disposition[k - 1]->y));
 960     }
 961     freeFFTImage(&mapImage);
 962 #else
 963     for (k = 1; k <= n; k++)
 964     {
 965         filterDispositionLevel(all_F[k], map,
 966                                &(disposition[k - 1]->score),
 967                                &(disposition[k - 1]->x),
 968                                &(disposition[k - 1]->y));
 969     }
 970 #endif
 971     (*kPoints) = 0;
 972     for (i = 0; i < diff1; i++)
 973     {
 974         for (j = 0; j < diff2; j++)
 975         {
 976             sumScorePartDisposition = 0.0;
 977             for (k = 1; k <= n; k++)
 978             {
 979                 // This condition takes on a value true
 980                 // when filter goes beyond the boundaries of block set
 981                 if ((2 * i + all_F[k]->V.y <
 982                             map->sizeY - all_F[k]->sizeY + 1) &&
 983                     (2 * j + all_F[k]->V.x <
 984                             map->sizeX - all_F[k]->sizeX + 1))
 985                 {
 986                     index = (2 * i + all_F[k]->V.y) *
 987                                 (map->sizeX - all_F[k]->sizeX + 1) +
 988                             (2 * j + all_F[k]->V.x);
 989                     sumScorePartDisposition += disposition[k - 1]->score[index];
 990                 }
 991             }
 992             scores[i * diff2 + j] = f[i * diff2 + j] - sumScorePartDisposition + b;
 993             if (scores[i * diff2 + j] > scoreThreshold)
 994             {
 995                 (*kPoints)++;
 996             }
 997         }
 998     }
 999
1000     // Allocation memory for saving positions of root filter and part filters
1001     (*points) = (CvPoint *)malloc(sizeof(CvPoint) * (*kPoints));
1002     (*partsDisplacement) = (CvPoint **)malloc(sizeof(CvPoint *) * (*kPoints));
1003     for (i = 0; i < (*kPoints); i++)
1004     {
1005         (*partsDisplacement)[i] = (CvPoint *)malloc(sizeof(CvPoint) * n);
1006     }
1007
1008     /*// DEBUG
1009     strcat(buf, tmp);
1010     file = fopen(buf, "w+");
1011     //*/
1012     // Construction of the set of positions for root filter
1013     // that correspond score function on the level that exceed threshold
1014     (*score) = (float *)malloc(sizeof(float) * (*kPoints));
1015     last = 0;
1016     for (i = 0; i < diff1; i++)
1017     {
1018         for (j = 0; j < diff2; j++)
1019         {
1020             if (scores[i * diff2 + j] > scoreThreshold)
1021             {
1022                 (*score)[last] = scores[i * diff2 + j];
1023                 (*points)[last].y = i;
1024                 (*points)[last].x = j;
1025                 for (k = 1; k <= n; k++)
1026                 {
1027                     if ((2 * i + all_F[k]->V.y <
1028                             map->sizeY - all_F[k]->sizeY + 1) &&
1029                         (2 * j + all_F[k]->V.x <
1030                             map->sizeX - all_F[k]->sizeX + 1))
1031                     {
1032                         index = (2 * i + all_F[k]->V.y) *
1033                                    (map->sizeX - all_F[k]->sizeX + 1) +
1034                                 (2 * j + all_F[k]->V.x);
1035                         (*partsDisplacement)[last][k - 1].x =
1036                                               disposition[k - 1]->x[index];
1037                         (*partsDisplacement)[last][k - 1].y =
1038                                               disposition[k - 1]->y[index];
1039                     }
1040                 }
1041                 last++;
1042             }
1043             //fprintf(file, "%lf;", scores[i * diff2 + j]);
1044         }
1045         //fprintf(file, "\n");
1046     }
1047     //fclose(file);
1048     //free(tmp);
1049
1050     // Release allocated memory
1051     for (i = 0; i < n ; i++)
1052     {
1053         free(disposition[i]->score);
1054         free(disposition[i]->x);
1055         free(disposition[i]->y);
1056         free(disposition[i]);
1057     }
1058     free(disposition);
1059     free(f);
1060     free(scores);
1061     freeFeatureMapObject(&map);
1062     return LATENT_SVM_OK;
1063 }
1064
1065 /*
1066 // Computation the maximum of the score function
1067 //
1068 // API
1069 // int maxFunctionalScore(const CvLSVMFilterObject **all_F, int n,
1070                           const featurePyramid *H, float b,
1071                           int maxXBorder, int maxYBorder,
1072                           float *score,
1073                           CvPoint **points, int **levels, int *kPoints,
1074                           CvPoint ***partsDisplacement);
1075 // INPUT
1076 // all_F             - the set of filters (the first element is root filter,
1077                        the other - part filters)
1078 // n                 - the number of part filters
1079 // H                 - feature pyramid
1080 // b                 - linear term of the score function
1081 // maxXBorder        - the largest root filter size (X-direction)
1082 // maxYBorder        - the largest root filter size (Y-direction)
1083 // OUTPUT
1084 // score             - the maximum of the score function
1085 // points            - the set of root filter positions (in the block space)
1086 // levels            - the set of levels
1087 // kPoints           - number of root filter positions
1088 // partsDisplacement - displacement of part filters (in the block space)
1089 // RESULT
1090 // Error status
1091 */
1092 int maxFunctionalScore(const CvLSVMFilterObject **all_F, int n,
1093                        const CvLSVMFeaturePyramid *H, float b,
1094                        int maxXBorder, int maxYBorder,
1095                        float *score,
1096                        CvPoint **points, int **levels, int *kPoints,
1097                        CvPoint ***partsDisplacement)
1098 {
1099     int l, i, j, k, s, f, level, numLevels;
1100     float *tmpScore;
1101     CvPoint ***tmpPoints;
1102     CvPoint ****tmpPartsDisplacement;
1103     int *tmpKPoints;
1104     float maxScore;
1105     int res;
1106
1107     /* DEBUG
1108     FILE *file;
1109     //*/
1110
1111     // Computation the number of levels for seaching object,
1112     // first lambda-levels are used for computation values
1113     // of score function for each position of root filter
1114     numLevels = H->numLevels - LAMBDA;
1115
1116     // Allocation memory for maximum value of score function for each level
1117     tmpScore = (float *)malloc(sizeof(float) * numLevels);
1118     // Allocation memory for the set of points that corresponds
1119     // to the maximum of score function
1120     tmpPoints = (CvPoint ***)malloc(sizeof(CvPoint **) * numLevels);
1121     for (i = 0; i < numLevels; i++)
1122     {
1123         tmpPoints[i] = (CvPoint **)malloc(sizeof(CvPoint *));
1124     }
1125     // Allocation memory for memory for saving parts displacement on each level
1126     tmpPartsDisplacement = (CvPoint ****)malloc(sizeof(CvPoint ***) * numLevels);
1127     for (i = 0; i < numLevels; i++)
1128     {
1129         tmpPartsDisplacement[i] = (CvPoint ***)malloc(sizeof(CvPoint **));
1130     }
1131     // Number of points that corresponds to the maximum
1132     // of score function on each level
1133     tmpKPoints = (int *)malloc(sizeof(int) * numLevels);
1134     for (i = 0; i < numLevels; i++)
1135     {
1136         tmpKPoints[i] = 0;
1137     }
1138
1139     // Set current value of the maximum of score function
1140     res = maxFunctionalScoreFixedLevel(all_F, n, H, LAMBDA, b,
1141             maxXBorder, maxYBorder,
1142             &(tmpScore[0]),
1143             tmpPoints[0],
1144             &(tmpKPoints[0]),
1145             tmpPartsDisplacement[0]);
1146     maxScore = tmpScore[0];
1147     (*kPoints) = tmpKPoints[0];
1148
1149     // Computation maxima of score function on each level
1150     // and getting the maximum on all levels
1151     /* DEBUG: maxScore
1152     file = fopen("maxScore.csv", "w+");
1153     fprintf(file, "%i;%lf;\n", H->lambda, tmpScore[0]);
1154     //*/
1155     for (l = LAMBDA + 1; l < H->numLevels; l++)
1156     {
1157         k = l - LAMBDA;
1158         res = maxFunctionalScoreFixedLevel(all_F, n, H, l, b,
1159                                            maxXBorder, maxYBorder,
1160                                            &(tmpScore[k]),
1161                                            tmpPoints[k],
1162                                            &(tmpKPoints[k]),
1163                                            tmpPartsDisplacement[k]);
1164         //fprintf(file, "%i;%lf;\n", l, tmpScore[k]);
1165         if (res != LATENT_SVM_OK)
1166         {
1167             continue;
1168         }
1169         if (maxScore < tmpScore[k])
1170         {
1171             maxScore = tmpScore[k];
1172             (*kPoints) = tmpKPoints[k];
1173         }
1174         else if ((maxScore - tmpScore[k]) * (maxScore - tmpScore[k]) <= EPS)
1175         {
1176             (*kPoints) += tmpKPoints[k];
1177         } /* if (maxScore < tmpScore[k]) else if (...)*/
1178     }
1179     //fclose(file);
1180
1181     // Allocation memory for levels
1182     (*levels) = (int *)malloc(sizeof(int) * (*kPoints));
1183     // Allocation memory for the set of points
1184     (*points) = (CvPoint *)malloc(sizeof(CvPoint) * (*kPoints));
1185     // Allocation memory for parts displacement
1186     (*partsDisplacement) = (CvPoint **)malloc(sizeof(CvPoint *) * (*kPoints));
1187
1188     // Filling the set of points, levels and parts displacement
1189     s = 0;
1190     f = 0;
1191     for (i = 0; i < numLevels; i++)
1192     {
1193         if ((tmpScore[i] - maxScore) * (tmpScore[i] - maxScore) <= EPS)
1194         {
1195             // Computation the number of level
1196             level = i + LAMBDA;
1197
1198             // Addition a set of points
1199             f += tmpKPoints[i];
1200             for (j = s; j < f; j++)
1201             {
1202                 (*levels)[j] = level;
1203                 (*points)[j] = (*tmpPoints[i])[j - s];
1204                 (*partsDisplacement)[j] = (*(tmpPartsDisplacement[i]))[j - s];
1205             }
1206             s = f;
1207         } /* if ((tmpScore[i] - maxScore) * (tmpScore[i] - maxScore) <= EPS) */
1208     }
1209     (*score) = maxScore;
1210
1211     // Release allocated memory
1212     for (i = 0; i < numLevels; i++)
1213     {
1214         free(tmpPoints[i]);
1215         free(tmpPartsDisplacement[i]);
1216     }
1217     free(tmpPoints);
1218     free(tmpPartsDisplacement);
1219     free(tmpScore);
1220     free(tmpKPoints);
1221
1222     return LATENT_SVM_OK;
1223 }
1224
1225 /*
1226 // Computation score function that exceed threshold
1227 //
1228 // API
1229 // int thresholdFunctionalScore(const CvLSVMFilterObject **all_F, int n,
1230                                 const featurePyramid *H,
1231                                 float b,
1232                                 int maxXBorder, int maxYBorder,
1233                                 float scoreThreshold,
1234                                 float **score,
1235                                 CvPoint **points, int **levels, int *kPoints,
1236                                 CvPoint ***partsDisplacement);
1237 // INPUT
1238 // all_F             - the set of filters (the first element is root filter,
1239                        the other - part filters)
1240 // n                 - the number of part filters
1241 // H                 - feature pyramid
1242 // b                 - linear term of the score function
1243 // maxXBorder        - the largest root filter size (X-direction)
1244 // maxYBorder        - the largest root filter size (Y-direction)
1245 // scoreThreshold    - score threshold
1246 // OUTPUT
1247 // score             - score function values that exceed threshold
1248 // points            - the set of root filter positions (in the block space)
1249 // levels            - the set of levels
1250 // kPoints           - number of root filter positions
1251 // partsDisplacement - displacement of part filters (in the block space)
1252 // RESULT
1253 // Error status
1254 */
1255 int thresholdFunctionalScore(const CvLSVMFilterObject **all_F, int n,
1256                              const CvLSVMFeaturePyramid *H,
1257                              float b,
1258                              int maxXBorder, int maxYBorder,
1259                              float scoreThreshold,
1260                              float **score,
1261                              CvPoint **points, int **levels, int *kPoints,
1262                              CvPoint ***partsDisplacement)
1263 {
1264     int l, i, j, k, s, f, level, numLevels;
1265     float **tmpScore;
1266     CvPoint ***tmpPoints;
1267     CvPoint ****tmpPartsDisplacement;
1268     int *tmpKPoints;
1269     int res;
1270
1271     /* DEBUG
1272     FILE *file;
1273     //*/
1274
1275     // Computation the number of levels for seaching object,
1276     // first lambda-levels are used for computation values
1277     // of score function for each position of root filter
1278     numLevels = H->numLevels - LAMBDA;
1279
1280     // Allocation memory for values of score function for each level
1281     // that exceed threshold
1282     tmpScore = (float **)malloc(sizeof(float*) * numLevels);
1283     // Allocation memory for the set of points that corresponds
1284     // to the maximum of score function
1285     tmpPoints = (CvPoint ***)malloc(sizeof(CvPoint **) * numLevels);
1286     for (i = 0; i < numLevels; i++)
1287     {
1288         tmpPoints[i] = (CvPoint **)malloc(sizeof(CvPoint *));
1289     }
1290     // Allocation memory for memory for saving parts displacement on each level
1291     tmpPartsDisplacement = (CvPoint ****)malloc(sizeof(CvPoint ***) * numLevels);
1292     for (i = 0; i < numLevels; i++)
1293     {
1294         tmpPartsDisplacement[i] = (CvPoint ***)malloc(sizeof(CvPoint **));
1295     }
1296     // Number of points that corresponds to the maximum
1297     // of score function on each level
1298     tmpKPoints = (int *)malloc(sizeof(int) * numLevels);
1299     for (i = 0; i < numLevels; i++)
1300     {
1301         tmpKPoints[i] = 0;
1302     }
1303
1304     // Computation maxima of score function on each level
1305     // and getting the maximum on all levels
1306     /* DEBUG: maxScore
1307     file = fopen("maxScore.csv", "w+");
1308     fprintf(file, "%i;%lf;\n", H->lambda, tmpScore[0]);
1309     //*/
1310     (*kPoints) = 0;
1311     for (l = LAMBDA; l < H->numLevels; l++)
1312     {
1313         k = l - LAMBDA;
1314         //printf("Score at the level %i\n", l);
1315         res = thresholdFunctionalScoreFixedLevel(all_F, n, H, l, b,
1316             maxXBorder, maxYBorder, scoreThreshold,
1317             &(tmpScore[k]),
1318             tmpPoints[k],
1319             &(tmpKPoints[k]),
1320             tmpPartsDisplacement[k]);
1321         //fprintf(file, "%i;%lf;\n", l, tmpScore[k]);
1322         if (res != LATENT_SVM_OK)
1323         {
1324             continue;
1325         }
1326         (*kPoints) += tmpKPoints[k];
1327     }
1328     //fclose(file);
1329
1330     // Allocation memory for levels
1331     (*levels) = (int *)malloc(sizeof(int) * (*kPoints));
1332     // Allocation memory for the set of points
1333     (*points) = (CvPoint *)malloc(sizeof(CvPoint) * (*kPoints));
1334     // Allocation memory for parts displacement
1335     (*partsDisplacement) = (CvPoint **)malloc(sizeof(CvPoint *) * (*kPoints));
1336     // Allocation memory for score function values
1337     (*score) = (float *)malloc(sizeof(float) * (*kPoints));
1338
1339     // Filling the set of points, levels and parts displacement
1340     s = 0;
1341     f = 0;
1342     for (i = 0; i < numLevels; i++)
1343     {
1344         // Computation the number of level
1345         level = i + LAMBDA;
1346
1347         // Addition a set of points
1348         f += tmpKPoints[i];
1349         for (j = s; j < f; j++)
1350         {
1351             (*levels)[j] = level;
1352             (*points)[j] = (*tmpPoints[i])[j - s];
1353             (*score)[j] = tmpScore[i][j - s];
1354             (*partsDisplacement)[j] = (*(tmpPartsDisplacement[i]))[j - s];
1355         }
1356         s = f;
1357     }
1358
1359     // Release allocated memory
1360     for (i = 0; i < numLevels; i++)
1361     {
1362         free(tmpPoints[i]);
1363         free(tmpPartsDisplacement[i]);
1364     }
1365     free(tmpPoints);
1366     free(tmpScore);
1367     free(tmpKPoints);
1368     free(tmpPartsDisplacement);
1369
1370     return LATENT_SVM_OK;
1371 }
1372
1373 #ifdef HAVE_TBB
1374 /*
1375 // Creating schedule of pyramid levels processing
1376 //
1377 // API
1378 // int createSchedule(const featurePyramid *H, const filterObject **all_F,
1379                       const int n, const int bx, const int by,
1380                       const int threadsNum, int *kLevels,
1381                       int **processingLevels)
1382 // INPUT
1383 // H                 - feature pyramid
1384 // all_F             - the set of filters (the first element is root filter,
1385                        the other - part filters)
1386 // n                 - the number of part filters
1387 // bx                - size of nullable border (X direction)
1388 // by                - size of nullable border (Y direction)
1389 // threadsNum        - number of threads that will be created in TBB version
1390 // OUTPUT
1391 // kLevels           - array that contains number of levels processed
1392                        by each thread
1393 // processingLevels  - array that contains lists of levels processed
1394                        by each thread
1395 // RESULT
1396 // Error status
1397 */
1398 static int createSchedule(const CvLSVMFeaturePyramid *H, const CvLSVMFilterObject **all_F,
1399                    const int n, const int bx, const int by,
1400                    const int threadsNum, int *kLevels, int **processingLevels)
1401 {
1402     int rootFilterDim, sumPartFiltersDim, i, numLevels, dbx, dby;
1403     int j, minValue, argMin, lambda, maxValue, k;
1404     int *dotProd, *weights, *disp;
1405     if (H == NULL || all_F == NULL)
1406     {
1407         return LATENT_SVM_TBB_SCHEDULE_CREATION_FAILED;
1408     }
1409     // Number of feature vectors in root filter
1410     rootFilterDim = all_F[0]->sizeX * all_F[0]->sizeY;
1411     // Number of feature vectors in all part filters
1412     sumPartFiltersDim = 0;
1413     for (i = 1; i <= n; i++)
1414     {
1415         sumPartFiltersDim += all_F[i]->sizeX * all_F[i]->sizeY;
1416     }
1417     // Number of levels which are used for computation of score function
1418     numLevels = H->numLevels - LAMBDA;
1419     // Allocation memory for saving number of dot products that will be
1420     // computed for each level of feature pyramid
1421     dotProd = (int *)malloc(sizeof(int) * numLevels);
1422     // Size of nullable border that's used in computing convolution
1423     // of feature map with part filter
1424     dbx = 2 * bx;
1425     dby = 2 * by;
1426     lambda = LAMBDA;
1427     for (i = 0; i < numLevels; i++)
1428     {
1429         dotProd[i] = H->pyramid[i + lambda]->sizeX *
1430                      H->pyramid[i + lambda]->sizeY * rootFilterDim +
1431                      (H->pyramid[i]->sizeX + dbx) *
1432                      (H->pyramid[i]->sizeY + dby) * sumPartFiltersDim;
1433     }
1434     // Allocation memory for saving dot product number performed by each thread
1435     weights = (int *)malloc(sizeof(int) * threadsNum);
1436     // Allocation memory for saving dispertion
1437     disp = (int *)malloc(sizeof(int) * threadsNum);
1438     // At the first step we think of first threadsNum levels will be processed
1439     // by different threads
1440     for (i = 0; i < threadsNum; i++)
1441     {
1442         kLevels[i] = 1;
1443         weights[i] = dotProd[i];
1444         disp[i] = 0;
1445     }
1446     // Computation number of levels that will be processed by each thread
1447     for (i = threadsNum; i < numLevels; i++)
1448     {
1449         // Search number of thread that will process level number i
1450         for (j = 0; j < threadsNum; j++)
1451         {
1452             weights[j] += dotProd[i];
1453             minValue = weights[0];
1454             maxValue = weights[0];
1455             for (k = 1; k < threadsNum; k++)
1456             {
1457                 minValue = min(minValue, weights[k]);
1458                 maxValue = max(maxValue, weights[k]);
1459             }
1460             disp[j] = maxValue - minValue;
1461             weights[j] -= dotProd[i];
1462         }
1463         minValue = disp[0];
1464         argMin = 0;
1465         for (j = 1; j < threadsNum; j++)
1466         {
1467             if (disp[j] < minValue)
1468             {
1469                 minValue = disp[j];
1470                 argMin = j;
1471             }
1472         }
1473         // Addition new level
1474         kLevels[argMin]++;
1475         weights[argMin] += dotProd[i];
1476     }
1477     for (i = 0; i < threadsNum; i++)
1478     {
1479         // Allocation memory for saving list of levels for each level
1480         processingLevels[i] = (int *)malloc(sizeof(int) * kLevels[i]);
1481         // At the first step we think of first threadsNum levels will be processed
1482         // by different threads
1483         processingLevels[i][0] = lambda + i;
1484         kLevels[i] = 1;
1485         weights[i] = dotProd[i];
1486     }
1487     // Creating list of levels
1488     for (i = threadsNum; i < numLevels; i++)
1489     {
1490         for (j = 0; j < threadsNum; j++)
1491         {
1492             weights[j] += dotProd[i];
1493             minValue = weights[0];
1494             maxValue = weights[0];
1495             for (k = 1; k < threadsNum; k++)
1496             {
1497                 minValue = min(minValue, weights[k]);
1498                 maxValue = max(maxValue, weights[k]);
1499             }
1500             disp[j] = maxValue - minValue;
1501             weights[j] -= dotProd[i];
1502         }
1503         minValue = disp[0];
1504         argMin = 0;
1505         for (j = 1; j < threadsNum; j++)
1506         {
1507             if (disp[j] < minValue)
1508             {
1509                 minValue = disp[j];
1510                 argMin = j;
1511             }
1512         }
1513         processingLevels[argMin][kLevels[argMin]] = lambda + i;
1514         kLevels[argMin]++;
1515         weights[argMin] += dotProd[i];
1516     }
1517     // Release allocated memory
1518     free(weights);
1519     free(dotProd);
1520     free(disp);
1521     return LATENT_SVM_OK;
1522 }
1523
1524 /*
1525 // int tbbThresholdFunctionalScore(const CvLSVMFilterObject **all_F, int n,
1526                                    const CvLSVMFeaturePyramid *H,
1527                                    const float b,
1528                                    const int maxXBorder, const int maxYBorder,
1529                                    const float scoreThreshold,
1530                                    const int threadsNum,
1531                                    float **score,
1532                                    CvPoint **points, int **levels, int *kPoints,
1533                                    CvPoint ***partsDisplacement);
1534 // INPUT
1535 // all_F             - the set of filters (the first element is root filter,
1536                        the other - part filters)
1537 // n                 - the number of part filters
1538 // H                 - feature pyramid
1539 // b                 - linear term of the score function
1540 // maxXBorder        - the largest root filter size (X-direction)
1541 // maxYBorder        - the largest root filter size (Y-direction)
1542 // scoreThreshold    - score threshold
1543 // threadsNum        - number of threads that will be created using TBB version
1544 // OUTPUT
1545 // score             - score function values that exceed threshold
1546 // points            - the set of root filter positions (in the block space)
1547 // levels            - the set of levels
1548 // kPoints           - number of root filter positions
1549 // partsDisplacement - displacement of part filters (in the block space)
1550 // RESULT
1551 // Error status
1552 */
1553 int tbbThresholdFunctionalScore(const CvLSVMFilterObject **all_F, int n,
1554                                 const CvLSVMFeaturePyramid *H,
1555                                 const float b,
1556                                 const int maxXBorder, const int maxYBorder,
1557                                 const float scoreThreshold,
1558                                 const int threadsNum,
1559                                 float **score,
1560                                 CvPoint **points, int **levels, int *kPoints,
1561                                 CvPoint ***partsDisplacement)
1562 {
1563     int i, j, s, f, level, numLevels;
1564     float **tmpScore;
1565     CvPoint ***tmpPoints;
1566     CvPoint ****tmpPartsDisplacement;
1567     int *tmpKPoints;
1568     int res;
1569
1570     int *kLevels, **procLevels;
1571     int bx, by;
1572
1573     // Computation the number of levels for seaching object,
1574     // first lambda-levels are used for computation values
1575     // of score function for each position of root filter
1576     numLevels = H->numLevels - LAMBDA;
1577     kLevels = (int *)malloc(sizeof(int) * threadsNum);
1578     procLevels = (int **)malloc(sizeof(int*) * threadsNum);
1579     computeBorderSize(maxXBorder, maxYBorder, &bx, &by);
1580     res = createSchedule(H, all_F, n, bx, by, threadsNum, kLevels, procLevels);
1581     if (res != LATENT_SVM_OK)
1582     {
1583         for (i = 0; i < threadsNum; i++)
1584         {
1585             if (procLevels[i] != NULL)
1586             {
1587                 free(procLevels[i]);
1588             }
1589         }
1590         free(procLevels);
1591         free(kLevels);
1592         return res;
1593     }
1594
1595     // Allocation memory for values of score function for each level
1596     // that exceed threshold
1597     tmpScore = (float **)malloc(sizeof(float*) * numLevels);
1598     // Allocation memory for the set of points that corresponds
1599     // to the maximum of score function
1600     tmpPoints = (CvPoint ***)malloc(sizeof(CvPoint **) * numLevels);
1601     for (i = 0; i < numLevels; i++)
1602     {
1603         tmpPoints[i] = (CvPoint **)malloc(sizeof(CvPoint *));
1604     }
1605     // Allocation memory for memory for saving parts displacement on each level
1606     tmpPartsDisplacement = (CvPoint ****)malloc(sizeof(CvPoint ***) * numLevels);
1607     for (i = 0; i < numLevels; i++)
1608     {
1609         tmpPartsDisplacement[i] = (CvPoint ***)malloc(sizeof(CvPoint **));
1610     }
1611     // Number of points that corresponds to the maximum
1612     // of score function on each level
1613     tmpKPoints = (int *)malloc(sizeof(int) * numLevels);
1614     for (i = 0; i < numLevels; i++)
1615     {
1616         tmpKPoints[i] = 0;
1617     }
1618
1619     // Computation maxima of score function on each level
1620     // and getting the maximum on all levels using TBB tasks
1621     tbbTasksThresholdFunctionalScore(all_F, n, H, b, maxXBorder, maxYBorder,
1622         scoreThreshold, kLevels, procLevels,
1623         threadsNum, tmpScore, tmpPoints,
1624         tmpKPoints, tmpPartsDisplacement);
1625     (*kPoints) = 0;
1626     for (i = 0; i < numLevels; i++)
1627     {
1628         (*kPoints) += tmpKPoints[i];
1629     }
1630
1631     // Allocation memory for levels
1632     (*levels) = (int *)malloc(sizeof(int) * (*kPoints));
1633     // Allocation memory for the set of points
1634     (*points) = (CvPoint *)malloc(sizeof(CvPoint) * (*kPoints));
1635     // Allocation memory for parts displacement
1636     (*partsDisplacement) = (CvPoint **)malloc(sizeof(CvPoint *) * (*kPoints));
1637     // Allocation memory for score function values
1638     (*score) = (float *)malloc(sizeof(float) * (*kPoints));
1639
1640     // Filling the set of points, levels and parts displacement
1641     s = 0;
1642     f = 0;
1643     for (i = 0; i < numLevels; i++)
1644     {
1645         // Computation the number of level
1646         level = i + LAMBDA;//H->lambda;
1647
1648         // Addition a set of points
1649         f += tmpKPoints[i];
1650         for (j = s; j < f; j++)
1651         {
1652             (*levels)[j] = level;
1653             (*points)[j] = (*tmpPoints[i])[j - s];
1654             (*score)[j] = tmpScore[i][j - s];
1655             (*partsDisplacement)[j] = (*(tmpPartsDisplacement[i]))[j - s];
1656         }
1657         s = f;
1658     }
1659
1660     // Release allocated memory
1661     for (i = 0; i < numLevels; i++)
1662     {
1663         free(tmpPoints[i]);
1664         free(tmpPartsDisplacement[i]);
1665     }
1666     for (i = 0; i < threadsNum; i++)
1667     {
1668         free(procLevels[i]);
1669     }
1670     free(procLevels);
1671     free(kLevels);
1672     free(tmpPoints);
1673     free(tmpScore);
1674     free(tmpKPoints);
1675     free(tmpPartsDisplacement);
1676
1677     return LATENT_SVM_OK;
1678 }
1679 #endif
1680
1681 static void sort(int n, const float* x, int* indices)
1682 {
1683     int i, j;
1684     for (i = 0; i < n; i++)
1685         for (j = i + 1; j < n; j++)
1686         {
1687             if (x[indices[j]] > x[indices[i]])
1688             {
1689                 //float x_tmp = x[i];
1690                 int index_tmp = indices[i];
1691                 //x[i] = x[j];
1692                 indices[i] = indices[j];
1693                 //x[j] = x_tmp;
1694                 indices[j] = index_tmp;
1695             }
1696         }
1697 }
1698
1699 /*
1700 // Perform non-maximum suppression algorithm (described in original paper)
1701 // to remove "similar" bounding boxes
1702 //
1703 // API
1704 // int nonMaximumSuppression(int numBoxes, const CvPoint *points,
1705                              const CvPoint *oppositePoints, const float *score,
1706                              float overlapThreshold,
1707                              int *numBoxesOut, CvPoint **pointsOut,
1708                              CvPoint **oppositePointsOut, float **scoreOut);
1709 // INPUT
1710 // numBoxes          - number of bounding boxes
1711 // points            - array of left top corner coordinates
1712 // oppositePoints    - array of right bottom corner coordinates
1713 // score             - array of detection scores
1714 // overlapThreshold  - threshold: bounding box is removed if overlap part
1715                        is greater than passed value
1716 // OUTPUT
1717 // numBoxesOut       - the number of bounding boxes algorithm returns
1718 // pointsOut         - array of left top corner coordinates
1719 // oppositePointsOut - array of right bottom corner coordinates
1720 // scoreOut          - array of detection scores
1721 // RESULT
1722 // Error status
1723 */
1724 int nonMaximumSuppression(int numBoxes, const CvPoint *points,
1725                           const CvPoint *oppositePoints, const float *score,
1726                           float overlapThreshold,
1727                           int *numBoxesOut, CvPoint **pointsOut,
1728                           CvPoint **oppositePointsOut, float **scoreOut)
1729 {
1730     int i, j, index;
1731     float* box_area = (float*)malloc(numBoxes * sizeof(float));
1732     int* indices = (int*)malloc(numBoxes * sizeof(int));
1733     int* is_suppressed = (int*)malloc(numBoxes * sizeof(int));
1734
1735     for (i = 0; i < numBoxes; i++)
1736     {
1737         indices[i] = i;
1738         is_suppressed[i] = 0;
1739         box_area[i] = (float)( (oppositePoints[i].x - points[i].x + 1) *
1740                                 (oppositePoints[i].y - points[i].y + 1));
1741     }
1742
1743     sort(numBoxes, score, indices);
1744     for (i = 0; i < numBoxes; i++)
1745     {
1746         if (!is_suppressed[indices[i]])
1747         {
1748             for (j = i + 1; j < numBoxes; j++)
1749             {
1750                 if (!is_suppressed[indices[j]])
1751                 {
1752                     int x1max = max(points[indices[i]].x, points[indices[j]].x);
1753                     int x2min = min(oppositePoints[indices[i]].x, oppositePoints[indices[j]].x);
1754                     int y1max = max(points[indices[i]].y, points[indices[j]].y);
1755                     int y2min = min(oppositePoints[indices[i]].y, oppositePoints[indices[j]].y);
1756                     int overlapWidth = x2min - x1max + 1;
1757                     int overlapHeight = y2min - y1max + 1;
1758                     if (overlapWidth > 0 && overlapHeight > 0)
1759                     {
1760                         float overlapPart = (overlapWidth * overlapHeight) / box_area[indices[j]];
1761                         if (overlapPart > overlapThreshold)
1762                         {
1763                             is_suppressed[indices[j]] = 1;
1764                         }
1765                     }
1766                 }
1767             }
1768         }
1769     }
1770
1771     *numBoxesOut = 0;
1772     for (i = 0; i < numBoxes; i++)
1773     {
1774         if (!is_suppressed[i]) (*numBoxesOut)++;
1775     }
1776
1777     *pointsOut = (CvPoint *)malloc((*numBoxesOut) * sizeof(CvPoint));
1778     *oppositePointsOut = (CvPoint *)malloc((*numBoxesOut) * sizeof(CvPoint));
1779     *scoreOut = (float *)malloc((*numBoxesOut) * sizeof(float));
1780     index = 0;
1781     for (i = 0; i < numBoxes; i++)
1782     {
1783         if (!is_suppressed[indices[i]])
1784         {
1785             (*pointsOut)[index].x = points[indices[i]].x;
1786             (*pointsOut)[index].y = points[indices[i]].y;
1787             (*oppositePointsOut)[index].x = oppositePoints[indices[i]].x;
1788             (*oppositePointsOut)[index].y = oppositePoints[indices[i]].y;
1789             (*scoreOut)[index] = score[indices[i]];
1790             index++;
1791         }
1792
1793     }
1794
1795     free(indices);
1796     free(box_area);
1797     free(is_suppressed);
1798
1799     return LATENT_SVM_OK;
1800 }