i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed an odometry crash when there is only 1 word in the dictionary (and NNDR is activated)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1056 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-12-17 22:36:09 +00:00
parent 4a150e3215
commit 6fc712c15d
1 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/VWDictionary.cpp
View File

@@ -454,7 +454,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
UTimer timerLocal; UTimer timerLocal;
timerLocal.start(); timerLocal.start();
if(!_dataTree.empty()) if(!_dataTree.empty() && _dataTree.rows >= (int)k)
{ {
//Find nearest neighbors //Find nearest neighbors
UDEBUG("newPts.total()=%d ", newPts.total()); UDEBUG("newPts.total()=%d ", newPts.total());
@@ -469,7 +469,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
std::map<float, int> fullResults; // Contains results from the kd-tree search and the naive search in new words std::map<float, int> fullResults; // Contains results from the kd-tree search and the naive search in new words
this->naiveNNSearch(newWords, newPts.ptr<float>(i), _dim, fullResults, k); this->naiveNNSearch(newWords, newPts.ptr<float>(i), _dim, fullResults, k);
if(!_dataTree.empty()) if(!_dataTree.empty() && _dataTree.rows >= (int)k)
{ {
for(unsigned int j=0; j<k; ++j) for(unsigned int j=0; j<k; ++j)
{ {
@@ -653,7 +653,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws, bool
} }
ULOGGER_DEBUG("Preparation time = %fs", timer.ticks()); ULOGGER_DEBUG("Preparation time = %fs", timer.ticks());
if(!_dataTree.empty()) if(!_dataTree.empty() && _dataTree.rows >= (int)k)
{ {
_nn->search(newPts, results, dists, k, _maxLeafs); _nn->search(newPts, results, dists, k, _maxLeafs);
} }
@@ -661,7 +661,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws, bool
std::map<int, int> mapIndexIdNotIndexed; std::map<int, int> mapIndexIdNotIndexed;
unsigned int unreferencedWordsCount = _visualWords.size() - _mapIndexId.size(); unsigned int unreferencedWordsCount = _visualWords.size() - _mapIndexId.size();
if(searchInNewlyAddedWords && unreferencedWordsCount) if(searchInNewlyAddedWords && unreferencedWordsCount && unreferencedWordsCount >= (int)k)
{ {
cv::Mat dataNotIndexed = cv::Mat::zeros(unreferencedWordsCount, _dim, CV_32F); cv::Mat dataNotIndexed = cv::Mat::zeros(unreferencedWordsCount, _dim, CV_32F);
unsigned int index = 0; unsigned int index = 0;
@@ -697,7 +697,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws, bool
for(unsigned int j=0; j<k; ++j) for(unsigned int j=0; j<k; ++j)
{ {
float dist; float dist;
if(!_dataTree.empty()) if(!_dataTree.empty() && _dataTree.rows >= (int)k)
{ {
dist = dists.at<float>(i,j); dist = dists.at<float>(i,j);
fullResults.insert(std::pair<float, int>(dist, uValue(_mapIndexId, results.at<int>(i,j)))); fullResults.insert(std::pair<float, int>(dist, uValue(_mapIndexId, results.at<int>(i,j))));
@@ -832,7 +832,7 @@ void VWDictionary::naiveNNSearch(const std::list<VisualWord *> & words, const fl
const float * dw = 0; const float * dw = 0;
bool goodMatch; bool goodMatch;
if(!words.size() || k == 0) if(!words.size() || k == 0 || words.size() < k)
{ {
return; return;
} }