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

MERGE branch STM 325:449 into trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@450 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-03-03 01:46:30 +00:00
parent c4aff5f20a
commit a2eb8bcab6
100 changed files with 76652 additions and 5164 deletions
Download Patch File Download Diff File Expand all files Collapse all files

255
corelib/src/DBDriver.cpp
View File

@@ -19,7 +19,7 @@
#include "rtabmap/core/DBDriver.h"
#include "Signature.h"
#include "rtabmap/core/Signature.h"
#include "VWDictionary.h"
#include "utilite/UConversion.h"
#include "utilite/UMath.h"
@@ -32,6 +32,7 @@ namespace rtabmap {
DBDriver::DBDriver(const ParametersMap & parameters) :
_minSignaturesToSave(Parameters::defaultDbMinSignaturesToSave()),
_minWordsToSave(Parameters::defaultDbMinWordsToSave()),
_imagesCompressed(Parameters::defaultDbImagesCompressed()),
_asyncWaiting(true),
_emptyTrashesTime(0)
{
@@ -40,7 +41,7 @@ DBDriver::DBDriver(const ParametersMap & parameters) :
DBDriver::~DBDriver()
{
this->kill();
join(true);
this->emptyTrashes();
}
@@ -55,24 +56,31 @@ void DBDriver::parseParameters(const ParametersMap & parameters)
{
_minWordsToSave = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kDbImagesCompressed())) != parameters.end())
{
_imagesCompressed = uStr2Bool((*iter).second.c_str());
}
}
void DBDriver::closeConnection()
{
this->kill();
UDEBUG("isRunning=%d", this->isRunning());
this->join(true);
UDEBUG("");
this->emptyTrashes();
_dbSafeAccessMutex.lock();
this->disconnectDatabaseQuery();
_dbSafeAccessMutex.unlock();
UDEBUG("");
}
bool DBDriver::openConnection(const std::string & url)
bool DBDriver::openConnection(const std::string & url, bool overwritten)
{
UDEBUG("");
_url = url;
_dbSafeAccessMutex.lock();
if(this->connectDatabaseQuery(url))
if(this->connectDatabaseQuery(url, overwritten))
{
this->start();
_dbSafeAccessMutex.unlock();
return true;
}
@@ -103,6 +111,7 @@ void DBDriver::mainLoop()
{
UDEBUG("");
this->emptyTrashes();
UDEBUG("");
this->kill(); // Do it only once
UDEBUG("");
}
@@ -243,19 +252,15 @@ bool DBDriver::getSignature(int signatureId, Signature ** s)
*s = 0;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::map<int, Signature*>::iterator i=_trashSignatures.begin(); i!=_trashSignatures.end();)
if(_trashSignatures.size())
{
if(i->first == signatureId)
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
std::map<int, Signature*>::iterator iter =_trashSignatures.find(signatureId);
if(iter != _trashSignatures.end())
{
*s = i->second;
_trashSignatures.erase(i++);
break;
}
else
{
++i;
*s = iter->second;
_trashSignatures.erase(iter);
}
}
}
@@ -278,15 +283,15 @@ bool DBDriver::getVisualWord(int wordId, VisualWord ** vw)
*vw = 0;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::map<int, VisualWord*>::iterator i=_trashVisualWords.begin(); i!=_trashVisualWords.end(); ++i)
if(_trashVisualWords.size())
{
if((*i).first == wordId)
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
std::map<int, VisualWord*>::iterator iter = _trashVisualWords.find(wordId);
if(iter != _trashVisualWords.end())
{
*vw = (*i).second;
_trashVisualWords.erase(i);
break;
*vw = iter->second;
_trashVisualWords.erase(iter);
}
}
}
@@ -308,7 +313,7 @@ bool DBDriver::getVisualWord(int wordId, VisualWord ** vw)
bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
{
ULOGGER_DEBUG("");
std::list<KeypointSignature *> toSaveK;
std::list<Signature *> toSave;
std::list<Signature *> toUpdate;
if(this->isConnected() && signatures.size())
{
@@ -318,13 +323,9 @@ bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
{
toUpdate.push_back(*i);
}
else if((*i)->signatureType().compare("KeypointSignature") == 0)
{
toSaveK.push_back((KeypointSignature *)(*i));
}
else
{
ULOGGER_ERROR("Unknown signature type ?!?");
toSave.push_back(*i);
}
}
@@ -332,9 +333,9 @@ bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
{
this->updateQuery(toUpdate);
}
if(toSaveK.size())
if(toSave.size())
{
this->saveQuery(toSaveK);
this->saveQuery(toSave);
}
}
return false;
@@ -358,7 +359,7 @@ bool DBDriver::loadLastSignatures(std::list<Signature *> & signatures) const
return r;
}
bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures, bool onlyParents)
bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
{
UDEBUG("");
// look up in the trash before the database
@@ -376,15 +377,9 @@ bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<
{
if(sIter->first == *iter)
{
if((onlyParents && sIter->second->getLoopClosureId() == 0) || !onlyParents)
{
signatures.push_back(sIter->second);
_trashSignatures.erase(sIter++);
}
else
{
++sIter;
}
signatures.push_back(sIter->second);
_trashSignatures.erase(sIter++);
valueFound = true;
break;
}
@@ -409,7 +404,64 @@ bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadKeypointSignaturesQuery(ids, signatures, onlyParents);
r = this->loadKeypointSignaturesQuery(ids, signatures);
_dbSafeAccessMutex.unlock();
return r;
}
else if(signatures.size())
{
return true;
}
return false;
}
// TODO the same code of method loadKeypointSignatures() above is used here
bool DBDriver::loadSMSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
{
UDEBUG("");
// look up in the trash before the database
std::list<int> ids = signIds;
std::list<Signature*>::iterator sIter;
bool valueFound = false;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
{
valueFound = false;
for(std::map<int, Signature*>::iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end();)
{
if(sIter->first == *iter)
{
signatures.push_back(sIter->second);
_trashSignatures.erase(sIter++);
valueFound = true;
break;
}
else
{
++sIter;
}
}
if(valueFound)
{
iter = ids.erase(iter);
}
else
{
++iter;
}
}
}
_trashesMutex.unlock();
UDEBUG("");
if(ids.size())
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadSMSignaturesQuery(ids, signatures);
_dbSafeAccessMutex.unlock();
return r;
}
@@ -429,23 +481,27 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
// look up in the trash before the database
std::list<int> ids = wordIds;
std::map<int, VisualWord*>::iterator wIter;
std::list<VisualWord *> puttedBack;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
if(_trashVisualWords.size())
{
wIter = _trashVisualWords.find(*iter);
if(wIter != _trashVisualWords.end())
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
{
//UDEBUG("put back word %d from trash", *iter);
vws.push_back(wIter->second);
_trashVisualWords.erase(wIter);
iter = ids.erase(iter);
}
else
{
++iter;
wIter = _trashVisualWords.find(*iter);
if(wIter != _trashVisualWords.end())
{
UDEBUG("put back word %d from trash", *iter);
puttedBack.push_back(wIter->second);
_trashVisualWords.erase(wIter);
iter = ids.erase(iter);
}
else
{
++iter;
}
}
}
}
@@ -456,10 +512,12 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
_dbSafeAccessMutex.lock();
r = this->loadWordsQuery(ids, vws);
_dbSafeAccessMutex.unlock();
uAppend(vws, puttedBack);
return r;
}
else if(vws.size())
else if(puttedBack.size())
{
uAppend(vws, puttedBack);
return true;
}
return false;
@@ -571,78 +629,27 @@ bool DBDriver::deleteUnreferencedWords() const
return false;
}
bool DBDriver::addNeighbor(int id, int newNeighbor, int oldNeighbor)
{
bool r = false;
Signature * s = 0;
_trashesMutex.lock();
s = uValue(_trashSignatures, id, s);
if(s)
{
std::list<std::vector<float> > actions = uValue(s->getNeighbors(), oldNeighbor, std::list<std::vector<float> >());
s->addNeighbor(newNeighbor, actions);
r = true;
}
_trashesMutex.unlock();
if(!r)
{
_dbSafeAccessMutex.lock();
r = this->addNeighborQuery(id, newNeighbor, oldNeighbor);
_dbSafeAccessMutex.unlock();
}
return r;
}
bool DBDriver::removeNeighbor(int id, int neighbor)
{
bool r = false;
Signature * s = 0;
_trashesMutex.lock();
s = uValue(_trashSignatures, id, s);
if(s)
{
s->removeNeighbor(neighbor);
r = true;
}
_trashesMutex.unlock();
if(!r)
{
r = executeNoResult("DELETE FROM Neighbor WHERE sid=" + uNumber2str(id) + " AND nid=" + uNumber2str(neighbor));
}
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getImage(int id, IplImage ** img) const
{
CvMat * compressed = 0;
_dbSafeAccessMutex.lock();
bool result = this->getImageCompressedQuery(id, &compressed);
if(compressed)
{
(*img) = cvDecodeImage(compressed, CV_LOAD_IMAGE_ANYCOLOR);
cvReleaseMat(&compressed);
}
bool result = this->getImageQuery(id, img);
_dbSafeAccessMutex.unlock();
return result;
}
//TODO Check also in the trash ?
bool DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors) const
bool DBDriver::getNeighborIds(int signatureId, std::list<int> & neighbors, bool onlyWithActions) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getNeighborIdsQuery(signatureId, neighbors);
r = this->getNeighborIdsQuery(signatureId, neighbors, onlyWithActions);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::loadNeighbors(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const
bool DBDriver::loadNeighbors(int signatureId, NeighborsMultiMap & neighbors) const
{
bool r;
_dbSafeAccessMutex.lock();
@@ -662,21 +669,11 @@ bool DBDriver::getWeight(int signatureId, int & weight) const
}
//TODO Check also in the trash ?
bool DBDriver::getLoopClosureId(int signatureId, int & loopId) const
bool DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLoopClosureIdQuery(signatureId, loopId);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getImageCompressed(int id, CvMat ** compressed) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getImageCompressedQuery(id, compressed);
r = this->getLoopClosureIdsQuery(signatureId, loopIds, childIds);
_dbSafeAccessMutex.unlock();
return r;
}
@@ -721,16 +718,6 @@ bool DBDriver::getSurfNi(int signatureId, int & ni) const
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getChildrenIds(int signatureId, std::list<int> & ids) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getChildrenIdsQuery(signatureId, ids);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::getHighestWeightedSignatures(unsigned int count, std::multimap<int, int> & ids) const
{
bool r;