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

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 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);
public:
@@ -126,7 +126,7 @@ private:
virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors) const = 0;
virtual bool getWeightQuery(int signatureId, int & weight) 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 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;
}
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;
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);
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;
}
_trashesMutex.unlock();
@@ -587,7 +589,7 @@ bool DBDriver::addNeighbor(int id, int neighbor, const std::list<std::vector<flo
if(!r)
{
_dbSafeAccessMutex.lock();
r = this->addNeighborQuery(id, neighbor, actuatorStates);
r = this->addNeighborQuery(id, newNeighbor, oldNeighbor);
_dbSafeAccessMutex.unlock();
}

11
corelib/src/DBDriverSqlite3.cpp
View File

@@ -804,11 +804,16 @@ bool DBDriverSqlite3::getLoopClosureIdQuery(int signatureId, int & loopId) const
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)
{
NeighborsMap neighbors;
this->loadNeighborsQuery(id, neighbors);
std::list<std::vector<float> > actuatorStates = uValue(neighbors, oldNeighbor, std::list<std::vector<float> >());
UTimer timer;
timer.start();
int rc = SQLITE_OK;
@@ -840,7 +845,7 @@ bool DBDriverSqlite3::addNeighborQuery(int id, int neighbor, const std::list<std
rc = sqlite3_finalize(ppStmt);
return false;
}
rc = sqlite3_bind_int(ppStmt, 2, neighbor);
rc = sqlite3_bind_int(ppStmt, 2, newNeighbor);
if (rc != SQLITE_OK)
{
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 getWeightQuery(int signatureId, int & weight) 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 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);
if(!newHasNeighbor)
{
//empty actions... they are not backward compatible
newS->addNeighbor(i->first, std::list<std::vector<float> >());
newS->addNeighbor(i->first, i->second);
}
if(neighbor)
{
@@ -1080,7 +1079,7 @@ void Memory::addLoopClosureLink(int oldId, int newId, bool rehearsal)
else if(_dbDriver)
{
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());
}
}
}