i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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
corelib/src/Memory.cpp
+16 -8
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;
}
corelib/src/VWDictionary.cpp
+75 -71
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());
}
examples/main.cpp
+39 -10
View File
@@ -25,8 +25,10 @@
void showUsage()
{
printf("\nUsage:\n"
"rtabmap-example \"path\"\n"
" path Path to a directory of images\n ");
"rtabmap-example [options] \"path\"\n"
" path Path to a directory of images\n "
" Options:"
" -l localization mode: use already built RTAB-Map database to localize\n ");
exit(1);
}
@@ -35,11 +37,28 @@ int main(int argc, char * argv[])
//ULogger::setType(ULogger::kTypeConsole);
//ULogger::setLevel(ULogger::kDebug);
std::string path;
bool localizationMode = false;
if(argc < 2)
{
showUsage();
}
std::string path = argv[1];
for(int i=1; i<argc-1; ++i)
{
if(strcmp(argv[i], "-l") == 0)
{
localizationMode = true;
}
else
{
printf("Unrecognized option \"%s\"\n", argv[i]);
showUsage();
}
}
path = argv[argc-1];
// rtabmap::Camera is simply a convenience wrapper of OpenCV cv::VideoCapture and cv::imread
rtabmap::CameraImages camera(path);
@@ -66,8 +85,15 @@ int main(int argc, char * argv[])
// Or SURF hessian treshold:
// parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFHessianThreshold(), "150"));
if(localizationMode)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));
}
// Initialize rtabmap: delete/create database...
rtabmap.init(parameters);
rtabmap.init(parameters, !localizationMode);
// Process each image of the directory...
printf("\nProcessing images... from directory \"%s\"\n", path.c_str());
@@ -75,10 +101,11 @@ int main(int argc, char * argv[])
int countLoopDetected=0;
int i=0;
cv::Mat img = camera.takeImage();
int nextIndex = rtabmap.getLastLocationId()+1;
while(!img.empty())
{
// Process image : Main loop of RTAB-Map
rtabmap.process(img);
rtabmap.process(img, nextIndex);
// Check if a loop closure is detected and print some info
if(rtabmap.getLoopClosureId())
@@ -91,11 +118,11 @@ int main(int argc, char * argv[])
printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f) *LOOP %d->%d*\n",
i,
rtabmap.getLastProcessTime(),
rtabmap.getSTM().size(), // short-term memory
rtabmap.getWM().size(), // working memory
(int)rtabmap.getSTM().size(), // short-term memory
(int)rtabmap.getWM().size(), // working memory
rtabmap.getLoopClosureId(),
rtabmap.getLcHypValue(),
rtabmap.getLastLocationId(),
nextIndex,
rtabmap.getLoopClosureId());
}
else
@@ -103,12 +130,14 @@ int main(int argc, char * argv[])
printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f)\n",
i,
rtabmap.getLastProcessTime(),
rtabmap.getSTM().size(), // short-term memory
rtabmap.getWM().size(), // working memory
(int)rtabmap.getSTM().size(), // short-term memory
(int)rtabmap.getWM().size(), // working memory
rtabmap.getRetrievedId(), // highest loop closure hypothesis
rtabmap.getLcHypValue());
}
++nextIndex;
//Get next image
img = camera.takeImage();
}
guilib/src/MainWindow.cpp
+3 -11
View File
@@ -487,7 +487,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
//means it will never used for a loop closure detection
if(_preferencesDialog->isImagesKept())
{
if(stat.loopClosureId()>0 || highestHypothesisId>0)
if(stat.loopClosureId()>0 || highestHypothesisId>0 || stat.localLoopClosureId()>0)
{
// order -> loop closure id -> local loop closure id -> highest loop closure hyp id
int id = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
@@ -506,19 +506,12 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
int matchId = 0;
if(highestHypothesisId > 0 || stat.localLoopClosureId())
if(highestHypothesisId > 0 || stat.localLoopClosureId()>0)
{
bool show = true;
if(stat.loopClosureId() > 0)
{
if(highestHypothesisId != stat.loopClosureId())
{
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::yellow));
}
else
{
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::green));
}
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::green));
_ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
if(highestHypothesisIsSaved)
{
@@ -531,7 +524,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::yellow));
_ui->label_matchId->setText(QString("Local match (%1)").arg(stat.localLoopClosureId()));
}
else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
{