VWDictionary.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/core/VWDictionary.h"
29 #include "rtabmap/core/VisualWord.h"
30 
31 #include "rtabmap/core/Signature.h"
32 #include "rtabmap/core/DBDriver.h"
33 #include "rtabmap/core/Parameters.h"
34 #include "rtabmap/core/FlannIndex.h"
35 
36 #include "rtabmap/utilite/UtiLite.h"
37 
38 #include <opencv2/opencv_modules.hpp>
39 
40 #if CV_MAJOR_VERSION < 3
41 #ifdef HAVE_OPENCV_GPU
42 #include <opencv2/gpu/gpu.hpp>
43 #endif
44 #else
45 #include <opencv2/core/cuda.hpp>
46 #ifdef HAVE_OPENCV_CUDAFEATURES2D
47 #include <opencv2/cudafeatures2d.hpp>
48 #endif
49 #endif
50 
51 #include <fstream>
52 #include <string>
53 
54 #define KDTREE_SIZE 4
55 #define KNN_CHECKS 32
56 
57 namespace rtabmap
58 {
59 
60 const int VWDictionary::ID_START = 1;
61 const int VWDictionary::ID_INVALID = 0;
62 
63 VWDictionary::VWDictionary(const ParametersMap & parameters) :
64  _totalActiveReferences(0),
65  _incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
66  _incrementalFlann(Parameters::defaultKpIncrementalFlann()),
67  _rebalancingFactor(Parameters::defaultKpFlannRebalancingFactor()),
68  _nndrRatio(Parameters::defaultKpNndrRatio()),
69  _dictionaryPath(Parameters::defaultKpDictionaryPath()),
70  _newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
71  _lastWordId(0),
72  useDistanceL1_(false),
73  _flannIndex(new FlannIndex()),
74  _strategy(kNNBruteForce)
75 {
76  this->setNNStrategy((NNStrategy)Parameters::defaultKpNNStrategy());
77  this->parseParameters(parameters);
78 }
79 
80 VWDictionary::~VWDictionary()
81 {
82  this->clear();
83  delete _flannIndex;
84 }
85 
86 void VWDictionary::parseParameters(const ParametersMap & parameters)
87 {
88  ParametersMap::const_iterator iter;
89  Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
90  Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
91  Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
92  Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
93 
94  UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
95 
96  std::string dictionaryPath = _dictionaryPath;
97  bool incrementalDictionary = _incrementalDictionary;
98  if((iter=parameters.find(Parameters::kKpDictionaryPath())) != parameters.end())
99  {
100  dictionaryPath = (*iter).second.c_str();
101  }
102  if((iter=parameters.find(Parameters::kKpIncrementalDictionary())) != parameters.end())
103  {
104  incrementalDictionary = uStr2Bool((*iter).second.c_str());
105  }
106 
107  // Verifying hypotheses strategy
108  if((iter=parameters.find(Parameters::kKpNNStrategy())) != parameters.end())
109  {
110  NNStrategy nnStrategy = (NNStrategy)std::atoi((*iter).second.c_str());
111  this->setNNStrategy(nnStrategy);
112  }
113 
114  if(incrementalDictionary)
115  {
116  this->setIncrementalDictionary();
117  }
118  else
119  {
120  this->setFixedDictionary(dictionaryPath);
121  }
122 
123 }
124 
125 void VWDictionary::setIncrementalDictionary()
126 {
127  if(!_incrementalDictionary)
128  {
129  _incrementalDictionary = true;
130  if(_visualWords.size())
131  {
132  UWARN("Incremental dictionary set: already loaded visual words (%d) from the fixed dictionary will be included in the incremental one.", _visualWords.size());
133  }
134  }
135  _dictionaryPath = "";
136 }
137 
138 void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
139 {
140  if(!dictionaryPath.empty())
141  {
142  if((!_incrementalDictionary && _dictionaryPath.compare(dictionaryPath) != 0) ||
143  _visualWords.size() == 0)
144  {
145  UDEBUG("incremental=%d, oldPath=%s newPath=%s, visual words=%d",
146  _incrementalDictionary?1:0, _dictionaryPath.c_str(), dictionaryPath.c_str(), (int)_visualWords.size());
147 
148  if(UFile::getExtension(dictionaryPath).compare("db") == 0)
149  {
150  UDEBUG("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str());
151  DBDriver * driver = DBDriver::create();
152  if(driver->openConnection(dictionaryPath, false))
153  {
154  driver->load(this, false);
155  for(std::map<int, VisualWord*>::iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
156  {
157  iter->second->setSaved(true);
158  }
159  _incrementalDictionary = _visualWords.size()==0;
160  driver->closeConnection(false);
161  }
162  else
163  {
164  UERROR("Could not load dictionary from database %s", dictionaryPath.c_str());
165  }
166  delete driver;
167  }
168  else
169  {
170  UWARN("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str());
171  std::ifstream file;
172  file.open(dictionaryPath.c_str(), std::ifstream::in);
173  if(file.good())
174  {
175  UDEBUG("Deleting old dictionary and loading the new one from \"%s\"", dictionaryPath.c_str());
176  UTimer timer;
177 
178  // first line is the header
179  std::string str;
180  std::list<std::string> strList;
181  std::getline(file, str);
182  strList = uSplitNumChar(str);
183  int dimension = 0;
184  for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
185  {
186  if(uIsDigit(iter->at(0)))
187  {
188  dimension = std::atoi(iter->c_str());
189  break;
190  }
191  }
192  UDEBUG("descriptor dimension = %d", dimension);
193 
194  if(dimension <= 0 || dimension > 1000)
195  {
196  UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
197  }
198  else
199  {
200  // Process all words
201  while(file.good())
202  {
203  std::getline(file, str);
204  strList = uSplit(str);
205  if((int)strList.size() == dimension+1)
206  {
207  //first one is the visual word id
208  std::list<std::string>::iterator iter = strList.begin();
209  int id = std::atoi(iter->c_str());
210  cv::Mat descriptor(1, dimension, CV_32F);
211  ++iter;
212  int i=0;
213 
214  //get descriptor
215  for(;i<dimension && iter != strList.end(); ++i, ++iter)
216  {
217  descriptor.at<float>(i) = uStr2Float(*iter);
218  }
219  if(i != dimension)
220  {
221  UERROR("Loaded word has not the same size (%d) than descriptor size previously detected (%d).", i, dimension);
222  }
223 
224  VisualWord * vw = new VisualWord(id, descriptor, 0);
225  vw->setSaved(true);
226  _visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
227  _notIndexedWords.insert(_notIndexedWords.end(), id);
228  _unusedWords.insert(_unusedWords.end(), std::pair<int, VisualWord*>(id, vw));
229  }
230  else if(!str.empty())
231  {
232  UWARN("Cannot parse line \"%s\"", str.c_str());
233  }
234  }
235  if(_visualWords.size())
236  {
237  UWARN("Loaded %d words!", (int)_visualWords.size());
238  }
239  }
240  }
241  else
242  {
243  UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
244  }
245  file.close();
246  }
247 
248  if(_visualWords.size() == 0)
249  {
250  _incrementalDictionary = _visualWords.size()==0;
251  UWARN("No words loaded, cannot set a fixed dictionary.", (int)_visualWords.size());
252  }
253  else
254  {
255  this->update();
256  _incrementalDictionary = false;
257  UDEBUG("Loaded %d words!", (int)_visualWords.size());
258  }
259  }
260  else if(!_incrementalDictionary)
261  {
262  UDEBUG("Dictionary \"%s\" already loaded...", dictionaryPath.c_str());
263  }
264  else
265  {
266  UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", _visualWords.size());
267  }
268  }
269  else if(_incrementalDictionary && _visualWords.size())
270  {
271  UWARN("Cannot change to fixed dictionary, %d words already loaded as incremental", (int)_visualWords.size());
272  }
273  else
274  {
275  _incrementalDictionary = false;
276  }
277  _dictionaryPath = dictionaryPath;
278 }
279 
280 void VWDictionary::setNNStrategy(NNStrategy strategy)
281 {
282  if(strategy!=kNNUndef)
283  {
284 #if CV_MAJOR_VERSION < 3
285 #ifdef HAVE_OPENCV_GPU
286  if(strategy == kNNBruteForceGPU && !cv::gpu::getCudaEnabledDeviceCount())
287  {
288  UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
289  strategy = kNNBruteForce;
290  }
291 #else
292  if(strategy == kNNBruteForceGPU)
293  {
294  UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV is not built with GPU/cuda module! Doing \"kNNBruteForce\" instead.");
295  strategy = kNNBruteForce;
296  }
297 #endif
298 #else
299 #ifdef HAVE_OPENCV_CUDAFEATURES2D
300  if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
301  {
302  UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
303  strategy = kNNBruteForce;
304  }
305 #else
306  if(strategy == kNNBruteForceGPU)
307  {
308  UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but OpenCV cudafeatures2d module is not found! Doing \"kNNBruteForce\" instead.");
309  strategy = kNNBruteForce;
310  }
311 #endif
312 #endif
313 
314  bool update = _strategy != strategy;
315  _strategy = strategy;
316  if(update)
317  {
318  _dataTree = cv::Mat();
319  _notIndexedWords = uKeysSet(_visualWords);
320  _removedIndexedWords.clear();
321  this->update();
322  }
323  }
324 }
325 
326 int VWDictionary::getLastIndexedWordId() const
327 {
328  if(_mapIndexId.size())
329  {
330  return _mapIndexId.rbegin()->second;
331  }
332  else
333  {
334  return 0;
335  }
336 }
337 
338 unsigned int VWDictionary::getIndexedWordsCount() const
339 {
340  return _flannIndex->indexedFeatures();
341 }
342 
343 unsigned int VWDictionary::getIndexMemoryUsed() const
344 {
345  return _flannIndex->memoryUsed();
346 }
347 
348 void VWDictionary::update()
349 {
350  ULOGGER_DEBUG("");
351  if(!_incrementalDictionary && !_notIndexedWords.size())
352  {
353  // No need to update the search index if we
354  // use a fixed dictionary and the index is
355  // already built
356  return;
357  }
358 
359  if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
360  {
361  if(_incrementalFlann &&
362  _strategy < kNNBruteForce &&
363  _visualWords.size())
364  {
365  ULOGGER_DEBUG("Incremental FLANN: Removing %d words...", (int)_removedIndexedWords.size());
366  for(std::set<int>::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter)
367  {
368  UASSERT(uContains(_mapIdIndex, *iter));
369  UASSERT(uContains(_mapIndexId, _mapIdIndex.at(*iter)));
370  _flannIndex->removePoint(_mapIdIndex.at(*iter));
371  _mapIndexId.erase(_mapIdIndex.at(*iter));
372  _mapIdIndex.erase(*iter);
373  }
374  ULOGGER_DEBUG("Incremental FLANN: Removing %d words... done!", (int)_removedIndexedWords.size());
375 
376  if(_notIndexedWords.size())
377  {
378  ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size());
379  for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
380  {
381  VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
382  UASSERT(w);
383 
384  cv::Mat descriptor;
385  if(w->getDescriptor().type() == CV_8U)
386  {
387  useDistanceL1_ = true;
388  if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
389  {
390  w->getDescriptor().convertTo(descriptor, CV_32F);
391  }
392  else
393  {
394  descriptor = w->getDescriptor();
395  }
396  }
397  else
398  {
399  descriptor = w->getDescriptor();
400  }
401 
402  int index = 0;
403  if(!_flannIndex->isBuilt())
404  {
405  UDEBUG("Building FLANN index...");
406  switch(_strategy)
407  {
408  case kNNFlannNaive:
409  _flannIndex->buildLinearIndex(descriptor, useDistanceL1_, _rebalancingFactor);
410  break;
411  case kNNFlannKdTree:
412  UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
413  _flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
414  break;
415  case kNNFlannLSH:
416  UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
417  _flannIndex->buildLSHIndex(descriptor, 12, 20, 2, _rebalancingFactor);
418  break;
419  default:
420  UFATAL("Not supposed to be here!");
421  break;
422  }
423  UDEBUG("Building FLANN index... done!");
424  }
425  else
426  {
427  UASSERT(descriptor.cols == _flannIndex->featuresDim());
428  UASSERT(descriptor.type() == _flannIndex->featuresType());
429  index = _flannIndex->addPoints(descriptor);
430  }
431  std::pair<std::map<int, int>::iterator, bool> inserted;
432  inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
433  UASSERT(inserted.second);
434  inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
435  UASSERT(inserted.second);
436  }
437  ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size());
438  }
439  }
440  else if(_strategy >= kNNBruteForce &&
441  _notIndexedWords.size() &&
442  _removedIndexedWords.size() == 0 &&
443  _visualWords.size() &&
444  _dataTree.rows)
445  {
446  //just add not indexed words
447  int i = _dataTree.rows;
448  _dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
449  for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
450  {
451  VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
452  UASSERT(w);
453  UASSERT(w->getDescriptor().cols == _dataTree.cols);
454  UASSERT(w->getDescriptor().type() == _dataTree.type());
455  _dataTree.push_back(w->getDescriptor());
456  _mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, w->id()));
457  std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), i));
458  UASSERT(inserted.second);
459  ++i;
460  }
461  }
462  else
463  {
464  _mapIndexId.clear();
465  _mapIdIndex.clear();
466  _dataTree = cv::Mat();
467  _flannIndex->release();
468 
469  if(_visualWords.size())
470  {
471  UTimer timer;
472  timer.start();
473 
474  int type;
475  if(_visualWords.begin()->second->getDescriptor().type() == CV_8U)
476  {
477  useDistanceL1_ = true;
478  if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
479  {
480  type = CV_32F;
481  }
482  else
483  {
484  type = _visualWords.begin()->second->getDescriptor().type();
485  }
486  }
487  else
488  {
489  type = _visualWords.begin()->second->getDescriptor().type();
490  }
491  int dim = _visualWords.begin()->second->getDescriptor().cols;
492 
493  UASSERT(type == CV_32F || type == CV_8U);
494  UASSERT(dim > 0);
495 
496  // Create the data matrix
497  _dataTree = cv::Mat(_visualWords.size(), dim, type); // SURF descriptors are CV_32F
498  std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
499  for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
500  {
501  cv::Mat descriptor;
502  if(iter->second->getDescriptor().type() == CV_8U)
503  {
504  if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
505  {
506  iter->second->getDescriptor().convertTo(descriptor, CV_32F);
507  }
508  else
509  {
510  descriptor = iter->second->getDescriptor();
511  }
512  }
513  else
514  {
515  descriptor = iter->second->getDescriptor();
516  }
517 
518  UASSERT(descriptor.cols == dim);
519  UASSERT(descriptor.type() == type);
520 
521  descriptor.copyTo(_dataTree.row(i));
522  _mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
523  _mapIdIndex.insert(_mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
524  }
525 
526  ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
527  ULOGGER_DEBUG("copying data = %f s", timer.ticks());
528 
529  switch(_strategy)
530  {
531  case kNNFlannNaive:
532  _flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _rebalancingFactor);
533  break;
534  case kNNFlannKdTree:
535  UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
536  _flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
537  break;
538  case kNNFlannLSH:
539  UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
540  _flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _rebalancingFactor);
541  break;
542  default:
543  break;
544  }
545 
546  ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
547  }
548  }
549  UDEBUG("Dictionary updated! (size=%d added=%d removed=%d)",
550  _dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size());
551  }
552  else
553  {
554  UDEBUG("Dictionary has not changed, so no need to update it! (size=%d)", _dataTree.rows);
555  }
556  _notIndexedWords.clear();
557  _removedIndexedWords.clear();
558  UDEBUG("");
559 }
560 
561 void VWDictionary::clear(bool printWarningsIfNotEmpty)
562 {
563  ULOGGER_DEBUG("");
564  if(printWarningsIfNotEmpty)
565  {
566  if(_visualWords.size() && _incrementalDictionary)
567  {
568  UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", (int)_visualWords.size());
569  }
570  if(_notIndexedWords.size())
571  {
572  UWARN("Not indexed words should be empty here (%d words still not indexed)", (int)_notIndexedWords.size());
573  }
574  }
575  for(std::map<int, VisualWord *>::iterator i=_visualWords.begin(); i!=_visualWords.end(); ++i)
576  {
577  delete (*i).second;
578  }
579  _visualWords.clear();
580  _notIndexedWords.clear();
581  _removedIndexedWords.clear();
582  _totalActiveReferences = 0;
583  _lastWordId = 0;
584  _dataTree = cv::Mat();
585  _mapIndexId.clear();
586  _mapIdIndex.clear();
587  _unusedWords.clear();
588  _flannIndex->release();
589  useDistanceL1_ = false;
590 
591  if(!_incrementalDictionary)
592  {
593  // reload the fixed dictionary
594  this->setFixedDictionary(_dictionaryPath);
595  }
596 }
597 
598 int VWDictionary::getNextId()
599 {
600  return ++_lastWordId;
601 }
602 
603 void VWDictionary::addWordRef(int wordId, int signatureId)
604 {
605  VisualWord * vw = 0;
606  vw = uValue(_visualWords, wordId, vw);
607  if(vw)
608  {
609  vw->addRef(signatureId);
610  _totalActiveReferences += 1;
611 
612  _unusedWords.erase(vw->id());
613  }
614  else
615  {
616  UERROR("Not found word %d (dict size=%d)", wordId, (int)_visualWords.size());
617  }
618 }
619 
620 void VWDictionary::removeAllWordRef(int wordId, int signatureId)
621 {
622  VisualWord * vw = 0;
623  vw = uValue(_visualWords, wordId, vw);
624  if(vw)
625  {
626  _totalActiveReferences -= vw->removeAllRef(signatureId);
627  if(vw->getReferences().size() == 0)
628  {
629  _unusedWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
630  }
631  }
632 }
633 
634 std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
635  int signatureId)
636 {
637  UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows);
638  UTimer timer;
639  std::list<int> wordIds;
640  if(descriptorsIn.rows == 0 || descriptorsIn.cols == 0)
641  {
642  UERROR("Descriptors size is null!");
643  return wordIds;
644  }
645 
646  if(!_incrementalDictionary && _visualWords.empty())
647  {
648  UERROR("Dictionary mode is set to fixed but no words are in it!");
649  return wordIds;
650  }
651 
652  // verify we have the same features
653  int dim = 0;
654  int type = -1;
655  if(_visualWords.size())
656  {
657  dim = _visualWords.begin()->second->getDescriptor().cols;
658  type = _visualWords.begin()->second->getDescriptor().type();
659  UASSERT(type == CV_32F || type == CV_8U);
660  }
661  if(dim && dim != descriptorsIn.cols)
662  {
663  UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptorsIn.cols, dim);
664  return wordIds;
665  }
666  if(type>=0 && type != descriptorsIn.type())
667  {
668  UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptorsIn.type(), type);
669  return wordIds;
670  }
671 
672  // now compare with the actual index
673  cv::Mat descriptors;
674  if(descriptorsIn.type() == CV_8U)
675  {
676  useDistanceL1_ = true;
677  if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
678  {
679  descriptorsIn.convertTo(descriptors, CV_32F);
680  }
681  else
682  {
683  descriptors = descriptorsIn;
684  }
685  }
686  else
687  {
688  descriptors = descriptorsIn;
689  }
690  dim = 0;
691  type = -1;
692  if(_dataTree.rows || _flannIndex->isBuilt())
693  {
694  dim = _flannIndex->isBuilt()?_flannIndex->featuresDim():_dataTree.cols;
695  type = _flannIndex->isBuilt()?_flannIndex->featuresType():_dataTree.type();
696  UASSERT(type == CV_32F || type == CV_8U);
697  }
698 
699  if(dim && dim != descriptors.cols)
700  {
701  UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptors.cols, dim);
702  return wordIds;
703  }
704 
705  if(type>=0 && type != descriptors.type())
706  {
707  UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptors.type(), type);
708  return wordIds;
709  }
710 
711  int dupWordsCountFromDict= 0;
712  int dupWordsCountFromLast= 0;
713 
714  unsigned int k=2; // k nearest neighbors
715 
716  cv::Mat newWords;
717  std::vector<int> newWordsId;
718 
719  cv::Mat results;
720  cv::Mat dists;
721  std::vector<std::vector<cv::DMatch> > matches;
722  bool bruteForce = false;
723 
724  UTimer timerLocal;
725  timerLocal.start();
726 
727  if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
728  {
729  //Find nearest neighbors
730  UDEBUG("newPts.total()=%d ", descriptors.rows);
731 
732  if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
733  {
734  _flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS);
735  }
736  else if(_strategy == kNNBruteForce)
737  {
738  bruteForce = true;
739  cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
740  matcher.knnMatch(descriptors, _dataTree, matches, k);
741  }
742  else if(_strategy == kNNBruteForceGPU)
743  {
744  bruteForce = true;
745 #if CV_MAJOR_VERSION < 3
746 #ifdef HAVE_OPENCV_GPU
747  cv::gpu::GpuMat newDescriptorsGpu(descriptors);
748  cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
749  if(descriptors.type()==CV_8U)
750  {
751  cv::gpu::BruteForceMatcher_GPU<cv::Hamming> gpuMatcher;
752  gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
753  }
754  else
755  {
756  cv::gpu::BruteForceMatcher_GPU<cv::L2<float> > gpuMatcher;
757  gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
758  }
759 #else
760  UERROR("Cannot use brute Force GPU because OpenCV is not built with gpu module.");
761 #endif
762 #else
763 #ifdef HAVE_OPENCV_CUDAFEATURES2D
764  cv::cuda::GpuMat newDescriptorsGpu(descriptors);
765  cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
766  cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
767  if(descriptors.type()==CV_8U)
768  {
769  gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
770  gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
771  }
772  else
773  {
774  gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2);
775  gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
776  }
777 #else
778  UERROR("Cannot use brute Force GPU because OpenCV is not built with cuda module.");
779 #endif
780 #endif
781  }
782  else
783  {
784  UFATAL("");
785  }
786 
787  // In case of binary descriptors
788  if(dists.type() == CV_32S)
789  {
790  cv::Mat temp;
791  dists.convertTo(temp, CV_32F);
792  dists = temp;
793  }
794 
795  UDEBUG("Time to find nn = %f s", timerLocal.ticks());
796  }
797 
798  // Process results
799  for(int i = 0; i < descriptors.rows; ++i)
800  {
801  std::multimap<float, int> fullResults; // Contains results from the kd-tree search and the naive search in new words
802  if(!bruteForce && dists.cols)
803  {
804  for(int j=0; j<dists.cols; ++j)
805  {
806  float d = dists.at<float>(i,j);
807  int index;
808  if (sizeof(size_t) == 8)
809  {
810  index = *((size_t*)&results.at<double>(i, j));
811  }
812  else
813  {
814  index = *((size_t*)&results.at<int>(i, j));
815  }
816  int id = uValue(_mapIndexId, index);
817  if(d >= 0.0f && id != 0)
818  {
819  fullResults.insert(std::pair<float, int>(d, id));
820  }
821  else
822  {
823  break;
824  }
825  }
826  }
827  else if(bruteForce && matches.size())
828  {
829  for(unsigned int j=0; j<matches.at(i).size(); ++j)
830  {
831  float d = matches.at(i).at(j).distance;
832  int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
833  if(d >= 0.0f && id != 0)
834  {
835  fullResults.insert(std::pair<float, int>(d, id));
836  }
837  else
838  {
839  break;
840  }
841  }
842  }
843 
844  // Check if this descriptor matches with a word from the last signature (a word not already added to the tree)
845  if(_newWordsComparedTogether && newWords.rows)
846  {
847  std::vector<std::vector<cv::DMatch> > matchesNewWords;
848  cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR);
849  UASSERT(descriptors.cols == newWords.cols && descriptors.type() == newWords.type());
850  matcher.knnMatch(descriptors.row(i), newWords, matchesNewWords, newWords.rows>1?2:1);
851  UASSERT(matchesNewWords.size() == 1);
852  for(unsigned int j=0; j<matchesNewWords.at(0).size(); ++j)
853  {
854  float d = matchesNewWords.at(0).at(j).distance;
855  int id = newWordsId[matchesNewWords.at(0).at(j).trainIdx];
856  if(d >= 0.0f && id != 0)
857  {
858  fullResults.insert(std::pair<float, int>(d, id));
859  }
860  else
861  {
862  break;
863  }
864  }
865  }
866 
867  if(_incrementalDictionary)
868  {
869  bool badDist = false;
870  if(fullResults.size() == 0)
871  {
872  badDist = true;
873  }
874  if(!badDist)
875  {
876  if(fullResults.size() >= 2)
877  {
878  // Apply NNDR
879  if(fullResults.begin()->first > _nndrRatio * (++fullResults.begin())->first)
880  {
881  badDist = true; // Rejected
882  }
883  }
884  else
885  {
886  badDist = true; // Rejected
887  }
888  }
889 
890  if(badDist)
891  {
892  // use original descriptor
893  VisualWord * vw = new VisualWord(getNextId(), descriptorsIn.row(i), signatureId);
894  _visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
895  _notIndexedWords.insert(_notIndexedWords.end(), vw->id());
896  newWords.push_back(descriptors.row(i));
897  newWordsId.push_back(vw->id());
898  wordIds.push_back(vw->id());
899  UASSERT(vw->id()>0);
900  }
901  else
902  {
903  if(_notIndexedWords.find(fullResults.begin()->second) != _notIndexedWords.end())
904  {
905  ++dupWordsCountFromLast;
906  }
907  else
908  {
909  ++dupWordsCountFromDict;
910  }
911 
912  this->addWordRef(fullResults.begin()->second, signatureId);
913  wordIds.push_back(fullResults.begin()->second);
914  }
915  }
916  else if(fullResults.size())
917  {
918  // If the dictionary is not incremental, just take the nearest word
919  ++dupWordsCountFromDict;
920  this->addWordRef(fullResults.begin()->second, signatureId);
921  wordIds.push_back(fullResults.begin()->second);
922  UASSERT(fullResults.begin()->second>0);
923  }
924  }
925  ULOGGER_DEBUG("naive search and add ref/words time = %f s", timerLocal.ticks());
926 
927  ULOGGER_DEBUG("%d new words added...", _notIndexedWords.size());
928  ULOGGER_DEBUG("%d duplicated words added (from current image = %d)...",
929  dupWordsCountFromDict+dupWordsCountFromLast, dupWordsCountFromLast);
930  UDEBUG("total time %fs", timer.ticks());
931 
932  _totalActiveReferences += _notIndexedWords.size();
933  return wordIds;
934 }
935 
936 std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
937 {
938  UTimer timer;
939  timer.start();
940 
941  if(_visualWords.size() && vws.size())
942  {
943  int type = (*vws.begin())->getDescriptor().type();
944  int dim = (*vws.begin())->getDescriptor().cols;
945 
946  if(dim != _visualWords.begin()->second->getDescriptor().cols)
947  {
948  UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", (*vws.begin())->getDescriptor().cols, dim);
949  return std::vector<int>(vws.size(), 0);
950  }
951 
952  if(type != _visualWords.begin()->second->getDescriptor().type())
953  {
954  UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", (*vws.begin())->getDescriptor().type(), type);
955  return std::vector<int>(vws.size(), 0);
956  }
957 
958  // fill the request matrix
959  int index = 0;
960  VisualWord * vw;
961  cv::Mat query(vws.size(), dim, type);
962  for(std::list<VisualWord *>::const_iterator iter=vws.begin(); iter!=vws.end(); ++iter, ++index)
963  {
964  vw = *iter;
965  UASSERT(vw);
966 
967  UASSERT(vw->getDescriptor().cols == dim);
968  UASSERT(vw->getDescriptor().type() == type);
969 
970  vw->getDescriptor().copyTo(query.row(index));
971  }
972  ULOGGER_DEBUG("Preparation time = %fs", timer.ticks());
973 
974  return findNN(query);
975  }
976  return std::vector<int>(vws.size(), 0);
977 }
978 std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
979 {
980  UTimer timer;
981  timer.start();
982  std::vector<int> resultIds(queryIn.rows, 0);
983  unsigned int k=2; // k nearest neighbor
984 
985  if(_visualWords.size() && queryIn.rows)
986  {
987  // verify we have the same features
988  int dim = _visualWords.begin()->second->getDescriptor().cols;
989  int type = _visualWords.begin()->second->getDescriptor().type();
990  UASSERT(type == CV_32F || type == CV_8U);
991 
992  if(dim != queryIn.cols)
993  {
994  UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", queryIn.cols, dim);
995  return resultIds;
996  }
997  if(type != queryIn.type())
998  {
999  UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", queryIn.type(), type);
1000  return resultIds;
1001  }
1002 
1003  // now compare with the actual index
1004  cv::Mat query;
1005  if(queryIn.type() == CV_8U)
1006  {
1007  if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
1008  {
1009  queryIn.convertTo(query, CV_32F);
1010  }
1011  else
1012  {
1013  query = queryIn;
1014  }
1015  }
1016  else
1017  {
1018  query = queryIn;
1019  }
1020  dim = 0;
1021  type = -1;
1022  if(_dataTree.rows || _flannIndex->isBuilt())
1023  {
1024  dim = _flannIndex->isBuilt()?_flannIndex->featuresDim():_dataTree.cols;
1025  type = _flannIndex->isBuilt()?_flannIndex->featuresType():_dataTree.type();
1026  UASSERT(type == CV_32F || type == CV_8U);
1027  }
1028 
1029  if(dim && dim != query.cols)
1030  {
1031  UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", query.cols, dim);
1032  return resultIds;
1033  }
1034 
1035  if(type>=0 && type != query.type())
1036  {
1037  UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", query.type(), type);
1038  return resultIds;
1039  }
1040 
1041  std::vector<std::vector<cv::DMatch> > matches;
1042  bool bruteForce = false;
1043  cv::Mat results;
1044  cv::Mat dists;
1045 
1046  if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
1047  {
1048  //Find nearest neighbors
1049  UDEBUG("query.rows=%d ", query.rows);
1050 
1051  if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
1052  {
1053  _flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS);
1054  }
1055  else if(_strategy == kNNBruteForce)
1056  {
1057  bruteForce = true;
1058  cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
1059  matcher.knnMatch(query, _dataTree, matches, k);
1060  }
1061  else if(_strategy == kNNBruteForceGPU)
1062  {
1063  bruteForce = true;
1064 #if CV_MAJOR_VERSION < 3
1065 #ifdef HAVE_OPENCV_GPU
1066  cv::gpu::GpuMat newDescriptorsGpu(query);
1067  cv::gpu::GpuMat lastDescriptorsGpu(_dataTree);
1068  if(query.type()==CV_8U)
1069  {
1070  cv::gpu::BruteForceMatcher_GPU<cv::Hamming> gpuMatcher;
1071  gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
1072  }
1073  else
1074  {
1075  cv::gpu::BruteForceMatcher_GPU<cv::L2<float> > gpuMatcher;
1076  gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
1077  }
1078 #else
1079  UERROR("Cannot use brute Force GPU because OpenCV is not built with gpu module.");
1080 #endif
1081 #else
1082 #ifdef HAVE_OPENCV_CUDAFEATURES2D
1083  cv::cuda::GpuMat newDescriptorsGpu(query);
1084  cv::cuda::GpuMat lastDescriptorsGpu(_dataTree);
1085  cv::Ptr<cv::cuda::DescriptorMatcher> gpuMatcher;
1086  if(query.type()==CV_8U)
1087  {
1088  gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING);
1089  gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
1090  }
1091  else
1092  {
1093  gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2);
1094  gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k);
1095  }
1096 #else
1097  UERROR("Cannot use brute Force GPU because OpenCV is not built with cuda module.");
1098 #endif
1099 #endif
1100  }
1101  else
1102  {
1103  UFATAL("");
1104  }
1105 
1106  // In case of binary descriptors
1107  if(dists.type() == CV_32S)
1108  {
1109  cv::Mat temp;
1110  dists.convertTo(temp, CV_32F);
1111  dists = temp;
1112  }
1113  }
1114  ULOGGER_DEBUG("Search dictionary time = %fs", timer.ticks());
1115 
1116  std::map<int, int> mapIndexIdNotIndexed;
1117  std::vector<std::vector<cv::DMatch> > matchesNotIndexed;
1118  if(_notIndexedWords.size())
1119  {
1120  cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), query.cols, query.type());
1121  unsigned int index = 0;
1122  VisualWord * vw;
1123  for(std::set<int>::iterator iter = _notIndexedWords.begin(); iter != _notIndexedWords.end(); ++iter, ++index)
1124  {
1125  vw = _visualWords.at(*iter);
1126 
1127  cv::Mat descriptor;
1128  if(vw->getDescriptor().type() == CV_8U)
1129  {
1130  if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
1131  {
1132  vw->getDescriptor().convertTo(descriptor, CV_32F);
1133  }
1134  else
1135  {
1136  descriptor = vw->getDescriptor();
1137  }
1138  }
1139  else
1140  {
1141  descriptor = vw->getDescriptor();
1142  }
1143 
1144  UASSERT(vw != 0 && descriptor.cols == query.cols && descriptor.type() == query.type());
1145  vw->getDescriptor().copyTo(dataNotIndexed.row(index));
1146  mapIndexIdNotIndexed.insert(mapIndexIdNotIndexed.end(), std::pair<int,int>(index, vw->id()));
1147  }
1148 
1149  // Find nearest neighbor
1150  ULOGGER_DEBUG("Searching in words not indexed...");
1151  cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR);
1152  matcher.knnMatch(query, dataNotIndexed, matchesNotIndexed, dataNotIndexed.rows>1?2:1);
1153  }
1154  ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks());
1155 
1156  for(int i=0; i<query.rows; ++i)
1157  {
1158  std::multimap<float, int> fullResults; // Contains results from the kd-tree search [and the naive search in new words]
1159  if(!bruteForce && dists.cols)
1160  {
1161  for(int j=0; j<dists.cols; ++j)
1162  {
1163  float d = dists.at<float>(i,j);
1164  int index;
1165 
1166  if (sizeof(size_t) == 8)
1167  {
1168  index = *((size_t*)&results.at<double>(i, j));
1169  }
1170  else
1171  {
1172  index = *((size_t*)&results.at<int>(i, j));
1173  }
1174  int id = uValue(_mapIndexId, index);
1175  if(d >= 0.0f && id != 0)
1176  {
1177  fullResults.insert(std::pair<float, int>(d, id));
1178  }
1179  }
1180  }
1181  else if(bruteForce && matches.size())
1182  {
1183  for(unsigned int j=0; j<matches.at(i).size(); ++j)
1184  {
1185  float d = matches.at(i).at(j).distance;
1186  int id = uValue(_mapIndexId, matches.at(i).at(j).trainIdx);
1187  if(d >= 0.0f && id != 0)
1188  {
1189  fullResults.insert(std::pair<float, int>(d, id));
1190  }
1191  }
1192  }
1193 
1194  // not indexed..
1195  if(matchesNotIndexed.size())
1196  {
1197  for(unsigned int j=0; j<matchesNotIndexed.at(i).size(); ++j)
1198  {
1199  float d = matchesNotIndexed.at(i).at(j).distance;
1200  int id = uValue(mapIndexIdNotIndexed, matchesNotIndexed.at(i).at(j).trainIdx);
1201  if(d >= 0.0f && id != 0)
1202  {
1203  fullResults.insert(std::pair<float, int>(d, id));
1204  }
1205  else
1206  {
1207  break;
1208  }
1209  }
1210  }
1211 
1212  if(_incrementalDictionary)
1213  {
1214  bool badDist = false;
1215  if(fullResults.size() == 0)
1216  {
1217  badDist = true;
1218  }
1219  if(!badDist)
1220  {
1221  if(fullResults.size() >= 2)
1222  {
1223  // Apply NNDR
1224  if(fullResults.begin()->first > _nndrRatio * (++fullResults.begin())->first)
1225  {
1226  badDist = true; // Rejected
1227  }
1228  }
1229  else
1230  {
1231  badDist = true; // Rejected
1232  }
1233  }
1234 
1235  if(!badDist)
1236  {
1237  resultIds[i] = fullResults.begin()->second; // Accepted
1238  }
1239  }
1240  else if(fullResults.size())
1241  {
1242  //Just take the nearest if the dictionary is not incremental
1243  resultIds[i] = fullResults.begin()->second; // Accepted
1244  }
1245  }
1246  ULOGGER_DEBUG("badDist check time = %fs", timer.ticks());
1247  }
1248  return resultIds;
1249 }
1250 
1251 void VWDictionary::addWord(VisualWord * vw)
1252 {
1253  if(vw)
1254  {
1255  _visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
1256  _notIndexedWords.insert(vw->id());
1257  if(vw->getReferences().size())
1258  {
1259  _totalActiveReferences += uSum(uValues(vw->getReferences()));
1260  }
1261  else
1262  {
1263  _unusedWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
1264  }
1265  if(_lastWordId < vw->id())
1266  {
1267  _lastWordId = vw->id();
1268  }
1269  }
1270 }
1271 
1272 const VisualWord * VWDictionary::getWord(int id) const
1273 {
1274  return uValue(_visualWords, id, (VisualWord *)0);
1275 }
1276 
1277 VisualWord * VWDictionary::getUnusedWord(int id) const
1278 {
1279  return uValue(_unusedWords, id, (VisualWord *)0);
1280 }
1281 
1282 std::vector<VisualWord*> VWDictionary::getUnusedWords() const
1283 {
1284  return uValues(_unusedWords);
1285 }
1286 
1287 std::vector<int> VWDictionary::getUnusedWordIds() const
1288 {
1289  return uKeys(_unusedWords);
1290 }
1291 
1292 void VWDictionary::removeWords(const std::vector<VisualWord*> & words)
1293 {
1294  UDEBUG("Removing %d words from dictionary (current size=%d)", (int)words.size(), (int)_visualWords.size());
1295  for(unsigned int i=0; i<words.size(); ++i)
1296  {
1297  _visualWords.erase(words[i]->id());
1298  _unusedWords.erase(words[i]->id());
1299  if(_notIndexedWords.erase(words[i]->id()) == 0)
1300  {
1301  _removedIndexedWords.insert(words[i]->id());
1302  }
1303  }
1304 }
1305 
1306 void VWDictionary::deleteUnusedWords()
1307 {
1308  std::vector<VisualWord*> unusedWords = uValues(_unusedWords);
1309  removeWords(unusedWords);
1310  for(unsigned int i=0; i<unusedWords.size(); ++i)
1311  {
1312  delete unusedWords[i];
1313  }
1314 }
1315 
1316 void VWDictionary::exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const
1317 {
1318  UDEBUG("");
1319  if(_visualWords.empty())
1320  {
1321  UWARN("Dictionary is empty, cannot export it!");
1322  return;
1323  }
1324  if(_visualWords.begin()->second->getDescriptor().type() != CV_32FC1)
1325  {
1326  UERROR("Exporting binary descriptors is not implemented!");
1327  return;
1328  }
1329  FILE* foutRef = 0;
1330  FILE* foutDesc = 0;
1331 #ifdef _MSC_VER
1332  fopen_s(&foutRef, fileNameReferences, "w");
1333  fopen_s(&foutDesc, fileNameDescriptors, "w");
1334 #else
1335  foutRef = fopen(fileNameReferences, "w");
1336  foutDesc = fopen(fileNameDescriptors, "w");
1337 #endif
1338 
1339  if(foutRef)
1340  {
1341  fprintf(foutRef, "WordID SignaturesID...\n");
1342  }
1343  if(foutDesc)
1344  {
1345  if(_visualWords.begin() == _visualWords.end())
1346  {
1347  fprintf(foutDesc, "WordID Descriptors...\n");
1348  }
1349  else
1350  {
1351  UDEBUG("");
1352  fprintf(foutDesc, "WordID Descriptors...%d\n", (*_visualWords.begin()).second->getDescriptor().cols);
1353  }
1354  }
1355 
1356  UDEBUG("Export %d words...", _visualWords.size());
1357  for(std::map<int, VisualWord *>::const_iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
1358  {
1359  // References
1360  if(foutRef)
1361  {
1362  fprintf(foutRef, "%d ", (*iter).first);
1363  const std::map<int, int> ref = (*iter).second->getReferences();
1364  for(std::map<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
1365  {
1366  for(int i=0; i<(*jter).second; ++i)
1367  {
1368  fprintf(foutRef, "%d ", (*jter).first);
1369  }
1370  }
1371  fprintf(foutRef, "\n");
1372  }
1373 
1374  //Descriptors
1375  if(foutDesc)
1376  {
1377  fprintf(foutDesc, "%d ", (*iter).first);
1378  const float * desc = (const float *)(*iter).second->getDescriptor().data;
1379  int dim = (*iter).second->getDescriptor().cols;
1380 
1381  for(int i=0; i<dim; i++)
1382  {
1383  fprintf(foutDesc, "%f ", desc[i]);
1384  }
1385  fprintf(foutDesc, "\n");
1386  }
1387  }
1388 
1389  if(foutRef)
1390  fclose(foutRef);
1391  if(foutDesc)
1392  fclose(foutDesc);
1393 }
1394 
1395 } // namespace rtabmap
d
d
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:446
UTimer
Definition: UTimer.h:46
rtabmap::FlannIndex::removePoint
void removePoint(unsigned int index)
Definition: FlannIndex.cpp:401
rtabmap::VWDictionary::getUnusedWordIds
std::vector< int > getUnusedWordIds() const
Definition: VWDictionary.cpp:1287
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
rtabmap::VWDictionary::getIndexMemoryUsed
unsigned int getIndexMemoryUsed() const
Definition: VWDictionary.cpp:343
uIsDigit
bool uIsDigit(const char c)
Definition: UStl.h:622
rtabmap::VisualWord::removeAllRef
int removeAllRef(int signatureId)
Definition: VisualWord.cpp:65
rtabmap::FlannIndex::buildLSHIndex
void buildLSHIndex(const cv::Mat &features, unsigned int table_number=12, unsigned int key_size=20, unsigned int multi_probe_level=2, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:280
f
f
rtabmap::VWDictionary::update
virtual void update()
Definition: VWDictionary.cpp:348
uStr2Bool
bool UTILITE_EXP uStr2Bool(const char *str)
Definition: UConversion.cpp:155
uKeysSet
std::set< K > uKeysSet(const std::map< K, V > &m)
Definition: UStl.h:186
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:123
rtabmap::FlannIndex::knnSearch
void knnSearch(const cv::Mat &query, cv::Mat &indices, cv::Mat &dists, int knn, int checks=32, float eps=0.0, bool sorted=true) const
Definition: FlannIndex.cpp:434
rtabmap::VWDictionary::addNewWords
virtual std::list< int > addNewWords(const cv::Mat &descriptors, int signatureId)
Definition: VWDictionary.cpp:634
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
rtabmap::VisualWord::getReferences
const std::map< int, int > & getReferences() const
Definition: VisualWord.h:50
rtabmap::VWDictionary::addWordRef
void addWordRef(int wordId, int signatureId)
Definition: VWDictionary.cpp:603
rtabmap::FlannIndex::featuresDim
int featuresDim() const
Definition: FlannIndex.h:75
rtabmap::VWDictionary::getWord
const VisualWord * getWord(int id) const
Definition: VWDictionary.cpp:1272
rtabmap::VWDictionary::exportDictionary
void exportDictionary(const char *fileNameReferences, const char *fileNameDescriptors) const
Definition: VWDictionary.cpp:1316
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
rtabmap::VWDictionary::_mapIdIndex
std::map< int,int > _mapIdIndex
Definition: VWDictionary.h:122
rtabmap::FlannIndex::addPoints
unsigned int addPoints(const cv::Mat &features)
Definition: FlannIndex.cpp:310
rtabmap::VWDictionary::getIndexedWordsCount
unsigned int getIndexedWordsCount() const
Definition: VWDictionary.cpp:338
rtabmap::VisualWord::id
int id() const
Definition: VisualWord.h:48
rtabmap::VWDictionary::setFixedDictionary
void setFixedDictionary(const std::string &dictionaryPath)
Definition: VWDictionary.cpp:138
UFATAL
#define UFATAL(...)
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
rtabmap::VWDictionary::getUnusedWords
std::vector< VisualWord * > getUnusedWords() const
Definition: VWDictionary.cpp:1282
UASSERT
#define UASSERT(condition)
rtabmap::VWDictionary::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: VWDictionary.cpp:86
rtabmap::FlannIndex::buildLinearIndex
void buildLinearIndex(const cv::Mat &features, bool useDistanceL1=false, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:136
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::FlannIndex::indexedFeatures
unsigned int indexedFeatures() const
Definition: FlannIndex.cpp:81
rtabmap::VWDictionary::_mapIndexId
std::map< int,int > _mapIndexId
Definition: VWDictionary.h:121
rtabmap::VisualWord::getDescriptor
const cv::Mat & getDescriptor() const
Definition: VisualWord.h:49
rtabmap::VWDictionary::findNN
std::vector< int > findNN(const std::list< VisualWord * > &vws) const
Definition: VWDictionary.cpp:936
rtabmap::VWDictionary::VWDictionary
VWDictionary(const ParametersMap &parameters=ParametersMap())
Definition: VWDictionary.cpp:63
rtabmap::FlannIndex::featuresType
int featuresType() const
Definition: FlannIndex.h:74
rtabmap::FlannIndex::memoryUsed
unsigned int memoryUsed() const
Definition: FlannIndex.cpp:109
rtabmap::VWDictionary::clear
void clear(bool printWarningsIfNotEmpty=true)
Definition: VWDictionary.cpp:561
uSplitNumChar
std::list< std::string > uSplitNumChar(const std::string &str)
Definition: UStl.h:672
UTimer::start
void start()
Definition: UTimer.cpp:80
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
false
#define false
Definition: ConvertUTF.c:56
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
UDEBUG
#define UDEBUG(...)
rtabmap::VWDictionary::removeWords
void removeWords(const std::vector< VisualWord * > &words)
Definition: VWDictionary.cpp:1292
rtabmap::VWDictionary::_notIndexedWords
std::set< int > _notIndexedWords
Definition: VWDictionary.h:124
rtabmap::VWDictionary::_unusedWords
std::map< int, VisualWord * > _unusedWords
Definition: VWDictionary.h:123
rtabmap::VWDictionary::getLastIndexedWordId
int getLastIndexedWordId() const
Definition: VWDictionary.cpp:326
rtabmap::VisualWord::addRef
void addRef(int signatureId)
Definition: VisualWord.cpp:51
UERROR
#define UERROR(...)
rtabmap::VWDictionary::_removedIndexedWords
std::set< int > _removedIndexedWords
Definition: VWDictionary.h:125
rtabmap::VWDictionary::ID_START
static const int ID_START
Definition: VWDictionary.h:56
UWARN
#define UWARN(...)
rtabmap::VWDictionary::removeAllWordRef
void removeAllWordRef(int wordId, int signatureId)
Definition: VWDictionary.cpp:620
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
KDTREE_SIZE
#define KDTREE_SIZE
Definition: VWDictionary.cpp:54
rtabmap::VisualWord::setSaved
void setSaved(bool saved)
Definition: VisualWord.h:53
uSum
T uSum(const std::list< T > &list)
Definition: UMath.h:320
rtabmap::FlannIndex::buildKDTreeIndex
void buildKDTreeIndex(const cv::Mat &features, int trees=4, bool useDistanceL1=false, float rebalancingFactor=2.0f)
Definition: FlannIndex.cpp:183
KNN_CHECKS
#define KNN_CHECKS
Definition: VWDictionary.cpp:55
rtabmap::VWDictionary::ID_INVALID
static const int ID_INVALID
Definition: VWDictionary.h:57
rtabmap::VWDictionary::addWord
virtual void addWord(VisualWord *vw)
Definition: VWDictionary.cpp:1251
rtabmap::VWDictionary::setNNStrategy
void setNNStrategy(NNStrategy strategy)
Definition: VWDictionary.cpp:280
rtabmap::VWDictionary::getUnusedWord
VisualWord * getUnusedWord(int id) const
Definition: VWDictionary.cpp:1277
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::VWDictionary::_dictionaryPath
std::string _dictionaryPath
Definition: VWDictionary.h:114
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::VWDictionary::_flannIndex
FlannIndex * _flannIndex
Definition: VWDictionary.h:118
rtabmap::VWDictionary::_visualWords
std::map< int, VisualWord * > _visualWords
Definition: VWDictionary.h:106

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:41