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

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

@@ -82,7 +82,6 @@ public:
// Load objects
void load(VWDictionary * dictionary) const;
void load(std::map<int, std::map<int, Transform> > & mapTransforms) const;
void loadLastNodes(std::list<Signature *> & signatures) const;
void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
@@ -99,7 +98,6 @@ public:
void getLastNodeId(int & id) const;
void getLastWordId(int & id) const;
void getInvertedIndexNi(int signatureId, int & ni) const;
void save(const std::map<int, std::map<int, Transform> > & mapTransforms) const;
protected:
DBDriver(const ParametersMap & parameters = ParametersMap());
@@ -118,12 +116,10 @@ private:
virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
virtual void updateQuery(const std::list<VisualWord *> & words) const = 0;
virtual void saveQuery(const std::map<int, std::map<int, Transform> > & mapTransforms) const = 0;
// Load objects
virtual void loadQuery(VWDictionary * dictionary) const = 0;
virtual void loadQuery(std::map<int, std::map<int, Transform> > & mapTransforms) const = 0;
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;

3
corelib/include/rtabmap/core/Memory.h
View File

@@ -86,8 +86,6 @@ public:
const std::set<int> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;}
int getMaxStMemSize() const {return _maxStMemSize;}
Transform getMapTransform(int sourceMapId, int targetMapId) const;
void removeMapTransform(int soureId, int targetId);
void getPose(int locationId,
int targetMapId,
Transform & pose,
@@ -209,7 +207,6 @@ private:
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem; // id
std::set<int> _workingMem; // id,age
std::map<int, std::map<int, Transform> > _mapTransforms; // Transform between maps <fromMapId, <toMapId, transform> >
//Keypoint stuff
VWDictionary * _vwd;

1
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -161,6 +161,7 @@ private:
std::map<int, Transform> _optimizedPoses;
Transform _mapCorrection;
Transform _mapTransform;
};
#endif /* RTABMAP_H_ */

61
corelib/include/rtabmap/core/Statistics.h
View File

@@ -104,21 +104,16 @@ public:
// setters
void setExtended(bool extended) {_extended = extended;}
void setRefImageId(int refImageId) {_refImageId = refImageId;}
void setRefImageMapId(int refImageMapId) {_refImageMapId = refImageMapId;}
void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
void setLoopClosureMapId(int loopClosureMapId) {_loopClosureMapId = loopClosureMapId;}
void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
void setLocalLoopClosureMapId(int localLoopClosureMapId) {_localLoopClosureMapId = localLoopClosureMapId;}
void setRefImage(const std::vector<unsigned char> & image) {_refImage = image;}
void setLoopImage(const std::vector<unsigned char> & image) {_loopImage = image;}
void setRefDepth(const std::vector<unsigned char> & depth) {_refDepth = depth;}
void setRefDepth2D(const std::vector<unsigned char> & depth2d) {_refDepth2d = depth2d;}
void setLoopDepth(const std::vector<unsigned char> & depth) {_loopDepth = depth;}
void setLoopDepth2D(const std::vector<unsigned char> & depth2d) {_loopDepth2d = depth2d;}
void setRefDepthConstant(float depthConstant) {_refDepthConstant = depthConstant;}
void setLoopDepthConstant(float depthConstant) {_loopDepthConstant = depthConstant;}
void setRefLocalTransform(const Transform & localTransform) {_refLocalTransform = localTransform;}
void setLoopLocalTransform(const Transform & localTransform) {_loopLocalTransform = localTransform;}
void setMapIds(const std::map<int, int> & mapIds) {_mapIds = mapIds;}
void setImages(const std::map<int, std::vector<unsigned char> > & images) {_images = images;}
void setDepths(const std::map<int, std::vector<unsigned char> > & depths) {_depths = depths;}
void setDepth2ds(const std::map<int, std::vector<unsigned char> > & depth2ds) {_depth2ds = depth2ds;}
void setDepthConstants(const std::map<int, float> & depthConstants) {_depthConstants = depthConstants;}
void setLocalTransforms(const std::map<int, Transform> & localTransforms) {_localTransforms = localTransforms;}
void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
void setCurrentPose(const Transform & pose) {_currentPose = pose;}
void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
@@ -133,21 +128,16 @@ public:
// getters
bool extended() const {return _extended;}
int refImageId() const {return _refImageId;}
int refImageMapId() const {return _refImageMapId;}
int loopClosureId() const {return _loopClosureId;}
int loopClosureMapId() const {return _loopClosureMapId;}
int localLoopClosureId() const {return _localLoopClosureId;}
int localLoopClosureMapId() const {return _localLoopClosureMapId;}
const std::vector<unsigned char> & refImage() const {return _refImage;}
const std::vector<unsigned char> & loopImage() const {return _loopImage;}
const std::vector<unsigned char> & refDepth() const {return _refDepth;}
const std::vector<unsigned char> & loopDepth() const {return _loopDepth;}
const std::vector<unsigned char> & refDepth2D() const {return _refDepth2d;}
const std::vector<unsigned char> & loopDepth2D() const {return _loopDepth2d;}
float refDepthConstant() const {return _refDepthConstant;}
float loopDepthConstant() const {return _loopDepthConstant;}
const Transform & refLocalTransform() const {return _refLocalTransform;}
const Transform & loopLocalTransform() const {return _loopLocalTransform;}
const std::map<int, int> & getMapIds() const {return _mapIds;}
const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
const std::map<int, std::vector<unsigned char> > & getDepth2ds() const {return _depth2ds;}
const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
const std::map<int, Transform> & poses() const {return _poses;}
const Transform & currentPose() const {return _currentPose;}
const Transform & mapCorrection() const {return _mapCorrection;}
@@ -165,25 +155,18 @@ private:
bool _extended; // 0 -> only loop closure and last signature ID fields are filled
int _refImageId;
int _refImageMapId;
int _loopClosureId;
int _loopClosureMapId;
int _localLoopClosureId;
int _localLoopClosureMapId;
// extended data start here...
std::vector<unsigned char> _refImage;
std::vector<unsigned char> _loopImage;
std::map<int, int> _mapIds;
std::map<int, std::vector<unsigned char> > _images;
// Metric data
std::vector<unsigned char> _refDepth;
std::vector<unsigned char> _refDepth2d;
std::vector<unsigned char> _loopDepth;
std::vector<unsigned char> _loopDepth2d;
float _refDepthConstant;
float _loopDepthConstant;
Transform _refLocalTransform;
Transform _loopLocalTransform;
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::map<int, Transform> _poses;
Transform _currentPose;

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,