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

Fixed the adding of neighbor's actions when adding a loop closure link

Browse Source 
Fixed loading config file with the ros gui_node (switch between rosparam and a config file )

git-svn-id: http://rtabmap.googlecode.com/svn/branches/0.3/rtabmap@97 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2011-06-10 19:56:07 +00:00
parent b01f3191fa
commit 196dc01a57
5 changed files with 18 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -71,7 +71,7 @@ public:
bool deleteAllObsoleteSSVWLinks() const; bool deleteAllObsoleteSSVWLinks() const;
bool deleteUnreferencedWords() const; bool deleteUnreferencedWords() const;
bool addNeighbor(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates); bool addNeighbor(int id, int newNeighbor, int oldNeighbor);
bool removeNeighbor(int id, int neighbor); bool removeNeighbor(int id, int neighbor);
public: public:
@@ -126,7 +126,7 @@ private:
virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors) const = 0; virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors) const = 0;
virtual bool getWeightQuery(int signatureId, int & weight) const = 0; virtual bool getWeightQuery(int signatureId, int & weight) const = 0;
virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const = 0; virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const = 0;
virtual bool addNeighborQuery(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates) const = 0; virtual bool addNeighborQuery(int id, int newNeighbor, int oldNeighbor) const = 0;
virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const = 0; virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const = 0;
virtual bool updateQuery(const std::list<Signature *> & signatures) const = 0; virtual bool updateQuery(const std::list<Signature *> & signatures) const = 0;

8
corelib/src/DBDriver.cpp
View File

@@ -571,7 +571,7 @@ bool DBDriver::deleteUnreferencedWords() const
return false; return false;
} }
bool DBDriver::addNeighbor(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates) bool DBDriver::addNeighbor(int id, int newNeighbor, int oldNeighbor)
{ {
bool r = false; bool r = false;
Signature * s = 0; Signature * s = 0;
@@ -579,7 +579,9 @@ bool DBDriver::addNeighbor(int id, int neighbor, const std::list<std::vector<flo
s = uValue(_trashSignatures, id, s); s = uValue(_trashSignatures, id, s);
if(s) if(s)
{ {
s->addNeighbor(neighbor, actuatorStates); const NeighborsMap & neighbors = s->getNeighbors();
std::list<std::vector<float> > actions = uValue(s->getNeighbors(), oldNeighbor, std::list<std::vector<float> >());
s->addNeighbor(newNeighbor, actions);
r = true; r = true;
} }
_trashesMutex.unlock(); _trashesMutex.unlock();
@@ -587,7 +589,7 @@ bool DBDriver::addNeighbor(int id, int neighbor, const std::list<std::vector<flo
if(!r) if(!r)
{ {
_dbSafeAccessMutex.lock(); _dbSafeAccessMutex.lock();
r = this->addNeighborQuery(id, neighbor, actuatorStates); r = this->addNeighborQuery(id, newNeighbor, oldNeighbor);
_dbSafeAccessMutex.unlock(); _dbSafeAccessMutex.unlock();
} }

11
corelib/src/DBDriverSqlite3.cpp
View File

@@ -804,11 +804,16 @@ bool DBDriverSqlite3::getLoopClosureIdQuery(int signatureId, int & loopId) const
return false; return false;
} }
bool DBDriverSqlite3::addNeighborQuery(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates) const bool DBDriverSqlite3::addNeighborQuery(int id, int newNeighbor, int oldNeighbor) const
{ {
ULOGGER_DEBUG("id=%d, neighbor=%d, actuatorStates=%d", id, neighbor, actuatorStates.size()); ULOGGER_DEBUG("id=%d, newNeighbor=%d, oldNeighbor=%d", id, newNeighbor, oldNeighbor);
if(_ppDb) if(_ppDb)
{ {
NeighborsMap neighbors;
this->loadNeighborsQuery(id, neighbors);
std::list<std::vector<float> > actuatorStates = uValue(neighbors, oldNeighbor, std::list<std::vector<float> >());
UTimer timer; UTimer timer;
timer.start(); timer.start();
int rc = SQLITE_OK; int rc = SQLITE_OK;
@@ -840,7 +845,7 @@ bool DBDriverSqlite3::addNeighborQuery(int id, int neighbor, const std::list<std
rc = sqlite3_finalize(ppStmt); rc = sqlite3_finalize(ppStmt);
return false; return false;
} }
rc = sqlite3_bind_int(ppStmt, 2, neighbor); rc = sqlite3_bind_int(ppStmt, 2, newNeighbor);
if (rc != SQLITE_OK) if (rc != SQLITE_OK)
{ {
ULOGGER_ERROR("DB error 2.2: %s", sqlite3_errmsg(_ppDb)); ULOGGER_ERROR("DB error 2.2: %s", sqlite3_errmsg(_ppDb));

2
corelib/src/DBDriverSqlite3.h
View File

@@ -50,7 +50,7 @@ private:
virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors) const; virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors) const;
virtual bool getWeightQuery(int signatureId, int & weight) const; virtual bool getWeightQuery(int signatureId, int & weight) const;
virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const; virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const;
virtual bool addNeighborQuery(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates) const; virtual bool addNeighborQuery(int id, int newNeighbor, int oldNeighbor) const;
virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const; virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const;
virtual bool updateQuery(const std::list<Signature *> & signatures) const; virtual bool updateQuery(const std::list<Signature *> & signatures) const;

5
corelib/src/Memory.cpp
View File

@@ -1056,8 +1056,7 @@ void Memory::addLoopClosureLink(int oldId, int newId, bool rehearsal)
bool newHasNeighbor = newS->hasNeighbor(i->first); bool newHasNeighbor = newS->hasNeighbor(i->first);
if(!newHasNeighbor) if(!newHasNeighbor)
{ {
//empty actions... they are not backward compatible newS->addNeighbor(i->first, i->second);
newS->addNeighbor(i->first, std::list<std::vector<float> >());
} }
if(neighbor) if(neighbor)
{ {
@@ -1080,7 +1079,7 @@ void Memory::addLoopClosureLink(int oldId, int newId, bool rehearsal)
else if(_dbDriver) else if(_dbDriver)
{ {
ULOGGER_DEBUG("*i=%d not found in WM or STM, modifying it in database...", i->first); ULOGGER_DEBUG("*i=%d not found in WM or STM, modifying it in database...", i->first);
_dbDriver->addNeighbor(i->first, newS->id(), i->second); _dbDriver->addNeighbor(i->first, newS->id(), oldS->id());
} }
} }
} }