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

Dictionary: new bin descriptor to 32f conversion. RegVis: removed octave comparisons.

Browse Source
This commit is contained in:
matlabbe
2019-04-24 01:24:55 -04:00
parent 1a71406f84
commit f9b7b54454
3 changed files with 109 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/VWDictionary.h
View File

@@ -99,6 +99,10 @@ public:
void removeWords(const std::vector<VisualWord*> & 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();

135
corelib/src/RegistrationVis.cpp
View File

@@ -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<int> descriptorsIndices(indices[i].size());
std::vector<int> 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<indices[i].size(); ++j)
{
int octaveFrom = kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave & 255;
octaveFrom = octaveFrom < 128 ? octaveFrom : (-128 | octaveFrom);
if(abs(octaveFrom-octave) <= octaveError)
{
descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
descriptorsOctave[oi] = octaveFrom;
descriptorsIndices[oi++] = indices[i].at(j);
}
descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
}
descriptorsIndices.resize(oi);
if(oi >=2)
UASSERT(oi >=2);
std::vector<std::vector<cv::DMatch> > 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<std::vector<cv::DMatch> > 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<int> addedOctaves;
for(unsigned int j=0; j<matches[0].size(); ++j)
{
int octave = descriptorsOctave.at(matches[0].at(j).trainIdx);
if(addedOctaves.find(octave) != addedOctaves.end())
{
secondDistance = matches[0].at(j).distance;
break;
}
addedOctaves.insert(octave);
}
if(secondDistance < 0.0f || matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
{
matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
}
}
else if(oi == 1)
{
matchedIndex = descriptorsIndices[0];
matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
}
}
else if(indices[i].size() == 1)
{
int octaveFrom = kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave & 255;
octaveFrom = octaveFrom < 128 ? octaveFrom : (-128 | octaveFrom);
if(abs(octaveFrom-octave) <= octaveError)
{
matchedIndex = indices[i].at(0);
}
matchedIndex = indices[i].at(0);
}
if(matchedIndex >= 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<int> descriptorsIndices(indices[i].size());
std::vector<int> descriptorsOctave(indices[i].size());
int oi=0;
if((int)indices[i].size() > descriptors.rows)
{
descriptors.resize(indices[i].size());
}
std::list<int> indicesToIgnoretmp;
for(unsigned int j=0; j<indices[i].size(); ++j)
{
int octave = kptsTo[indices[i].at(j)].octave & 255;
octave = octave < 128 ? octave : (-128 | octave);
if(abs(octaveFrom-octave) <= octaveError)
{
descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
descriptorsOctave[oi] = octave;
descriptorsIndices[oi++] = indices[i].at(j);
indicesToIgnoretmp.push_back(indices[i].at(j));
}
descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
}
bruteForceDescCopy += bruteForceTimer.ticks();
if(oi >=2)
UASSERT(oi >=2);
std::vector<std::vector<cv::DMatch> > 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<std::vector<cv::DMatch> > 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<int> addedOctaves;
for(unsigned int j=0; j<matches[0].size(); ++j)
{
int octave = descriptorsOctave.at(matches[0].at(j).trainIdx);
if(addedOctaves.find(octave) != addedOctaves.end())
{
secondDistance = matches[0].at(j).distance;
break;
}
addedOctaves.insert(octave);
}
if(secondDistance < 0.0f || matches[0].at(0).distance < _nndr * secondDistance)
{
matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
}
}
else if(oi == 1)
{
matchedIndexTo = descriptorsIndices[0];
matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
}
}
else if(indices[i].size() == 1)
{
int octave = kptsTo[indices[i].at(0)].octave & 255;
octave = octave < 128 ? octave : (-128 | octave);
if(abs(octaveFrom-octave) <= octaveError)
{
matchedIndexTo = indices[i].at(0);
}
matchedIndexTo = indices[i].at(0);
}
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
@@ -1147,11 +1077,18 @@ Transform RegistrationVis::computeTransformationImpl(
// match between all descriptors
VWDictionary dictionary(_featureParameters);
std::list<int> 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<int> toWordIds;

82
corelib/src/VWDictionary.cpp
View File

@@ -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<descriptorsIn.rows; ++i)
{
const unsigned char * ptrIn = descriptorsIn.ptr(i);
float * ptrOut = descriptorsOut.ptr<float>(i);
for(int j=0; j<descriptorsIn.cols; ++j)
{
int jo = j*8;
ptrOut[jo] = (ptrIn[j] & 1) == 1?1.0f:0.0f;
ptrOut[jo+1] = (ptrIn[j] & (1<<1)) != 0?1.0f:0.0f;
ptrOut[jo+2] = (ptrIn[j] & (1<<2)) != 0?1.0f:0.0f;
ptrOut[jo+3] = (ptrIn[j] & (1<<3)) != 0?1.0f:0.0f;
ptrOut[jo+4] = (ptrIn[j] & (1<<4)) != 0?1.0f:0.0f;
ptrOut[jo+5] = (ptrIn[j] & (1<<5)) != 0?1.0f:0.0f;
ptrOut[jo+6] = (ptrIn[j] & (1<<6)) != 0?1.0f:0.0f;
ptrOut[jo+7] = (ptrIn[j] & (1<<7)) != 0?1.0f:0.0f;
}
}
return descriptorsOut;
}
cv::Mat VWDictionary::convert32FToBin(const cv::Mat & descriptorsIn)
{
UASSERT(descriptorsIn.type() == CV_32FC1 && descriptorsIn.cols % 8 == 0);
cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols/8, CV_8UC1);
for(int i=0; i<descriptorsIn.rows; ++i)
{
const float * ptrIn = descriptorsIn.ptr<float>(i);
unsigned char * ptrOut = descriptorsOut.ptr(i);
for(int j=0; j<descriptorsOut.cols; ++j)
{
int jo = j*8;
ptrOut[j] =
(unsigned char)(ptrIn[jo] == 0?0:1) |
(ptrIn[jo+1] == 0?0:(1<<1)) |
(ptrIn[jo+2] == 0?0:(1<<2)) |
(ptrIn[jo+3] == 0?0:(1<<3)) |
(ptrIn[jo+4] == 0?0:(1<<4)) |
(ptrIn[jo+5] == 0?0:(1<<5)) |
(ptrIn[jo+6] == 0?0:(1<<6)) |
(ptrIn[jo+7] == 0?0:(1<<7));
}
}
return descriptorsOut;
}
void VWDictionary::update()
{
ULOGGER_DEBUG("");
@@ -385,9 +440,9 @@ void VWDictionary::update()
if(w->getDescriptor().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<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
int signatureId)
std::list<int> VWDictionary::addNewWords(
const cv::Mat & descriptorsIn,
int signatureId)
{
UDEBUG("id=%d descriptors=%d", signatureId, descriptorsIn.rows);
UTimer timer;
@@ -674,9 +730,9 @@ std::list<int> 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<int> 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<int> 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
{