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/include/rtabmap/core/DBDriver.h
+18
View File
@@ -98,6 +98,15 @@ public:
bool isConnected() const;
long getMemoryUsed() const; // In bytes
std::string getDatabaseVersion() const;
long getImagesMemoryUsed() const;
long getDepthImagesMemoryUsed() const;
long getLaserScansMemoryUsed() const;
long getUserDataMemoryUsed() const;
long getWordsMemoryUsed() const;
int getLastNodesSize() const; // working memory
int getLastDictionarySize() const; // working memory
int getTotalNodesSize() const;
int getTotalDictionarySize() const;
void executeNoResult(const std::string & sql) const;
@@ -130,6 +139,15 @@ private:
virtual bool isConnectedQuery() const = 0;
virtual long getMemoryUsedQuery() const = 0; // In bytes
virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
virtual long getImagesMemoryUsedQuery() const = 0;
virtual long getDepthImagesMemoryUsedQuery() const = 0;
virtual long getLaserScansMemoryUsedQuery() const = 0;
virtual long getUserDataMemoryUsedQuery() const = 0;
virtual long getWordsMemoryUsedQuery() const = 0;
virtual int getLastNodesSizeQuery() const = 0;
virtual int getLastDictionarySizeQuery() const = 0;
virtual int getTotalNodesSizeQuery() const = 0;
virtual int getTotalDictionarySizeQuery() const = 0;
virtual void executeNoResultQuery(const std::string & sql) const = 0;
corelib/include/rtabmap/core/Graph.h
+31 -4
View File
@@ -244,19 +244,23 @@ bool RTABMAP_EXP exportPoses(
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
int to);
int to,
bool checkBothWays = true);
std::multimap<int, int>::iterator RTABMAP_EXP findLink(
std::multimap<int, int> & links,
int from,
int to);
int to,
bool checkBothWays = true);
std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
const std::multimap<int, Link> & links,
int from,
int to);
int to,
bool checkBothWays = true);
std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
const std::multimap<int, int> & links,
int from,
int to);
int to,
bool checkBothWays = true);
/**
* Get only the the most recent or older poses in the defined radius.
@@ -284,6 +288,12 @@ std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
float radius,
float angle);
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);
/**
* Perform A* path planning in the graph.
* @param poses The graph's poses
@@ -300,6 +310,23 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
int to,
bool updateNewCosts = false);
/**
* Perform Dijkstra path planning in the graph.
* @param poses The graph's poses
* @param links The graph's links (from node id -> to node id)
* @param from initial node
* @param to final node
* @param updateNewCosts Keep up-to-date costs while traversing the graph.
* @param useSameCostForAllLinks Ignore distance between nodes
* @return the path ids from id "from" to id "to" including initial and final nodes.
*/
std::list<int> RTABMAP_EXP computePath(
const std::multimap<int, Link> & links,
int from,
int to,
bool updateNewCosts = false,
bool useSameCostForAllLinks = false);
/**
* Perform Dijkstra path planning in the graph.
* @param fromId initial node
corelib/include/rtabmap/core/Link.h
+9 -1
View File
@@ -38,7 +38,15 @@ namespace rtabmap {
class RTABMAP_EXP Link
{
public:
enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kVirtualClosure, kUndef};
enum Type {
kNeighbor,
kGlobalClosure,
kLocalSpaceClosure,
kLocalTimeClosure,
kUserClosure,
kVirtualClosure,
kNeighborMerged,
kUndef};
Link();
Link(int from,
int to,
corelib/include/rtabmap/core/Memory.h
+5 -2
View File
@@ -77,7 +77,7 @@ public:
bool postInitClosingEvents = false);
std::map<int, float> computeLikelihood(const Signature * signature,
const std::list<int> & ids);
int incrementMapId();
int incrementMapId(std::map<int, int> * reducedIds = 0);
void updateAge(int signatureId);
std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
@@ -155,6 +155,7 @@ public:
bool isIDsGenerated() const {return _generateIds;}
int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
const Feature2D * getFeature2D() const {return _feature2D;}
bool isGraphReduced() const {return _reduceGraph;}
void setRoi(const std::string & roi);
@@ -197,7 +198,8 @@ private:
void clear();
void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
void addSignatureToWm(Signature * signature);
void moveSignatureToWMFromSTM(int id, int * reducedTo = 0);
void addSignatureToWmFromLTM(Signature * signature);
Signature * _getSignature(int id) const;
std::list<Signature *> getRemovableSignatures(int count,
const std::set<int> & ignoredIds = std::set<int>());
@@ -231,6 +233,7 @@ private:
bool _saveDepth16Format;
bool _notLinkedNodesKeptInDb;
bool _incrementalMemory;
bool _reduceGraph;
int _maxStMemSize;
float _recentWmRatio;
bool _transferSortingByWeightId;
corelib/include/rtabmap/core/Parameters.h
+5 -4
View File
@@ -189,7 +189,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, SaveDepth16Format, bool, true, "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size.");
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode.");
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode.");
RTABMAP_PARAM(Mem, ReduceGraph, bool, false, "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2, "Ratio of locations after the last loop closure in WM that cannot be transferred.");
RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false, "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
@@ -286,8 +287,8 @@ class RTABMAP_EXP Parameters
// RGB-D SLAM
RTABMAP_PARAM(RGBD, Enabled, bool, true, "");
RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.1, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.1, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1.0, "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.");
@@ -303,7 +304,7 @@ class RTABMAP_EXP Parameters
// Local loop closure detection
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, true, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth, int, 50, "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius, float, 0.5, "Path filtering radius.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
corelib/include/rtabmap/core/Statistics.h
+4
View File
@@ -154,6 +154,7 @@ public:
void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
void setCurrentGoalId(int goal) {_currentGoalId=goal;}
void setReducedIds(const std::map<int, int> & reducedIds) {_reducedIds = reducedIds;}
// getters
bool extended() const {return _extended;}
@@ -173,6 +174,7 @@ public:
const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
const std::vector<int> & localPath() const {return _localPath;}
int currentGoalId() const {return _currentGoalId;}
const std::map<int, int> & reducedIds() const {return _reducedIds;}
const std::map<std::string, float> & data() const {return _data;}
@@ -198,6 +200,8 @@ private:
std::vector<int> _localPath;
int _currentGoalId;
std::map<int, int> _reducedIds;
// Format for statistics (Plottable statistics must go in that map) :
// {"Group/Name/Unit", value}
// Example : {"Timing/Total time/ms", 500.0f}
corelib/include/rtabmap/core/VWDictionary.h
+8 -1
View File
@@ -46,7 +46,13 @@ class FlannIndex;
class RTABMAP_EXP VWDictionary
{
public:
enum NNStrategy{kNNFlannNaive, kNNFlannKdTree, kNNFlannLSH, kNNBruteForce, kNNBruteForceGPU, kNNUndef};
enum NNStrategy{
kNNFlannNaive,
kNNFlannKdTree,
kNNFlannLSH,
kNNBruteForce,
kNNBruteForceGPU,
kNNUndef};
static const int ID_START;
static const int ID_INVALID;
@@ -79,6 +85,7 @@ public:
unsigned int getIndexMemoryUsed() const;
void setNNStrategy(NNStrategy strategy);
bool isIncremental() const {return _incrementalDictionary;}
bool isIncrementalFlann() const {return _incrementalFlann;}
void setIncrementalDictionary();
void setFixedDictionary(const std::string & dictionaryPath);
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;
}
}