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

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 // Load objects
void load(VWDictionary * dictionary) const; void load(VWDictionary * dictionary) const;
void load(std::map<int, std::map<int, Transform> > & mapTransforms) const;
void loadLastNodes(std::list<Signature *> & signatures) const; void loadLastNodes(std::list<Signature *> & signatures) const;
void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures); void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws); void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
@@ -99,7 +98,6 @@ public:
void getLastNodeId(int & id) const; void getLastNodeId(int & id) const;
void getLastWordId(int & id) const; void getLastWordId(int & id) const;
void getInvertedIndexNi(int signatureId, int & ni) const; void getInvertedIndexNi(int signatureId, int & ni) const;
void save(const std::map<int, std::map<int, Transform> > & mapTransforms) const;
protected: protected:
DBDriver(const ParametersMap & parameters = ParametersMap()); DBDriver(const ParametersMap & parameters = ParametersMap());
@@ -118,12 +116,10 @@ private:
virtual void saveQuery(const std::list<VisualWord *> & words) const = 0; 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<Signature *> & signatures) const = 0;
virtual void updateQuery(const std::list<VisualWord *> & words) 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 // Load objects
virtual void loadQuery(VWDictionary * dictionary) const = 0; 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 loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
virtual void loadSignaturesQuery(const std::list<int> & ids, 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; 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> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;} const std::set<int> & getStMem() const {return _stMem;}
int getMaxStMemSize() const {return _maxStMemSize;} int getMaxStMemSize() const {return _maxStMemSize;}
Transform getMapTransform(int sourceMapId, int targetMapId) const;
void removeMapTransform(int soureId, int targetId);
void getPose(int locationId, void getPose(int locationId,
int targetMapId, int targetMapId,
Transform & pose, 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::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> _stMem; // id
std::set<int> _workingMem; // id,age std::set<int> _workingMem; // id,age
std::map<int, std::map<int, Transform> > _mapTransforms; // Transform between maps <fromMapId, <toMapId, transform> >
//Keypoint stuff //Keypoint stuff
VWDictionary * _vwd; VWDictionary * _vwd;

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

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

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

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

14
corelib/src/DBDriver.cpp
View File

@@ -280,13 +280,6 @@ void DBDriver::load(VWDictionary * dictionary) const
_dbSafeAccessMutex.unlock(); _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 void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
{ {
_dbSafeAccessMutex.lock(); _dbSafeAccessMutex.lock();
@@ -480,13 +473,6 @@ void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
_dbSafeAccessMutex.unlock(); _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 void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const
{ {
ULOGGER_DEBUG(""); 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 //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 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 std::string DBDriverSqlite3::queryStepNode() const
{ {
return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);"; 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 saveQuery(const std::list<VisualWord *> & words) const;
virtual void updateQuery(const std::list<Signature *> & signatures) const; virtual void updateQuery(const std::list<Signature *> & signatures) const;
virtual void updateQuery(const std::list<VisualWord *> & words) 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 // Load objects
virtual void loadQuery(VWDictionary * dictionary) const; 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 loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual void loadSignaturesQuery(const std::list<int> & ids, 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; 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()))); 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..."))); if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references...")));
// Enable loaded signatures // Enable loaded signatures
Signature * ss; Signature * ss;
@@ -642,42 +633,6 @@ int Memory::getVWDictionarySize() const
return _vwd->getVisualWords().size(); 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 void Memory::getPose(int locationId, int targetMapId, Transform & pose, bool lookInDatabase) const
{ {
const Signature * s = getSignature(locationId); 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); _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 std::map<int, Transform> Memory::getNeighborLinks(int signatureId, bool ignoreNeighborByLoopClosure, bool lookInDatabase) const
@@ -1011,11 +941,6 @@ void Memory::clear()
this->cleanUnusedWords(); this->cleanUnusedWords();
if(_dbDriver)
{
_dbDriver->save(_mapTransforms);
}
if(_dbDriver) if(_dbDriver)
{ {
_dbDriver->emptyTrashes(); _dbDriver->emptyTrashes();
@@ -1084,7 +1009,6 @@ void Memory::clear()
_idCount = kIdStart; _idCount = kIdStart;
_idMapCount = kIdStart; _idMapCount = kIdStart;
_memoryChanged = false; _memoryChanged = false;
_mapTransforms.clear();
if(_dbDriver) if(_dbDriver)
{ {
@@ -1720,12 +1644,12 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
} }
else if(inliersCount < _bowMinInliers) 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 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()) 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()); 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()); oldS->addLoopClosureId(newS->id(), transform.inverse());
newS->addChildLoopClosureId(oldS->id(), transform); newS->addChildLoopClosureId(oldS->id(), transform);

107
corelib/src/Rtabmap.cpp
View File

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

4
corelib/src/Statistics.cpp
View File

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

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

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

101
guilib/src/MainWindow.cpp
View File

@@ -614,6 +614,12 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
time.start(); time.start();
totalTime.start(); totalTime.start();
//Affichage des stats et images //Affichage des stats et images
int refMapId = uValue(stat.getMapIds(), stat.refImageId(), -1);
int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), -1);
_ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
_ui->label_matchId->clear();
if(stat.extended()) if(stat.extended())
{ {
float totalTime = static_cast<float>(uValue(stat.data(), Statistics::kTimingTotal(), 0.0f)); float totalTime = static_cast<float>(uValue(stat.data(), Statistics::kTimingTotal(), 0.0f));
@@ -623,7 +629,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
} }
UDEBUG(""); UDEBUG("");
_ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(stat.refImageMapId()));
int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f)); int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f); bool highestHypothesisIsSaved = (bool)uValue(stat.data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
@@ -635,53 +640,48 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_ui->imageView_source->setBackgroundBrush(QBrush(Qt::black)); _ui->imageView_source->setBackgroundBrush(QBrush(Qt::black));
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black)); _ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
// get images // update cache
std::vector<unsigned char> refImage = stat.refImage();
std::vector<unsigned char> refDepth = stat.refDepth();
const std::vector<unsigned char> & refDepth2D = stat.refDepth2D();
std::vector<unsigned char> loopImage = stat.loopImage();
std::vector<unsigned char> loopDepth = stat.loopDepth();
const std::vector<unsigned char> & loopDepth2D = stat.loopDepth2D();
//Added to cache
if(_preferencesDialog->isImagesKept()) if(_preferencesDialog->isImagesKept())
{ {
//ref // images
if(!refImage.empty()) for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stat.getImages().begin();
iter != stat.getImages().end();
++iter)
{ {
_imagesMap.insert(stat.refImageId(), refImage); if(!iter->second.empty() && !_imagesMap.contains(iter->first))
}
if(!refDepth.empty())
{
_depthsMap.insert(stat.refImageId(), refDepth);
_depthConstantsMap.insert(stat.refImageId(), stat.refDepthConstant());
_localTransformsMap.insert(stat.refImageId(), stat.refLocalTransform());
}
if(!refDepth2D.empty())
{
_depths2DMap.insert(stat.refImageId(), refDepth2D);
}
// loop
if(stat.loopClosureId()>0 || highestHypothesisId>0 || stat.localLoopClosureId()>0)
{
// order -> loop closure id -> local loop closure id -> highest loop closure hyp id
int id = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
if(!_imagesMap.contains(id))
{ {
_imagesMap.insert(id, loopImage); _imagesMap.insert(iter->first, iter->second);
} }
}
if(!_depthsMap.contains(id) && !loopDepth.empty()) // depths
for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stat.getDepths().begin();
iter != stat.getDepths().end();
++iter)
{
if(!iter->second.empty() && !_depthsMap.contains(iter->first))
{ {
_depthsMap.insert(id, loopDepth); float constant = uValue(stat.getDepthConstants(), iter->first, 0.0f);
_depthConstantsMap.insert(id, stat.loopDepthConstant()); Transform transform = uValue(stat.getLocalTransforms(), iter->first, Transform());
_localTransformsMap.insert(id, stat.loopLocalTransform()); if(constant != 0.0f && !transform.isNull())
{
_depthsMap.insert(iter->first, iter->second);
_depthConstantsMap.insert(iter->first, constant);
_localTransformsMap.insert(iter->first, transform);
}
else
{
UERROR("Invalid depth data for id=%d", iter->first);
}
} }
}
if(!_depths2DMap.contains(id) && !loopDepth2D.empty()) // depths2d
for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stat.getDepth2ds().begin();
iter != stat.getDepth2ds().end();
++iter)
{
if(!iter->second.empty() && !_depths2DMap.contains(iter->first))
{ {
_depths2DMap.insert(id, loopDepth2D); _depths2DMap.insert(iter->first, iter->second);
} }
} }
} }
@@ -690,7 +690,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f); int localTimeClosures = (int)uValue(stat.data(), Statistics::kLocalLoopTime_closures(), 0.0f);
bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kLocalLoopScan_matching_success(), 0.0f); bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kLocalLoopScan_matching_success(), 0.0f);
_ui->label_matchId->clear(); _ui->label_matchId->clear();
_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(stat.refImageMapId())); _ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
if(rehearsed > 0) if(rehearsed > 0)
{ {
@@ -707,6 +707,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms", time.restart()); UDEBUG("time= %d ms", time.restart());
std::vector<unsigned char> refImage = uValue(stat.getImages(), stat.refImageId(), std::vector<unsigned char>());
std::vector<unsigned char> refDepth = uValue(stat.getDepths(), stat.refImageId(), std::vector<unsigned char>());
std::vector<unsigned char> refDepth2D = uValue(stat.getDepth2ds(), stat.refImageId(), std::vector<unsigned char>());
std::vector<unsigned char> loopImage = uValue(stat.getImages(), stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId(), std::vector<unsigned char>());
std::vector<unsigned char> loopDepth = uValue(stat.getDepths(), stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId(), std::vector<unsigned char>());
std::vector<unsigned char> loopDepth2D = uValue(stat.getDepth2ds(), stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId(), std::vector<unsigned char>());
int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f)); int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f); float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
int matchId = 0; int matchId = 0;
@@ -721,13 +728,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{ {
_ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1)); _ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
} }
_ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(stat.loopClosureMapId())); _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
matchId = stat.loopClosureId(); matchId = stat.loopClosureId();
} }
else if(stat.localLoopClosureId()) else if(stat.localLoopClosureId())
{ {
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::yellow)); _ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::yellow));
_ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.localLoopClosureId()).arg(stat.localLoopClosureMapId())); _ui->label_matchId->setText(QString("Local match = %1 [%2]").arg(stat.localLoopClosureId()).arg(loopMapId));
matchId = stat.localLoopClosureId(); matchId = stat.localLoopClosureId();
} }
else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr()) else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
@@ -932,7 +939,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
// Add to loop closure viewer if all data is saved // Add to loop closure viewer if all data is saved
Signature * loopOld = new Signature( Signature * loopOld = new Signature(
loopOldId, loopOldId,
stat.loopClosureMapId(), loopMapId,
std::multimap<int, cv::KeyPoint>(), std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(), std::multimap<int, pcl::PointXYZ>(),
Transform(), Transform(),
@@ -944,7 +951,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
Signature * loopNew = new Signature( Signature * loopNew = new Signature(
loopNewId, loopNewId,
stat.refImageMapId(), refMapId,
std::multimap<int, cv::KeyPoint>(), std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(), std::multimap<int, pcl::PointXYZ>(),
loopClosureTransform, loopClosureTransform,
@@ -971,7 +978,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
else if(!stat.extended() && stat.loopClosureId()>0) else if(!stat.extended() && stat.loopClosureId()>0)
{ {
_ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1)); _ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
_ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(stat.loopClosureMapId())); _ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
} }
float elapsedTime = static_cast<float>(totalTime.elapsed()); float elapsedTime = static_cast<float>(totalTime.elapsed());
UINFO("Processing statistics time = %fs", elapsedTime/1000.0f); UINFO("Processing statistics time = %fs", elapsedTime/1000.0f);
@@ -996,7 +1003,7 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & poses, const Tr
} }
// Map updated! regenerate the assembled cloud, last pose is the new one // Map updated! regenerate the assembled cloud, last pose is the new one
UDEBUG("Update map with %d locations", poses.size()); UDEBUG("Update map with %d locations (currentPose=%s)", poses.size(), currentPose.prettyPrint().c_str());
QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds(); QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {