From f9b7b54454a024920b5bf31c25f3b1d6935bbec3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 24 Apr 2019 01:24:55 -0400 Subject: [PATCH] Dictionary: new bin descriptor to 32f conversion. RegVis: removed octave comparisons. --- corelib/include/rtabmap/core/VWDictionary.h | 4 + corelib/src/RegistrationVis.cpp | 135 ++++++-------------- corelib/src/VWDictionary.cpp | 82 ++++++++++-- 3 files changed, 109 insertions(+), 112 deletions(-) diff --git a/corelib/include/rtabmap/core/VWDictionary.h b/corelib/include/rtabmap/core/VWDictionary.h index a095f338..b56b31a4 100644 --- a/corelib/include/rtabmap/core/VWDictionary.h +++ b/corelib/include/rtabmap/core/VWDictionary.h @@ -99,6 +99,10 @@ public: void removeWords(const std::vector & words); // caller must delete the words void deleteUnusedWords(); +public: + static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn); + static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn); + protected: int getNextId(); diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index 69f6c02d..6a2ade2d 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -794,7 +794,6 @@ Transform RegistrationVis::computeTransformationImpl( UDEBUG("guessMatchToProjection=%d, cornersProjected=%d", _guessMatchToProjection?1:0, (int)cornersProjected.size()); if(cornersProjected.size()) { - int octaveError = 1; if(_guessMatchToProjection) { // match frame to projected @@ -826,14 +825,10 @@ Transform RegistrationVis::computeTransformationImpl( cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type()); for(unsigned int i = 0; i < pointsToMat.rows; ++i) { - // Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554) - int octave = kptsTo[i].octave & 255; - octave = octave < 128 ? octave : (-128 | octave); int matchedIndex = -1; if(indices[i].size() >= 2) { std::vector descriptorsIndices(indices[i].size()); - std::vector descriptorsOctave(indices[i].size()); int oi=0; if((int)indices[i].size() > descriptors.rows) { @@ -841,54 +836,25 @@ Transform RegistrationVis::computeTransformationImpl( } for(unsigned int j=0; j=2) + UASSERT(oi >=2); + + std::vector > matches; + cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2); + UASSERT(matches.size() == 1); + UASSERT(matches[0].size() == 2); + if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance) { - std::vector > matches; - cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); - matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2 + octaveError*2); - UASSERT(matches.size() == 1); - UASSERT(matches[0].size() >= 2); - float secondDistance = -1.0f; - std::set addedOctaves; - for(unsigned int j=0; j= 0) @@ -994,74 +960,38 @@ Transform RegistrationVis::computeTransformationImpl( if(util3d::isFinite(kptsFrom3D[matchedIndexFrom])) { - // Make octave compatible with SIFT packed octave (https://github.com/opencv/opencv/issues/4554) - int octaveFrom = kptsFrom.at(matchedIndexFrom).octave & 255; - octaveFrom = octaveFrom < 128 ? octaveFrom : (-128 | octaveFrom); - int matchedIndexTo = -1; if(indices[i].size() >= 2) { bruteForceTimer.restart(); std::vector descriptorsIndices(indices[i].size()); - std::vector descriptorsOctave(indices[i].size()); int oi=0; if((int)indices[i].size() > descriptors.rows) { descriptors.resize(indices[i].size()); } - std::list indicesToIgnoretmp; for(unsigned int j=0; j=2) + UASSERT(oi >=2); + + std::vector > matches; + cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); + matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2); + UASSERT(matches.size() == 1); + UASSERT(matches[0].size() == 2); + bruteForceTotalTime+=bruteForceTimer.elapsed(); + if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance) { - std::vector > matches; - cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR); - matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2 + octaveError*2); - UASSERT(matches.size() == 1); - UASSERT(matches[0].size() >= 2); - bruteForceTotalTime+=bruteForceTimer.elapsed(); - float secondDistance = -1.0f; - std::set addedOctaves; - for(unsigned int j=0; j fromWordIds; - for (int i = 0; i < descriptorsFrom.rows; ++i) + if(orignalWordsFromIds.empty()) { - int id = orignalWordsFromIds.size() ? orignalWordsFromIds[i] : i; - dictionary.addWord(new VisualWord(id, descriptorsFrom.row(i), 1)); - fromWordIds.push_back(id); + fromWordIds = dictionary.addNewWords(descriptorsFrom, 1); + } + else + { + for (int i = 0; i < descriptorsFrom.rows; ++i) + { + int id = orignalWordsFromIds[i]; + dictionary.addWord(new VisualWord(id, descriptorsFrom.row(i), 1)); + fromWordIds.push_back(id); + } } std::list toWordIds; diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index c7d2f9e3..563b976f 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -345,6 +345,61 @@ unsigned int VWDictionary::getIndexMemoryUsed() const return _flannIndex->memoryUsed(); } +cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn) +{ + // Old approach + //cv::Mat descriptorsOut; + //descriptorsIn.convertTo(descriptorsOut, CV_32F); + //return descriptorsOut; + + // New approach + UASSERT(descriptorsIn.type() == CV_8UC1); + cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols*8, CV_32FC1); + for(int i=0; i(i); + for(int j=0; j(i); + unsigned char * ptrOut = descriptorsOut.ptr(i); + for(int j=0; jgetDescriptor().type() == CV_8U) { useDistanceL1_ = true; - if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + if(_strategy == kNNFlannKdTree) { - w->getDescriptor().convertTo(descriptor, CV_32F); + descriptor = convertBinTo32F(w->getDescriptor()); } else { @@ -475,7 +530,7 @@ void VWDictionary::update() if(_visualWords.begin()->second->getDescriptor().type() == CV_8U) { useDistanceL1_ = true; - if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + if(_strategy == kNNFlannKdTree) { type = CV_32F; } @@ -501,9 +556,9 @@ void VWDictionary::update() cv::Mat descriptor; if(iter->second->getDescriptor().type() == CV_8U) { - if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + if(_strategy == kNNFlannKdTree) { - iter->second->getDescriptor().convertTo(descriptor, CV_32F); + descriptor = convertBinTo32F(iter->second->getDescriptor()); } else { @@ -631,8 +686,9 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId) } } -std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, - int signatureId) +std::list VWDictionary::addNewWords( + const cv::Mat & descriptorsIn, + int signatureId) { UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows); UTimer timer; @@ -674,9 +730,9 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, if(descriptorsIn.type() == CV_8U) { useDistanceL1_ = true; - if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + if(_strategy == kNNFlannKdTree) { - descriptorsIn.convertTo(descriptors, CV_32F); + descriptors = convertBinTo32F(descriptorsIn); } else { @@ -1004,9 +1060,9 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const cv::Mat query; if(queryIn.type() == CV_8U) { - if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + if(_strategy == kNNFlannKdTree) { - queryIn.convertTo(query, CV_32F); + query = convertBinTo32F(queryIn); } else { @@ -1129,9 +1185,9 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const cv::Mat descriptor; if(vw->getDescriptor().type() == CV_8U) { - if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive) + if(_strategy == kNNFlannKdTree) { - vw->getDescriptor().convertTo(descriptor, CV_32F); + descriptor = convertBinTo32F(vw->getDescriptor()); } else {