modules/legacy/src/calonder.cpp

   1 //*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42 #include "precomp.hpp"
  43 #include <cstdio>
  44 #include <iostream>
  45 #include <fstream>
  46
  47 class CSMatrixGenerator {
  48 public:
  49    typedef enum { PDT_GAUSS=1, PDT_BERNOULLI, PDT_DBFRIENDLY } PHI_DISTR_TYPE;
  50    ~CSMatrixGenerator();
  51    static float* getCSMatrix(int m, int n, PHI_DISTR_TYPE dt);     // do NOT free returned pointer
  52
  53
  54 private:
  55    static float *cs_phi_;    // matrix for compressive sensing
  56    static int cs_phi_m_, cs_phi_n_;
  57 };
  58
  59 float* CSMatrixGenerator::getCSMatrix(int m, int n, PHI_DISTR_TYPE dt)
  60 {
  61    assert(m <= n);
  62
  63    if (cs_phi_m_!=m || cs_phi_n_!=n || cs_phi_==NULL) {
  64       if (cs_phi_) delete [] cs_phi_;
  65       cs_phi_ = new float[m*n];
  66    }
  67
  68    #if 0 // debug - load the random matrix from a file (for reproducability of results)
  69       //assert(m == 176);
  70       //assert(n == 500);
  71       //const char *phi = "/u/calonder/temp/dim_red/kpca_phi.txt";
  72       const char *phi = "/u/calonder/temp/dim_red/debug_phi.txt";
  73       std::ifstream ifs(phi);
  74       for (size_t i=0; i<m*n; ++i) {
  75          if (!ifs.good()) {
  76             printf("[ERROR] RandomizedTree::makeRandomMeasMatrix: problem reading '%s'\n", phi);
  77             exit(0);
  78          }
  79          ifs >> cs_phi[i];
  80       }
  81       ifs.close();
  82
  83       static bool warned=false;
  84       if (!warned) {
  85          printf("[NOTE] RT: reading %ix%i PHI matrix from '%s'...\n", m, n, phi);
  86          warned=true;
  87       }
  88
  89       return;
  90    #endif
  91
  92    float *cs_phi = cs_phi_;
  93
  94    if (m == n) {
  95       // special case - set to 0 for safety
  96       memset(cs_phi, 0, m*n*sizeof(float));
  97       printf("[WARNING] %s:%i: square CS matrix (-> no reduction)\n", __FILE__, __LINE__);
  98    }
  99    else {
 100        cv::RNG rng(23);
 101
 102       // par is distr param, cf 'Favorable JL Distributions' (Baraniuk et al, 2006)
 103       if (dt == PDT_GAUSS) {
 104          float par = (float)(1./m);
 105          for (int i=0; i<m*n; ++i)
 106             *cs_phi++ = (float)rng.gaussian(par);
 107       }
 108       else if (dt == PDT_BERNOULLI) {
 109          float par = (float)(1./sqrt((float)m));
 110          for (int i=0; i<m*n; ++i)
 111             *cs_phi++ = (rng(2)==0 ? par : -par);
 112       }
 113       else if (dt == PDT_DBFRIENDLY) {
 114          float par = (float)sqrt(3./m);
 115          for (int i=0; i<m*n; ++i) {
 116             int r = rng(6);
 117             *cs_phi++ = (r==0 ? par : (r==1 ? -par : 0.f));
 118          }
 119       }
 120       else
 121          throw("PHI_DISTR_TYPE not implemented.");
 122    }
 123
 124    return cs_phi_;
 125 }
 126
 127 CSMatrixGenerator::~CSMatrixGenerator()
 128 {
 129    if (cs_phi_) delete [] cs_phi_;
 130    cs_phi_ = NULL;
 131 }
 132
 133 float *CSMatrixGenerator::cs_phi_   = NULL;
 134 int    CSMatrixGenerator::cs_phi_m_ = 0;
 135 int    CSMatrixGenerator::cs_phi_n_ = 0;
 136
 137
 138 inline void addVec(int size, const float* src1, const float* src2, float* dst)
 139 {
 140   while(--size >= 0) {
 141     *dst = *src1 + *src2;
 142     ++dst; ++src1; ++src2;
 143   }
 144 }
 145
 146
 147 // sum up 50 byte vectors of length 176
 148 // assume 4 bits max for input vector values
 149 // final shift is 2 bits right
 150 // temp buffer should be twice as long as signature
 151 // sig and buffer need not be initialized
 152 inline void sum_50t_176c(uchar **pp, uchar *sig, unsigned short *temp)
 153 {
 154 #if CV_SSE2
 155   __m128i acc, *acc1, *acc2, *acc3, *acc4, tzero;
 156   __m128i *ssig, *ttemp;
 157
 158   ssig = (__m128i *)sig;
 159   ttemp = (__m128i *)temp;
 160
 161   // empty ttemp[]
 162   tzero = _mm_set_epi32(0, 0, 0, 0);
 163   for (int i=0; i<22; i++)
 164     ttemp[i] = tzero;
 165
 166   for (int j=0; j<48; j+=16)
 167     {
 168       // empty ssig[]
 169       for (int i=0; i<11; i++)
 170     ssig[i] = tzero;
 171
 172       for (int i=j; i<j+16; i+=4) // 4 columns at a time, to 16
 173     {
 174       acc1 = (__m128i *)pp[i];
 175       acc2 = (__m128i *)pp[i+1];
 176       acc3 = (__m128i *)pp[i+2];
 177       acc4 = (__m128i *)pp[i+3];
 178
 179       // add next four columns
 180       acc = _mm_adds_epu8(acc1[0],acc2[0]);
 181       acc = _mm_adds_epu8(acc,acc3[0]);
 182       acc = _mm_adds_epu8(acc,acc4[1]);
 183       ssig[0] = _mm_adds_epu8(acc,ssig[0]);
 184       // add four columns
 185       acc = _mm_adds_epu8(acc1[1],acc2[1]);
 186       acc = _mm_adds_epu8(acc,acc3[1]);
 187       acc = _mm_adds_epu8(acc,acc4[1]);
 188       ssig[1] = _mm_adds_epu8(acc,ssig[1]);
 189       // add four columns
 190       acc = _mm_adds_epu8(acc1[2],acc2[2]);
 191       acc = _mm_adds_epu8(acc,acc3[2]);
 192       acc = _mm_adds_epu8(acc,acc4[2]);
 193       ssig[2] = _mm_adds_epu8(acc,ssig[2]);
 194       // add four columns
 195       acc = _mm_adds_epu8(acc1[3],acc2[3]);
 196       acc = _mm_adds_epu8(acc,acc3[3]);
 197       acc = _mm_adds_epu8(acc,acc4[3]);
 198       ssig[3] = _mm_adds_epu8(acc,ssig[3]);
 199       // add four columns
 200       acc = _mm_adds_epu8(acc1[4],acc2[4]);
 201       acc = _mm_adds_epu8(acc,acc3[4]);
 202       acc = _mm_adds_epu8(acc,acc4[4]);
 203       ssig[4] = _mm_adds_epu8(acc,ssig[4]);
 204       // add four columns
 205       acc = _mm_adds_epu8(acc1[5],acc2[5]);
 206       acc = _mm_adds_epu8(acc,acc3[5]);
 207       acc = _mm_adds_epu8(acc,acc4[5]);
 208       ssig[5] = _mm_adds_epu8(acc,ssig[5]);
 209       // add four columns
 210       acc = _mm_adds_epu8(acc1[6],acc2[6]);
 211       acc = _mm_adds_epu8(acc,acc3[6]);
 212       acc = _mm_adds_epu8(acc,acc4[6]);
 213       ssig[6] = _mm_adds_epu8(acc,ssig[6]);
 214       // add four columns
 215       acc = _mm_adds_epu8(acc1[7],acc2[7]);
 216       acc = _mm_adds_epu8(acc,acc3[7]);
 217       acc = _mm_adds_epu8(acc,acc4[7]);
 218       ssig[7] = _mm_adds_epu8(acc,ssig[7]);
 219       // add four columns
 220       acc = _mm_adds_epu8(acc1[8],acc2[8]);
 221       acc = _mm_adds_epu8(acc,acc3[8]);
 222       acc = _mm_adds_epu8(acc,acc4[8]);
 223       ssig[8] = _mm_adds_epu8(acc,ssig[8]);
 224       // add four columns
 225       acc = _mm_adds_epu8(acc1[9],acc2[9]);
 226       acc = _mm_adds_epu8(acc,acc3[9]);
 227       acc = _mm_adds_epu8(acc,acc4[9]);
 228       ssig[9] = _mm_adds_epu8(acc,ssig[9]);
 229       // add four columns
 230       acc = _mm_adds_epu8(acc1[10],acc2[10]);
 231       acc = _mm_adds_epu8(acc,acc3[10]);
 232       acc = _mm_adds_epu8(acc,acc4[10]);
 233       ssig[10] = _mm_adds_epu8(acc,ssig[10]);
 234     }
 235
 236       // unpack to ttemp buffer and add
 237       ttemp[0] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[0],tzero),ttemp[0]);
 238       ttemp[1] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[0],tzero),ttemp[1]);
 239       ttemp[2] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[1],tzero),ttemp[2]);
 240       ttemp[3] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[1],tzero),ttemp[3]);
 241       ttemp[4] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[2],tzero),ttemp[4]);
 242       ttemp[5] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[2],tzero),ttemp[5]);
 243       ttemp[6] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[3],tzero),ttemp[6]);
 244       ttemp[7] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[3],tzero),ttemp[7]);
 245       ttemp[8] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[4],tzero),ttemp[8]);
 246       ttemp[9] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[4],tzero),ttemp[9]);
 247       ttemp[10] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[5],tzero),ttemp[10]);
 248       ttemp[11] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[5],tzero),ttemp[11]);
 249       ttemp[12] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[6],tzero),ttemp[12]);
 250       ttemp[13] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[6],tzero),ttemp[13]);
 251       ttemp[14] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[7],tzero),ttemp[14]);
 252       ttemp[15] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[7],tzero),ttemp[15]);
 253       ttemp[16] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[8],tzero),ttemp[16]);
 254       ttemp[17] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[8],tzero),ttemp[17]);
 255       ttemp[18] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[9],tzero),ttemp[18]);
 256       ttemp[19] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[9],tzero),ttemp[19]);
 257       ttemp[20] = _mm_add_epi16(_mm_unpacklo_epi8(ssig[10],tzero),ttemp[20]);
 258       ttemp[21] = _mm_add_epi16(_mm_unpackhi_epi8(ssig[10],tzero),ttemp[21]);
 259     }
 260
 261   // create ssignature from 16-bit result
 262   ssig[0] =_mm_packus_epi16(_mm_srai_epi16(ttemp[0],2),_mm_srai_epi16(ttemp[1],2));
 263   ssig[1] =_mm_packus_epi16(_mm_srai_epi16(ttemp[2],2),_mm_srai_epi16(ttemp[3],2));
 264   ssig[2] =_mm_packus_epi16(_mm_srai_epi16(ttemp[4],2),_mm_srai_epi16(ttemp[5],2));
 265   ssig[3] =_mm_packus_epi16(_mm_srai_epi16(ttemp[6],2),_mm_srai_epi16(ttemp[7],2));
 266   ssig[4] =_mm_packus_epi16(_mm_srai_epi16(ttemp[8],2),_mm_srai_epi16(ttemp[9],2));
 267   ssig[5] =_mm_packus_epi16(_mm_srai_epi16(ttemp[10],2),_mm_srai_epi16(ttemp[11],2));
 268   ssig[6] =_mm_packus_epi16(_mm_srai_epi16(ttemp[12],2),_mm_srai_epi16(ttemp[13],2));
 269   ssig[7] =_mm_packus_epi16(_mm_srai_epi16(ttemp[14],2),_mm_srai_epi16(ttemp[15],2));
 270   ssig[8] =_mm_packus_epi16(_mm_srai_epi16(ttemp[16],2),_mm_srai_epi16(ttemp[17],2));
 271   ssig[9] =_mm_packus_epi16(_mm_srai_epi16(ttemp[18],2),_mm_srai_epi16(ttemp[19],2));
 272   ssig[10] =_mm_packus_epi16(_mm_srai_epi16(ttemp[20],2),_mm_srai_epi16(ttemp[21],2));
 273 #else
 274   (void)pp;
 275   (void)sig;
 276   (void)temp;
 277   CV_Error( CV_StsNotImplemented, "Not supported without SSE2" );
 278 #endif
 279 }
 280
 281 namespace cv
 282 {
 283 RandomizedTree::RandomizedTree()
 284   : posteriors_(NULL), posteriors2_(NULL)
 285 {
 286 }
 287
 288 RandomizedTree::~RandomizedTree()
 289 {
 290    freePosteriors(3);
 291 }
 292
 293 void RandomizedTree::createNodes(int num_nodes, RNG &rng)
 294 {
 295   nodes_.reserve(num_nodes);
 296   for (int i = 0; i < num_nodes; ++i) {
 297     nodes_.push_back( RTreeNode((uchar)rng(RandomizedTree::PATCH_SIZE),
 298                                 (uchar)rng(RandomizedTree::PATCH_SIZE),
 299                                 (uchar)rng(RandomizedTree::PATCH_SIZE),
 300                                 (uchar)rng(RandomizedTree::PATCH_SIZE)) );
 301   }
 302 }
 303
 304 int RandomizedTree::getIndex(uchar* patch_data) const
 305 {
 306   int index = 0;
 307   for (int d = 0; d < depth_; ++d) {
 308     int child_offset = nodes_[index](patch_data);
 309     index = 2*index + 1 + child_offset;
 310   }
 311   return (int)(index - nodes_.size());
 312 }
 313
 314 void RandomizedTree::train(std::vector<BaseKeypoint> const& base_set,
 315                            RNG &rng, int _depth, int views, size_t reduced_num_dim,
 316                            int num_quant_bits)
 317 {
 318   PatchGenerator make_patch;
 319   train(base_set, rng, make_patch, _depth, views, reduced_num_dim, num_quant_bits);
 320 }
 321
 322 void RandomizedTree::train(std::vector<BaseKeypoint> const& base_set,
 323                            RNG &rng, PatchGenerator &make_patch,
 324                            int _depth, int views, size_t reduced_num_dim,
 325                            int num_quant_bits)
 326 {
 327   init((int)base_set.size(), _depth, rng);
 328
 329   Mat patch;
 330
 331   // Estimate posterior probabilities using random affine views
 332   std::vector<BaseKeypoint>::const_iterator keypt_it;
 333   int class_id = 0;
 334   Size patchSize(PATCH_SIZE, PATCH_SIZE);
 335   for (keypt_it = base_set.begin(); keypt_it != base_set.end(); ++keypt_it, ++class_id) {
 336     for (int i = 0; i < views; ++i) {
 337       make_patch( cv::cvarrToMat(keypt_it->image), Point(keypt_it->x, keypt_it->y ), patch, patchSize, rng );
 338       IplImage iplPatch = patch;
 339       addExample(class_id, getData(&iplPatch));
 340     }
 341   }
 342
 343   finalize(reduced_num_dim, num_quant_bits);
 344 }
 345
 346 void RandomizedTree::allocPosteriorsAligned(int num_leaves, int num_classes)
 347 {
 348   freePosteriors(3);
 349
 350   posteriors_ = new float*[num_leaves]; //(float**) malloc(num_leaves*sizeof(float*));
 351   for (int i=0; i<num_leaves; ++i) {
 352     posteriors_[i] = (float*)cvAlloc(num_classes*sizeof(posteriors_[i][0]));
 353     memset(posteriors_[i], 0, num_classes*sizeof(float));
 354   }
 355
 356   posteriors2_ = new uchar*[num_leaves];
 357   for (int i=0; i<num_leaves; ++i) {
 358     posteriors2_[i] = (uchar*)cvAlloc(num_classes*sizeof(posteriors2_[i][0]));
 359     memset(posteriors2_[i], 0, num_classes*sizeof(uchar));
 360   }
 361
 362   classes_ = num_classes;
 363 }
 364
 365 void RandomizedTree::freePosteriors(int which)
 366 {
 367    if (posteriors_ && (which&1)) {
 368       for (int i=0; i<num_leaves_; ++i)
 369          if (posteriors_[i])
 370             cvFree( &posteriors_[i] );
 371       delete [] posteriors_;
 372       posteriors_ = NULL;
 373    }
 374
 375    if (posteriors2_ && (which&2)) {
 376       for (int i=0; i<num_leaves_; ++i)
 377          cvFree( &posteriors2_[i] );
 378       delete [] posteriors2_;
 379       posteriors2_ = NULL;
 380    }
 381
 382    classes_ = -1;
 383 }
 384
 385 void RandomizedTree::init(int num_classes, int _depth, RNG &rng)
 386 {
 387   depth_ = _depth;
 388   num_leaves_ = 1 << _depth;       // 2**d
 389   int num_nodes = num_leaves_ - 1; // 2**d - 1
 390
 391   // Initialize probabilities and counts to 0
 392   allocPosteriorsAligned(num_leaves_, num_classes);      // will set classes_ correctly
 393   for (int i = 0; i < num_leaves_; ++i)
 394     memset((void*)posteriors_[i], 0, num_classes*sizeof(float));
 395   leaf_counts_.resize(num_leaves_);
 396
 397   for (int i = 0; i < num_leaves_; ++i)
 398     memset((void*)posteriors2_[i], 0, num_classes*sizeof(uchar));
 399
 400   createNodes(num_nodes, rng);
 401 }
 402
 403 void RandomizedTree::addExample(int class_id, uchar* patch_data)
 404 {
 405   int index = getIndex(patch_data);
 406   float* posterior = getPosteriorByIndex(index);
 407   ++leaf_counts_[index];
 408   ++posterior[class_id];
 409 }
 410
 411 // returns the p% percentile of data (length n vector)
 412 static float percentile(float *data, int n, float p)
 413 {
 414    assert(n>0);
 415    assert(p>=0 && p<=1);
 416    std::vector<float> vec(data, data+n);
 417    std::sort(vec.begin(), vec.end());
 418    int ix = (int)(p*(n-1));
 419    return vec[ix];
 420 }
 421
 422 void RandomizedTree::finalize(size_t reduced_num_dim, int num_quant_bits)
 423 {
 424    // Normalize by number of patches to reach each leaf
 425    for (int index = 0; index < num_leaves_; ++index) {
 426       float* posterior = posteriors_[index];
 427       assert(posterior != NULL);
 428       int count = leaf_counts_[index];
 429       if (count != 0) {
 430          float normalizer = 1.0f / count;
 431          for (int c = 0; c < classes_; ++c) {
 432             *posterior *= normalizer;
 433             ++posterior;
 434          }
 435       }
 436    }
 437    leaf_counts_.clear();
 438
 439    // apply compressive sensing
 440    if ((int)reduced_num_dim != classes_)
 441       compressLeaves(reduced_num_dim);
 442    else {
 443       static bool notified = false;
 444       if (!notified)
 445          printf("\n[OK] NO compression to leaves applied, dim=%i\n", (int)reduced_num_dim);
 446       notified = true;
 447    }
 448
 449    // convert float-posteriors to char-posteriors (quantization step)
 450    makePosteriors2(num_quant_bits);
 451 }
 452
 453 void RandomizedTree::compressLeaves(size_t reduced_num_dim)
 454 {
 455    static bool warned = false;
 456    if (!warned) {
 457       printf("\n[OK] compressing leaves with phi %i x %i\n", (int)reduced_num_dim, (int)classes_);
 458       warned = true;
 459    }
 460
 461    static bool warned2 = false;
 462    if ((int)reduced_num_dim == classes_) {
 463      if (!warned2)
 464        printf("[WARNING] RandomizedTree::compressLeaves:  not compressing because reduced_dim == classes()\n");
 465      warned2 = true;
 466      return;
 467    }
 468
 469    // DO NOT FREE RETURNED POINTER
 470    float *cs_phi = CSMatrixGenerator::getCSMatrix((int)reduced_num_dim, classes_, CSMatrixGenerator::PDT_BERNOULLI);
 471
 472    float *cs_posteriors = new float[num_leaves_ * reduced_num_dim];         // temp, num_leaves_ x reduced_num_dim
 473    for (int i=0; i<num_leaves_; ++i) {
 474       float *post = getPosteriorByIndex(i);
 475       float *prod = &cs_posteriors[i*reduced_num_dim];
 476       Mat A( (int)reduced_num_dim, classes_, CV_32FC1, cs_phi );
 477       Mat X( classes_, 1, CV_32FC1, post );
 478       Mat Y( (int)reduced_num_dim, 1, CV_32FC1, prod );
 479       Y = A*X;
 480    }
 481
 482    // copy new posteriors
 483    freePosteriors(3);
 484    allocPosteriorsAligned(num_leaves_, (int)reduced_num_dim);
 485    for (int i=0; i<num_leaves_; ++i)
 486       memcpy(posteriors_[i], &cs_posteriors[i*reduced_num_dim], reduced_num_dim*sizeof(float));
 487    classes_ = (int)reduced_num_dim;
 488
 489    delete [] cs_posteriors;
 490 }
 491
 492 void RandomizedTree::makePosteriors2(int num_quant_bits)
 493 {
 494    int N = (1<<num_quant_bits) - 1;
 495
 496    float perc[2];
 497    estimateQuantPercForPosteriors(perc);
 498
 499    assert(posteriors_ != NULL);
 500    for (int i=0; i<num_leaves_; ++i)
 501       quantizeVector(posteriors_[i], classes_, N, perc, posteriors2_[i]);
 502
 503    // printf("makePosteriors2 quantization bounds: %.3e, %.3e (num_leaves=%i, N=%i)\n",
 504    //        perc[0], perc[1], num_leaves_, N);
 505 }
 506
 507 void RandomizedTree::estimateQuantPercForPosteriors(float perc[2])
 508 {
 509    // _estimate_ percentiles for this tree
 510    // TODO: do this more accurately
 511    assert(posteriors_ != NULL);
 512    perc[0] = perc[1] = .0f;
 513    for (int i=0; i<num_leaves_; i++) {
 514       perc[0] += percentile(posteriors_[i], classes_, GET_LOWER_QUANT_PERC());
 515       perc[1] += percentile(posteriors_[i], classes_, GET_UPPER_QUANT_PERC());
 516    }
 517    perc[0] /= num_leaves_;
 518    perc[1] /= num_leaves_;
 519 }
 520
 521
 522 float* RandomizedTree::getPosterior(uchar* patch_data)
 523 {
 524   return const_cast<float*>(const_cast<const RandomizedTree*>(this)->getPosterior(patch_data));
 525 }
 526
 527 const float* RandomizedTree::getPosterior(uchar* patch_data) const
 528 {
 529   return getPosteriorByIndex( getIndex(patch_data) );
 530 }
 531
 532 uchar* RandomizedTree::getPosterior2(uchar* patch_data)
 533 {
 534   return const_cast<uchar*>(const_cast<const RandomizedTree*>(this)->getPosterior2(patch_data));
 535 }
 536
 537 const uchar* RandomizedTree::getPosterior2(uchar* patch_data) const
 538 {
 539   return getPosteriorByIndex2( getIndex(patch_data) );
 540 }
 541
 542 void RandomizedTree::quantizeVector(float *vec, int dim, int N, float bnds[2], int clamp_mode)
 543 {
 544    float map_bnd[2] = {0.f,(float)N};          // bounds of quantized target interval we're mapping to
 545    for (int k=0; k<dim; ++k, ++vec) {
 546       *vec = float(int((*vec - bnds[0])/(bnds[1] - bnds[0])*(map_bnd[1] - map_bnd[0]) + map_bnd[0]));
 547       // 0: clamp both, lower and upper values
 548       if (clamp_mode == 0)      *vec = (*vec<map_bnd[0]) ? map_bnd[0] : ((*vec>map_bnd[1]) ? map_bnd[1] : *vec);
 549       // 1: clamp lower values only
 550       else if (clamp_mode == 1) *vec = (*vec<map_bnd[0]) ? map_bnd[0] : *vec;
 551       // 2: clamp upper values only
 552       else if (clamp_mode == 2) *vec = (*vec>map_bnd[1]) ? map_bnd[1] : *vec;
 553       // 4: no clamping
 554       else if (clamp_mode == 4) ; // yep, nothing
 555       else {
 556          printf("clamp_mode == %i is not valid (%s:%i).\n", clamp_mode, __FILE__, __LINE__);
 557          exit(1);
 558       }
 559    }
 560
 561 }
 562
 563 void RandomizedTree::quantizeVector(float *vec, int dim, int N, float bnds[2], uchar *dst)
 564 {
 565    int map_bnd[2] = {0, N};          // bounds of quantized target interval we're mapping to
 566    int tmp;
 567    for (int k=0; k<dim; ++k) {
 568       tmp = int((*vec - bnds[0])/(bnds[1] - bnds[0])*(map_bnd[1] - map_bnd[0]) + map_bnd[0]);
 569       *dst = (uchar)((tmp<0) ? 0 : ((tmp>N) ? N : tmp));
 570       ++vec;
 571       ++dst;
 572    }
 573 }
 574
 575
 576 void RandomizedTree::read(const char* file_name, int num_quant_bits)
 577 {
 578   std::ifstream file(file_name, std::ifstream::binary);
 579   read(file, num_quant_bits);
 580   file.close();
 581 }
 582
 583 void RandomizedTree::read(std::istream &is, int num_quant_bits)
 584 {
 585   is.read((char*)(&classes_), sizeof(classes_));
 586   is.read((char*)(&depth_), sizeof(depth_));
 587
 588   num_leaves_ = 1 << depth_;
 589   int num_nodes = num_leaves_ - 1;
 590
 591   nodes_.resize(num_nodes);
 592   is.read((char*)(&nodes_[0]), num_nodes * sizeof(nodes_[0]));
 593
 594   //posteriors_.resize(classes_ * num_leaves_);
 595   //freePosteriors(3);
 596   //printf("[DEBUG] reading: %i leaves, %i classes\n", num_leaves_, classes_);
 597   allocPosteriorsAligned(num_leaves_, classes_);
 598   for (int i=0; i<num_leaves_; i++)
 599     is.read((char*)posteriors_[i], classes_ * sizeof(*posteriors_[0]));
 600
 601   // make char-posteriors from float-posteriors
 602   makePosteriors2(num_quant_bits);
 603 }
 604
 605 void RandomizedTree::write(const char* file_name) const
 606 {
 607   std::ofstream file(file_name, std::ofstream::binary);
 608   write(file);
 609   file.close();
 610 }
 611
 612 void RandomizedTree::write(std::ostream &os) const
 613 {
 614   if (!posteriors_) {
 615     printf("WARNING: Cannot write float posteriors (posteriors_ = NULL).\n");
 616     return;
 617   }
 618
 619   os.write((char*)(&classes_), sizeof(classes_));
 620   os.write((char*)(&depth_), sizeof(depth_));
 621
 622   os.write((char*)(&nodes_[0]), (int)(nodes_.size() * sizeof(nodes_[0])));
 623   for (int i=0; i<num_leaves_; i++) {
 624     os.write((char*)posteriors_[i], classes_ * sizeof(*posteriors_[0]));
 625   }
 626 }
 627
 628
 629 void RandomizedTree::savePosteriors(String url, bool append)
 630 {
 631    std::ofstream file(url.c_str(), (append?std::ios::app:std::ios::out));
 632    for (int i=0; i<num_leaves_; i++) {
 633       float *post = posteriors_[i];
 634       char buf[20];
 635       for (int j=0; j<classes_; j++) {
 636          sprintf(buf, "%.10e", *post++);
 637          file << buf << ((j<classes_-1) ? " " : "");
 638       }
 639       file << std::endl;
 640    }
 641    file.close();
 642 }
 643
 644 void RandomizedTree::savePosteriors2(String url, bool append)
 645 {
 646    std::ofstream file(url.c_str(), (append?std::ios::app:std::ios::out));
 647    for (int i=0; i<num_leaves_; i++) {
 648       uchar *post = posteriors2_[i];
 649       for (int j=0; j<classes_; j++)
 650          file << int(*post++) << (j<classes_-1?" ":"");
 651       file << std::endl;
 652    }
 653    file.close();
 654 }
 655
 656 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 657
 658 RTreeClassifier::RTreeClassifier()
 659   : classes_(0)
 660 {
 661   posteriors_ = NULL;
 662 }
 663
 664 void RTreeClassifier::train(std::vector<BaseKeypoint> const& base_set,
 665                             RNG &rng, int num_trees, int depth,
 666                             int views, size_t reduced_num_dim,
 667                             int num_quant_bits)
 668 {
 669   PatchGenerator make_patch;
 670   train(base_set, rng, make_patch, num_trees, depth, views, reduced_num_dim, num_quant_bits);
 671 }
 672
 673 // Single-threaded version of train(), with progress output
 674 void RTreeClassifier::train(std::vector<BaseKeypoint> const& base_set,
 675                             RNG &rng, PatchGenerator &make_patch, int num_trees,
 676                             int depth, int views, size_t reduced_num_dim,
 677                             int num_quant_bits)
 678 {
 679   if (reduced_num_dim > base_set.size()) {
 680     printf("INVALID PARAMS in RTreeClassifier::train: reduced_num_dim{%i} > base_set.size(){%i}\n",
 681            (int)reduced_num_dim, (int)base_set.size());
 682     return;
 683   }
 684
 685   num_quant_bits_ = num_quant_bits;
 686   classes_ = (int)reduced_num_dim; // base_set.size();
 687   original_num_classes_ = (int)base_set.size();
 688   trees_.resize(num_trees);
 689
 690   printf("[OK] Training trees: base size=%i, reduced size=%i\n", (int)base_set.size(), (int)reduced_num_dim);
 691
 692   int count = 1;
 693   printf("[OK] Trained 0 / %i trees", num_trees);  fflush(stdout);
 694   for( int ti = 0; ti < num_trees; ti++ ) {
 695     trees_[ti].train(base_set, rng, make_patch, depth, views, reduced_num_dim, num_quant_bits_);
 696     printf("\r[OK] Trained %i / %i trees", count++, num_trees);
 697     fflush(stdout);
 698   }
 699
 700   printf("\n");
 701   countZeroElements();
 702   printf("\n\n");
 703 }
 704
 705 void RTreeClassifier::getSignature(IplImage* patch, float *sig) const
 706 {
 707   // Need pointer to 32x32 patch data
 708   uchar buffer[RandomizedTree::PATCH_SIZE * RandomizedTree::PATCH_SIZE];
 709   uchar* patch_data;
 710   if (patch->widthStep != RandomizedTree::PATCH_SIZE) {
 711     //printf("[INFO] patch is padded, data will be copied (%i/%i).\n",
 712     //       patch->widthStep, RandomizedTree::PATCH_SIZE);
 713     uchar* data = getData(patch);
 714     patch_data = buffer;
 715     for (int i = 0; i < RandomizedTree::PATCH_SIZE; ++i) {
 716       memcpy((void*)patch_data, (void*)data, RandomizedTree::PATCH_SIZE);
 717       data += patch->widthStep;
 718       patch_data += RandomizedTree::PATCH_SIZE;
 719     }
 720     patch_data = buffer;
 721   }
 722   else {
 723     patch_data = getData(patch);
 724   }
 725
 726   memset((void*)sig, 0, classes_ * sizeof(float));
 727   std::vector<RandomizedTree>::const_iterator tree_it;
 728
 729   // get posteriors
 730   float **posteriors = new float*[trees_.size()];  // TODO: move alloc outside this func
 731   float **pp = posteriors;
 732   for (tree_it = trees_.begin(); tree_it != trees_.end(); ++tree_it, pp++) {
 733     *pp = const_cast<float*>(tree_it->getPosterior(patch_data));
 734     assert(*pp != NULL);
 735   }
 736
 737   // sum them up
 738   pp = posteriors;
 739   for (tree_it = trees_.begin(); tree_it != trees_.end(); ++tree_it, pp++)
 740     addVec(classes_, sig, *pp, sig);
 741
 742   delete [] posteriors;
 743   posteriors = NULL;
 744
 745   // full quantization (experimental)
 746   #if 0
 747     int n_max = 1<<8 - 1;
 748     int sum_max = (1<<4 - 1)*trees_.size();
 749     int shift = 0;
 750     while ((sum_max>>shift) > n_max) shift++;
 751
 752     for (int i = 0; i < classes_; ++i) {
 753       sig[i] = int(sig[i] + .5) >> shift;
 754       if (sig[i]>n_max) sig[i] = n_max;
 755     }
 756
 757     static bool warned = false;
 758     if (!warned) {
 759       printf("[WARNING] Using full quantization (RTreeClassifier::getSignature)! shift=%i\n", shift);
 760       warned = true;
 761     }
 762   #else
 763     // TODO: get rid of this multiply (-> number of trees is known at train
 764     // time, exploit it in RandomizedTree::finalize())
 765     float normalizer = 1.0f / trees_.size();
 766     for (int i = 0; i < classes_; ++i)
 767       sig[i] *= normalizer;
 768   #endif
 769 }
 770
 771 void RTreeClassifier::getSignature(IplImage* patch, uchar *sig) const
 772 {
 773   // Need pointer to 32x32 patch data
 774   uchar buffer[RandomizedTree::PATCH_SIZE * RandomizedTree::PATCH_SIZE];
 775   uchar* patch_data;
 776   if (patch->widthStep != RandomizedTree::PATCH_SIZE) {
 777     //printf("[INFO] patch is padded, data will be copied (%i/%i).\n",
 778     //       patch->widthStep, RandomizedTree::PATCH_SIZE);
 779     uchar* data = getData(patch);
 780     patch_data = buffer;
 781     for (int i = 0; i < RandomizedTree::PATCH_SIZE; ++i) {
 782       memcpy((void*)patch_data, (void*)data, RandomizedTree::PATCH_SIZE);
 783       data += patch->widthStep;
 784       patch_data += RandomizedTree::PATCH_SIZE;
 785     }
 786     patch_data = buffer;
 787   } else {
 788     patch_data = getData(patch);
 789   }
 790
 791   std::vector<RandomizedTree>::const_iterator tree_it;
 792
 793   // get posteriors
 794   if (posteriors_ == NULL)
 795     {
 796       posteriors_ = (uchar**)cvAlloc( trees_.size()*sizeof(posteriors_[0]) );
 797       ptemp_ = (unsigned short*)cvAlloc( classes_*sizeof(ptemp_[0]) );
 798     }
 799   /// @todo What is going on in the next 4 lines?
 800   uchar **pp = posteriors_;
 801   for (tree_it = trees_.begin(); tree_it != trees_.end(); ++tree_it, pp++)
 802     *pp = const_cast<uchar*>(tree_it->getPosterior2(patch_data));
 803   pp = posteriors_;
 804
 805 #if 1
 806      // SSE2 optimized code
 807      sum_50t_176c(pp, sig, ptemp_);    // sum them up
 808 #else
 809      static bool warned = false;
 810
 811      memset((void*)sig, 0, classes_ * sizeof(sig[0]));
 812      unsigned short *sig16 = new unsigned short[classes_];           // TODO: make member, no alloc here
 813      memset((void*)sig16, 0, classes_ * sizeof(sig16[0]));
 814      for (tree_it = trees_.begin(); tree_it != trees_.end(); ++tree_it, pp++)
 815        addVec(classes_, sig16, *pp, sig16);
 816
 817      // squeeze signatures into an uchar
 818      const bool full_shifting = true;
 819      int shift;
 820      if (full_shifting) {
 821         float num_add_bits_f = log((float)trees_.size())/log(2.f);     // # additional bits required due to summation
 822         int num_add_bits = int(num_add_bits_f);
 823         if (num_add_bits_f != float(num_add_bits)) ++num_add_bits;
 824         shift = num_quant_bits_ + num_add_bits - 8*sizeof(uchar);
 825 //shift = num_quant_bits_ + num_add_bits - 2;
 826 //shift = 6;
 827         if (shift>0)
 828           for (int i = 0; i < classes_; ++i)
 829             sig[i] = (sig16[i] >> shift);      // &3 cut off all but lowest 2 bits, 3(dec) = 11(bin)
 830
 831         if (!warned)
 832            printf("[OK] RTC: quantizing by FULL RIGHT SHIFT, shift = %i\n", shift);
 833      }
 834      else {
 835         printf("[ERROR] RTC: not implemented!\n");
 836         exit(0);
 837      }
 838
 839      if (!warned)
 840         printf("[WARNING] RTC: unoptimized signature computation\n");
 841      warned = true;
 842 #endif
 843 }
 844
 845
 846 void RTreeClassifier::getSparseSignature(IplImage *patch, float *sig, float thresh) const
 847 {
 848    getFloatSignature(patch, sig);
 849    for (int i=0; i<classes_; ++i, sig++)
 850       if (*sig < thresh) *sig = 0.f;
 851 }
 852
 853 int RTreeClassifier::countNonZeroElements(float *vec, int n, double tol)
 854 {
 855    int res = 0;
 856    while (n-- > 0)
 857       res += (fabs(*vec++) > tol);
 858    return res;
 859 }
 860
 861 void RTreeClassifier::read(const char* file_name)
 862 {
 863   std::ifstream file(file_name, std::ifstream::binary);
 864   if( file.is_open() )
 865   {
 866       read(file);
 867       file.close();
 868   }
 869 }
 870
 871 void RTreeClassifier::read(std::istream &is)
 872 {
 873   int num_trees = 0;
 874   is.read((char*)(&num_trees), sizeof(num_trees));
 875   is.read((char*)(&classes_), sizeof(classes_));
 876   is.read((char*)(&original_num_classes_), sizeof(original_num_classes_));
 877   is.read((char*)(&num_quant_bits_), sizeof(num_quant_bits_));
 878
 879   if (num_quant_bits_<1 || num_quant_bits_>8) {
 880     printf("[WARNING] RTC: suspicious value num_quant_bits_=%i found; setting to %i.\n",
 881            num_quant_bits_, (int)DEFAULT_NUM_QUANT_BITS);
 882     num_quant_bits_ = DEFAULT_NUM_QUANT_BITS;
 883   }
 884
 885   trees_.resize(num_trees);
 886   std::vector<RandomizedTree>::iterator tree_it;
 887
 888   for (tree_it = trees_.begin(); tree_it != trees_.end(); ++tree_it) {
 889     tree_it->read(is, num_quant_bits_);
 890   }
 891
 892   printf("[OK] Loaded RTC, quantization=%i bits\n", num_quant_bits_);
 893
 894   countZeroElements();
 895 }
 896
 897 void RTreeClassifier::write(const char* file_name) const
 898 {
 899   std::ofstream file(file_name, std::ofstream::binary);
 900   write(file);
 901   file.close();
 902 }
 903
 904 void RTreeClassifier::write(std::ostream &os) const
 905 {
 906   int num_trees = (int)trees_.size();
 907   os.write((char*)(&num_trees), sizeof(num_trees));
 908   os.write((char*)(&classes_), sizeof(classes_));
 909   os.write((char*)(&original_num_classes_), sizeof(original_num_classes_));
 910   os.write((char*)(&num_quant_bits_), sizeof(num_quant_bits_));
 911 printf("RTreeClassifier::write: num_quant_bits_=%i\n", num_quant_bits_);
 912
 913   std::vector<RandomizedTree>::const_iterator tree_it;
 914   for (tree_it = trees_.begin(); tree_it != trees_.end(); ++tree_it)
 915     tree_it->write(os);
 916 }
 917
 918 void RTreeClassifier::saveAllFloatPosteriors(String url)
 919 {
 920   printf("[DEBUG] writing all float posteriors to %s...\n", url.c_str());
 921   for (int i=0; i<(int)trees_.size(); ++i)
 922     trees_[i].savePosteriors(url, (i==0 ? false : true));
 923   printf("[DEBUG] done\n");
 924 }
 925
 926 void RTreeClassifier::saveAllBytePosteriors(String url)
 927 {
 928   printf("[DEBUG] writing all byte posteriors to %s...\n", url.c_str());
 929   for (int i=0; i<(int)trees_.size(); ++i)
 930     trees_[i].savePosteriors2(url, (i==0 ? false : true));
 931   printf("[DEBUG] done\n");
 932 }
 933
 934
 935 void RTreeClassifier::setFloatPosteriorsFromTextfile_176(String url)
 936 {
 937    std::ifstream ifs(url.c_str());
 938
 939    for (int i=0; i<(int)trees_.size(); ++i) {
 940       int num_classes = trees_[i].classes_;
 941       assert(num_classes == 176);     // TODO: remove this limitation (arose due to SSE2 optimizations)
 942       for (int k=0; k<trees_[i].num_leaves_; ++k) {
 943          float *post = trees_[i].getPosteriorByIndex(k);
 944          for (int j=0; j<num_classes; ++j, ++post)
 945             ifs >> *post;
 946          assert(ifs.good());
 947       }
 948    }
 949    classes_ = 176;
 950
 951    //setQuantization(num_quant_bits_);
 952
 953    ifs.close();
 954    printf("[EXPERIMENTAL] read entire tree from '%s'\n", url.c_str());
 955 }
 956
 957
 958 float RTreeClassifier::countZeroElements()
 959 {
 960    size_t flt_zeros = 0;
 961    size_t ui8_zeros = 0;
 962    size_t num_elem = trees_[0].classes();
 963    for (int i=0; i<(int)trees_.size(); ++i)
 964       for (int k=0; k<(int)trees_[i].num_leaves_; ++k) {
 965          float *p = trees_[i].getPosteriorByIndex(k);
 966          uchar *p2 = trees_[i].getPosteriorByIndex2(k);
 967          assert(p); assert(p2);
 968          for (int j=0; j<(int)num_elem; ++j, ++p, ++p2) {
 969             if (*p == 0.f) flt_zeros++;
 970             if (*p2 == 0) ui8_zeros++;
 971          }
 972       }
 973    num_elem = trees_.size()*trees_[0].num_leaves_*num_elem;
 974    float flt_perc = 100.f*flt_zeros/num_elem;
 975    float ui8_perc = 100.f*ui8_zeros/num_elem;
 976    printf("[OK] RTC: overall %i/%i (%.3f%%) zeros in float leaves\n", (int)flt_zeros, (int)num_elem, flt_perc);
 977    printf("          overall %i/%i (%.3f%%) zeros in uint8 leaves\n", (int)ui8_zeros, (int)num_elem, ui8_perc);
 978
 979    return flt_perc;
 980 }
 981
 982 void RTreeClassifier::setQuantization(int num_quant_bits)
 983 {
 984    for (int i=0; i<(int)trees_.size(); ++i)
 985       trees_[i].applyQuantization(num_quant_bits);
 986
 987    printf("[OK] signature quantization is now %i bits (before: %i)\n", num_quant_bits, num_quant_bits_);
 988    num_quant_bits_ = num_quant_bits;
 989 }
 990
 991 void RTreeClassifier::discardFloatPosteriors()
 992 {
 993    for (int i=0; i<(int)trees_.size(); ++i)
 994       trees_[i].discardFloatPosteriors();
 995    printf("[OK] RTC: discarded float posteriors of all trees\n");
 996 }
 997
 998 }