modules/contrib/src/openfabmap.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 // This file originates from the openFABMAP project:
  10 // [http://code.google.com/p/openfabmap/]
  11 //
  12 // For published work which uses all or part of OpenFABMAP, please cite:
  13 // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843]
  14 //
  15 // Original Algorithm by Mark Cummins and Paul Newman:
  16 // [http://ijr.sagepub.com/content/27/6/647.short]
  17 // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942]
  18 // [http://ijr.sagepub.com/content/30/9/1100.abstract]
  19 //
  20 //                           License Agreement
  21 //
  22 // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and
  23 //                    Will Maddern [w.maddern@qut.edu.au], all rights reserved.
  24 //
  25 //
  26 // Redistribution and use in source and binary forms, with or without modification,
  27 // are permitted provided that the following conditions are met:
  28 //
  29 //   * Redistribution's of source code must retain the above copyright notice,
  30 //     this list of conditions and the following disclaimer.
  31 //
  32 //   * Redistribution's in binary form must reproduce the above copyright notice,
  33 //     this list of conditions and the following disclaimer in the documentation
  34 //     and/or other materials provided with the distribution.
  35 //
  36 //   * The name of the copyright holders may not be used to endorse or promote products
  37 //     derived from this software without specific prior written permission.
  38 //
  39 // This software is provided by the copyright holders and contributors "as is" and
  40 // any express or implied warranties, including, but not limited to, the implied
  41 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  42 // In no event shall the Intel Corporation or contributors be liable for any direct,
  43 // indirect, incidental, special, exemplary, or consequential damages
  44 // (including, but not limited to, procurement of substitute goods or services;
  45 // loss of use, data, or profits; or business interruption) however caused
  46 // and on any theory of liability, whether in contract, strict liability,
  47 // or tort (including negligence or otherwise) arising in any way out of
  48 // the use of this software, even if advised of the possibility of such damage.
  49 //
  50 //M*/
  51
  52 #include "precomp.hpp"
  53 #include "opencv2/contrib/openfabmap.hpp"
  54
  55
  56 /*
  57     Calculate the sum of two log likelihoods
  58 */
  59 namespace cv {
  60
  61 namespace of2 {
  62
  63 static double logsumexp(double a, double b) {
  64     return a > b ? log(1 + exp(b - a)) + a : log(1 + exp(a - b)) + b;
  65 }
  66
  67 FabMap::FabMap(const Mat& _clTree, double _PzGe,
  68         double _PzGNe, int _flags, int _numSamples) :
  69     clTree(_clTree), PzGe(_PzGe), PzGNe(_PzGNe), flags(
  70             _flags), numSamples(_numSamples) {
  71
  72     CV_Assert(flags & MEAN_FIELD || flags & SAMPLED);
  73     CV_Assert(flags & NAIVE_BAYES || flags & CHOW_LIU);
  74     if (flags & NAIVE_BAYES) {
  75         PzGL = &FabMap::PzqGL;
  76     } else {
  77         PzGL = &FabMap::PzqGzpqL;
  78     }
  79
  80     //check for a valid Chow-Liu tree
  81     CV_Assert(clTree.type() == CV_64FC1);
  82     cv::checkRange(clTree.row(0), false, NULL, 0, clTree.cols);
  83     cv::checkRange(clTree.row(1), false, NULL, DBL_MIN, 1);
  84     cv::checkRange(clTree.row(2), false, NULL, DBL_MIN, 1);
  85     cv::checkRange(clTree.row(3), false, NULL, DBL_MIN, 1);
  86
  87     // TODO: Add default values for member variables
  88     Pnew = 0.9;
  89     sFactor = 0.99;
  90     mBias = 0.5;
  91 }
  92
  93 FabMap::~FabMap() {
  94 }
  95
  96 const std::vector<cv::Mat>& FabMap::getTrainingImgDescriptors() const {
  97     return trainingImgDescriptors;
  98 }
  99
 100 const std::vector<cv::Mat>& FabMap::getTestImgDescriptors() const {
 101     return testImgDescriptors;
 102 }
 103
 104 void FabMap::addTraining(const Mat& queryImgDescriptor) {
 105     CV_Assert(!queryImgDescriptor.empty());
 106     vector<Mat> queryImgDescriptors;
 107     for (int i = 0; i < queryImgDescriptor.rows; i++) {
 108         queryImgDescriptors.push_back(queryImgDescriptor.row(i));
 109     }
 110     addTraining(queryImgDescriptors);
 111 }
 112
 113 void FabMap::addTraining(const vector<Mat>& queryImgDescriptors) {
 114     for (size_t i = 0; i < queryImgDescriptors.size(); i++) {
 115         CV_Assert(!queryImgDescriptors[i].empty());
 116         CV_Assert(queryImgDescriptors[i].rows == 1);
 117         CV_Assert(queryImgDescriptors[i].cols == clTree.cols);
 118         CV_Assert(queryImgDescriptors[i].type() == CV_32F);
 119         trainingImgDescriptors.push_back(queryImgDescriptors[i]);
 120     }
 121 }
 122
 123 void FabMap::add(const cv::Mat& queryImgDescriptor) {
 124     CV_Assert(!queryImgDescriptor.empty());
 125     vector<Mat> queryImgDescriptors;
 126     for (int i = 0; i < queryImgDescriptor.rows; i++) {
 127         queryImgDescriptors.push_back(queryImgDescriptor.row(i));
 128     }
 129     add(queryImgDescriptors);
 130 }
 131
 132 void FabMap::add(const std::vector<cv::Mat>& queryImgDescriptors) {
 133     for (size_t i = 0; i < queryImgDescriptors.size(); i++) {
 134         CV_Assert(!queryImgDescriptors[i].empty());
 135         CV_Assert(queryImgDescriptors[i].rows == 1);
 136         CV_Assert(queryImgDescriptors[i].cols == clTree.cols);
 137         CV_Assert(queryImgDescriptors[i].type() == CV_32F);
 138         testImgDescriptors.push_back(queryImgDescriptors[i]);
 139     }
 140 }
 141
 142 void FabMap::compare(const Mat& queryImgDescriptor,
 143             vector<IMatch>& matches, bool addQuery,
 144             const Mat& mask) {
 145     CV_Assert(!queryImgDescriptor.empty());
 146     vector<Mat> queryImgDescriptors;
 147     for (int i = 0; i < queryImgDescriptor.rows; i++) {
 148         queryImgDescriptors.push_back(queryImgDescriptor.row(i));
 149     }
 150     compare(queryImgDescriptors,matches,addQuery,mask);
 151 }
 152
 153 void FabMap::compare(const Mat& queryImgDescriptor,
 154             const Mat& testImgDescriptor, vector<IMatch>& matches,
 155             const Mat& mask) {
 156     CV_Assert(!queryImgDescriptor.empty());
 157     vector<Mat> queryImgDescriptors;
 158     for (int i = 0; i < queryImgDescriptor.rows; i++) {
 159         queryImgDescriptors.push_back(queryImgDescriptor.row(i));
 160     }
 161
 162     CV_Assert(!testImgDescriptor.empty());
 163     vector<Mat> _testImgDescriptors;
 164     for (int i = 0; i < testImgDescriptor.rows; i++) {
 165         _testImgDescriptors.push_back(testImgDescriptor.row(i));
 166     }
 167     compare(queryImgDescriptors,_testImgDescriptors,matches,mask);
 168
 169 }
 170
 171 void FabMap::compare(const Mat& queryImgDescriptor,
 172         const vector<Mat>& _testImgDescriptors,
 173         vector<IMatch>& matches, const Mat& mask) {
 174     CV_Assert(!queryImgDescriptor.empty());
 175     vector<Mat> queryImgDescriptors;
 176     for (int i = 0; i < queryImgDescriptor.rows; i++) {
 177         queryImgDescriptors.push_back(queryImgDescriptor.row(i));
 178     }
 179     compare(queryImgDescriptors,_testImgDescriptors,matches,mask);
 180 }
 181
 182 void FabMap::compare(const vector<Mat>& queryImgDescriptors,
 183                      vector<IMatch>& matches, bool addQuery, const Mat& /*mask*/) {
 184
 185     // TODO: add first query if empty (is this necessary)
 186
 187     for (size_t i = 0; i < queryImgDescriptors.size(); i++) {
 188         CV_Assert(!queryImgDescriptors[i].empty());
 189         CV_Assert(queryImgDescriptors[i].rows == 1);
 190         CV_Assert(queryImgDescriptors[i].cols == clTree.cols);
 191         CV_Assert(queryImgDescriptors[i].type() == CV_32F);
 192
 193         // TODO: add mask
 194
 195         compareImgDescriptor(queryImgDescriptors[i],
 196                              (int)i, testImgDescriptors, matches);
 197         if (addQuery)
 198             add(queryImgDescriptors[i]);
 199     }
 200 }
 201
 202 void FabMap::compare(const vector<Mat>& queryImgDescriptors,
 203         const vector<Mat>& _testImgDescriptors,
 204         vector<IMatch>& matches, const Mat& /*mask*/) {
 205
 206     CV_Assert(!(flags & MOTION_MODEL));
 207     for (size_t i = 0; i < _testImgDescriptors.size(); i++) {
 208         CV_Assert(!_testImgDescriptors[i].empty());
 209         CV_Assert(_testImgDescriptors[i].rows == 1);
 210         CV_Assert(_testImgDescriptors[i].cols == clTree.cols);
 211         CV_Assert(_testImgDescriptors[i].type() == CV_32F);
 212     }
 213
 214     for (size_t i = 0; i < queryImgDescriptors.size(); i++) {
 215         CV_Assert(!queryImgDescriptors[i].empty());
 216         CV_Assert(queryImgDescriptors[i].rows == 1);
 217         CV_Assert(queryImgDescriptors[i].cols == clTree.cols);
 218         CV_Assert(queryImgDescriptors[i].type() == CV_32F);
 219
 220         // TODO: add mask
 221
 222         compareImgDescriptor(queryImgDescriptors[i],
 223                 (int)i, _testImgDescriptors, matches);
 224     }
 225 }
 226
 227 void FabMap::compareImgDescriptor(const Mat& queryImgDescriptor,
 228         int queryIndex, const vector<Mat>& _testImgDescriptors,
 229         vector<IMatch>& matches) {
 230
 231     vector<IMatch> queryMatches;
 232     queryMatches.push_back(IMatch(queryIndex,-1,
 233         getNewPlaceLikelihood(queryImgDescriptor),0));
 234     getLikelihoods(queryImgDescriptor,_testImgDescriptors,queryMatches);
 235     normaliseDistribution(queryMatches);
 236     for (size_t j = 1; j < queryMatches.size(); j++) {
 237         queryMatches[j].queryIdx = queryIndex;
 238     }
 239     matches.insert(matches.end(), queryMatches.begin(), queryMatches.end());
 240 }
 241
 242 void FabMap::getLikelihoods(const Mat& /*queryImgDescriptor*/,
 243         const vector<Mat>& /*testImgDescriptors*/, vector<IMatch>& /*matches*/) {
 244
 245 }
 246
 247 double FabMap::getNewPlaceLikelihood(const Mat& queryImgDescriptor) {
 248     if (flags & MEAN_FIELD) {
 249         double logP = 0;
 250         bool zq, zpq;
 251         if(flags & NAIVE_BAYES) {
 252             for (int q = 0; q < clTree.cols; q++) {
 253                 zq = queryImgDescriptor.at<float>(0,q) > 0;
 254
 255                 logP += log(Pzq(q, false) * PzqGeq(zq, false) +
 256                         Pzq(q, true) * PzqGeq(zq, true));
 257             }
 258         } else {
 259             for (int q = 0; q < clTree.cols; q++) {
 260                 zq = queryImgDescriptor.at<float>(0,q) > 0;
 261                 zpq = queryImgDescriptor.at<float>(0,pq(q)) > 0;
 262
 263                 double alpha, beta, p;
 264                 alpha = Pzq(q, zq) * PzqGeq(!zq, false) * PzqGzpq(q, !zq, zpq);
 265                 beta = Pzq(q, !zq) * PzqGeq(zq, false) * PzqGzpq(q, zq, zpq);
 266                 p = Pzq(q, false) * beta / (alpha + beta);
 267
 268                 alpha = Pzq(q, zq) * PzqGeq(!zq, true) * PzqGzpq(q, !zq, zpq);
 269                 beta = Pzq(q, !zq) * PzqGeq(zq, true) * PzqGzpq(q, zq, zpq);
 270                 p += Pzq(q, true) * beta / (alpha + beta);
 271
 272                 logP += log(p);
 273             }
 274         }
 275         return logP;
 276     }
 277
 278     if (flags & SAMPLED) {
 279         CV_Assert(!trainingImgDescriptors.empty());
 280         CV_Assert(numSamples > 0);
 281
 282         vector<Mat> sampledImgDescriptors;
 283
 284         // TODO: this method can result in the same sample being added
 285         // multiple times. Is this desired?
 286
 287         for (int i = 0; i < numSamples; i++) {
 288             int index = rand() % trainingImgDescriptors.size();
 289             sampledImgDescriptors.push_back(trainingImgDescriptors[index]);
 290         }
 291
 292         vector<IMatch> matches;
 293         getLikelihoods(queryImgDescriptor,sampledImgDescriptors,matches);
 294
 295         double averageLogLikelihood = -DBL_MAX + matches.front().likelihood + 1;
 296         for (int i = 0; i < numSamples; i++) {
 297             averageLogLikelihood =
 298                 logsumexp(matches[i].likelihood, averageLogLikelihood);
 299         }
 300
 301         return averageLogLikelihood - log((double)numSamples);
 302     }
 303     return 0;
 304 }
 305
 306 void FabMap::normaliseDistribution(vector<IMatch>& matches) {
 307     CV_Assert(!matches.empty());
 308
 309     if (flags & MOTION_MODEL) {
 310
 311         matches[0].match = matches[0].likelihood + log(Pnew);
 312
 313         if (priorMatches.size() > 2) {
 314             matches[1].match = matches[1].likelihood;
 315             matches[1].match += log(
 316                 (2 * (1-mBias) * priorMatches[1].match +
 317                 priorMatches[1].match +
 318                 2 * mBias * priorMatches[2].match) / 3);
 319             for (size_t i = 2; i < priorMatches.size()-1; i++) {
 320                 matches[i].match = matches[i].likelihood;
 321                 matches[i].match += log(
 322                     (2 * (1-mBias) * priorMatches[i-1].match +
 323                     priorMatches[i].match +
 324                     2 * mBias * priorMatches[i+1].match)/3);
 325             }
 326             matches[priorMatches.size()-1].match =
 327                 matches[priorMatches.size()-1].likelihood;
 328             matches[priorMatches.size()-1].match += log(
 329                 (2 * (1-mBias) * priorMatches[priorMatches.size()-2].match +
 330                 priorMatches[priorMatches.size()-1].match +
 331                 2 * mBias * priorMatches[priorMatches.size()-1].match)/3);
 332
 333             for(size_t i = priorMatches.size(); i < matches.size(); i++) {
 334                 matches[i].match = matches[i].likelihood;
 335             }
 336         } else {
 337             for(size_t i = 1; i < matches.size(); i++) {
 338                 matches[i].match = matches[i].likelihood;
 339             }
 340         }
 341
 342         double logsum = -DBL_MAX + matches.front().match + 1;
 343
 344         //calculate the normalising constant
 345         for (size_t i = 0; i < matches.size(); i++) {
 346             logsum = logsumexp(logsum, matches[i].match);
 347         }
 348
 349         //normalise
 350         for (size_t i = 0; i < matches.size(); i++) {
 351             matches[i].match = exp(matches[i].match - logsum);
 352         }
 353
 354         //smooth final probabilities
 355         for (size_t i = 0; i < matches.size(); i++) {
 356             matches[i].match = sFactor*matches[i].match +
 357             (1 - sFactor)/matches.size();
 358         }
 359
 360         //update our location priors
 361         priorMatches = matches;
 362
 363     } else {
 364
 365         double logsum = -DBL_MAX + matches.front().likelihood + 1;
 366
 367         for (size_t i = 0; i < matches.size(); i++) {
 368             logsum = logsumexp(logsum, matches[i].likelihood);
 369         }
 370         for (size_t i = 0; i < matches.size(); i++) {
 371             matches[i].match = exp(matches[i].likelihood - logsum);
 372         }
 373         for (size_t i = 0; i < matches.size(); i++) {
 374             matches[i].match = sFactor*matches[i].match +
 375             (1 - sFactor)/matches.size();
 376         }
 377     }
 378 }
 379
 380 int FabMap::pq(int q) {
 381     return (int)clTree.at<double>(0,q);
 382 }
 383
 384 double FabMap::Pzq(int q, bool zq) {
 385     return (zq) ? clTree.at<double>(1,q) : 1 - clTree.at<double>(1,q);
 386 }
 387
 388 double FabMap::PzqGzpq(int q, bool zq, bool zpq) {
 389     if (zpq) {
 390         return (zq) ? clTree.at<double>(2,q) : 1 - clTree.at<double>(2,q);
 391     } else {
 392         return (zq) ? clTree.at<double>(3,q) : 1 - clTree.at<double>(3,q);
 393     }
 394 }
 395
 396 double FabMap::PzqGeq(bool zq, bool eq) {
 397     if (eq) {
 398         return (zq) ? PzGe : 1 - PzGe;
 399     } else {
 400         return (zq) ? PzGNe : 1 - PzGNe;
 401     }
 402 }
 403
 404 double FabMap::PeqGL(int q, bool Lzq, bool eq) {
 405     double alpha, beta;
 406     alpha = PzqGeq(Lzq, true) * Pzq(q, true);
 407     beta = PzqGeq(Lzq, false) * Pzq(q, false);
 408
 409     if (eq) {
 410         return alpha / (alpha + beta);
 411     } else {
 412         return 1 - (alpha / (alpha + beta));
 413     }
 414 }
 415
 416 double FabMap::PzqGL(int q, bool zq, bool /*zpq*/, bool Lzq) {
 417     return PeqGL(q, Lzq, false) * PzqGeq(zq, false) +
 418         PeqGL(q, Lzq, true) * PzqGeq(zq, true);
 419 }
 420
 421
 422 double FabMap::PzqGzpqL(int q, bool zq, bool zpq, bool Lzq) {
 423     double p;
 424     double alpha, beta;
 425
 426     alpha = Pzq(q,  zq) * PzqGeq(!zq, false) * PzqGzpq(q, !zq, zpq);
 427     beta  = Pzq(q, !zq) * PzqGeq( zq, false) * PzqGzpq(q,  zq, zpq);
 428     p = PeqGL(q, Lzq, false) * beta / (alpha + beta);
 429
 430     alpha = Pzq(q,  zq) * PzqGeq(!zq, true) * PzqGzpq(q, !zq, zpq);
 431     beta  = Pzq(q, !zq) * PzqGeq( zq, true) * PzqGzpq(q,  zq, zpq);
 432     p += PeqGL(q, Lzq, true) * beta / (alpha + beta);
 433
 434     return p;
 435 }
 436
 437
 438 FabMap1::FabMap1(const Mat& _clTree, double _PzGe, double _PzGNe, int _flags,
 439         int _numSamples) : FabMap(_clTree, _PzGe, _PzGNe, _flags,
 440                 _numSamples) {
 441 }
 442
 443 FabMap1::~FabMap1() {
 444 }
 445
 446 void FabMap1::getLikelihoods(const Mat& queryImgDescriptor,
 447         const vector<Mat>& testImageDescriptors, vector<IMatch>& matches) {
 448
 449     for (size_t i = 0; i < testImageDescriptors.size(); i++) {
 450         bool zq, zpq, Lzq;
 451         double logP = 0;
 452         for (int q = 0; q < clTree.cols; q++) {
 453
 454             zq = queryImgDescriptor.at<float>(0,q) > 0;
 455             zpq = queryImgDescriptor.at<float>(0,pq(q)) > 0;
 456             Lzq = testImageDescriptors[i].at<float>(0,q) > 0;
 457
 458             logP += log((this->*PzGL)(q, zq, zpq, Lzq));
 459
 460         }
 461         matches.push_back(IMatch(0,(int)i,logP,0));
 462     }
 463 }
 464
 465 FabMapLUT::FabMapLUT(const Mat& _clTree, double _PzGe, double _PzGNe,
 466         int _flags, int _numSamples, int _precision) :
 467 FabMap(_clTree, _PzGe, _PzGNe, _flags, _numSamples), precision(_precision) {
 468
 469     int nWords = clTree.cols;
 470     double precFactor = (double)pow(10.0, precision);
 471
 472     table = new int[nWords][8];
 473
 474     for (int q = 0; q < nWords; q++) {
 475         for (unsigned char i = 0; i < 8; i++) {
 476
 477             bool Lzq = (bool) ((i >> 2) & 0x01);
 478             bool zq = (bool) ((i >> 1) & 0x01);
 479             bool zpq = (bool) (i & 1);
 480
 481             table[q][i] = -(int)(log((this->*PzGL)(q, zq, zpq, Lzq))
 482                     * precFactor);
 483         }
 484     }
 485 }
 486
 487 FabMapLUT::~FabMapLUT() {
 488     delete[] table;
 489 }
 490
 491 void FabMapLUT::getLikelihoods(const Mat& queryImgDescriptor,
 492         const vector<Mat>& testImageDescriptors, vector<IMatch>& matches) {
 493
 494     double precFactor = (double)pow(10.0, -precision);
 495
 496     for (size_t i = 0; i < testImageDescriptors.size(); i++) {
 497         unsigned long long int logP = 0;
 498         for (int q = 0; q < clTree.cols; q++) {
 499             logP += table[q][(queryImgDescriptor.at<float>(0,pq(q)) > 0) +
 500             ((queryImgDescriptor.at<float>(0, q) > 0) << 1) +
 501             ((testImageDescriptors[i].at<float>(0,q) > 0) << 2)];
 502         }
 503         matches.push_back(IMatch(0,(int)i,-precFactor*(double)logP,0));
 504     }
 505 }
 506
 507 FabMapFBO::FabMapFBO(const Mat& _clTree, double _PzGe, double _PzGNe,
 508         int _flags, int _numSamples, double _rejectionThreshold,
 509         double _PsGd, int _bisectionStart, int _bisectionIts) :
 510 FabMap(_clTree, _PzGe, _PzGNe, _flags, _numSamples), PsGd(_PsGd),
 511     rejectionThreshold(_rejectionThreshold), bisectionStart(_bisectionStart),
 512         bisectionIts(_bisectionIts) {
 513 }
 514
 515
 516 FabMapFBO::~FabMapFBO() {
 517 }
 518
 519 void FabMapFBO::getLikelihoods(const Mat& queryImgDescriptor,
 520         const vector<Mat>& testImageDescriptors, vector<IMatch>& matches) {
 521
 522     std::multiset<WordStats> wordData;
 523     setWordStatistics(queryImgDescriptor, wordData);
 524
 525     vector<int> matchIndices;
 526     vector<IMatch> queryMatches;
 527
 528     for (size_t i = 0; i < testImageDescriptors.size(); i++) {
 529         queryMatches.push_back(IMatch(0,(int)i,0,0));
 530         matchIndices.push_back((int)i);
 531     }
 532
 533     double currBest  = -DBL_MAX;
 534     double bailedOut = DBL_MAX;
 535
 536     for (std::multiset<WordStats>::iterator wordIter = wordData.begin();
 537             wordIter != wordData.end(); wordIter++) {
 538         bool zq = queryImgDescriptor.at<float>(0,wordIter->q) > 0;
 539         bool zpq = queryImgDescriptor.at<float>(0,pq(wordIter->q)) > 0;
 540
 541         currBest = -DBL_MAX;
 542
 543         for (size_t i = 0; i < matchIndices.size(); i++) {
 544             bool Lzq =
 545                 testImageDescriptors[matchIndices[i]].at<float>(0,wordIter->q) > 0;
 546             queryMatches[matchIndices[i]].likelihood +=
 547                 log((this->*PzGL)(wordIter->q,zq,zpq,Lzq));
 548             currBest =
 549                 std::max(queryMatches[matchIndices[i]].likelihood, currBest);
 550         }
 551
 552         if (matchIndices.size() == 1)
 553             continue;
 554
 555         double delta = std::max(limitbisection(wordIter->V, wordIter->M),
 556             -log(rejectionThreshold));
 557
 558         vector<int>::iterator matchIter = matchIndices.begin();
 559         while (matchIter != matchIndices.end()) {
 560             if (currBest - queryMatches[*matchIter].likelihood > delta) {
 561                 queryMatches[*matchIter].likelihood = bailedOut;
 562                 matchIter = matchIndices.erase(matchIter);
 563             } else {
 564                 matchIter++;
 565             }
 566         }
 567     }
 568
 569     for (size_t i = 0; i < queryMatches.size(); i++) {
 570         if (queryMatches[i].likelihood == bailedOut) {
 571             queryMatches[i].likelihood = currBest + log(rejectionThreshold);
 572         }
 573     }
 574     matches.insert(matches.end(), queryMatches.begin(), queryMatches.end());
 575
 576 }
 577
 578 void FabMapFBO::setWordStatistics(const Mat& queryImgDescriptor,
 579         std::multiset<WordStats>& wordData) {
 580     //words are sorted according to information = -ln(P(zq|zpq))
 581     //in non-log format this is lowest probability first
 582     for (int q = 0; q < clTree.cols; q++) {
 583         wordData.insert(WordStats(q,PzqGzpq(q,
 584                 queryImgDescriptor.at<float>(0,q) > 0,
 585                 queryImgDescriptor.at<float>(0,pq(q)) > 0)));
 586     }
 587
 588     double d = 0, V = 0, M = 0;
 589     bool zq, zpq;
 590
 591     for (std::multiset<WordStats>::reverse_iterator wordIter =
 592             wordData.rbegin();
 593             wordIter != wordData.rend(); wordIter++) {
 594
 595         zq = queryImgDescriptor.at<float>(0,wordIter->q) > 0;
 596         zpq = queryImgDescriptor.at<float>(0,pq(wordIter->q)) > 0;
 597
 598         d = log((this->*PzGL)(wordIter->q, zq, zpq, true)) -
 599             log((this->*PzGL)(wordIter->q, zq, zpq, false));
 600
 601         V += pow(d, 2.0) * 2 *
 602             (Pzq(wordIter->q, true) - pow(Pzq(wordIter->q, true), 2.0));
 603         M = std::max(M, fabs(d));
 604
 605         wordIter->V = V;
 606         wordIter->M = M;
 607     }
 608 }
 609
 610 double FabMapFBO::limitbisection(double v, double m) {
 611     double midpoint, left_val, mid_val;
 612     double left = 0, right = bisectionStart;
 613
 614     left_val = bennettInequality(v, m, left) - PsGd;
 615
 616     for(int i = 0; i < bisectionIts; i++) {
 617
 618         midpoint = (left + right)*0.5;
 619         mid_val = bennettInequality(v, m, midpoint)- PsGd;
 620
 621         if(left_val * mid_val > 0) {
 622             left = midpoint;
 623             left_val = mid_val;
 624         } else {
 625             right = midpoint;
 626         }
 627     }
 628
 629     return (right + left) * 0.5;
 630 }
 631
 632 double FabMapFBO::bennettInequality(double v, double m, double delta) {
 633     double DMonV = delta * m / v;
 634     double f_delta = log(DMonV + sqrt(pow(DMonV, 2.0) + 1));
 635     return exp((v / pow(m, 2.0))*(cosh(f_delta) - 1 - DMonV * f_delta));
 636 }
 637
 638 bool FabMapFBO::compInfo(const WordStats& first, const WordStats& second) {
 639     return first.info < second.info;
 640 }
 641
 642 FabMap2::FabMap2(const Mat& _clTree, double _PzGe, double _PzGNe,
 643         int _flags) :
 644 FabMap(_clTree, _PzGe, _PzGNe, _flags) {
 645     CV_Assert(flags & SAMPLED);
 646
 647     children.resize(clTree.cols);
 648
 649     for (int q = 0; q < clTree.cols; q++) {
 650         d1.push_back(log((this->*PzGL)(q, false, false, true) /
 651                 (this->*PzGL)(q, false, false, false)));
 652         d2.push_back(log((this->*PzGL)(q, false, true, true) /
 653                 (this->*PzGL)(q, false, true, false)) - d1[q]);
 654         d3.push_back(log((this->*PzGL)(q, true, false, true) /
 655                 (this->*PzGL)(q, true, false, false))- d1[q]);
 656         d4.push_back(log((this->*PzGL)(q, true, true, true) /
 657                 (this->*PzGL)(q, true, true, false))- d1[q]);
 658         children[pq(q)].push_back(q);
 659     }
 660
 661 }
 662
 663 FabMap2::~FabMap2() {
 664 }
 665
 666
 667 void FabMap2::addTraining(const vector<Mat>& queryImgDescriptors) {
 668     for (size_t i = 0; i < queryImgDescriptors.size(); i++) {
 669         CV_Assert(!queryImgDescriptors[i].empty());
 670         CV_Assert(queryImgDescriptors[i].rows == 1);
 671         CV_Assert(queryImgDescriptors[i].cols == clTree.cols);
 672         CV_Assert(queryImgDescriptors[i].type() == CV_32F);
 673         trainingImgDescriptors.push_back(queryImgDescriptors[i]);
 674         addToIndex(queryImgDescriptors[i], trainingDefaults, trainingInvertedMap);
 675     }
 676 }
 677
 678
 679 void FabMap2::add(const vector<Mat>& queryImgDescriptors) {
 680     for (size_t i = 0; i < queryImgDescriptors.size(); i++) {
 681         CV_Assert(!queryImgDescriptors[i].empty());
 682         CV_Assert(queryImgDescriptors[i].rows == 1);
 683         CV_Assert(queryImgDescriptors[i].cols == clTree.cols);
 684         CV_Assert(queryImgDescriptors[i].type() == CV_32F);
 685         testImgDescriptors.push_back(queryImgDescriptors[i]);
 686         addToIndex(queryImgDescriptors[i], testDefaults, testInvertedMap);
 687     }
 688 }
 689
 690 void FabMap2::getLikelihoods(const Mat& queryImgDescriptor,
 691         const vector<Mat>& testImageDescriptors, vector<IMatch>& matches) {
 692
 693     if (&testImageDescriptors == &testImgDescriptors) {
 694         getIndexLikelihoods(queryImgDescriptor, testDefaults, testInvertedMap,
 695             matches);
 696     } else {
 697         CV_Assert(!(flags & MOTION_MODEL));
 698         vector<double> defaults;
 699         std::map<int, vector<int> > invertedMap;
 700         for (size_t i = 0; i < testImageDescriptors.size(); i++) {
 701             addToIndex(testImageDescriptors[i],defaults,invertedMap);
 702         }
 703         getIndexLikelihoods(queryImgDescriptor, defaults, invertedMap, matches);
 704     }
 705 }
 706
 707 double FabMap2::getNewPlaceLikelihood(const Mat& queryImgDescriptor) {
 708
 709     CV_Assert(!trainingImgDescriptors.empty());
 710
 711     vector<IMatch> matches;
 712     getIndexLikelihoods(queryImgDescriptor, trainingDefaults,
 713             trainingInvertedMap, matches);
 714
 715     double averageLogLikelihood = -DBL_MAX + matches.front().likelihood + 1;
 716     for (size_t i = 0; i < matches.size(); i++) {
 717         averageLogLikelihood =
 718             logsumexp(matches[i].likelihood, averageLogLikelihood);
 719     }
 720
 721     return averageLogLikelihood - log((double)trainingDefaults.size());
 722
 723 }
 724
 725 void FabMap2::addToIndex(const Mat& queryImgDescriptor,
 726         vector<double>& defaults,
 727         std::map<int, vector<int> >& invertedMap) {
 728     defaults.push_back(0);
 729     for (int q = 0; q < clTree.cols; q++) {
 730         if (queryImgDescriptor.at<float>(0,q) > 0) {
 731             defaults.back() += d1[q];
 732             invertedMap[q].push_back((int)defaults.size()-1);
 733         }
 734     }
 735 }
 736
 737 void FabMap2::getIndexLikelihoods(const Mat& queryImgDescriptor,
 738         std::vector<double>& defaults,
 739         std::map<int, vector<int> >& invertedMap,
 740         std::vector<IMatch>& matches) {
 741
 742     vector<int>::iterator LwithI, child;
 743
 744     std::vector<double> likelihoods = defaults;
 745
 746     for (int q = 0; q < clTree.cols; q++) {
 747         if (queryImgDescriptor.at<float>(0,q) > 0) {
 748             for (LwithI = invertedMap[q].begin();
 749                 LwithI != invertedMap[q].end(); LwithI++) {
 750
 751                 if (queryImgDescriptor.at<float>(0,pq(q)) > 0) {
 752                     likelihoods[*LwithI] += d4[q];
 753                 } else {
 754                     likelihoods[*LwithI] += d3[q];
 755                 }
 756             }
 757             for (child = children[q].begin(); child != children[q].end();
 758                 child++) {
 759
 760                 if (queryImgDescriptor.at<float>(0,*child) == 0) {
 761                     for (LwithI = invertedMap[*child].begin();
 762                         LwithI != invertedMap[*child].end(); LwithI++) {
 763
 764                         likelihoods[*LwithI] += d2[*child];
 765                     }
 766                 }
 767             }
 768         }
 769     }
 770
 771     for (size_t i = 0; i < likelihoods.size(); i++) {
 772         matches.push_back(IMatch(0,(int)i,likelihoods[i],0));
 773     }
 774 }
 775
 776 }
 777
 778 }