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

Retrieved data are now included in the rtabmap info message

Browse Source 
Map transform is only defined with the last loop closure for localization mode: on loop closure between two maps, there is only a TORO link added between the locations (no need to keep the transform between the maps) 

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1058 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-01-08 16:11:00 +00:00
parent 6fc712c15d
commit 11eed189c9
12 changed files with 153 additions and 377 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
corelib/src/DBDriver.cpp
View File

@@ -280,13 +280,6 @@ void DBDriver::load(VWDictionary * dictionary) const
_dbSafeAccessMutex.unlock();
}
void DBDriver::load(std::map<int, std::map<int, Transform> > & mapTransforms) const
{
_dbSafeAccessMutex.lock();
this->loadQuery(mapTransforms);
_dbSafeAccessMutex.unlock();
}
void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
{
_dbSafeAccessMutex.lock();
@@ -480,13 +473,6 @@ void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
_dbSafeAccessMutex.unlock();
}
void DBDriver::save(const std::map<int, std::map<int, Transform> > & mapTransforms) const
{
_dbSafeAccessMutex.lock();
saveQuery(mapTransforms);
_dbSafeAccessMutex.unlock();
}
void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const
{
ULOGGER_DEBUG("");

116
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1178,71 +1178,6 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
}
}
void DBDriverSqlite3::loadQuery(std::map<int, std::map<int, Transform> > & mapTransforms) const
{
ULOGGER_DEBUG("");
if(_ppDb)
{
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT source_map_id, target_map_id, transform "
"FROM MapLink;";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
int sourceId;
int targetId;
int dataSize;
const void * data;
// Process the result if one
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index = 0;
sourceId = sqlite3_column_int(ppStmt, index++);
targetId = sqlite3_column_int(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize/int(sizeof(float)) != 12)
{
UERROR("Transform size (bytes=%d, length=%d) != 12 !?!", dataSize, dataSize/int(sizeof(float)));
}
else
{
const float * datafloat = (const float *)data;
Transform transform(datafloat[0], datafloat[1], datafloat[2], datafloat[3],
datafloat[4], datafloat[5], datafloat[6], datafloat[7],
datafloat[8], datafloat[9], datafloat[10], datafloat[11]);
if(!uContains(mapTransforms, sourceId))
{
mapTransforms.insert(std::make_pair(sourceId, std::map<int, Transform>()));
}
UASSERT(!uContains(mapTransforms.at(sourceId), targetId));
mapTransforms.at(sourceId).insert(std::make_pair(targetId, transform));
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%fs", timer.ticks());
}
}
//may be slower than the previous version but don't have a limit of words that can be loaded at the same time
void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const
{
@@ -1804,57 +1739,6 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
}
}
void DBDriverSqlite3::saveQuery(const std::map<int, std::map<int, Transform> > & mapTransforms) const
{
UDEBUG("mapTransforms size=%d", mapTransforms.size());
if(_ppDb)
{
// First delete all transforms
this->executeNoResult("DELETE FROM MapLink;");
std::string type;
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::string query;
// Create new entries in table MapLink
if(mapTransforms.size()>0)
{
query = std::string("INSERT INTO MapLink(source_map_id, target_map_id, transform) VALUES(?,?,?);");
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
for(std::map<int, std::map<int, Transform> >::const_iterator iter=mapTransforms.begin(); iter!=mapTransforms.end(); ++iter)
{
for(std::map<int, Transform>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
int index=1;
rc = sqlite3_bind_int(ppStmt, index++, iter->first);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, index++, jter->first);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_blob(ppStmt, index++, jter->second.data(), jter->second.size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
//execute query
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
UDEBUG("Time=%fs", timer.ticks());
}
}
std::string DBDriverSqlite3::queryStepNode() const
{
return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);";

2
corelib/src/DBDriverSqlite3.h
View File

@@ -54,11 +54,9 @@ private:
virtual void saveQuery(const std::list<VisualWord *> & words) const;
virtual void updateQuery(const std::list<Signature *> & signatures) const;
virtual void updateQuery(const std::list<VisualWord *> & words) const;
virtual void saveQuery(const std::map<int, std::map<int, Transform> > & mapTransforms) const;
// Load objects
virtual void loadQuery(VWDictionary * dictionary) const;
virtual void loadQuery(std::map<int, std::map<int, Transform> > & mapTransforms) const;
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;

111
corelib/src/Memory.cpp
View File

@@ -202,15 +202,6 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize())));
}
// Get map transforms
if(_dbDriver && _dbDriver->isConnected())
{
if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Loading map transforms..."));
_dbDriver->load(_mapTransforms);
UDEBUG("%d map links loaded!", (int)_mapTransforms.size());
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading map transforms, done! (%d links)", (int)_mapTransforms.size())));
}
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
// Enable loaded signatures
Signature * ss;
@@ -642,42 +633,6 @@ int Memory::getVWDictionarySize() const
return _vwd->getVisualWords().size();
}
Transform Memory::getMapTransform(int sourceMapId, int targetMapId) const
{
Transform t;
if(sourceMapId!=targetMapId)
{
std::map<int, std::map<int, Transform> >::const_iterator iter = _mapTransforms.find(sourceMapId);
if(iter != _mapTransforms.end())
{
std::map<int, Transform>::const_iterator jter = iter->second.find(targetMapId);
if(jter != iter->second.end())
{
t = jter->second;
}
}
}
else
{
t.setIdentity();
}
return t;
}
void Memory::removeMapTransform(int sourceId, int targetId)
{
std::map<int, std::map<int, Transform> >::iterator iter = _mapTransforms.find(sourceId);
if(iter != _mapTransforms.end())
{
iter->second.erase(targetId);
}
iter = _mapTransforms.find(targetId);
if(iter != _mapTransforms.end())
{
iter->second.erase(sourceId);
}
}
void Memory::getPose(int locationId, int targetMapId, Transform & pose, bool lookInDatabase) const
{
const Signature * s = getSignature(locationId);
@@ -691,31 +646,6 @@ void Memory::getPose(int locationId, int targetMapId, Transform & pose, bool loo
{
_dbDriver->getPose(locationId, pose, mapId);
}
if(!pose.isNull() && mapId >= 0 && mapId != targetMapId)
{
std::map<int, std::map<int, Transform> >::const_iterator iter = _mapTransforms.find(mapId);
if(iter != _mapTransforms.end())
{
std::map<int, Transform>::const_iterator jter = iter->second.find(targetMapId);
if(jter != iter->second.end())
{
// transform pose to target map
pose = jter->second * pose;
}
else
{
pose.setNull();
UWARN("No map transforms from map %d to target map %d!", mapId, targetMapId);
}
}
else
{
pose.setNull();
UWARN("No map transforms for map %d!", mapId);
}
}
}
std::map<int, Transform> Memory::getNeighborLinks(int signatureId, bool ignoreNeighborByLoopClosure, bool lookInDatabase) const
@@ -1011,11 +941,6 @@ void Memory::clear()
this->cleanUnusedWords();
if(_dbDriver)
{
_dbDriver->save(_mapTransforms);
}
if(_dbDriver)
{
_dbDriver->emptyTrashes();
@@ -1084,7 +1009,6 @@ void Memory::clear()
_idCount = kIdStart;
_idMapCount = kIdStart;
_memoryChanged = false;
_mapTransforms.clear();
if(_dbDriver)
{
@@ -1720,12 +1644,12 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
}
else if(inliersCount < _bowMinInliers)
{
UWARN("Not enough inliers %d/%d", inliersCount, _bowMinInliers);
UINFO("Not enough inliers %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
}
}
else
{
UWARN("Not enough inliers %d/%d", (int)inliersOld->size(), _bowMinInliers);
UDEBUG("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
}
}
else if(!oldS.isBadSignature() && !newS.isBadSignature())
@@ -2088,37 +2012,6 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
UDEBUG("Add loop closure link between %d and %d", oldS->id(), newS->id());
if(oldS->mapId() != newS->mapId())
{
Transform oldMapToNewMap = oldS->getPose() * transform.inverse() * newS->getPose().inverse();
oldMapToNewMap = oldMapToNewMap.inverse(); // FIXME: why ?!?
UINFO("Adding loop closure between %d and %d which don't belong to the same map (%d vs %d). "
"Add map transform between map %d and %d (%s).",
oldId, newId,
oldS->mapId(), newS->mapId(),
oldS->mapId(), newS->mapId(),
oldMapToNewMap.prettyPrint().c_str());
//update to new transform if it is already set
removeMapTransform(oldS->mapId(), newS->mapId());
if(!uContains(_mapTransforms, oldS->mapId()))
{
_mapTransforms.insert(std::make_pair(oldS->mapId(), std::map<int, Transform>()));
}
if(!uContains(_mapTransforms, newS->mapId()))
{
_mapTransforms.insert(std::make_pair(newS->mapId(), std::map<int, Transform>()));
}
UASSERT(!uContains(_mapTransforms.at(oldS->mapId()), newS->mapId()));
UASSERT(!uContains(_mapTransforms.at(newS->mapId()), oldS->mapId()));
_mapTransforms.at(oldS->mapId()).insert(std::make_pair(newS->mapId(), oldMapToNewMap));
_mapTransforms.at(newS->mapId()).insert(std::make_pair(oldS->mapId(), oldMapToNewMap.inverse()));
}
oldS->addLoopClosureId(newS->id(), transform.inverse());
newS->addChildLoopClosureId(oldS->id(), transform);

107
corelib/src/Rtabmap.cpp
View File

@@ -96,7 +96,8 @@ Rtabmap::Rtabmap() :
_foutFloat(0),
_foutInt(0),
_wDir(std::string(".")+UDirectory::separator()),
_mapCorrection(Transform::getIdentity())
_mapCorrection(Transform::getIdentity()),
_mapTransform(Transform::getIdentity())
{
}
@@ -593,6 +594,7 @@ void Rtabmap::resetMemory(bool dbOverwritten)
_lastProcessTime = 0.0;
_optimizedPoses.clear();
_mapCorrection.setIdentity();
_mapTransform.setIdentity();
if(_memory)
{
@@ -1187,6 +1189,25 @@ bool Rtabmap::process(const Image & image)
{
_lcHypothesisId = 0;
}
else
{
// used for localization mode
_mapTransform.setIdentity();
const Signature * oldS = _memory->getSignature(_lcHypothesisId);
UASSERT(oldS != 0);
if(oldS->mapId() != signature->mapId())
{
// New map -> old map
_mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
UINFO("Adding loop closure between %d and %d which don't belong to the same map (%d vs %d). "
"Add map transform between map %d and %d (%s).",
oldS->id(), signature->id(),
oldS->mapId(), signature->mapId(),
oldS->mapId(), signature->mapId(),
_mapTransform.prettyPrint().c_str());
}
}
}
timeAddLoopClosureLink = timer.ticks();
ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
@@ -1262,9 +1283,7 @@ bool Rtabmap::process(const Image & image)
const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0);
Transform correction = _optimizedPoses.at(oldId) * oldS->getPose().inverse();
Transform mapTransform = _memory->getMapTransform(signature->mapId(), oldS->mapId());
UASSERT(!mapTransform.isNull());
_mapCorrection = correction*mapTransform;
_mapCorrection = correction * _mapTransform;
}
}
else
@@ -1304,11 +1323,9 @@ bool Rtabmap::process(const Image & image)
{
ULOGGER_INFO("sending stats...");
statistics_.setRefImageId(signature->id());
statistics_.setRefImageMapId(signature->mapId());
if(_lcHypothesisId != Memory::kIdInvalid)
{
statistics_.setLoopClosureId(_lcHypothesisId);
statistics_.setLoopClosureMapId(_memory->getSignature(_lcHypothesisId)->mapId());
ULOGGER_INFO("Loop closure detected! With id=%d", _lcHypothesisId);
}
if(_publishStats)
@@ -1330,7 +1347,12 @@ bool Rtabmap::process(const Image & image)
if(localSpaceClosureId)
{
statistics_.setLocalLoopClosureId(localSpaceClosureId);
statistics_.setLocalLoopClosureMapId(_memory->getSignature(localSpaceClosureId)->mapId());
}
if(_lcHypothesisId || localSpaceClosureId)
{
UASSERT(uContains(sLoop->getLoopClosureIds(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLoopClosureIds().at(signature->id()).prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLoopClosureIds().at(signature->id()));
}
statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
@@ -1359,48 +1381,66 @@ bool Rtabmap::process(const Image & image)
if(_publishImage)
{
statistics_.setRefImage(signature->getImage()); // raw data
if(_rgbdSlamMode)
{
statistics_.setRefDepth(signature->getDepth());
statistics_.setRefDepth2D(signature->getDepth2D());
statistics_.setRefDepthConstant(signature->getDepthConstant());
statistics_.setRefLocalTransform(signature->getLocalTransform());
}
std::map<int, int> mapIds;
std::map<int, std::vector<unsigned char> > images;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depth2ds;
std::map<int, float> depthConstants;
std::map<int, Transform> localTransforms;
std::vector<int> ids(signaturesRetrieved.begin(), signaturesRetrieved.end());
ids.push_back(signature->id());
if(sLoop)
{
UTimer tmpTimer;
ids.push_back(sLoop->id());
}
UTimer tmpTimer;
for(unsigned int i=0; i<ids.size(); ++i)
{
// Add data
std::vector<unsigned char> im;
if(_rgbdSlamMode)
{
std::vector<unsigned char> depth, depth2d;
float depthConstant;
Transform localTransform;
_memory->getImageDepth(sLoop->id(), im, depth, depth2d, depthConstant, localTransform);
_memory->getImageDepth(ids[i], im, depth, depth2d, depthConstant, localTransform);
if(tmpTimer.elapsed() > 0.03)
if(!depth.empty())
{
UWARN("getting image time = %fs", tmpTimer.ticks());
depths.insert(std::make_pair(ids[i], depth));
depthConstants.insert(std::make_pair(ids[i], depthConstant));
localTransforms.insert(std::make_pair(ids[i], localTransform));
}
statistics_.setLoopDepth(depth);
statistics_.setLoopDepth2D(depth2d);
statistics_.setLoopDepthConstant(depthConstant);
statistics_.setLoopLocalTransform(localTransform);
if(_lcHypothesisId || localSpaceClosureId)
if(!depth2d.empty())
{
UASSERT(uContains(sLoop->getLoopClosureIds(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLoopClosureIds().at(signature->id()).prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLoopClosureIds().at(signature->id()));
depth2ds.insert(std::make_pair(ids[i], depth2d));
}
}
else
{
im = _memory->getImage(sLoop->id());
im = _memory->getImage(ids[i]);
}
UASSERT(_memory->getSignature(ids[i]) != 0);
mapIds.insert(std::make_pair(ids[i], _memory->getSignature(ids[i])->mapId()));
if(!im.empty())
{
images.insert(std::make_pair(ids[i], im));
}
statistics_.setLoopImage(im);
}
if(tmpTimer.elapsed() > 0.03)
{
UWARN("getting data[%d] time = %fs", (int)ids.size(), tmpTimer.ticks());
}
statistics_.setMapIds(mapIds);
statistics_.setImages(images);
statistics_.setDepths(depths);
statistics_.setDepth2ds(depth2ds);
statistics_.setDepthConstants(depthConstants);
statistics_.setLocalTransforms(localTransforms);
}
if(_publishLikelihood || _publishPdf)
@@ -1686,9 +1726,8 @@ std::map<int, Transform> Rtabmap::getOptimizedWMPosesInRadius(
const std::set<int> & stm = _memory->getStMem();
for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
// Only locations in Working Memory and in the same map or a map link exist between them (only
// global loop closure detection can do loop closure between not joined maps).
if(stm.find(iter->first) == stm.end() && !_memory->getMapTransform(fromS->mapId(), _memory->getSignature(iter->first)->mapId()).isNull())
// Only locations in Working Memory and in the same map.
if(stm.find(iter->first) == stm.end() && fromS->mapId() == _memory->getSignature(iter->first)->mapId())
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;

4
corelib/src/Statistics.cpp
View File

@@ -34,9 +34,7 @@ Statistics::Statistics() :
_extended(0),
_refImageId(0),
_loopClosureId(0),
_localLoopClosureId(0),
_refDepthConstant(0),
_loopDepthConstant(0)
_localLoopClosureId(0)
{
_defaultDataInitialized = true;
}

6
corelib/src/resources/DatabaseSchema.sql
View File

@@ -22,12 +22,6 @@ CREATE TABLE Node (
PRIMARY KEY (id)
);
CREATE TABLE MapLink (
source_map_id INTEGER NOT NULL,
target_map_id INTEGER NOT NULL,
transform BLOB
);
CREATE TABLE Image (
id INTEGER NOT NULL,
data BLOB,