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

rtabmap: fixed support of fixed memory and fixed dictionary (localization-only mode from an already built database) -> updated rtabmap example with "-l" option for localization mode

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@878 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-06-06 13:41:49 +00:00
parent 504e51db12
commit 30ab6dca0e
4 changed files with 133 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
corelib/src/Memory.cpp
View File

@@ -435,10 +435,13 @@ bool Memory::update(const Image & image, std::map<std::string, float> & stats)
// signature like a parent to the memory tree, otherwise add
// it as a child to the similar signature.
//============================================================
this->rehearsal(signature, stats);
t=timer.ticks()*1000;
stats.insert(std::pair<std::string, float>(std::string("TimingMem/Rehearsal/ms"), t));
ULOGGER_DEBUG("time rehearsal=%f ms", t);
if(_incrementalMemory)
{
this->rehearsal(signature, stats);
t=timer.ticks()*1000;
stats.insert(std::pair<std::string, float>(std::string("TimingMem/Rehearsal/ms"), t));
ULOGGER_DEBUG("time rehearsal=%f ms", t);
}
//============================================================
// Transfer the oldest signature of the short-term memory to the working memory
@@ -451,7 +454,7 @@ bool Memory::update(const Image & image, std::map<std::string, float> & stats)
++_signaturesAdded;
}
if(!_memoryChanged)
if(!_memoryChanged && _incrementalMemory)
{
_memoryChanged = true;
}
@@ -1138,7 +1141,7 @@ int Memory::cleanup(const std::list<int> & ignoredIds)
// bad signature
if(_lastSignature->isBadSignature() || !_incrementalMemory)
{
moveToTrash(_lastSignature);
moveToTrash(_lastSignature, _incrementalMemory);
++signaturesRemoved;
}
@@ -1359,7 +1362,7 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase)
// Remove neighbor links with bad signatures (assuming that is done in cleanup),
// the bad signatures are always the last signature added.
if(s->isBadSignature())
if(s->isBadSignature() || !saveToDatabase)
{
const std::set<int> & neighbors = s->getNeighbors();
for(std::set<int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
@@ -1466,7 +1469,7 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
ULOGGER_INFO("old=%d, new=%d", oldId, newId);
Signature * oldS = _getSignature(oldId);
Signature * newS = _getSignature(newId);
if(oldS && newS)
if(oldS && newS && _incrementalMemory)
{
const std::set<int> & oldLoopclosureIds = oldS->getLoopClosureIds();
if(oldLoopclosureIds.size() && oldLoopclosureIds.find(newS->id()) != oldLoopclosureIds.end())
@@ -1612,6 +1615,11 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
{
UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
}
if(oldS && newS && !_incrementalMemory)
{
// Accept loop closure while we don't add a link
return true;
}
}
return false;
}

146
corelib/src/VWDictionary.cpp
View File

@@ -123,93 +123,97 @@ void VWDictionary::setIncrementalDictionary(bool incrementalDictionary, const st
{
if(!incrementalDictionary)
{
if((!_incrementalDictionary && _dictionaryPath.compare(dictionaryPath) != 0) ||
_visualWords.size() == 0)
if(!dictionaryPath.empty())
{
std::ifstream file;
if(!dictionaryPath.empty())
if((!_incrementalDictionary && _dictionaryPath.compare(dictionaryPath) != 0) ||
_visualWords.size() == 0)
{
std::ifstream file;
file.open(dictionaryPath.c_str(), std::ifstream::in);
}
if(file.good())
{
UDEBUG("Deleting old dictionary and loading the new one from \"%s\"", dictionaryPath.c_str());
UTimer timer;
// first line is the header
std::string str;
std::list<std::string> strList;
std::getline(file, str);
strList = uSplitNumChar(str);
unsigned int dimension = 0;
for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
if(file.good())
{
if(uIsDigit(iter->at(0)))
UDEBUG("Deleting old dictionary and loading the new one from \"%s\"", dictionaryPath.c_str());
UTimer timer;
// first line is the header
std::string str;
std::list<std::string> strList;
std::getline(file, str);
strList = uSplitNumChar(str);
unsigned int dimension = 0;
for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
{
dimension = std::atoi(iter->c_str());
break;
if(uIsDigit(iter->at(0)))
{
dimension = std::atoi(iter->c_str());
break;
}
}
}
if(dimension == 0 || dimension > 1000)
{
UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
if(dimension == 0 || dimension > 1000)
{
UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
}
else
{
// Process all words
while(file.good())
{
std::getline(file, str);
strList = uSplit(str);
if(strList.size() == dimension+1)
{
//first one is the visual word id
std::list<std::string>::iterator iter = strList.begin();
int id = std::atoi(iter->c_str());
std::vector<float> descriptor(dimension);
++iter;
unsigned int i=0;
//get descriptor
for(;i<dimension && iter != strList.end(); ++i, ++iter)
{
descriptor[i] = std::atof(iter->c_str());
}
if(i != dimension)
{
UERROR("");
}
// laplacian not used
VisualWord * vw = new VisualWord(id, &(descriptor[0]), dimension, 0);
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
}
else
{
UWARN("Cannot parse line \"%s\"", str.c_str());
}
}
this->update();
_incrementalDictionary = false;
}
UDEBUG("Time changing dictionary = %fs", timer.ticks());
}
else
{
// Process all words
while(file.good())
{
std::getline(file, str);
strList = uSplit(str);
if(strList.size() == dimension+1)
{
//first one is the visual word id
std::list<std::string>::iterator iter = strList.begin();
int id = std::atoi(iter->c_str());
std::vector<float> descriptor(dimension);
++iter;
unsigned int i=0;
//get descriptor
for(;i<dimension && iter != strList.end(); ++i, ++iter)
{
descriptor[i] = std::atof(iter->c_str());
}
if(i != dimension)
{
UERROR("");
}
// laplacian not used
VisualWord * vw = new VisualWord(id, &(descriptor[0]), dimension, 0);
_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
}
else
{
UWARN("Cannot parse line \"%s\"", str.c_str());
}
}
this->update();
_incrementalDictionary = false;
UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
}
UDEBUG("Time changing dictionary = %fs", timer.ticks());
file.close();
}
else if(!_incrementalDictionary)
{
UDEBUG("Dictionary \"%s\" already loaded...", dictionaryPath.c_str());
}
else
{
UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", _visualWords.size());
}
file.close();
}
else if(!_incrementalDictionary)
else if(_visualWords.size() == 0)
{
UDEBUG("Dictionary \"%s\" already loaded...", dictionaryPath.c_str());
}
else
{
UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", _visualWords.size());
_incrementalDictionary = false;
}
}
else if(incrementalDictionary && !_incrementalDictionary)
@@ -358,7 +362,7 @@ void VWDictionary::update()
void VWDictionary::clear()
{
if(_visualWords.size())
if(_visualWords.size() && _incrementalDictionary)
{
UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", _visualWords.size());
}