From 748360aac7a4777b51caf27a2621671678ddb43a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 7 Apr 2016 11:48:07 -0400 Subject: [PATCH] Vocabulary: Binary descriptors are saved as is even if there is a float conversion for flann --- corelib/src/VWDictionary.cpp | 241 +++++++++++++++++++++++++++-------- guilib/src/MainWindow.cpp | 13 +- 2 files changed, 197 insertions(+), 57 deletions(-) diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 50c1191e..1ac51a18 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -626,6 +626,25 @@ void VWDictionary::update() { VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); UASSERT(w); + + cv::Mat descriptor; + if(w->getDescriptor().type() == CV_8U) + { + useDistanceL1_ = true; + if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + { + w->getDescriptor().convertTo(descriptor, CV_32F); + } + else + { + descriptor = w->getDescriptor(); + } + } + else + { + descriptor = w->getDescriptor(); + } + int index = 0; if(!_flannIndex->isBuilt()) { @@ -633,15 +652,15 @@ void VWDictionary::update() switch(_strategy) { case kNNFlannNaive: - _flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams(), useDistanceL1_); + _flannIndex->build(descriptor, rtflann::LinearIndexParams(), useDistanceL1_); break; case kNNFlannKdTree: - UASSERT_MSG(w->getDescriptor().type() == CV_32F, "To use KdTree dictionary, float descriptors are required!"); - _flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams(), useDistanceL1_); + UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!"); + _flannIndex->build(descriptor, rtflann::KDTreeIndexParams(), useDistanceL1_); break; case kNNFlannLSH: - UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!"); - _flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2), useDistanceL1_); + UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!"); + _flannIndex->build(descriptor, rtflann::LshIndexParams(12, 20, 2), useDistanceL1_); break; default: UFATAL("Not supposed to be here!"); @@ -651,9 +670,9 @@ void VWDictionary::update() } else { - UASSERT(w->getDescriptor().cols == _flannIndex->featuresDim()); - UASSERT(w->getDescriptor().type() == _flannIndex->featuresType()); - index = _flannIndex->addPoint(w->getDescriptor()); + UASSERT(descriptor.cols == _flannIndex->featuresDim()); + UASSERT(descriptor.type() == _flannIndex->featuresType()); + index = _flannIndex->addPoint(descriptor); } std::pair::iterator, bool> inserted; inserted = _mapIndexId.insert(std::pair(index, w->id())); @@ -698,7 +717,23 @@ void VWDictionary::update() UTimer timer; timer.start(); - int type = _visualWords.begin()->second->getDescriptor().type(); + int type; + if(_visualWords.begin()->second->getDescriptor().type() == CV_8U) + { + useDistanceL1_ = true; + if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + { + type = CV_32F; + } + else + { + type = _visualWords.begin()->second->getDescriptor().type(); + } + } + else + { + type = _visualWords.begin()->second->getDescriptor().type(); + } int dim = _visualWords.begin()->second->getDescriptor().cols; UASSERT(type == CV_32F || type == CV_8U); @@ -709,10 +744,27 @@ void VWDictionary::update() std::map::const_iterator iter = _visualWords.begin(); for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter) { - UASSERT(iter->second->getDescriptor().cols == dim); - UASSERT(iter->second->getDescriptor().type() == type); + cv::Mat descriptor; + if(iter->second->getDescriptor().type() == CV_8U) + { + if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + { + iter->second->getDescriptor().convertTo(descriptor, CV_32F); + } + else + { + descriptor = iter->second->getDescriptor(); + } + } + else + { + descriptor = iter->second->getDescriptor(); + } - iter->second->getDescriptor().copyTo(_dataTree.row(i)); + UASSERT(descriptor.cols == dim); + UASSERT(descriptor.type() == type); + + descriptor.copyTo(_dataTree.row(i)); _mapIndexId.insert(_mapIndexId.end(), std::pair(i, iter->second->id())); _mapIdIndex.insert(_mapIdIndex.end(), std::pair(iter->second->id(), i)); } @@ -827,6 +879,42 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, { UASSERT(signatureId > 0); + UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows); + UTimer timer; + std::list wordIds; + if(descriptorsIn.rows == 0 || descriptorsIn.cols == 0) + { + UERROR("Descriptors size is null!"); + return wordIds; + } + + if(!_incrementalDictionary && _visualWords.empty()) + { + UERROR("Dictionary mode is set to fixed but no words are in it!"); + return wordIds; + } + + // verify we have the same features + int dim = 0; + int type = -1; + if(_visualWords.size()) + { + dim = _visualWords.begin()->second->getDescriptor().cols; + type = _visualWords.begin()->second->getDescriptor().type(); + UASSERT(type == CV_32F || type == CV_8U); + } + if(dim && dim != descriptorsIn.cols) + { + UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptorsIn.cols, dim); + return wordIds; + } + if(type>=0 && type != descriptorsIn.type()) + { + UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptorsIn.type(), type); + return wordIds; + } + + // now compare with the actual index cv::Mat descriptors; if(descriptorsIn.type() == CV_8U) { @@ -844,21 +932,12 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, { descriptors = descriptorsIn; } - - UDEBUG("id=%d descriptors=%d", signatureId, descriptors.rows); - UTimer timer; - std::list wordIds; - if(descriptors.rows == 0 || descriptors.cols == 0) + dim = 0; + type = -1; + if(_dataTree.rows || _flannIndex->isBuilt()) { - UERROR("Descriptors size is null!"); - return wordIds; - } - int dim = 0; - int type = -1; - if(_visualWords.size()) - { - dim = _visualWords.begin()->second->getDescriptor().cols; - type = _visualWords.begin()->second->getDescriptor().type(); + dim = _flannIndex->isBuilt()?_flannIndex->featuresDim():_dataTree.cols; + type = _flannIndex->isBuilt()?_flannIndex->featuresType():_dataTree.type(); UASSERT(type == CV_32F || type == CV_8U); } @@ -867,20 +946,12 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptors.cols, dim); return wordIds; } - dim = descriptors.cols; if(type>=0 && type != descriptors.type()) { UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptors.type(), type); return wordIds; } - type = descriptors.type(); - - if(!_incrementalDictionary && _visualWords.empty()) - { - UERROR("Dictionary mode is set to fixed but no words are in it!"); - return wordIds; - } int dupWordsCountFromDict= 0; int dupWordsCountFromLast= 0; @@ -910,7 +981,7 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, else if(_strategy == kNNBruteForce) { bruteForce = true; - cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); matcher.knnMatch(descriptors, _dataTree, matches, k); } else if(_strategy == kNNBruteForceGPU) @@ -920,7 +991,7 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, #ifdef HAVE_OPENCV_GPU cv::gpu::GpuMat newDescriptorsGpu(descriptors); cv::gpu::GpuMat lastDescriptorsGpu(_dataTree); - if(type==CV_8U) + if(descriptors.type()==CV_8U) { cv::gpu::BruteForceMatcher_GPU gpuMatcher; gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); @@ -938,7 +1009,7 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, cv::cuda::GpuMat newDescriptorsGpu(descriptors); cv::cuda::GpuMat lastDescriptorsGpu(_dataTree); cv::Ptr gpuMatcher; - if(type==CV_8U) + if(descriptors.type()==CV_8U) { gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING); gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); @@ -1010,7 +1081,8 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, if(_newWordsComparedTogether && newWords.rows) { std::vector > matchesNewWords; - cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR); + UASSERT(descriptors.cols == newWords.cols && descriptors.type() == newWords.type()); matcher.knnMatch(descriptors.row(i), newWords, matchesNewWords, newWords.rows>1?2:1); UASSERT(matchesNewWords.size() == 1); for(unsigned int j=0; j VWDictionary::addNewWords(const cv::Mat & descriptorsIn, if(badDist) { - VisualWord * vw = new VisualWord(getNextId(), descriptors.row(i), signatureId); + // use original descriptor + VisualWord * vw = new VisualWord(getNextId(), descriptorsIn.row(i), signatureId); _visualWords.insert(_visualWords.end(), std::pair(vw->id(), vw)); _notIndexedWords.insert(_notIndexedWords.end(), vw->id()); - newWords.push_back(vw->getDescriptor()); + newWords.push_back(descriptors.row(i)); newWordsId.push_back(vw->id()); wordIds.push_back(vw->id()); UASSERT(vw->id()>0); @@ -1103,16 +1176,16 @@ std::vector VWDictionary::findNN(const std::list & vws) const if(_visualWords.size() && vws.size()) { - int dim = _visualWords.begin()->second->getDescriptor().cols; - int type = _visualWords.begin()->second->getDescriptor().type(); + int type = (*vws.begin())->getDescriptor().type(); + int dim = (*vws.begin())->getDescriptor().cols; - if(dim != (*vws.begin())->getDescriptor().cols) + if(dim != _visualWords.begin()->second->getDescriptor().cols) { UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", (*vws.begin())->getDescriptor().cols, dim); return std::vector(vws.size(), 0); } - if(type != (*vws.begin())->getDescriptor().type()) + if(type != _visualWords.begin()->second->getDescriptor().type()) { UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", (*vws.begin())->getDescriptor().type(), type); return std::vector(vws.size(), 0); @@ -1126,6 +1199,7 @@ std::vector VWDictionary::findNN(const std::list & vws) const { vw = *iter; UASSERT(vw); + UASSERT(vw->getDescriptor().cols == dim); UASSERT(vw->getDescriptor().type() == type); @@ -1137,25 +1211,64 @@ std::vector VWDictionary::findNN(const std::list & vws) const } return std::vector(vws.size(), 0); } -std::vector VWDictionary::findNN(const cv::Mat & query) const +std::vector VWDictionary::findNN(const cv::Mat & queryIn) const { UTimer timer; timer.start(); - std::vector resultIds(query.rows, 0); + std::vector resultIds(queryIn.rows, 0); unsigned int k=2; // k nearest neighbor - if(_visualWords.size() && query.rows) + if(_visualWords.size() && queryIn.rows) { + // verify we have the same features int dim = _visualWords.begin()->second->getDescriptor().cols; int type = _visualWords.begin()->second->getDescriptor().type(); + UASSERT(type == CV_32F || type == CV_8U); - if(dim != query.cols) + if(dim != queryIn.cols) + { + UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", queryIn.cols, dim); + return resultIds; + } + if(type != queryIn.type()) + { + UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", queryIn.type(), type); + return resultIds; + } + + // now compare with the actual index + cv::Mat query; + if(queryIn.type() == CV_8U) + { + if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + { + queryIn.convertTo(query, CV_32F); + } + else + { + query = queryIn; + } + } + else + { + query = queryIn; + } + dim = 0; + type = -1; + if(_dataTree.rows || _flannIndex->isBuilt()) + { + dim = _flannIndex->isBuilt()?_flannIndex->featuresDim():_dataTree.cols; + type = _flannIndex->isBuilt()?_flannIndex->featuresType():_dataTree.type(); + UASSERT(type == CV_32F || type == CV_8U); + } + + if(dim && dim != query.cols) { UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", query.cols, dim); return resultIds; } - if(type != query.type()) + if(type>=0 && type != query.type()) { UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", query.type(), type); return resultIds; @@ -1178,7 +1291,7 @@ std::vector VWDictionary::findNN(const cv::Mat & query) const else if(_strategy == kNNBruteForce) { bruteForce = true; - cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); matcher.knnMatch(query, _dataTree, matches, k); } else if(_strategy == kNNBruteForceGPU) @@ -1188,7 +1301,7 @@ std::vector VWDictionary::findNN(const cv::Mat & query) const #ifdef HAVE_OPENCV_GPU cv::gpu::GpuMat newDescriptorsGpu(query); cv::gpu::GpuMat lastDescriptorsGpu(_dataTree); - if(type==CV_8U) + if(query.type()==CV_8U) { cv::gpu::BruteForceMatcher_GPU gpuMatcher; gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); @@ -1206,7 +1319,7 @@ std::vector VWDictionary::findNN(const cv::Mat & query) const cv::cuda::GpuMat newDescriptorsGpu(query); cv::cuda::GpuMat lastDescriptorsGpu(_dataTree); cv::Ptr gpuMatcher; - if(type==CV_8U) + if(query.type()==CV_8U) { gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING); gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k); @@ -1240,20 +1353,38 @@ std::vector VWDictionary::findNN(const cv::Mat & query) const std::vector > matchesNotIndexed; if(_notIndexedWords.size()) { - cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), dim, type); + cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), query.cols, query.type()); unsigned int index = 0; VisualWord * vw; for(std::set::iterator iter = _notIndexedWords.begin(); iter != _notIndexedWords.end(); ++iter, ++index) { vw = _visualWords.at(*iter); - UASSERT(vw != 0 && vw->getDescriptor().cols == dim && vw->getDescriptor().type() == type); + + cv::Mat descriptor; + if(vw->getDescriptor().type() == CV_8U) + { + if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + { + vw->getDescriptor().convertTo(descriptor, CV_32F); + } + else + { + descriptor = vw->getDescriptor(); + } + } + else + { + descriptor = vw->getDescriptor(); + } + + UASSERT(vw != 0 && descriptor.cols == query.cols && descriptor.type() == query.type()); vw->getDescriptor().copyTo(dataNotIndexed.row(index)); mapIndexIdNotIndexed.insert(mapIndexIdNotIndexed.end(), std::pair(index, vw->id())); } // Find nearest neighbor ULOGGER_DEBUG("Searching in words not indexed..."); - cv::BFMatcher matcher(type==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR); matcher.knnMatch(query, dataNotIndexed, matchesNotIndexed, dataNotIndexed.rows>1?2:1); } ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks()); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 08e8fe53..5c53c3ec 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1303,7 +1303,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) " Dark Blue = Weight Update\n" " Dark Yellow = Proximity Detection in Time\n" " Dark Cyan = Neighbor Link Refined\n" - " Gray = Small Movement"); + " Gray = Small Movement\n" + "Feature Color code:\n" + " Green = New\n" + " Yellow = New but Not Unique\n" + " Red = In Vocabulary\n" + " Blue = In Vocabulary and in Previous Signature\n" + " Pink = In Vocabulary and in Loop Closure Signature"); } // Set color code as tooltip if(_ui->label_matchId->toolTip().isEmpty()) @@ -1312,7 +1318,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) "Background Color Code:\n" " Green = Accepted Loop Closure Detection\n" " Red = Rejected Loop Closure Detection\n" - " Yellow = Proximity Detection in Space"); + " Yellow = Proximity Detection in Space\n" + "Feature Color code:\n" + " Red = In Vocabulary\n" + " Pink = In Vocabulary and in Loop Closure Signature"); } UDEBUG("time= %d ms", time.restart());