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: localization mode (run under fixed memory and fixed dictionary), optional retrieving of the last extracted words when calling rtabmap.process() (useful when in localization mode where the last location is deleted after the rtabmap process)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@882 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-06-10 18:54:11 +00:00
parent 30ab6dca0e
commit 6c656e7c57
6 changed files with 56 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/src/DBDriverSqlite3.cpp
View File

@@ -822,8 +822,19 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
}
else
{
ULOGGER_ERROR("Node %d not found in database", *iter);
rc = sqlite3_finalize(ppStmt);
UWARN("Creating a empty signature for node %d", *iter);
Signature * ss = new Signature(*iter, visualWords);
if(ss)
{
ss->setWeight(weight);
ss->setSaved(true);
nodes.push_back(ss);
}
else
{
UFATAL("?Cannot allocate memory !!!");
}
++loaded;
}
//reset

34
corelib/src/Memory.cpp
View File

@@ -35,7 +35,7 @@
namespace rtabmap {
const int Memory::kIdStart = 1;
const int Memory::kIdStart = 0;
const int Memory::kIdVirtual = -1;
const int Memory::kIdInvalid = 0;
@@ -115,14 +115,22 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
_dbDriver->loadLastNodes(dbSignatures);
for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
{
_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
if((int)_stMem.size() <= _maxStMemSize)
// ignore bad signatures
if(!(*iter)->isBadSignature())
{
_stMem.insert((*iter)->id());
_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
if((int)_stMem.size() <= _maxStMemSize)
{
_stMem.insert((*iter)->id());
}
else
{
_workingMem.insert((*iter)->id());
}
}
else
{
_workingMem.insert((*iter)->id());
delete *iter;
}
}
UEventsManager::post(new RtabmapEventInit(std::string("Loading last signatures, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
@@ -135,7 +143,6 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
// Last id
_dbDriver->getLastNodeId(_idCount);
_idCount += 1;
}
else
{
@@ -149,7 +156,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
_workingMem.insert(kIdVirtual);
ULOGGER_DEBUG("ids start with %d", _idCount);
ULOGGER_DEBUG("ids start with %d", _idCount+1);
// Now load the dictionary if we have a connection
@@ -810,7 +817,7 @@ void Memory::getLoopClosureIds(int signatureId, std::set<int> & loopClosureIds,
int Memory::getNextId()
{
return _idCount++;
return ++_idCount;
}
int Memory::getDatabaseMemoryUsed() const
@@ -1390,7 +1397,12 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase)
}
}
const Signature * Memory::getLastSignature() const
int Memory::getLastSignatureId() const
{
return _idCount;
}
const Signature * Memory::getLastWorkingSignature() const
{
ULOGGER_DEBUG("");
return _lastSignature;
@@ -2229,6 +2241,10 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
{
id = this->getNextId();
}
else
{
_idCount = id;
}
int treeSize= _workingMem.size() + _stMem.size();
int nbCommonWords = 0;

19
corelib/src/Rtabmap.cpp
View File

@@ -394,9 +394,9 @@ int Rtabmap::getRetrievedId() const
int Rtabmap::getLastLocationId() const
{
int id = 0;
if(_memory && _memory->getLastSignature())
if(_memory)
{
id = _memory->getLastSignature()->id();
id = _memory->getLastSignatureId();
}
return id;
}
@@ -454,7 +454,7 @@ int Rtabmap::getTotalMemSize() const
{
if(_memory)
{
const Signature * s =_memory->getLastSignature();
const Signature * s =_memory->getLastWorkingSignature();
if(s)
{
return s->id();
@@ -557,7 +557,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
//============================================================
// MAIN LOOP
//============================================================
void Rtabmap::process(const Image & image)
void Rtabmap::process(const Image & image, std::multimap<int, cv::KeyPoint> * words)
{
UDEBUG("");
@@ -630,7 +630,7 @@ void Rtabmap::process(const Image & image)
{
return;
}
signature = _memory->getLastSignature();
signature = _memory->getLastWorkingSignature();
if(!signature)
{
UFATAL("Not supposed to be here...last signature is null?!?");
@@ -640,6 +640,11 @@ void Rtabmap::process(const Image & image)
timeMemoryUpdate = timer.ticks();
ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
if(words)
{
*words = signature->getWords();
}
//============================================================
// Bayes filter update
//============================================================
@@ -1229,10 +1234,10 @@ void Rtabmap::rejectLastLoopClosure()
}
}
void Rtabmap::process(const cv::Mat & image, int id)
void Rtabmap::process(const cv::Mat & image, int id, std::multimap<int, cv::KeyPoint> * words)
{
this->process(Image(image, id));
this->process(Image(image, id), words);
}
void Rtabmap::dumpData() const