binary_descriptor_matcher.cpp 27.3 KB
Newer Older
biagio montesano's avatar
biagio montesano committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
/*M///////////////////////////////////////////////////////////////////////////////////////
 //
 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 //
 //  By downloading, copying, installing or using the software you agree to this license.
 //  If you do not agree to this license, do not download, install,
 //  copy or use the software.
 //
 //
 //                           License Agreement
 //                For Open Source Computer Vision Library
 //
 // Copyright (C) 2014, Biagio Montesano, all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
 // are permitted provided that the following conditions are met:
 //
 //   * Redistribution's of source code must retain the above copyright notice,
 //     this list of conditions and the following disclaimer.
 //
 //   * Redistribution's in binary form must reproduce the above copyright notice,
 //     this list of conditions and the following disclaimer in the documentation
 //     and/or other materials provided with the distribution.
 //
 //   * The name of the copyright holders may not be used to endorse or promote products
 //     derived from this software without specific prior written permission.
 //
 // This software is provided by the copyright holders and contributors "as is" and
 // any express or implied warranties, including, but not limited to, the implied
 // warranties of merchantability and fitness for a particular purpose are disclaimed.
 // In no event shall the Intel Corporation or contributors be liable for any direct,
 // indirect, incidental, special, exemplary, or consequential damages
 // (including, but not limited to, procurement of substitute goods or services;
 // loss of use, data, or profits; or business interruption) however caused
 // and on any theory of liability, whether in contract, strict liability,
 // or tort (including negligence or otherwise) arising in any way out of
 // the use of this software, even if advised of the possibility of such damage.
 //
 //M*/

42 43
#include "precomp.hpp"

44 45 46 47
#define MAX_B 37
double ARRAY_RESIZE_FACTOR = 1.1;    // minimum is 1.0
double ARRAY_RESIZE_ADD_FACTOR = 4;  // minimum is 1

48 49 50 51 52
//using namespace cv;
namespace cv
{
namespace line_descriptor
{
53

54 55 56
/* constructor */
BinaryDescriptorMatcher::BinaryDescriptorMatcher()
{
57 58 59 60
  dataset = new Mihasher( 256, 32 );
  nextAddedIndex = 0;
  numImages = 0;
  descrInDS = 0;
61 62
}

63 64 65
/* constructor with smart pointer */
Ptr<BinaryDescriptorMatcher> BinaryDescriptorMatcher::createBinaryDescriptorMatcher()
{
66
  return Ptr < BinaryDescriptorMatcher > ( new BinaryDescriptorMatcher() );
67 68
}

69 70 71
/* store new descriptors to be inserted in dataset */
void BinaryDescriptorMatcher::add( const std::vector<Mat>& descriptors )
{
72 73 74 75 76 77 78 79
  for ( size_t i = 0; i < descriptors.size(); i++ )
  {
    descriptorsMat.push_back( descriptors[i] );

    indexesMap.insert( std::pair<int, int>( nextAddedIndex, numImages ) );
    nextAddedIndex += descriptors[i].rows;
    numImages++;
  }
80 81 82 83 84
}

/* store new descriptors into dataset */
void BinaryDescriptorMatcher::train()
{
85 86
  if( !dataset )
    dataset = new Mihasher( 256, 32 );
87

88 89
  if( descriptorsMat.rows > 0 )
    dataset->populate( descriptorsMat, descriptorsMat.rows, descriptorsMat.cols );
90

91 92
  descrInDS = descriptorsMat.rows;
  descriptorsMat.release();
93 94 95 96 97
}

/* clear dataset and internal data */
void BinaryDescriptorMatcher::clear()
{
98 99 100 101 102 103
  descriptorsMat.release();
  indexesMap.clear();
  dataset = 0;
  nextAddedIndex = 0;
  numImages = 0;
  descrInDS = 0;
104 105 106
}

/* retrieve Hamming distances */
107
void BinaryDescriptorMatcher::checkKDistances( UINT32 * numres, int k, std::vector<int> & k_distances, int row, int string_length ) const
108
{
109
  int k_to_found = k;
110

111 112 113 114
  UINT32 * numres_tmp = numres + ( ( string_length + 1 ) * row );
  for ( int j = 0; j < ( string_length + 1 ) && k_to_found > 0; j++ )
  {
    if( ( * ( numres_tmp + j ) ) > 0 )
115
    {
116
      for ( int i = 0; i < (int) ( * ( numres_tmp + j ) ) && k_to_found > 0; i++ )
117 118 119 120
      {
        k_distances.push_back( j );
        k_to_found--;
      }
121
    }
122
  }
123 124 125
}

/* for every input descriptor,
126 127
 find the best matching one (from one image to a set) */
void BinaryDescriptorMatcher::match( const Mat& queryDescriptors, std::vector<DMatch>& matches, const std::vector<Mat>& masks )
128
{
129
  /* check data validity */
130 131 132 133 134 135
  if( queryDescriptors.rows == 0 )
  {
    std::cout << "Error: query descriptors'matrix is empty" << std::endl;
    return;
  }

136 137 138
  if( masks.size() != 0 && (int) masks.size() != numImages )
  {
    std::cout << "Error: the number of images in dataset is " << numImages << " but match function received " << masks.size()
139
        << " masks. Program will be terminated" << std::endl;
140

141 142
    return;
  }
143

144 145
  /* add new descriptors to dataset, if needed */
  train();
146

147 148
  /* set number of requested matches to return for each query */
  dataset->setK( 1 );
biagio montesano's avatar
biagio montesano committed
149

150 151 152
  /* prepare structures for query */
  UINT32 *results = new UINT32[queryDescriptors.rows];
  UINT32 * numres = new UINT32[ ( 256 + 1 ) * ( queryDescriptors.rows )];
153

154 155 156 157 158 159 160 161 162 163 164 165 166 167
  /* execute query */
  dataset->batchquery( results, numres, queryDescriptors, queryDescriptors.rows, queryDescriptors.cols );
  /* compose matches */
  for ( int counter = 0; counter < queryDescriptors.rows; counter++ )
  {
    /* create a map iterator */
    std::map<int, int>::iterator itup;

    /* get info about original image of each returned descriptor */
    itup = indexesMap.upper_bound( results[counter] - 1 );
    itup--;
    /* data validity check */
    if( !masks.empty() && ( masks[itup->second].rows != queryDescriptors.rows || masks[itup->second].cols != 1 ) )
    {
168 169
      std::stringstream ss;
      ss << "Error: mask " << itup->second << " in knnMatch function " << "should have " << queryDescriptors.rows << " and "
170
          << "1 column. Program will be terminated";
171
      //throw std::runtime_error( ss.str() );
172
    }
173
    /* create a DMatch object if required by mask or if there is
174
     no mask at all */
175
    else if( masks.empty() || masks[itup->second].at < uchar > ( counter ) != 0 )
176
    {
177 178
      std::vector<int> k_distances;
      checkKDistances( numres, 1, k_distances, counter, 256 );
179

180 181 182 183
      DMatch dm;
      dm.queryIdx = counter;
      dm.trainIdx = results[counter] - 1;
      dm.imgIdx = itup->second;
184
      dm.distance = (float) k_distances[0];
185 186

      matches.push_back( dm );
187 188
    }

189 190 191
  }

  /* delete data */
192 193
  delete[] results;
  delete[] numres;
194
}
195 196

/* for every input descriptor, find the best matching one (for a pair of images) */
197
void BinaryDescriptorMatcher::match( const Mat& queryDescriptors, const Mat& trainDescriptors, std::vector<DMatch>& matches, const Mat& mask ) const
198
{
199

200
  /* check data validity */
201 202 203 204 205 206
  if( queryDescriptors.rows == 0 || trainDescriptors.rows == 0 )
  {
    std::cout << "Error: descriptors matrices cannot be void" << std::endl;
    return;
  }

207 208 209
  if( !mask.empty() && ( mask.rows != queryDescriptors.rows && mask.cols != 1 ) )
  {
    std::cout << "Error: input mask should have " << queryDescriptors.rows << " rows and 1 column. " << "Program will be terminated" << std::endl;
210

211 212
    return;
  }
213

214 215
  /* create a new mihasher object */
  Mihasher *mh = new Mihasher( 256, 32 );
216

217 218 219 220
  /* populate mihasher */
  cv::Mat copy = trainDescriptors.clone();
  mh->populate( copy, copy.rows, copy.cols );
  mh->setK( 1 );
221

222 223 224
  /* prepare structures for query */
  UINT32 *results = new UINT32[queryDescriptors.rows];
  UINT32 * numres = new UINT32[ ( 256 + 1 ) * ( queryDescriptors.rows )];
225

226 227
  /* execute query */
  mh->batchquery( results, numres, queryDescriptors, queryDescriptors.rows, queryDescriptors.cols );
228

229 230 231
  /* compose matches */
  for ( int counter = 0; counter < queryDescriptors.rows; counter++ )
  {
232
    /* create a DMatch object if required by mask or if there is
233
     no mask at all */
234
    if( mask.empty() || ( !mask.empty() && mask.at < uchar > ( counter ) != 0 ) )
235
    {
236 237
      std::vector<int> k_distances;
      checkKDistances( numres, 1, k_distances, counter, 256 );
238

239 240 241 242
      DMatch dm;
      dm.queryIdx = counter;
      dm.trainIdx = results[counter] - 1;
      dm.imgIdx = 0;
243
      dm.distance = (float) k_distances[0];
244

245
      matches.push_back( dm );
246
    }
247
  }
248

249 250
  /* delete data */
  delete mh;
251 252
  delete[] results;
  delete[] numres;
253 254 255 256

}

/* for every input descriptor,
257 258 259
 find the best k matching descriptors (for a pair of images) */
void BinaryDescriptorMatcher::knnMatch( const Mat& queryDescriptors, const Mat& trainDescriptors, std::vector<std::vector<DMatch> >& matches, int k,
                                        const Mat& mask, bool compactResult ) const
260 261

{
262
  /* check data validity */
263 264 265 266 267 268
  if( queryDescriptors.rows == 0 || trainDescriptors.rows == 0 )
  {
    std::cout << "Error: descriptors matrices cannot be void" << std::endl;
    return;
  }

269 270 271
  if( !mask.empty() && ( mask.rows != queryDescriptors.rows || mask.cols != 1 ) )
  {
    std::cout << "Error: input mask should have " << queryDescriptors.rows << " rows and 1 column. " << "Program will be terminated" << std::endl;
272

273 274
    return;
  }
275

276 277
  /* create a new mihasher object */
  Mihasher *mh = new Mihasher( 256, 32 );
278

279 280 281
  /* populate mihasher */
  cv::Mat copy = trainDescriptors.clone();
  mh->populate( copy, copy.rows, copy.cols );
282

283 284
  /* set K */
  mh->setK( k );
285

286 287 288
  /* prepare structures for query */
  UINT32 *results = new UINT32[k * queryDescriptors.rows];
  UINT32 * numres = new UINT32[ ( 256 + 1 ) * ( queryDescriptors.rows )];
289

290 291
  /* execute query */
  mh->batchquery( results, numres, queryDescriptors, queryDescriptors.rows, queryDescriptors.cols );
292

293 294 295 296 297
  /* compose matches */
  int index = 0;
  for ( int counter = 0; counter < queryDescriptors.rows; counter++ )
  {
    /* initialize a vector of matches */
298
    std::vector < DMatch > tempVec;
299

300
    /* chech whether query should be ignored */
301
    if( !mask.empty() && mask.at < uchar > ( counter ) == 0 )
302 303 304 305 306
    {
      /* if compact result is not requested, add an empty vector */
      if( !compactResult )
        matches.push_back( tempVec );
    }
307

308 309 310 311 312 313 314 315 316 317 318
    /* query matches must be considered */
    else
    {
      std::vector<int> k_distances;
      checkKDistances( numres, k, k_distances, counter, 256 );
      for ( int j = index; j < index + k; j++ )
      {
        DMatch dm;
        dm.queryIdx = counter;
        dm.trainIdx = results[j] - 1;
        dm.imgIdx = 0;
319
        dm.distance = (float) k_distances[j - index];
320 321 322 323 324

        tempVec.push_back( dm );
      }

      matches.push_back( tempVec );
325 326
    }

327 328 329 330 331 332
    /* increment pointer */
    index += k;
  }

  /* delete data */
  delete mh;
333 334
  delete[] results;
  delete[] numres;
335 336 337
}

/* for every input descriptor,
338 339
 find the best k matching descriptors (from one image to a set) */
void BinaryDescriptorMatcher::knnMatch( const Mat& queryDescriptors, std::vector<std::vector<DMatch> >& matches, int k, const std::vector<Mat>& masks,
340 341 342
                                        bool compactResult )
{

343
  /* check data validity */
344 345 346 347 348 349
  if( queryDescriptors.rows == 0 )
  {
    std::cout << "Error: descriptors matrix cannot be void" << std::endl;
    return;
  }

350 351 352
  if( masks.size() != 0 && (int) masks.size() != numImages )
  {
    std::cout << "Error: the number of images in dataset is " << numImages << " but knnMatch function received " << masks.size()
353
        << " masks. Program will be terminated" << std::endl;
354

355 356
    return;
  }
357

358 359
  /* add new descriptors to dataset, if needed */
  train();
360

361 362
  /* set number of requested matches to return for each query */
  dataset->setK( k );
biagio montesano's avatar
biagio montesano committed
363

364 365 366
  /* prepare structures for query */
  UINT32 *results = new UINT32[k * queryDescriptors.rows];
  UINT32 * numres = new UINT32[ ( 256 + 1 ) * ( queryDescriptors.rows )];
367

368 369
  /* execute query */
  dataset->batchquery( results, numres, queryDescriptors, queryDescriptors.rows, queryDescriptors.cols );
370

371 372 373 374 375
  /* compose matches */
  int index = 0;
  for ( int counter = 0; counter < queryDescriptors.rows; counter++ )
  {
    /* create a void vector of matches */
376
    std::vector < DMatch > tempVector;
377 378 379

    /* loop over k results returned for every query */
    for ( int j = index; j < index + k; j++ )
380
    {
381 382 383 384 385 386 387 388 389 390
      /* retrieve which image returned index refers to */
      int currentIndex = results[j] - 1;
      std::map<int, int>::iterator itup;
      itup = indexesMap.upper_bound( currentIndex );
      itup--;

      /* data validity check */
      if( !masks.empty() && ( masks[itup->second].rows != queryDescriptors.rows || masks[itup->second].cols != 1 ) )
      {
        std::cout << "Error: mask " << itup->second << " in knnMatch function " << "should have " << queryDescriptors.rows << " and "
391
            << "1 column. Program will be terminated" << std::endl;
392

393 394 395 396 397
        return;
      }

      /* decide if, according to relative mask, returned match should be
       considered */
398
      else if( masks.size() == 0 || masks[itup->second].at < uchar > ( counter ) != 0 )
399 400 401
      {
        std::vector<int> k_distances;
        checkKDistances( numres, k, k_distances, counter, 256 );
402

403 404 405 406
        DMatch dm;
        dm.queryIdx = counter;
        dm.trainIdx = results[j] - 1;
        dm.imgIdx = itup->second;
407
        dm.distance = (float) k_distances[j - index];
408

409 410
        tempVector.push_back( dm );
      }
411 412
    }

413 414 415 416 417 418 419 420 421
    /* decide whether temporary vector should be saved */
    if( ( tempVector.size() == 0 && !compactResult ) || tempVector.size() > 0 )
      matches.push_back( tempVector );

    /* increment pointer */
    index += k;
  }

  /* delete data */
422 423
  delete[] results;
  delete[] numres;
424 425 426
}

/* for every input desciptor, find all the ones falling in a
427 428 429
 certaing matching radius (for a pair of images) */
void BinaryDescriptorMatcher::radiusMatch( const Mat& queryDescriptors, const Mat& trainDescriptors, std::vector<std::vector<DMatch> >& matches,
                                           float maxDistance, const Mat& mask, bool compactResult ) const
430 431 432

{

433
  /* check data validity */
434 435 436 437 438 439
  if( queryDescriptors.rows == 0 || trainDescriptors.rows == 0 )
  {
    std::cout << "Error: descriptors matrices cannot be void" << std::endl;
    return;
  }

440 441 442
  if( !mask.empty() && ( mask.rows != queryDescriptors.rows && mask.cols != 1 ) )
  {
    std::cout << "Error: input mask should have " << queryDescriptors.rows << " rows and 1 column. " << "Program will be terminated" << std::endl;
443

444 445
    return;
  }
446

447 448
  /* create a new Mihasher */
  Mihasher* mh = new Mihasher( 256, 32 );
449

450
  /* populate Mihasher */
451 452
  //Mat copy = queryDescriptors.clone();
  Mat copy = trainDescriptors.clone();
453
  mh->populate( copy, copy.rows, copy.cols );
454

455 456
  /* set K */
  mh->setK( trainDescriptors.rows );
457

458 459 460
  /* prepare structures for query */
  UINT32 *results = new UINT32[trainDescriptors.rows * queryDescriptors.rows];
  UINT32 * numres = new UINT32[ ( 256 + 1 ) * ( queryDescriptors.rows )];
461

462 463
  /* execute query */
  mh->batchquery( results, numres, queryDescriptors, queryDescriptors.rows, queryDescriptors.cols );
464

465 466 467 468 469 470
  /* compose matches */
  int index = 0;
  for ( int i = 0; i < queryDescriptors.rows; i++ )
  {
    std::vector<int> k_distances;
    checkKDistances( numres, trainDescriptors.rows, k_distances, i, 256 );
471

472
    std::vector < DMatch > tempVector;
473
    for ( int j = index; j < index + trainDescriptors.rows; j++ )
474
    {
475 476
//      if( numres[j] <= maxDistance )
      if( k_distances[j - index] <= maxDistance )
477
      {
478
        if( mask.empty() || mask.at < uchar > ( i ) != 0 )
479
        {
480 481
          DMatch dm;
          dm.queryIdx = i;
482
          dm.trainIdx = (int) ( results[j] - 1 );
483
          dm.imgIdx = 0;
484
          dm.distance = (float) k_distances[j - index];
485 486

          tempVector.push_back( dm );
487
        }
488 489
      }
    }
490

491 492 493
    /* decide whether temporary vector should be saved */
    if( ( tempVector.size() == 0 && !compactResult ) || tempVector.size() > 0 )
      matches.push_back( tempVector );
494

495 496
    /* increment pointer */
    index += trainDescriptors.rows;
497

498
  }
499

500 501
  /* delete data */
  delete mh;
502 503
  delete[] results;
  delete[] numres;
504 505
}

506 507
/* for every input descriptor, find all the ones falling in a
 certain matching radius (from one image to a set) */
508 509
void BinaryDescriptorMatcher::radiusMatch( const Mat& queryDescriptors, std::vector<std::vector<DMatch> >& matches, float maxDistance,
                                           const std::vector<Mat>& masks, bool compactResult )
510 511
{

512
  /* check data validity */
513 514 515 516 517 518
  if( queryDescriptors.rows == 0 )
  {
    std::cout << "Error: descriptors matrices cannot be void" << std::endl;
    return;
  }

519 520 521
  if( masks.size() != 0 && (int) masks.size() != numImages )
  {
    std::cout << "Error: the number of images in dataset is " << numImages << " but radiusMatch function received " << masks.size()
522
        << " masks. Program will be terminated" << std::endl;
523

524 525
    return;
  }
526

527 528
  /* populate dataset */
  train();
529

530 531
  /* set K */
  dataset->setK( descrInDS );
532

533 534 535
  /* prepare structures for query */
  UINT32 *results = new UINT32[descrInDS * queryDescriptors.rows];
  UINT32 * numres = new UINT32[ ( 256 + 1 ) * ( queryDescriptors.rows )];
536

537 538
  /* execute query */
  dataset->batchquery( results, numres, queryDescriptors, queryDescriptors.rows, queryDescriptors.cols );
539

540 541 542 543
  /* compose matches */
  int index = 0;
  for ( int counter = 0; counter < queryDescriptors.rows; counter++ )
  {
544
    std::vector < DMatch > tempVector;
545
    for ( int j = index; j < index + descrInDS; j++ )
546
    {
547 548 549 550 551 552 553 554 555 556 557 558
      std::vector<int> k_distances;
      checkKDistances( numres, descrInDS, k_distances, counter, 256 );

      if( k_distances[j - index] <= maxDistance )
      {
        int currentIndex = results[j] - 1;
        std::map<int, int>::iterator itup;
        itup = indexesMap.upper_bound( currentIndex );
        itup--;

        /* data validity check */
        if( !masks.empty() && ( masks[itup->second].rows != queryDescriptors.rows || masks[itup->second].cols != 1 ) )
559
        {
560
          std::cout << "Error: mask " << itup->second << " in radiusMatch function " << "should have " << queryDescriptors.rows << " and "
561
              << "1 column. Program will be terminated" << std::endl;
562 563

          return;
564 565
        }

566
        /* add match if necessary */
567
        else if( masks.empty() || masks[itup->second].at < uchar > ( counter ) != 0 )
568 569 570 571 572 573
        {

          DMatch dm;
          dm.queryIdx = counter;
          dm.trainIdx = results[j] - 1;
          dm.imgIdx = itup->second;
574
          dm.distance = (float) k_distances[j - index];
575

576 577 578
          tempVector.push_back( dm );
        }
      }
579 580
    }

581 582 583 584 585 586 587 588 589
    /* decide whether temporary vector should be saved */
    if( ( tempVector.size() == 0 && !compactResult ) || tempVector.size() > 0 )
      matches.push_back( tempVector );

    /* increment pointer */
    index += descrInDS;
  }

  /* delete data */
590 591
  delete[] results;
  delete[] numres;
592 593

}
594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755

/* execute a batch query */
void BinaryDescriptorMatcher::Mihasher::batchquery( UINT32 * results, UINT32 *numres, const cv::Mat & queries, UINT32 numq, int dim1queries )
{
  /* create and initialize a bitarray */
  counter = new bitarray;
  counter->init( N );

  UINT32 *res = new UINT32[K * ( D + 1 )];
  UINT64 *chunks = new UINT64[m];
  UINT32 * presults = results;
  UINT32 *pnumres = numres;

  /* make a copy of input queries */
  cv::Mat queries_clone = queries.clone();

  /* set a pointer to first query (row) */
  UINT8 *pq = queries_clone.ptr();

  /* loop over number of descriptors */
  for ( size_t i = 0; i < numq; i++ )
  {
    /* for every descriptor, query database */
    query( presults, pnumres, pq, chunks, res );

    /* move pointer to write next K indeces */
    presults += K;
    pnumres += B + 1;

    /* move forward pointer to current row in descriptors matrix */
    pq += dim1queries;

  }

  delete[] res;
  delete[] chunks;

  delete counter;
}

/* execute a single query */
void BinaryDescriptorMatcher::Mihasher::query( UINT32* results, UINT32* numres, UINT8 * Query, UINT64 *chunks, UINT32 *res )
{
  /* if K == 0 that means we want everything to be processed.
   So maxres = N in that case. Otherwise K limits the results processed */
  UINT32 maxres = K ? K : (UINT32) N;

  /* number of results so far obtained (up to a distance of s per chunk) */
  UINT32 n = 0;

  /* number of candidates tested with full codes (not counting duplicates) */
  UINT32 nc = 0;

  /* counting everything retrieved (duplicates are counted multiple times)
   number of lookups (and xors) */
  UINT32 nl = 0;

  UINT32 nd = 0;
  UINT32 *arr;
  int size = 0;
  UINT32 index;
  int hammd;

  counter->erase();
  memset( numres, 0, ( B + 1 ) * sizeof ( *numres ) );

  split( chunks, Query, m, mplus, b );

  /* the growing search radius per substring */
  int s;

  /* current b: for the first mplus substrings it is b, for the rest it is (b-1) */
  int curb = b;

  for ( s = 0; s <= d && n < maxres; s++ )
  {
    for ( int k = 0; k < m; k++ )
    {
      if( k < mplus )
        curb = b;
      else
        curb = b - 1;
      UINT64 chunksk = chunks[k];
      /* number of bit-strings with s number of 1s */
      nl += xornum[s + 1] - xornum[s];

      /* the bit-string with s number of 1s */
      UINT64 bitstr = 0;
      for ( int i = 0; i < s; i++ )
        /* power[i] stores the location of the i'th 1 */
        power[i] = i;
      /* used for stopping criterion (location of (s+1)th 1) */
      power[s] = curb + 1;

      /* bit determines the 1 that should be moving to the left */
      int bit = s - 1;

      /* start from the left-most 1, and move it to the left until
       it touches another one */

      /* the loop for changing bitstr */
      bool infiniteWhile = true;
      while ( infiniteWhile )
      {
        if( bit != -1 )
        {
          bitstr ^= ( power[bit] == bit ) ? (UINT64) 1 << power[bit] : (UINT64) 3 << ( power[bit] - 1 );
          power[bit]++;
          bit--;
        }

        else
        { /* bit == -1 */
          /* the binary code bitstr is available for processing */
          arr = H[k].query( chunksk ^ bitstr, &size );  // lookup
          if( size )
          { /* the corresponding bucket is not empty */
            nd += size;
            for ( int c = 0; c < size; c++ )
            {
              index = arr[c];
              if( !counter->get( index ) )
              { /* if it is not a duplicate */
                counter->set( index );
                hammd = cv::line_descriptor::match( codes.ptr() + (UINT64) index * ( B_over_8 ), Query, B_over_8 );

                nc++;
                if( hammd <= D && numres[hammd] < maxres )
                  res[hammd * K + numres[hammd]] = index + 1;

                numres[hammd]++;
              }
            }
          }

          /* end of processing */
          while ( ++bit < s && power[bit] == power[bit + 1] - 1 )
          {
            bitstr ^= (UINT64) 1 << ( power[bit] - 1 );
            power[bit] = bit;
          }
          if( bit == s )
            break;
        }
      }

      n = n + numres[s * m + k];
      if( n >= maxres )
        break;
    }
  }

  n = 0;
  for ( s = 0; s <= D && (int) n < K; s++ )
  {
    for ( int c = 0; c < (int) numres[s] && (int) n < K; c++ )
      results[n++] = res[s * K + c];
  }

}

/* constructor 2 */
756
BinaryDescriptorMatcher::Mihasher::Mihasher( int B_val, int _m )
757
{
758
  B = B_val;
759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786
  B_over_8 = B / 8;
  m = _m;
  b = (int) ceil( (double) B / m );

  /* assuming that B/2 is large enough radius to include
   all of the k nearest neighbors */
  D = (int) ceil( B / 2.0 );
  d = (int) ceil( (double) D / m );

  /* mplus is the number of chunks with b bits
   (m-mplus) is the number of chunks with (b-1) bits */
  mplus = B - m * ( b - 1 );

  xornum = new UINT32[d + 2];
  xornum[0] = 0;
  for ( int i = 0; i <= d; i++ )
    xornum[i + 1] = xornum[i] + (UINT32) choose( b, i );

  H = new SparseHashtable[m];

  /* H[i].init might fail */
  for ( int i = 0; i < mplus; i++ )
    H[i].init( b );
  for ( int i = mplus; i < m; i++ )
    H[i].init( b - 1 );
}

/* K setter */
787
void BinaryDescriptorMatcher::Mihasher::setK( int K_val )
788
{
789
  K = K_val;
790 791 792 793 794 795 796 797 798 799
}

/* desctructor */
BinaryDescriptorMatcher::Mihasher::~Mihasher()
{
  delete[] xornum;
  delete[] H;
}

/* populate tables */
800
void BinaryDescriptorMatcher::Mihasher::populate( cv::Mat & _codes, UINT32 N_val, int dim1codes )
801
{
802
  N = N_val;
803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837
  codes = _codes;
  UINT64 * chunks = new UINT64[m];

  UINT8 * pcodes = codes.ptr();
  for ( UINT64 i = 0; i < N; i++, pcodes += dim1codes )
  {
    split( chunks, pcodes, m, mplus, b );

    for ( int k = 0; k < m; k++ )
      H[k].insert( chunks[k], (UINT32) i );

    if( i % (int) ceil( N / 1000.0 ) == 0 )
      fflush (stdout);
  }

  delete[] chunks;
}

/* constructor */
BinaryDescriptorMatcher::SparseHashtable::SparseHashtable()
{
  table = NULL;
  size = 0;
  b = 0;
}

/* initializer */
int BinaryDescriptorMatcher::SparseHashtable::init( int _b )
{
  b = _b;

  if( b < 5 || b > MAX_B || b > (int) ( sizeof(UINT64) * 8 ) )
    return 1;

  size = UINT64_1 << ( b - 5 );  // size = 2 ^ b
838
  table = (BucketGroup*) calloc( (size_t)size, sizeof(BucketGroup) );
839 840 841 842 843 844 845 846

  return 0;

}

/* destructor */
BinaryDescriptorMatcher::SparseHashtable::~SparseHashtable()
{
847
  free (table);
848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865
}

/* insert data */
void BinaryDescriptorMatcher::SparseHashtable::insert( UINT64 index, UINT32 data )
{
  table[index >> 5].insert( (int) ( index % 32 ), data );
}

/* query data */
UINT32* BinaryDescriptorMatcher::SparseHashtable::query( UINT64 index, int *Size )
{
  return table[index >> 5].query( (int) ( index % 32 ), Size );
}

/* constructor */
BinaryDescriptorMatcher::BucketGroup::BucketGroup()
{
  empty = 0;
866
  group = std::vector < uint32_t > ( 2, 0 );
867 868 869 870 871 872 873
}

/* destructor */
BinaryDescriptorMatcher::BucketGroup::~BucketGroup()
{
}

874
void BinaryDescriptorMatcher::BucketGroup::insert_value( std::vector<uint32_t>& vec, int index, UINT32 data )
875
{
876
  if( vec.size() > 1 )
877
  {
878 879 880 881 882
    if( vec[0] == vec[1] )
    {
      vec[1] = (UINT32) ceil( vec[0] * 1.1 );
      for ( int i = 0; i < (int) ( 2 + vec[1] - vec.size() ); i++ )
        vec.push_back( 0 );
883

884
    }
885

886 887 888
    vec.insert( vec.begin() + 2 + index, data );
    vec[2 + index] = data;
    vec[0]++;
889 890 891 892
  }

  else
  {
893 894 895 896
    vec = std::vector < uint32_t > ( 3, 0 );
    vec[0] = 1;
    vec[1] = 1;
    vec[2] = data;
897 898 899
  }
}

900
void BinaryDescriptorMatcher::BucketGroup::push_value( std::vector<uint32_t>& vec, UINT32 Data )
901
{
902
  if( vec.size() > 0 )
903
  {
904
    if( vec[0] == vec[1] )
905
    {
906 907 908
      vec[1] = (UINT32) std::max( ceil( vec[1] * ARRAY_RESIZE_FACTOR ), vec[1] + ARRAY_RESIZE_ADD_FACTOR );
      for ( int i = 0; i < (int) ( 2 + vec[1] - vec.size() ); i++ )
        vec.push_back( 0 );
909 910
    }

911 912
    vec[2 + vec[0]] = Data;
    vec[0]++;
913 914 915 916 917

  }

  else
  {
918
    vec = std::vector < uint32_t > ( 2 + (uint32_t) ARRAY_RESIZE_ADD_FACTOR, 0 );
919 920 921
    vec[0] = 1;
    vec[1] = 1;
    vec[2] = Data;
922 923 924
  }
}

925 926
/* insert data into the bucket */
void BinaryDescriptorMatcher::BucketGroup::insert( int subindex, UINT32 data )
927
{
928
  if( group.size() == 0 )
929
  {
930
    push_value( group, 0 );
931 932
  }

933 934 935 936
  UINT32 lowerbits = ( (UINT32) 1 << subindex ) - 1;
  int end = popcnt( empty & lowerbits );

  if( ! ( empty & ( (UINT32) 1 << subindex ) ) )
937
  {
938 939
    insert_value( group, end, group[end + 2] );
    empty |= (UINT32) 1 << subindex;
940 941
  }

942 943
  int totones = popcnt( empty );
  insert_value( group, totones + 1 + group[2 + end + 1], data );
944

945 946
  for ( int i = end + 1; i < totones + 1; i++ )
    group[2 + i]++;
947 948
}

949 950
/* perform a query to the bucket */
UINT32* BinaryDescriptorMatcher::BucketGroup::query( int subindex, int *size )
951
{
952 953 954 955 956
  if( empty & ( (UINT32) 1 << subindex ) )
  {
    UINT32 lowerbits = ( (UINT32) 1 << subindex ) - 1;
    int end = popcnt( empty & lowerbits );
    int totones = popcnt( empty );
957

958 959 960
    *size = group[2 + end + 1] - group[2 + end];
    return & ( * ( group.begin() + 2 + totones + 1 + (int) group[2 + end] ) );
  }
961

962
  else
963
  {
964 965
    *size = 0;
    return NULL;
966
  }
967
}
968

969
}
970 971
}