i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update version 0.10.11:

Browse Source 
-Added parameter "Mem/ReduceGraph" with new link type (NeighborMerged)
-DatabaseViewer: add Info panel, fixed empty extracted database when "RGB images" is not checked
-ConsoleWidget: added buffer size and refresh rate options
-DBDriver: new database info getters
-Graph: added reduceGraph() method, added Dijsktra computePath() with links only
-Updated default parameters: RGBD/LocalLoopDetectionSpace=true, RGBD/LinearUpdate=0.1 and RGBD/AngularUpdate=0.1
-Goals are saved with label instead of ID if the target node has a label
-DataRecorder: fixed saving incrementally the database (instead of only at the end)
-MainWindow: When waypoints are used, resend the current goal (1 sec delay) if it has failed setting it
This commit is contained in:
matlabbe
2015-10-24 17:03:44 -04:00
parent 957498b5a3
commit 463a5d973c
32 changed files with 1820 additions and 314 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/DBDriver.cpp
+98 -3
View File
@@ -106,6 +106,79 @@ long DBDriver::getMemoryUsed() const
return bytes;
}
long DBDriver::getImagesMemoryUsed() const
{
long bytes;
_dbSafeAccessMutex.lock();
bytes = getImagesMemoryUsedQuery();
_dbSafeAccessMutex.unlock();
return bytes;
}
long DBDriver::getDepthImagesMemoryUsed() const
{
long bytes;
_dbSafeAccessMutex.lock();
bytes = getDepthImagesMemoryUsedQuery();
_dbSafeAccessMutex.unlock();
return bytes;
}
long DBDriver::getLaserScansMemoryUsed() const
{
long bytes;
_dbSafeAccessMutex.lock();
bytes = getLaserScansMemoryUsedQuery();
_dbSafeAccessMutex.unlock();
return bytes;
}
long DBDriver::getUserDataMemoryUsed() const
{
long bytes;
_dbSafeAccessMutex.lock();
bytes = getUserDataMemoryUsedQuery();
_dbSafeAccessMutex.unlock();
return bytes;
}
long DBDriver::getWordsMemoryUsed() const
{
long bytes;
_dbSafeAccessMutex.lock();
bytes = getWordsMemoryUsedQuery();
_dbSafeAccessMutex.unlock();
return bytes;
}
int DBDriver::getLastNodesSize() const
{
int nodes;
_dbSafeAccessMutex.lock();
nodes = getLastNodesSizeQuery();
_dbSafeAccessMutex.unlock();
return nodes;
}
int DBDriver::getLastDictionarySize() const
{
int words;
_dbSafeAccessMutex.lock();
words = getLastDictionarySizeQuery();
_dbSafeAccessMutex.unlock();
return words;
}
int DBDriver::getTotalNodesSize() const
{
int words;
_dbSafeAccessMutex.lock();
words = getTotalNodesSizeQuery();
_dbSafeAccessMutex.unlock();
return words;
}
int DBDriver::getTotalDictionarySize() const
{
int words;
_dbSafeAccessMutex.lock();
words = getTotalDictionarySizeQuery();
_dbSafeAccessMutex.unlock();
return words;
}
std::string DBDriver::getDatabaseVersion() const
{
std::string version = "0.0.0";
@@ -473,7 +546,8 @@ void DBDriver::getNodeData(
}
}
bool DBDriver::getNodeInfo(int signatureId,
bool DBDriver::getNodeInfo(
int signatureId,
Transform & pose,
int & mapId,
int & weight,
@@ -568,7 +642,8 @@ void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
nIter!=sIter->second->getLinks().end();
++nIter)
{
if(nIter->second.type() == Link::kNeighbor)
if(nIter->second.type() == Link::kNeighbor ||
nIter->second.type() == Link::kNeighborMerged)
{
hasNeighbors = true;
break;
@@ -778,7 +853,8 @@ void DBDriver::generateGraph(
const char * colorG = "green";
const char * colorP = "pink";
; UINFO("Generating map with %d locations", ids.size());
const char * colorNM = "blue";
UINFO("Generating map with %d locations", ids.size());
fprintf(fout, "digraph G {\n");
for(std::set<int>::iterator i=ids.begin(); i!=ids.end(); ++i)
{
@@ -809,6 +885,16 @@ void DBDriver::generateGraph(
iter->first,
weightNeighbor);
}
else if(iter->second.type() == Link::kNeighborMerged)
{
//merged neighbor
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"M\", fontcolor=%s, fontsize=8];\n",
id,
weight,
iter->first,
weightNeighbor,
colorNM);
}
else if(iter->first > id)
{
//loop
@@ -860,6 +946,15 @@ void DBDriver::generateGraph(
iter->first,
weightNeighbor);
}
else if(iter->second.type() == Link::kNeighborMerged)
{
fprintf(fout, " \"%d\\n%d\" -> \"%d\\n%d\" [label=\"M\", fontcolor=%s, fontsize=8];\n",
id,
weight,
iter->first,
weightNeighbor,
colorNM);
}
else if(iter->first > id)
{
//loop
corelib/src/DBDriverSqlite3.cpp
+262 -6
View File
@@ -446,6 +446,259 @@ long DBDriverSqlite3::getMemoryUsedQuery() const
}
}
long DBDriverSqlite3::getImagesMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
query = "SELECT sum(length(image)) from Data;";
}
else
{
query = "SELECT sum(length(data)) from Image;";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
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());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getDepthImagesMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
query = "SELECT sum(length(depth)) from Data;";
}
else
{
query = "SELECT sum(length(data)) from Depth;";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
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());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getLaserScansMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
query = "SELECT sum(length(scan)) from Data;";
}
else
{
query = "SELECT sum(length(data2d)) from Depth;";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
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());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getUserDataMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
query = "SELECT sum(length(user_data)) from Data;";
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)
{
query = "SELECT sum(length(user_data)) from Node;";
}
else
{
return size; // no user_data
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
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());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
long DBDriverSqlite3::getWordsMemoryUsedQuery() const
{
UDEBUG("");
long size = 0L;
if(_ppDb)
{
std::string query = "SELECT sum(length(descriptor)) from Word;";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
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());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int64(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
int DBDriverSqlite3::getLastNodesSizeQuery() const
{
UDEBUG("");
int size = 0;
if(_ppDb)
{
std::string query = "SELECT count(id) from Node WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
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());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
int DBDriverSqlite3::getLastDictionarySizeQuery() const
{
UDEBUG("");
int size = 0;
if(_ppDb)
{
std::string query = "SELECT count(id) from Word WHERE time_enter >= (SELECT MAX(time_enter) FROM Statistics);";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
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());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
int DBDriverSqlite3::getTotalNodesSizeQuery() const
{
UDEBUG("");
int size = 0;
if(_ppDb)
{
std::string query = "SELECT count(id) from Node;";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
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());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
int DBDriverSqlite3::getTotalDictionarySizeQuery() const
{
UDEBUG("");
int size = 0;
if(_ppDb)
{
std::string query = "SELECT count(id) from Word;";
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
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());
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
size = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
return size;
}
void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) const
{
UDEBUG("load data for %d signatures", (int)signatures.size());
@@ -830,7 +1083,7 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
query << "SELECT id "
<< "FROM Node "
<< "INNER JOIN Link "
<< "ON id = to_id " // use to_id tp ignore all children (which don't have link pointing on them)
<< "ON id = to_id " // use to_id to ignore all children (which don't have link pointing on them)
<< "ORDER BY id";
}
@@ -2067,7 +2320,10 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
if(!(*i)->sensorData().imageCompressed().empty())
if(!(*i)->sensorData().imageCompressed().empty() ||
!(*i)->sensorData().depthOrRightCompressed().empty() ||
!(*i)->sensorData().laserScanCompressed().empty() ||
!(*i)->sensorData().userDataCompressed().empty())
{
UASSERT((*i)->id() == (*i)->sensorData().id());
stepSensorData(ppStmt, (*i)->sensorData());
@@ -2506,7 +2762,7 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2517,7 +2773,7 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2578,7 +2834,7 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2591,7 +2847,7 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
corelib/src/DBDriverSqlite3.h
+9
View File
@@ -54,6 +54,15 @@ private:
virtual bool isConnectedQuery() const;
virtual long getMemoryUsedQuery() const; // In bytes
virtual bool getDatabaseVersionQuery(std::string & version) const;
virtual long getImagesMemoryUsedQuery() const;
virtual long getDepthImagesMemoryUsedQuery() const;
virtual long getLaserScansMemoryUsedQuery() const;
virtual long getUserDataMemoryUsedQuery() const;
virtual long getWordsMemoryUsedQuery() const;
virtual int getLastNodesSizeQuery() const;
virtual int getLastDictionarySizeQuery() const;
virtual int getTotalNodesSizeQuery() const;
virtual int getTotalDictionarySizeQuery() const;
virtual void executeNoResultQuery(const std::string & sql) const;
corelib/src/DBReader.cpp
+26 -14
View File
@@ -157,12 +157,13 @@ void DBReader::mainLoop()
OdometryEvent odom = this->getNextData();
if(odom.data().id())
{
int goalId = 0;
std::string goalId;
double previousStamp = odom.data().stamp();
if(previousStamp == 0)
{
odom.data().setStamp(UTimer::now());
}
if(!_goalsIgnored &&
odom.data().userDataRaw().type() == CV_8SC1 &&
odom.data().userDataRaw().cols >= 7 && // including null str ending
@@ -176,7 +177,7 @@ void DBReader::mainLoop()
std::list<std::string> strs = uSplit(goalStr, ':');
if(strs.size() == 2)
{
goalId = atoi(strs.rbegin()->c_str());
goalId = *strs.rbegin();
odom.data().setUserData(cv::Mat());
}
}
@@ -196,8 +197,9 @@ void DBReader::mainLoop()
this->post(new CameraEvent(odom.data()));
}
if(goalId > 0)
if(!goalId.empty())
{
double delay = 0.0;
if(!_ignoreGoalDelay && _currentId != _ids.end())
{
// get stamp for the next signature to compute the delay
@@ -210,23 +212,33 @@ void DBReader::mainLoop()
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
if(previousStamp && stamp && stamp > previousStamp)
{
double delay = stamp - previousStamp;
UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
goalId, delay);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
uSleep(delay*1000);
}
else
{
UWARN("Goal %d detected, posting it!", goalId);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
delay = stamp - previousStamp;
}
}
if(delay > 0.0)
{
UWARN("Goal \"%s\" detected, posting it! Waiting %f seconds before sending next data...",
goalId.c_str(), delay);
}
else
{
UWARN("Goal \"%s\" detected, posting it!", goalId.c_str());
}
if(uIsInteger(goalId))
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, atoi(goalId.c_str())));
}
else
{
UWARN("Goal %d detected, posting it!", goalId);
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, goalId));
}
if(delay > 0.0)
{
uSleep(delay*1000);
}
}
}
corelib/src/Features2d.cpp
+2 -2
View File
@@ -72,7 +72,7 @@ void Feature2D::filterKeypointsByDepth(
const cv::Mat & depth,
float maxDepth)
{
if(!depth.empty() && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
if(!depth.empty() && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
{
std::vector<cv::KeyPoint> output(keypoints.size());
std::vector<int> indexes(keypoints.size(), 0);
@@ -85,7 +85,7 @@ void Feature2D::filterKeypointsByDepth(
if(u >=0 && u<depth.cols && v >=0 && v<depth.rows)
{
float d = isInMM?(float)depth.at<uint16_t>(v,u)*0.001f:depth.at<float>(v,u);
if(d!=0.0f && uIsFinite(d) && d < maxDepth)
if(uIsFinite(d) && d>0.0f && (maxDepth <= 0.0f || d < maxDepth))
{
output[oi++] = keypoints[i];
indexes[i] = 1;
corelib/src/Graph.cpp
+363 -39
View File
@@ -240,7 +240,9 @@ void Optimizer::getConnectedGraph(
std::multimap<int, int> biLinks;
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
{
UASSERT_MSG(findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end(), "Input links should be unique between two poses.");
UASSERT_MSG(findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end(),
uFormat("Input links should be unique between two poses (%d->%d).",
iter->second.from(), iter->second.to()).c_str());
biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
}
@@ -396,12 +398,14 @@ std::map<int, Transform> TOROOptimizer::optimize(
}
}
}
UDEBUG("buildMST...");
UDEBUG("buildMST... root=%d", rootId);
UASSERT(uContains(poses, rootId));
if(isSlam2d())
{
pg2.buildMST(rootId); // pg.buildSimpleTree();
UDEBUG("initializeOnTree()");
pg2.initializeOnTree();
UDEBUG("initializeTreeParameters()");
pg2.initializeTreeParameters();
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
"TORO is not able to find the root of the graph!)");
@@ -410,7 +414,9 @@ std::map<int, Transform> TOROOptimizer::optimize(
else
{
pg3.buildMST(rootId); // pg.buildSimpleTree();
UDEBUG("initializeOnTree()");
pg3.initializeOnTree();
UDEBUG("initializeTreeParameters()");
pg3.initializeTreeParameters();
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
"TORO is not able to find the root of the graph!)");
@@ -799,7 +805,9 @@ std::map<int, Transform> G2OOptimizer::optimize(
g2o::HyperGraph::Edge * edge = 0;
VertexSwitchLinear * v = 0;
if(this->isRobust() && iter->second.type() != Link::kNeighbor)
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// For loop closure links, add switchable edges
@@ -840,7 +848,9 @@ std::map<int, Transform> G2OOptimizer::optimize(
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
if(this->isRobust() && iter->second.type() != Link::kNeighbor)
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE2Switchable * e = new EdgeSE2Switchable();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
@@ -881,7 +891,9 @@ std::map<int, Transform> G2OOptimizer::optimize(
constraint = a.rotation();
constraint.translation() = a.translation();
if(this->isRobust() && iter->second.type() != Link::kNeighbor)
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
EdgeSE3Switchable * e = new EdgeSE3Switchable();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
@@ -1079,7 +1091,9 @@ bool G2OOptimizer::saveGraph(
std::string prefix = "EDGE_SE3:QUAT";
std::string suffix = "";
if(useRobustConstraints && iter->second.type() != Link::kNeighbor)
if(useRobustConstraints &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
prefix = "EDGE_SE3_SWITCHABLE";
fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId);
@@ -1196,7 +1210,9 @@ std::map<int, Transform> GTSAMOptimizer::optimize(
UASSERT(!iter->second.transform().isNull());
if(this->isRobust() && iter->second.type()!=Link::kNeighbor)
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// create new switch variable
// Sunderhauf IROS 2012:
@@ -1233,7 +1249,9 @@ std::map<int, Transform> GTSAMOptimizer::optimize(
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
if(this->isRobust() && iter->second.type()!=Link::kNeighbor)
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
@@ -1255,7 +1273,9 @@ std::map<int, Transform> GTSAMOptimizer::optimize(
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
if(this->isRobust() && iter->second.type()!=Link::kNeighbor)
if(this->isRobust() &&
iter->second.type()!=Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
@@ -1683,7 +1703,8 @@ bool exportPoses(
std::multimap<int, Link>::iterator findLink(
std::multimap<int, Link> & links,
int from,
int to)
int to,
bool checkBothWays)
{
std::multimap<int, Link>::iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
@@ -1695,15 +1716,18 @@ std::multimap<int, Link>::iterator findLink(
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
if(checkBothWays)
{
if(iter->second.to() == from)
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
return iter;
if(iter->second.to() == from)
{
return iter;
}
++iter;
}
++iter;
}
return links.end();
}
@@ -1711,7 +1735,8 @@ std::multimap<int, Link>::iterator findLink(
std::multimap<int, int>::iterator findLink(
std::multimap<int, int> & links,
int from,
int to)
int to,
bool checkBothWays)
{
std::multimap<int, int>::iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
@@ -1723,22 +1748,26 @@ std::multimap<int, int>::iterator findLink(
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
if(checkBothWays)
{
if(iter->second == from)
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
return iter;
if(iter->second == from)
{
return iter;
}
++iter;
}
++iter;
}
return links.end();
}
std::multimap<int, Link>::const_iterator findLink(
const std::multimap<int, Link> & links,
int from,
int to)
int to,
bool checkBothWays)
{
std::multimap<int, Link>::const_iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
@@ -1750,15 +1779,18 @@ std::multimap<int, Link>::const_iterator findLink(
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
if(checkBothWays)
{
if(iter->second.to() == from)
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
return iter;
if(iter->second.to() == from)
{
return iter;
}
++iter;
}
++iter;
}
return links.end();
}
@@ -1766,7 +1798,8 @@ std::multimap<int, Link>::const_iterator findLink(
std::multimap<int, int>::const_iterator findLink(
const std::multimap<int, int> & links,
int from,
int to)
int to,
bool checkBothWays)
{
std::multimap<int, int>::const_iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
@@ -1778,15 +1811,18 @@ std::multimap<int, int>::const_iterator findLink(
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
if(checkBothWays)
{
if(iter->second == from)
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
return iter;
if(iter->second == from)
{
return iter;
}
++iter;
}
++iter;
}
return links.end();
}
@@ -1955,6 +1991,192 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & p
return clusters;
}
void reduceGraph(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
std::multimap<int, Link> & hyperLinks)
{
UINFO("Input: poses=%d links=%d", (int)poses.size(), (int)links.size());
UTimer timer;
std::map<int, int> posesToHyperNodes;
std::map<int, std::multimap<int, Link> > clusterloopClosureLinks;
{
std::multimap<int, Link> bidirectionalLoopClosureLinks;
for(std::multimap<int, Link>::const_iterator jter=links.begin(); jter!=links.end(); ++jter)
{
if(jter->second.type() != Link::kNeighbor &&
jter->second.type() != Link::kNeighborMerged &&
jter->second.userDataCompressed().empty())
{
if(uContains(poses, jter->second.from()) &&
uContains(poses, jter->second.to()))
{
UASSERT_MSG(graph::findLink(links, jter->second.to(), jter->second.from(), false) == links.end(), "Input links should be unique!");
bidirectionalLoopClosureLinks.insert(std::make_pair(jter->second.from(), jter->second));
//bidirectionalLoopClosureLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
}
}
}
UINFO("Clustering hyper nodes...");
// largest ID to smallest ID
for(std::map<int, Transform>::const_reverse_iterator iter=poses.rbegin(); iter!=poses.rend(); ++iter)
{
if(posesToHyperNodes.find(iter->first) == posesToHyperNodes.end())
{
int hyperNodeId = iter->first;
std::list<int> loopClosures;
std::set<int> loopClosuresAdded;
loopClosures.push_back(iter->first);
std::multimap<int, Link> clusterLinks;
while(loopClosures.size())
{
int id = loopClosures.front();
loopClosures.pop_front();
UASSERT(posesToHyperNodes.find(id) == posesToHyperNodes.end());
posesToHyperNodes.insert(std::make_pair(id, hyperNodeId));
hyperNodes.insert(std::make_pair(hyperNodeId, id));
for(std::multimap<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.find(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter)
{
if(posesToHyperNodes.find(jter->second.to()) == posesToHyperNodes.end() &&
loopClosuresAdded.find(jter->second.to()) == loopClosuresAdded.end())
{
loopClosures.push_back(jter->second.to());
loopClosuresAdded.insert(jter->second.to());
clusterLinks.insert(*jter);
clusterLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
if(jter->second.from() < jter->second.to())
{
UWARN("Child to Parent link? %d->%d (type=%d)",
jter->second.from(),
jter->second.to(),
jter->second.type());
}
}
}
}
UASSERT(clusterloopClosureLinks.find(hyperNodeId) == clusterloopClosureLinks.end());
clusterloopClosureLinks.insert(std::make_pair(hyperNodeId, clusterLinks));
UDEBUG("Created hyper node %d with %d children (%f%%)",
hyperNodeId, (int)loopClosuresAdded.size(), float(posesToHyperNodes.size())/float(poses.size())*100.0f);
}
}
UINFO("Clustering hyper nodes... done! (%f s)", timer.ticks());
}
UINFO("Creating hyper links...");
int i=0;
for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin(); jter!=links.rend(); ++jter)
{
if((jter->second.type() == Link::kNeighbor ||
jter->second.type() == Link::kNeighborMerged ||
!jter->second.userDataCompressed().empty()) &&
uContains(poses, jter->second.from()) &&
uContains(poses, jter->second.to()))
{
UASSERT_MSG(uContains(posesToHyperNodes, jter->second.from()), uFormat("%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
UASSERT_MSG(uContains(posesToHyperNodes, jter->second.to()), uFormat("%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
int hyperNodeIDFrom = posesToHyperNodes.at(jter->second.from());
int hyperNodeIDTo = posesToHyperNodes.at(jter->second.to());
// ignore links inside a hyper node
if(hyperNodeIDFrom != hyperNodeIDTo)
{
std::multimap<int, Link>::iterator tmpIter = graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo);
if(tmpIter!=hyperLinks.end() &&
hyperNodeIDFrom == jter->second.from() &&
hyperNodeIDTo == jter->second.to() &&
tmpIter->second.type() > Link::kNeighbor &&
jter->second.type() == Link::kNeighbor)
{
// neighbor links have priority, so remove the previously added link
hyperLinks.erase(tmpIter);
}
// only add unique link between two hyper nodes (keeping only the more recent)
if(graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo) == hyperLinks.end())
{
UASSERT(clusterloopClosureLinks.find(hyperNodeIDFrom) != clusterloopClosureLinks.end());
UASSERT(clusterloopClosureLinks.find(hyperNodeIDTo) != clusterloopClosureLinks.end());
std::multimap<int, Link> tmpLinks = clusterloopClosureLinks.at(hyperNodeIDFrom);
tmpLinks.insert(clusterloopClosureLinks.at(hyperNodeIDTo).begin(), clusterloopClosureLinks.at(hyperNodeIDTo).end());
tmpLinks.insert(std::make_pair(jter->second.from(), jter->second));
std::list<int> path = computePath(tmpLinks, hyperNodeIDFrom, hyperNodeIDTo, false, true);
UASSERT_MSG(path.size()>1,
uFormat("path.size()=%d, hyperNodeIDFrom=%d, hyperNodeIDTo=%d",
(int)path.size(),
hyperNodeIDFrom,
hyperNodeIDTo).c_str());
if(path.size() > 10)
{
UWARN("Large path! %d nodes", (int)path.size());
std::stringstream stream;
for(std::list<int>::const_iterator iter=path.begin(); iter!=path.end();++iter)
{
if(iter!=path.begin())
{
stream << ",";
}
stream << *iter;
}
UWARN("Path = [%s]", stream.str().c_str());
}
// create the hyperlink
std::list<int>::iterator iter=path.begin();
int from = *iter;
++iter;
int to = *iter;
std::multimap<int, Link>::const_iterator foundIter = graph::findLink(tmpLinks, from, to, false);
UASSERT(foundIter != tmpLinks.end());
Link hyperLink = foundIter->second;
++iter;
from = to;
for(; iter!=path.end(); ++iter)
{
to = *iter;
std::multimap<int, Link>::const_iterator foundIter = graph::findLink(tmpLinks, from, to, false);
UASSERT(foundIter != tmpLinks.end());
hyperLink = hyperLink.merge(foundIter->second, jter->second.type());
from = to;
}
UASSERT(hyperLink.from() == hyperNodeIDFrom);
UASSERT(hyperLink.to() == hyperNodeIDTo);
hyperLinks.insert(std::make_pair(hyperNodeIDFrom, hyperLink));
UDEBUG("Created hyper link %d->%d (%f%%)",
hyperLink.from(), hyperLink.to(), float(i)/float(links.size())*100.0f);
if(hyperLink.transform().getNorm() > jter->second.transform().getNorm()+1)
{
UWARN("Large hyper link %d->%d (%f m)! original %d->%d (%f m)",
hyperLink.from(),
hyperLink.to(),
hyperLink.transform().getNorm(),
jter->second.from(),
jter->second.to(),
jter->second.transform().getNorm());
}
}
}
}
++i;
}
UINFO("Creating hyper links... done! (%f s)", timer.ticks());
UINFO("Output: poses=%d links=%d", (int)uUniqueKeys(hyperNodes).size(), (int)links.size());
}
class Node
{
@@ -2005,6 +2227,7 @@ struct Order
}
};
// A*
std::list<std::pair<int, Transform> > computePath(
const std::map<int, rtabmap::Transform> & poses,
const std::multimap<int, int> & links,
@@ -2014,7 +2237,6 @@ std::list<std::pair<int, Transform> > computePath(
{
std::list<std::pair<int, Transform> > path;
//A*
int startNode = from;
int endNode = to;
rtabmap::Transform endPose = poses.at(endNode);
@@ -2076,6 +2298,7 @@ std::list<std::pair<int, Transform> > computePath(
Node n(iter->second, currentNode->id(), poseIter->second);
n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second));
n.setDistToEnd(n.distFrom(endPose));
nodes.insert(std::make_pair(iter->second, n));
if(updateNewCosts)
{
@@ -2110,6 +2333,107 @@ std::list<std::pair<int, Transform> > computePath(
return path;
}
// Dijksta
std::list<int> RTABMAP_EXP computePath(
const std::multimap<int, Link> & links,
int from,
int to,
bool updateNewCosts,
bool useSameCostForAllLinks)
{
std::list<int> path;
int startNode = from;
int endNode = to;
std::map<int, Node> nodes;
nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform())));
std::priority_queue<Pair, std::vector<Pair>, Order> pq;
std::multimap<float, int> pqmap;
if(updateNewCosts)
{
pqmap.insert(std::make_pair(0, startNode));
}
else
{
pq.push(Pair(startNode, 0));
}
while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
{
Node * currentNode;
if(updateNewCosts)
{
currentNode = &nodes.find(pqmap.begin()->second)->second;
pqmap.erase(pqmap.begin());
}
else
{
currentNode = &nodes.find(pq.top().first)->second;
pq.pop();
}
currentNode->setClosed(true);
if(currentNode->id() == endNode)
{
while(currentNode->id()!=startNode)
{
path.push_front(currentNode->id());
currentNode = &nodes.find(currentNode->fromId())->second;
}
path.push_front(startNode);
break;
}
// lookup neighbors
for(std::multimap<int, Link>::const_iterator iter = links.find(currentNode->id());
iter!=links.end() && iter->first == currentNode->id();
++iter)
{
std::map<int, Node>::iterator nodeIter = nodes.find(iter->second.to());
float cost = 1;
if(!useSameCostForAllLinks)
{
cost = iter->second.transform().getNorm();
}
if(nodeIter == nodes.end())
{
Node n(iter->second.to(), currentNode->id(), Transform());
n.setCostSoFar(currentNode->costSoFar() + cost);
nodes.insert(std::make_pair(iter->second.to(), n));
if(updateNewCosts)
{
pqmap.insert(std::make_pair(n.totalCost(), n.id()));
}
else
{
pq.push(Pair(n.id(), n.totalCost()));
}
}
else if(!useSameCostForAllLinks && updateNewCosts && nodeIter->second.isOpened())
{
float newCostSoFar = currentNode->costSoFar() + cost;
if(nodeIter->second.costSoFar() > newCostSoFar)
{
// update the cost in the priority queue
for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
{
if(mapIter->second == nodeIter->first)
{
pqmap.erase(mapIter);
nodeIter->second.setCostSoFar(newCostSoFar);
pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
break;
}
}
}
}
}
}
return path;
}
// return path starting from "fromId" (Identity pose for the first node)
std::list<std::pair<int, Transform> > computePath(
@@ -2503,7 +2827,7 @@ std::list<std::map<int, Transform> > getPaths(
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
{
std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
if(path.size() == 0 || (jter != links.end() && jter->second.type() == Link::kNeighbor))
if(path.size() == 0 || (jter != links.end() && (jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)))
{
path.insert(*iter);
poses.erase(iter++);
corelib/src/Memory.cpp
+268 -125
View File
@@ -71,6 +71,7 @@ Memory::Memory(const ParametersMap & parameters) :
_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
_reduceGraph(Parameters::defaultMemReduceGraph()),
_maxStMemSize(Parameters::defaultMemSTMSize()),
_recentWmRatio(Parameters::defaultMemRecentWmRatio()),
_transferSortingByWeightId(Parameters::defaultMemTransferSortingByWeightId()),
@@ -402,6 +403,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(parameters, Parameters::kMemReduceGraph(), _reduceGraph);
Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
@@ -626,49 +628,36 @@ bool Memory::update(
//============================================================
// Transfer the oldest signature of the short-term memory to the working memory
//============================================================
int validSignaturesCount = 0;
int notIntermediateNodesCount = 0;
for(std::set<int>::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter)
{
const Signature * s = this->getSignature(*iter);
UASSERT(s != 0);
if(!s->isBadSignature())
if(s->getWeight() >= 0)
{
++validSignaturesCount;
++notIntermediateNodesCount;
}
}
while(_stMem.size() && _maxStMemSize>0 && validSignaturesCount > _maxStMemSize)
std::map<int, int> reducedIds;
while(_stMem.size() && _maxStMemSize>0 && notIntermediateNodesCount > _maxStMemSize)
{
UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin());
Signature * s = this->_getSignature(*_stMem.begin());
if(!_localSpaceLinksKeptInWM)
int id = *_stMem.begin();
Signature * s = this->_getSignature(id);
UASSERT(s != 0);
if(s->getWeight() >= 0)
{
// remove local space links outside STM
UASSERT(s!=0);
std::map<int, Link> links = s->getLinks(); // get a copy because we will remove some links in "s"
for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kLocalSpaceClosure)
{
Signature * sTo = this->_getSignature(iter->first);
if(sTo)
{
sTo->removeLink(s->id());
}
else
{
UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
}
s->removeLink(iter->first);
}
}
--notIntermediateNodesCount;
}
if(!s->isBadSignature())
int reducedTo = 0;
moveSignatureToWMFromSTM(id, &reducedTo);
if(reducedTo > 0)
{
--validSignaturesCount;
reducedIds.insert(std::make_pair(id, reducedTo));
}
_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
_stMem.erase(*_stMem.begin());
}
if(stats) stats->setReducedIds(reducedIds);
if(!_memoryChanged && _incrementalMemory)
{
@@ -784,7 +773,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
UDEBUG("time = %fs", timer.ticks());
}
void Memory::addSignatureToWm(Signature * signature)
void Memory::addSignatureToWmFromLTM(Signature * signature)
{
if(signature)
{
@@ -799,6 +788,127 @@ void Memory::addSignatureToWm(Signature * signature)
}
}
void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
{
UDEBUG("Inserting node %d from STM in WM...", id);
UASSERT(_stMem.find(id) != _stMem.end());
Signature * s = this->_getSignature(id);
UASSERT(s!=0);
if(!_localSpaceLinksKeptInWM)
{
// remove local space links outside STM
std::map<int, Link> links = s->getLinks(); // get a copy because we will remove some links in "s"
for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kLocalSpaceClosure)
{
Signature * sTo = this->_getSignature(iter->first);
if(sTo)
{
sTo->removeLink(s->id());
}
else
{
UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
}
s->removeLink(iter->first);
}
}
}
if(_reduceGraph)
{
bool merge = false;
const std::map<int, Link> & links = s->getLinks();
std::map<int, Link> neighbors;
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(!merge)
{
merge = iter->second.to() < s->id() && // should be a parent->child link
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.userDataCompressed().empty() &&
iter->second.type() != Link::kUndef &&
iter->second.type() != Link::kVirtualClosure;
if(merge)
{
UDEBUG("Reduce %d to %d", s->id(), iter->second.to());
if(reducedTo)
{
*reducedTo = iter->second.to();
}
}
}
if(iter->second.type() == Link::kNeighbor)
{
neighbors.insert(*iter);
}
}
if(merge)
{
if(s->getLabel().empty())
{
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
merge = true;
Signature * sTo = this->_getSignature(iter->first);
UASSERT(sTo!=0);
sTo->removeLink(s->id());
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kUndef)
{
// link to all neighbors
for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
{
if(!sTo->hasLink(jter->second.to()))
{
Link l = iter->second.inverse().merge(
jter->second,
iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
sTo->addLink(l);
Signature * sB = this->_getSignature(l.to());
UASSERT(sB!=0);
UASSERT(!sB->hasLink(l.to()));
sB->addLink(l.inverse());
}
}
}
}
//remove neighbor links
std::map<int, Link> linksCopy = links;
for(std::map<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
{
s->removeLink(iter->first);
if(iter->second.type() == Link::kNeighbor)
{
if(_lastGlobalLoopClosureId == s->id())
{
_lastGlobalLoopClosureId = iter->first;
}
}
}
}
this->moveToTrash(s, _notLinkedNodesKeptInDb);
s = 0;
}
}
}
if(s != 0)
{
_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
_stMem.erase(*_stMem.begin());
}
// else already removed from STM/WM in moveToTrash()
}
const Signature * Memory::getSignature(int id) const
{
return _getSignature(id);
@@ -825,7 +935,8 @@ std::map<int, Link> Memory::getNeighborLinks(
const std::map<int, Link> & allLinks = s->getLinks();
for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor)
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
{
links.insert(*iter);
}
@@ -834,8 +945,19 @@ std::map<int, Link> Memory::getNeighborLinks(
else if(lookInDatabase && _dbDriver)
{
std::map<int, Link> neighbors;
_dbDriver->loadLinks(signatureId, neighbors, Link::kNeighbor);
links.insert(neighbors.begin(), neighbors.end());
_dbDriver->loadLinks(signatureId, neighbors);
for(std::map<int, Link>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
neighbors.erase(iter++);
}
else
{
++iter;
}
}
}
else
{
@@ -855,7 +977,8 @@ std::map<int, Link> Memory::getLoopClosureLinks(
const std::map<int, Link> & allLinks = s->getLinks();
for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(iter->second.type() > Link::kNeighbor &&
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kUndef)
{
loopClosures.insert(*iter);
@@ -867,7 +990,9 @@ std::map<int, Link> Memory::getLoopClosureLinks(
_dbDriver->loadLinks(signatureId, loopClosures);
for(std::map<int, Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end();)
{
if(iter->second.type() == Link::kNeighbor)
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged ||
iter->second.type() == Link::kUndef )
{
loopClosures.erase(iter++);
}
@@ -931,7 +1056,8 @@ std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNul
// return map<Id,Margin>, including signatureId
// maxCheckedInDatabase = -1 means no limit to check in database (default)
// maxCheckedInDatabase = 0 means don't check in database
std::map<int, int> Memory::getNeighborsId(int signatureId,
std::map<int, int> Memory::getNeighborsId(
int signatureId,
int maxGraphDepth, // 0 means infinite margin
int maxCheckedInDatabase, // default -1 (no limit)
bool incrementMarginOnLoop, // default false
@@ -992,7 +1118,7 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
ids.insert(std::pair<int, int>(*jter, m));
UTimer timer;
_dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kNeighbor:Link::kUndef);
_dbDriver->loadLinks(*jter, tmpLinks);
if(dbAccessTime)
{
*dbAccessTime += timer.getElapsedTime();
@@ -1005,7 +1131,8 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end())
{
UASSERT(iter->second.type() != Link::kUndef);
if(iter->second.type() == Link::kNeighbor)
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
{
if(ignoreIntermediateNodes && s->getWeight()==-1)
{
@@ -1116,7 +1243,7 @@ int Memory::getNextId()
return ++_idCount;
}
int Memory::incrementMapId()
int Memory::incrementMapId(std::map<int, int> * reducedIds)
{
//don't increment if there is no location in the current map
const Signature * s = getLastWorkingSignature();
@@ -1125,32 +1252,13 @@ int Memory::incrementMapId()
// New session! move all signatures from the STM to WM
while(_stMem.size())
{
UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin());
if(!_localSpaceLinksKeptInWM)
int reducedId = 0;
int id = *_stMem.begin();
moveSignatureToWMFromSTM(id, &reducedId);
if(reducedIds && reducedId > 0)
{
// remove local space links outside STM
Signature * s = this->_getSignature(*_stMem.begin());
UASSERT(s!=0);
std::map<int, Link> links = s->getLinks(); // get a copy because we will remove some links in "s"
for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kLocalSpaceClosure)
{
Signature * sTo = this->_getSignature(iter->first);
if(sTo)
{
sTo->removeLink(s->id());
}
else
{
UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
}
s->removeLink(iter->first);
}
}
reducedIds->insert(std::make_pair(id, reducedId));
}
_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
_stMem.erase(*_stMem.begin());
}
return ++_idMapCount;
@@ -1210,6 +1318,17 @@ void Memory::clear()
{
UDEBUG("");
// empty the STM
while(_stMem.size())
{
moveSignatureToWMFromSTM(*_stMem.begin());
}
if(_stMem.size() != 0)
{
ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
}
_stMem.clear();
this->cleanUnusedWords();
if(_dbDriver)
@@ -1217,6 +1336,12 @@ void Memory::clear()
_dbDriver->emptyTrashes();
_dbDriver->join();
}
if(_dbDriver)
{
// make sure time_enter in database is at least 1 second
// after for the next stuf added to database
uSleep(1500);
}
// Save some stats to the db, save only when the mem is not empty
if(_dbDriver && (_stMem.size() || _workingMem.size()))
@@ -1261,11 +1386,6 @@ void Memory::clear()
ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size());
}
_workingMem.clear();
if(_stMem.size() != 0)
{
ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
}
_stMem.clear();
if(_signatures.size()!=0)
{
ULOGGER_ERROR("_signatures must be empty here, size=%d", _signatures.size());
@@ -1450,8 +1570,15 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
{
UDEBUG("");
std::list<int> signaturesRemoved;
if(this->isIncremental() && _vwd->isIncremental() && _vwd->getVisualWords().size())
if(this->isIncremental() &&
_vwd->isIncremental() &&
_vwd->getVisualWords().size() &&
!_vwd->isIncrementalFlann())
{
// Note that when using incremental FLANN, the number of words
// is not the biggest issue, so use the number of signatures instead
// of the number of words
int newWords = 0;
int wordsRemoved = 0;
@@ -1719,32 +1846,33 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
if(!keepLinkedToGraph || (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
{
UASSERT_MSG(this->isInSTM(s->id()),
uFormat("Deleting location (%d) outside the STM is not implemented!", s->id()).c_str());
uFormat("Deleting location (%d) outside the "
"STM is not implemented!", s->id()).c_str());
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
Signature * sTo = this->_getSignature(iter->first);
// neighbor to s
if(sTo)
{
if(iter->first > s->id() && (sTo->getLinks().size() == 1 || !sTo->hasLink(s->id())))
{
UWARN("Link %d of %d is newer, removing neighbor link may split the map!",
iter->first, s->id());
}
UASSERT_MSG(sTo!=0,
uFormat("A neighbor (%d) of the deleted location %d is "
"not found in WM/STM! Are you deleting a location "
"outside the STM?", iter->first, s->id()).c_str());
// child
if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
{
sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
}
sTo->removeLink(s->id());
}
else
if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
{
UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
UWARN("Link %d of %d is newer, removing neighbor link "
"may split the map!",
iter->first, s->id());
}
// child
if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id())
{
sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
}
sTo->removeLink(s->id());
}
s->removeLinks(); // remove all links
s->setWeight(0);
@@ -1801,6 +1929,11 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
}
}
if(_lastGlobalLoopClosureId == s->id())
{
_lastGlobalLoopClosureId = 0;
}
if( (_notLinkedNodesKeptInDb || keepLinkedToGraph) &&
_dbDriver &&
s->id()>0 &&
@@ -1959,7 +2092,9 @@ void Memory::removeLink(int oldId, int newId)
bool noChildrenAnymore = true;
for(std::map<int, Link>::const_iterator iter=newS->getLinks().begin(); iter!=newS->getLinks().end(); ++iter)
{
if(iter->second.type() > Link::kNeighbor && iter->first < newS->id())
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->first < newS->id())
{
noChildrenAnymore = false;
break;
@@ -2554,7 +2689,11 @@ Transform Memory::computeIcpTransform(
fabs(ipitch) > _icpMaxRotation ||
fabs(iyaw) > _icpMaxRotation)))
{
msg = uFormat("Cannot compute transform (ICP correction too large)");
msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
uMax3(fabs(ix), fabs(iy), fabs(iz)),
uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)),
_icpMaxTranslation,
_icpMaxRotation);
UINFO(msg.c_str());
}
else
@@ -2888,23 +3027,28 @@ bool Memory::addLink(const Link & link)
if(link.type()!=Link::kVirtualClosure)
{
_linksChanged = true;
}
if(link.type() == Link::kGlobalClosure)
{
_lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id();
// update weight
// ignore scan matching loop closures
if(link.type() != Link::kLocalSpaceClosure ||
link.userDataCompressed().empty())
{
_lastGlobalLoopClosureId = fromS->id()>toS->id()?fromS->id():toS->id();
// update weights only if the memory is incremental
UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0);
if(fromS->id() > toS->id())
{
fromS->setWeight(fromS->getWeight() + toS->getWeight());
toS->setWeight(0);
}
else
{
toS->setWeight(toS->getWeight() + fromS->getWeight());
fromS->setWeight(0);
// update weights only if the memory is incremental
// When reducing the graph, transfer weight to the oldest signature
UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0);
if((_reduceGraph && fromS->id() < toS->id()) ||
(!_reduceGraph && fromS->id() > toS->id()))
{
fromS->setWeight(fromS->getWeight() + toS->getWeight());
toS->setWeight(0);
}
else
{
toS->setWeight(toS->getWeight() + fromS->getWeight());
fromS->setWeight(0);
}
}
}
}
@@ -3101,7 +3245,8 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
iter!=i->second->getLinks().end();
++iter)
{
if(iter->second.type() > Link::kNeighbor)
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
if(iter->first < i->first)
{
@@ -3199,7 +3344,9 @@ bool Memory::rehearsalMerge(int oldId, int newId)
if(oldS && newS && _incrementalMemory)
{
std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
if(iter != oldS->getLinks().end() && iter->second.type() > Link::kNeighbor)
if(iter != oldS->getLinks().end() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// do nothing, already merged
UWARN("already merged, old=%d, new=%d", oldId, newId);
@@ -3608,8 +3755,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.imageRaw().type() == CV_8UC3);
UASSERT_MSG(data.depthOrRightRaw().empty() ||
( (data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_8UC1) &&
data.depthOrRightRaw().rows == data.imageRaw().rows &&
data.depthOrRightRaw().cols == data.imageRaw().cols),
((data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) || (data.depthOrRightRaw().rows == data.imageRaw().rows && data.depthOrRightRaw().cols == data.imageRaw().cols))),
uFormat("image=(%d/%d) depth=(%d/%d, type=%d [accepted=%d,%d,%d])",
data.imageRaw().cols,
data.imageRaw().rows,
@@ -3912,7 +4058,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
descriptors = data.descriptors().clone();
// filter by depth
if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid())
if(!data.depthOrRightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValid())
{
//stereo
cv::Mat imageMono;
@@ -4104,6 +4250,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
Signature * s;
if(this->isBinDataKept())
{
UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
image.empty()?0:1,
depthOrRightImage.empty()?0:1,
laserScan.empty()?0:1,
data.userDataRaw().empty()?0:1);
std::vector<unsigned char> imageBytes;
std::vector<unsigned char> depthBytes;
@@ -4398,7 +4550,7 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
{
idsLoaded.push_back((*i)->id());
//append to working memory
this->addSignatureToWm(*i);
this->addSignatureToWmFromLTM(*i);
}
this->enableWordsRef(idsLoaded);
UDEBUG("time = %fs", timer.ticks());
@@ -4427,14 +4579,16 @@ void Memory::getMetricConstraints(
{
if(uContains(poses, *iter))
{
std::map<int, Link> neighbors = this->getNeighborLinks(*iter, lookInDatabase); // only direct neighbors
for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
std::map<int, Link> tmpLinks = getLinks(*iter, lookInDatabase);
for(std::map<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
{
if( jter->second.isValid() &&
uContains(poses, jter->first) &&
graph::findLink(links, *iter, jter->first) == links.end())
{
if(!lookInDatabase)
if(!lookInDatabase &&
(jter->second.type() == Link::kNeighbor ||
jter->second.type() == Link::kNeighborMerged))
{
Link link = jter->second;
const Signature * s = this->getSignature(jter->first);
@@ -4471,17 +4625,6 @@ void Memory::getMetricConstraints(
}
}
}
std::map<int, Link> loops = this->getLoopClosureLinks(*iter, lookInDatabase);
for(std::map<int, Link>::iterator jter=loops.begin(); jter!=loops.end(); ++jter)
{
if( jter->second.isValid() && // null transform means a rehearsed location
jter->first < *iter && // Loop parent to child
uContains(poses, jter->first))
{
links.insert(std::make_pair(*iter, jter->second));
}
}
}
}
}
corelib/src/OdometryOpticalFlow.cpp
+1
View File
@@ -505,6 +505,7 @@ Transform OdometryOpticalFlow::computeTransform(
data.cameraModels()[0].fy(),
true);
if(pcl::isFinite(pt) &&
pt.z > 0 &&
(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
{
newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform());
corelib/src/Rtabmap.cpp
+129 -19
View File
@@ -658,12 +658,27 @@ int Rtabmap::triggerNewMap()
int mapId = -1;
if(_memory)
{
mapId = _memory->incrementMapId();
std::map<int, int> reducedIds;
mapId = _memory->incrementMapId(&reducedIds);
UINFO("New map triggered, new map = %d", mapId);
_optimizedPoses.clear();
_constraints.clear();
_lastLocalizationNodeId = 0;
_distanceTravelled = 0.0f;
//Verify if there are nodes that were merged through graph reduction
if(reducedIds.size() && _path.size())
{
for(unsigned int i=0; i<_path.size(); ++i)
{
std::map<int, int>::const_iterator iter = reducedIds.find(_path[i].first);
if(iter!= reducedIds.end())
{
// change path ID to loop closure ID
_path[i].first = iter->second;
}
}
}
}
return mapId;
}
@@ -942,7 +957,6 @@ bool Rtabmap::process(
}
}
signature = _memory->getLastWorkingSignature();
if(!signature)
{
@@ -957,6 +971,7 @@ bool Rtabmap::process(
// Metric
//============================================================
bool smallDisplacement = false;
std::list<int> signaturesRemoved;
if(_rgbdSlamMode)
{
//Verify if there was a rehearsal
@@ -1080,9 +1095,10 @@ bool Rtabmap::process(
{
newPose = _mapCorrection * signature->getPose();
}
// Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // used in localization mode only (path planning)
if(signature->getLinks().size() == 1)
{
// link should be old to new
@@ -1108,6 +1124,43 @@ bool Rtabmap::process(
}
_constraints.insert(std::make_pair(tmp.from(), tmp));
}
//============================================================
// Reduced graph
//============================================================
//Verify if there are nodes that were merged through graph reduction
if(statistics_.reducedIds().size())
{
for(unsigned int i=0; i<_path.size(); ++i)
{
std::map<int, int>::const_iterator iter = statistics_.reducedIds().find(_path[i].first);
if(iter!= statistics_.reducedIds().end())
{
// change path ID to loop closure ID
_path[i].first = iter->second;
}
}
for(std::map<int, int>::const_iterator iter=statistics_.reducedIds().begin();
iter!=statistics_.reducedIds().end();
++iter)
{
int erased = _optimizedPoses.erase(iter->first);
if(erased)
{
for(std::multimap<int, Link>::iterator jter = _constraints.begin(); jter!=_constraints.end();)
{
if(jter->second.from() == iter->first || jter->second.to() == iter->first)
{
_constraints.erase(jter++);
}
else
{
++jter;
}
}
}
}
}
//============================================================
// Local loop closure in TIME
@@ -1132,7 +1185,6 @@ bool Rtabmap::process(
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
variance = 1.0f; // ICP, set variance to 1 // FIXME why? all other links based on visual keep the variance
}
if(!transform.isNull())
{
@@ -2097,9 +2149,10 @@ bool Rtabmap::process(
if(_rgbdSlamMode &&
(_loopClosureHypothesis.first>0 ||
lastLocalSpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() ||
localLoopClosuresInTimeFound>0 ||
((_memory->isIncremental() || signature->getLinks().size()) && // In localization mode, the new node should be linked
(localLoopClosuresInTimeFound>0 || // only same map of the current one
signaturesRetrieved.size())))) // can be different map of the current one
signaturesRetrieved.size()))) // can be different map of the current one
{
UASSERT(uContains(_optimizedPoses, signature->id()));
@@ -2345,7 +2398,6 @@ bool Rtabmap::process(
}
// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
std::list<int> signaturesRemoved;
int signatureRemoved = _memory->cleanup();
if(signatureRemoved)
{
@@ -2898,11 +2950,46 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
{
UTimer timer;
std::map<int, Transform> optimizedPoses;
std::map<int, Transform> poses;
std::multimap<int, Link> edgeConstraints;
std::map<int, Transform> poses, posesOut;
std::multimap<int, Link> edgeConstraints, linksOut;
UDEBUG("ids=%d", (int)ids.size());
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
// The constraints must be all already connected! Only check in debug
if(ULogger::level() == ULogger::kDebug)
{
_graphOptimizer->getConnectedGraph(fromId, poses, edgeConstraints, posesOut, linksOut);
if(poses.size() != posesOut.size())
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(posesOut.find(iter->first) == posesOut.end())
{
UERROR("Not found %d in posesOut", iter->first);
for(std::multimap<int, Link>::iterator jter=edgeConstraints.begin(); jter!=edgeConstraints.end(); ++jter)
{
if(jter->second.from() == iter->first || jter->second.to()==iter->first)
{
UERROR("Found link %d->%d", jter->second.from(), jter->second.to());
}
}
}
}
}
if(edgeConstraints.size() != linksOut.size())
{
for(std::multimap<int, Link>::iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(graph::findLink(linksOut, iter->second.from(), iter->second.to()) == linksOut.end())
{
UERROR("Not found link %d->%d in linksOut", iter->second.from(), iter->second.to());
}
}
}
UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size() == linksOut.size(),
uFormat("nodes %d->%d, links %d->%d", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size()).c_str());
}
if(constraints)
{
@@ -3285,6 +3372,24 @@ bool Rtabmap::computePath(int targetNode, bool global)
{
// set goal to latest signature
std::string goalStr = uFormat("GOAL:%d", targetNode);
// use label is exist
if(_memory->getSignature(targetNode))
{
if(!_memory->getSignature(targetNode)->getLabel().empty())
{
goalStr = std::string("GOAL:")+_memory->getSignature(targetNode)->getLabel();
}
}
else if(global)
{
std::map<int, std::string> labels = _memory->getAllLabels();
std::map<int, std::string>::iterator iter = labels.find(targetNode);
if(iter != labels.end() && !iter->second.empty())
{
goalStr = std::string("GOAL:")+labels.at(targetNode);
}
}
setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone());
}
updateGoalIndex();
@@ -3474,17 +3579,19 @@ void Rtabmap::updateGoalIndex()
// remove all previous virtual links
for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i)
{
if(_memory->getSignature(_path[i].first))
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
{
_memory->removeVirtualLinks(_path[i].first);
_memory->removeVirtualLinks(s->id());
}
}
// for the current index, only keep the newest virtual link
// This will make sure that the path is still connected even
// if the new signature is removed (e.g., because of a small displacement)
UASSERT(_pathCurrentIndex < _path.size());
const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first);
UASSERT(currentIndexS != 0);
UASSERT_MSG(currentIndexS != 0, uFormat("_path[%d].first=%d", _pathCurrentIndex, _path[_pathCurrentIndex].first).c_str());
std::map<int, Link> links = currentIndexS->getLinks(); // make a copy
bool latestVirtualLinkFound = false;
for(std::map<int, Link>::reverse_iterator iter=links.rbegin(); iter!=links.rend(); ++iter)
@@ -3516,14 +3623,17 @@ void Rtabmap::updateGoalIndex()
}
if(distanceSoFar <= _localRadius)
{
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
if(_path[i].first != _path[i-1].first)
{
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, 100, 100)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
}
}
}
corelib/src/RtabmapThread.cpp
+10 -2
View File
@@ -257,10 +257,18 @@ void RtabmapThread::mainLoop()
if(id == 0 && !parameters.at("label").empty() && _rtabmap->getMemory())
{
id = _rtabmap->getMemory()->getSignatureIdByLabel(parameters.at("label"));
if(id <= 0)
{
UERROR("Failed to find a node with label \"%s\".", parameters.at("label").c_str());
}
}
if(id <= 0 || !_rtabmap->computePath(id, true))
else if(id < 0)
{
UERROR("Failed to set a goal to location=%d.", id);
UERROR("Failed to set a goal. ID (%d) should be positive > 0", id);
}
if(id > 0 && !_rtabmap->computePath(id, true))
{
UERROR("Failed to compute a path to goal %d.", id);
}
this->post(new RtabmapGlobalPathEvent(id, parameters.at("label"), _rtabmap->getPath()));
break;
corelib/src/Signature.cpp
+3 -1
View File
@@ -97,7 +97,8 @@ void Signature::addLinks(const std::map<int, Link> & links)
void Signature::addLink(const Link & link)
{
UDEBUG("Add link %d to %d (type=%d)", link.to(), this->id(), (int)link.type());
UASSERT(link.from() == this->id());
UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG(link.to() != this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
_linksModified = true;
@@ -134,6 +135,7 @@ void Signature::removeLink(int idTo)
int count = (int)_links.erase(idTo);
if(count)
{
UDEBUG("Removed link %d from %d", idTo, this->id());
_linksModified = true;
}
}