diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index 847caa50..cc451dd3 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -162,7 +162,7 @@ public: void executeNoResult(const std::string & sql) const; // Load objects - void load(VWDictionary & dictionary, bool lastStateOnly = true) const; + void load(VWDictionary & dictionary, bool lastStateOnly = true, bool idsOnly = false) const; void loadLastNodes(std::list & signatures, bool loadWordIdsOnly = false) const; // returned signatures must be freed after usage Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded void loadSignatures(const std::list & ids, std::list & signatures, std::set * loadedFromTrash = 0, bool loadWordIdsOnly = false); // returned signatures must be freed after usage @@ -279,7 +279,7 @@ protected: virtual void saveFlannIndexQuery(const std::vector & indexData) const = 0; // Load objects - virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const = 0; + virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true, bool idsOnly = false) const = 0; virtual void loadLastNodesQuery(std::list & signatures, bool loadWordIdsOnly) const = 0; virtual void loadSignaturesQuery(const std::list & ids, std::list & signatures, bool loadWordIdsOnly) const = 0; virtual void loadWordsQuery(const std::set & wordIds, std::list & vws) const = 0; diff --git a/corelib/include/rtabmap/core/DBDriverSqlite3.h b/corelib/include/rtabmap/core/DBDriverSqlite3.h index a5df8e92..e73bab86 100644 --- a/corelib/include/rtabmap/core/DBDriverSqlite3.h +++ b/corelib/include/rtabmap/core/DBDriverSqlite3.h @@ -138,7 +138,7 @@ protected: virtual void saveFlannIndexQuery(const std::vector & indexData) const; // Load objects - virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true) const; + virtual void loadQuery(VWDictionary & dictionary, bool lastStateOnly = true, bool idsOnly = false) const; virtual void loadLastNodesQuery(std::list & signatures, bool loadWordIdsOnly) const; virtual void loadSignaturesQuery(const std::list & ids, std::list & signatures, bool loadWordIdsOnly) const; virtual void loadWordsQuery(const std::set & wordIds, std::list & vws) const; diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index ab3df0a7..040b8516 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -141,11 +141,12 @@ public: const std::map & optimizedPoses, int maxGraphDepth) const; void convertToIntermediate(int locationId); - void deleteLocation(int locationId, std::list * deletedWords = 0); + void deleteLocation(int locationId, std::list * deletedWords = 0, bool keepLinkedInDb = false); void saveLocationData(int locationId); void removeLink(int idA, int idB); void removeRawData(int id, bool image = true, bool scan = true, bool userData = true, bool occupancyGrid = true); int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false, int direction = 0); + void setDummyDictionary(bool enabled); //getters const std::map & getWorkingMem() const {return _workingMem;} @@ -279,7 +280,6 @@ private: std::list getRemovableSignatures(int count, const std::set & ignoredIds = std::set()); int getNextId(); - void initCountId(); void rehearsal(Signature * signature, Statistics * stats = 0); bool rehearsalMerge(int oldId, int newId); bool canBeReduced(const Link & link, float maxDistance, int direction); @@ -396,6 +396,8 @@ private: MarkerDetector * _markerDetector; GlobalDescriptorExtractor * _globalDescriptorExtractor; + + bool _dummyDictionary; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index b3d438e8..930aac21 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -229,6 +229,7 @@ public: bool addLink(const Link & link); cv::Mat getInformation(const cv::Mat & covariance) const; void addNodesToRepublish(const std::vector & ids); + void setDummyDictionary(bool enabled); int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success void clearPath(int status); // -1=failed 0=idle/executing 1=success @@ -402,6 +403,8 @@ private: int _pathStuckCount; float _pathStuckDistance; + bool _dummyDictionary; + #ifdef RTABMAP_PYTHON PythonInterface * _python; #endif diff --git a/corelib/include/rtabmap/core/Signature.h b/corelib/include/rtabmap/core/Signature.h index 9dbbe532..fbb3e217 100644 --- a/corelib/include/rtabmap/core/Signature.h +++ b/corelib/include/rtabmap/core/Signature.h @@ -62,7 +62,7 @@ public: virtual ~Signature(); /** - * Must return a value between >=0 and <=1 (1 means 100% similarity). + * Must return a value between >=0 and <=1 (1 means 100% similarity). */ float compareTo(const Signature & signature) const; bool isBadSignature() const; @@ -70,10 +70,10 @@ public: int id() const {return _id;} int mapId() const {return _mapId;} - void setWeight(int weight) {_modified=_weight!=weight;_weight = weight;} + void setWeight(int weight) {_modified=_modified || _weight!=weight;_weight = weight;} int getWeight() const {return _weight;} - void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;} + void setLabel(const std::string & label) {_modified=_modified || _label.compare(label)!=0;_label = label;} const std::string & getLabel() const {return _label;} double getStamp() const {return _stamp;} diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index d71c1400..a78b181e 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -539,10 +539,10 @@ void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan) _dbSafeAccessMutex.unlock(); } -void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly) const +void DBDriver::load(VWDictionary & dictionary, bool lastStateOnly, bool idsOnly) const { _dbSafeAccessMutex.lock(); - this->loadQuery(dictionary, lastStateOnly); + this->loadQuery(dictionary, lastStateOnly, idsOnly); _dbSafeAccessMutex.unlock(); } diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 66986051..1bce54a2 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -3538,7 +3538,7 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list & nodes, bool lo } } -void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) const +void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly, bool idsOnly) const { ULOGGER_DEBUG(""); if(_ppDb) @@ -3552,7 +3552,12 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c std::list visualWords; // Get the visual words - query << "SELECT id, descriptor_size, descriptor FROM Word "; + query << "SELECT id"; + if(!idsOnly) + { + query << ", descriptor_size, descriptor"; + } + query << " FROM Word "; if(lastStateOnly) { if(uStrNumCmp(_version, "0.11.11") >= 0) @@ -3581,27 +3586,29 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c int index=0; id = sqlite3_column_int(ppStmt, index++); // VisualWord Id - descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size - descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array - dRealSize = sqlite3_column_bytes(ppStmt, index++); - cv::Mat d; - if(dRealSize == descriptorSize) - { - // CV_8U binary descriptors - d = cv::Mat(1, descriptorSize, CV_8U); - } - else if(dRealSize/int(sizeof(float)) == descriptorSize) - { - // CV_32F - d = cv::Mat(1, descriptorSize, CV_32F); - } - else - { - UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize); - } + if(!idsOnly) { + descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size + descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array + dRealSize = sqlite3_column_bytes(ppStmt, index++); - memcpy(d.data, descriptor, dRealSize); + if(dRealSize == descriptorSize) + { + // CV_8U binary descriptors + d = cv::Mat(1, descriptorSize, CV_8U); + } + else if(dRealSize/int(sizeof(float)) == descriptorSize) + { + // CV_32F + d = cv::Mat(1, descriptorSize, CV_32F); + } + else + { + UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize); + } + + memcpy(d.data, descriptor, dRealSize); + } VisualWord * vw = new VisualWord(id, d); vw->setSaved(true); dictionary.addWord(vw); @@ -3621,7 +3628,7 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly) c getLastWordId(id); dictionary.setLastWordId(id); - if(uStrNumCmp(_version, "0.23.0") >= 0) { + if(!idsOnly && uStrNumCmp(_version, "0.23.0") >= 0) { // load dictionary index std::stringstream query3; query3 << "SELECT dictionary_index " diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index e0ffac39..5b7dc5db 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -138,7 +138,8 @@ Memory::Memory(const ParametersMap & parameters) : _badSignRatio(Parameters::defaultKpBadSignRatio()), _tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()), _parallelized(Parameters::defaultKpParallelized()), - _registrationVis(0) + _registrationVis(0), + _dummyDictionary(false) { _feature2D = Feature2D::create(parameters); _vwd = new VWDictionary(parameters); @@ -411,31 +412,55 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) { if(wordIds.size()) { - std::list words; - _dbDriver->loadWords(wordIds, words); - for(std::list::iterator iter = words.begin(); iter!=words.end(); ++iter) + if(_dummyDictionary) { - _vwd->addWord(*iter); + for(std::set::iterator iter = wordIds.begin(); iter!=wordIds.end(); ++iter) + { + VisualWord * w = new VisualWord(*iter, cv::Mat()); + w->setSaved(true); + _vwd->addWord(w); // placeholder descriptor + } + } + else + { + std::list words; + _dbDriver->loadWords(wordIds, words); + for(std::list::iterator iter = words.begin(); iter!=words.end(); ++iter) + { + _vwd->addWord(*iter); + } } // Get Last word id int id = 0; _dbDriver->getLastWordId(id); _vwd->setLastWordId(id); } + else { + _dummyDictionary = false; + } } else { - _dbDriver->load(*_vwd, false); + _dbDriver->load(*_vwd, false, _dummyDictionary); } } else { UDEBUG("load words"); // load the last dictionary - _dbDriver->load(*_vwd, _vwd->isIncremental()); + _dbDriver->load(*_vwd, _vwd->isIncremental(), _dummyDictionary); + } + UDEBUG("%d words loaded! (type=%s, dim=%d)", + _vwd->getUnusedWordsSize(), + _vwd->getVisualWords().empty()?"NA":_vwd->getVisualWords().begin()->second->getDescriptor().empty()?"dummy":_vwd->getVisualWords().begin()->second->getDescriptor().type() == CV_32FC1?"float":"binary", + _vwd->getVisualWords().empty()?0:_vwd->getVisualWords().begin()->second->getDescriptor().cols); + UDEBUG("Dictionary memory usage: %ld Bytes (%ld MB)", _vwd->getMemoryUsed(), _vwd->getMemoryUsed()/(1024*1024)); + if(!_dummyDictionary) { + _vwd->update(); + } + else { + UDEBUG("Dictionary update skipped (dummy dictionary is enabled)"); } - UDEBUG("%d words loaded!", _vwd->getUnusedWordsSize()); - _vwd->update(); if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Loading dictionary, done! (%d words)", (int)_vwd->getUnusedWordsSize()))); if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Adding word references..."))); @@ -496,6 +521,24 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) UWARN("%s", msg.c_str()); if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(msg)); + if(_dummyDictionary) + { + UWARN("Dummy dictionary cannot be used when repairing the dictionary, disabling dummy dictionary."); + for(std::map::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) + { + Signature * s = this->_getSignature(i->first); + UASSERT(s != 0); + if(!s->isEnabled()) + { + break; + } + this->disableWordsRef(s->id()); + } + _vwd->deleteUnusedWords(); + _vwd->clear(); + _dummyDictionary = false; + } + //remove all words ref const std::map & addedWords = _vwd->getVisualWords(); @@ -595,6 +638,21 @@ void Memory::loadDataFromDb(bool postInitClosingEvents) UDEBUG("map ids start with %d", _idMapCount); } +void Memory::setDummyDictionary(bool enabled) +{ + if(_dbDriver != 0) { + UERROR("Dummy dictionary can only be set if the memory is not yet initialized. Ignoring."); + return; + } + if(enabled) { + UINFO("Dummy dictionary enabled."); + } + else { + UINFO("Dummy dictionary disabled."); + } + _dummyDictionary = enabled; +} + void Memory::saveFlannIndex(bool postInitClosingEvents) { if(!_dbDriver) { @@ -1324,6 +1382,11 @@ int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direc UWARN("Node %d is not in WM/STM, cannot reduce it.", id); return 0; } + else if(s->getWeight() == -1) + { + UWARN("Cannot reduce intermediate node %d (not supported).", id); + return 0; + } if(!s->getLabel().empty()) { @@ -1340,6 +1403,11 @@ int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direc { float distance = iter->second.transform().getNorm(); reducedTo = iter->second.to(); + if(this->_getSignature(reducedTo) == 0) + { + UWARN("Node %d is not in WM/STM, cannot reduce %d to it.", reducedTo, id); + return 0; + } UDEBUG("Reduce %d to %d (distance=%f)", s->id(), iter->second.to(), distance); } @@ -1347,6 +1415,18 @@ int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direc if(iter->second.type() == Link::kNeighbor) { neighbors.insert(*iter); + // neighbors should not be intermediate nodes + Signature * sTo = this->_getSignature(iter->first); + if(sTo == 0) + { + UWARN("Neighbor node %d is not in WM/STM, cannot reduce %d.", iter->first, id); + return 0; + } + else if(sTo->getWeight() == -1) + { + UWARN("Neighbor node %d is an intermediate node (not supported), cannot reduce %d.", iter->first, id); + return 0; + } } } if(reducedTo>0) @@ -1455,10 +1535,10 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut) else { std::multimap links = s->getLinks(); - // Setting true to make sure we save all visual + // Setting keepLinkedInDb=true to make sure we save all visual // words that could be referenced in a previously // transferred node in LTM (#979) - reducedId = reduceNode(s->id(), 0, true); + reducedId = reduceNode(s->id(), 0, /*keepLinkedInDb*/ true); if(reducedToOut) { *reducedToOut = reducedId; } @@ -3132,13 +3212,18 @@ void Memory::convertToIntermediate(int locationId) } } -void Memory::deleteLocation(int locationId, std::list * deletedWords) +void Memory::deleteLocation(int locationId, std::list * deletedWords, bool keepLinkedInDb) { - UDEBUG("Deleting location %d", locationId); + UDEBUG("Deleting location %d (keepLinkedInDb=%s)", locationId, keepLinkedInDb?"true":"false"); Signature * location = _getSignature(locationId); if(location) { - this->moveToTrash(location, false, deletedWords); + this->moveToTrash(location, keepLinkedInDb, deletedWords); + _memoryChanged = true; + } + else + { + UWARN("Location %d has not been found in STM/WM, cannot delete it.", locationId); } } @@ -5092,6 +5177,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor data.depthOrRightRaw().rows, data.depthOrRightRaw().type(), CV_16UC1, CV_32FC1, CV_8UC1, CV_8UC3).c_str()); + UASSERT_MSG(!_dummyDictionary, "Memory::createSignature() cannot be called if the memory has been initialized with a dummy dictionary."); if(!data.depthOrRightRaw().empty() && data.cameraModels().empty() && diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 1c64de81..89351adc 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -180,7 +180,8 @@ Rtabmap::Rtabmap() : _pathGoalIndex(0), _pathTransformToGoal(Transform::getIdentity()), _pathStuckCount(0), - _pathStuckDistance(0.0f) + _pathStuckDistance(0.0f), + _dummyDictionary(false) #ifdef RTABMAP_PYTHON ,_python(new PythonInterface()) #endif @@ -360,6 +361,10 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas if(!_memory) { _memory = new Memory(allParameters); + if(_dummyDictionary) + { + _memory->setDummyDictionary(true); + } _memory->init(_databasePath, false, allParameters, true); } @@ -6934,6 +6939,18 @@ void Rtabmap::addNodesToRepublish(const std::vector & ids) } } +void Rtabmap::setDummyDictionary(bool enabled) +{ + if(_memory) { + UERROR("Memory is already initialized, cannot set dummy dictionary. This " + "function can only be called after Rtabmap object is created, but " + "before init() is called."); + } + else { + _dummyDictionary = true; + } +} + void Rtabmap::clearPath(int status) { UINFO("status=%d", status); diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index 952e012c..ff979825 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -108,7 +108,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters) incrementalDictionary = uStr2Bool((*iter).second.c_str()); } - // Verifying hypotheses strategy + // Verifying NN strategy bool treeUpdated = false; if((iter=parameters.find(Parameters::kKpNNStrategy())) != parameters.end()) { diff --git a/tools/DetectMoreLoopClosures/main.cpp b/tools/DetectMoreLoopClosures/main.cpp index 2f4fb31f..d15374e7 100644 --- a/tools/DetectMoreLoopClosures/main.cpp +++ b/tools/DetectMoreLoopClosures/main.cpp @@ -249,9 +249,15 @@ int main(int argc, char * argv[]) UTimer timer; ParametersMap originalParameters = parameters; uInsert(parameters, inputParams); + + // This avoids to load original descriptors in the dictionary + // to save RAM and intialization time (we don't need the dictionary for this tool) + rtabmap.setDummyDictionary(true); // should be set before Rtabmap::init() + rtabmap.init(parameters, dbPath); printf("Initialization... done! (%f sec)\n", timer.ticks()); + // detectMoreLoopClosures would clear the optimized map if loop closures are detected float xMin, yMin, cellSize; bool haveOptimizedMap = !rtabmap.getMemory()->load2DMap(xMin, yMin, cellSize).empty(); @@ -311,7 +317,7 @@ int main(int argc, char * argv[]) // Restore original parameters before saving back the database rtabmap.parseParameters(originalParameters); - rtabmap.close(); + rtabmap.close(detected>0); - return 0; + return detected>=0; } diff --git a/tools/ReduceGraph/main.cpp b/tools/ReduceGraph/main.cpp index 5d44ab9e..2046b643 100644 --- a/tools/ReduceGraph/main.cpp +++ b/tools/ReduceGraph/main.cpp @@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include #include #include @@ -56,9 +57,19 @@ void showUsage(const char * exec) "%s [Options] database.db\n" "Options:\n" " --keep_latest Merge old nodes to newer nodes, thus keeping only latest nodes.\n" - " --keep_linked Keep reduced nodes linked to graph.\n" - " --pre_cleanup Remove all user loop closures linking nodes closer than %s in the graph before reducing the graph.\n" + " --keep_linked Keep reduced nodes linked to graph (by only child->parent link)\n" + " instead of invalidating them. When --remove_orphan_nodes is used,\n" + " orphan nodes are simply transfered to LTM instead of being invalidated.\n" + " --pre_cleanup Remove all user loop closures linking nodes closer than %s nodes in the graph before reducing the graph.\n" " --radius #.# Maximum loop closure distance that can be merged. Default is 1 m. Should be > 0.\n" + " --remove_orphan_nodes Remove all orphan nodes created by graph reduction \n" + " from WM and LTM. This assumes that the original global \n" + " graph connected all nodes in WM and LTM, otherwise it is skipped.\n" + " --remove_all_orphan_nodes Remove all orphan nodes created or not by graph \n" + " reduction from WM and LTM. Warning: this could remove \n" + " completly unconnected graphes from WM and LTM. Usage of \n" + " --remove_orphan_nodes is safer. Backup your database \n" + " before trying this.\n" " --udebug/--uinfo/--warn can also be used to change verbosity.\n" "\n", exec, Parameters::kMemSTMSize().c_str()); exit(1); @@ -78,6 +89,8 @@ int main(int argc, char * argv[]) bool keepLinked = false; float radius = 1.0f; bool preCleanup = false; + bool removeOrphanNodes = false; + bool removeAllOrphanNodes = false; for(int i=1; i wm; if(driver->openConnection(dbPath)) { parameters = driver->getLastParameters(); + driver->getLastNodeIds(wm); driver->closeConnection(false); } else @@ -154,7 +180,13 @@ int main(int argc, char * argv[]) } delete driver; + size_t wmOrgSize = wm.size(); Memory memory; + + // This avoids to load original descriptors in the dictionary + // to save RAM and intialization time (we don't need the dictionary for this tool) + memory.setDummyDictionary(true); // should be set before Memory::init() + printf("Initialization...\n"); UTimer timer; ParametersMap originalParameters = parameters; @@ -173,10 +205,53 @@ int main(int argc, char * argv[]) return 1; } - Transform lastLocalizationPose; - std::map optimizedPoses = memory.loadOptimizedPoses(&lastLocalizationPose); - float xMin, yMin, cellSize; - bool hasOptimizedMap = !memory.load2DMap(xMin, yMin, cellSize).empty(); + bool isWholeGraphConnected = false; + std::shared_ptr optimizer(Optimizer::create(parameters)); + + std::map poses; + std::multimap constraints; + memory.getMetricConstraints(ids, poses, constraints, false, true); + if(!poses.empty()) + { + std::map posesOut; + std::multimap linksOut; + optimizer->getConnectedGraph( + keepLatest?poses.rbegin()->first:poses.begin()->first, + poses, + constraints, + posesOut, + linksOut); + isWholeGraphConnected = posesOut.size() == poses.size(); + + if(isWholeGraphConnected) + { + printf("The whole global graph is connected to all nodes of WM/LTM (%ld/%ld).\n", + posesOut.size(), ids.size()); + } + else { + //Count number of nodes not in global graph that are in WM + int missing = 0; + for(auto id:wm) + { + if(posesOut.find(id) == posesOut.end()) { + ++missing; + } + } + if(missing>0) + { + printf("The whole global graph is not connected to all nodes of WM/LTM (%ld/%ld) " + "with some (%d) of the disconnected nodes in WM.%s\n", + posesOut.size(), ids.size(), missing, + removeAllOrphanNodes?"":" You may consider using --remove_all_orphan_nodes to remove these nodes from WM if necessary."); + } + else + { + printf("The whole global graph is not connected to all nodes of WM/LTM (%ld/%ld), " + "though no disconnected nodes are in WM.\n", + posesOut.size(), ids.size()); + } + } + } int totalNodesReduced = 0; std::vector vids; @@ -192,6 +267,7 @@ int main(int argc, char * argv[]) vids.insert(vids.end(), ids.rbegin(), ids.rend()); } + int totalLinksRemoved = 0; if(preCleanup) { if(memory.getMaxStMemSize() <= 1) @@ -200,7 +276,6 @@ int main(int argc, char * argv[]) } else { - int totalRemoved = 0; for(auto id: vids) { auto nids = memory.getNeighborsId(id, memory.getMaxStMemSize(), -1, true, true, true); @@ -211,15 +286,20 @@ int main(int argc, char * argv[]) nids.find(link.first)!=nids.end()) { memory.removeLink(id, link.first); - ++totalRemoved; + ++totalLinksRemoved; } } } printf("Removed %d user links that were linking nodes that were close in the graph (below %s=%d)\n", - totalRemoved, Parameters::kMemSTMSize().c_str(), memory.getMaxStMemSize()); + totalLinksRemoved, Parameters::kMemSTMSize().c_str(), memory.getMaxStMemSize()); } } + // Get local optimized graph before reduction + Transform lastLocalizationPose; + std::map optimizedPoses = memory.loadOptimizedPoses(&lastLocalizationPose); + + // Graph reduction for(auto id: vids) { // Nodes can be already reduced by other nodes, check if they are still there @@ -234,6 +314,82 @@ int main(int argc, char * argv[]) } } printf("Reduced a total of %d nodes out of %ld nodes\n", totalNodesReduced, ids.size()); + if(totalNodesReduced==0 && totalLinksRemoved==0 && !removeOrphanNodes) + { + printf("Nothing to do, exiting without updating the database.\n"); + memory.close(false); + return 0; + } + memory.emptyTrash(); + memory.joinTrashThread(); + + if(isWholeGraphConnected || removeAllOrphanNodes) + { + // refetch the global graph after graph reduction + ids = memory.getAllSignatureIds(); + + // Check if some nodes got disconnected from the global graph + size_t totalBefore = poses.size(); + poses.clear(); + constraints.clear(); + memory.getMetricConstraints(ids, poses, constraints, false, true); + std::map posesOut; + std::multimap linksOut; + optimizer->getConnectedGraph( + keepLatest?poses.rbegin()->first:poses.lower_bound(0)->first, + poses, + constraints, + posesOut, + linksOut); + + isWholeGraphConnected = posesOut.size() == poses.size(); + + if(!isWholeGraphConnected) + { + if(removeOrphanNodes) + { + printf("Option %s is enabled, let's cleanup WM and LTM " + "%ld from nodes not in the global graph anymore (%ld -> reduced to %ld -> %ld connected globally).\n", + removeAllOrphanNodes?"--remove_all_orphan_nodes":"--remove_orphan_nodes", + poses.size()-posesOut.size(), + totalBefore, ids.size(), posesOut.size()); + // Not all graph is connected anymore while it was before reduction: + // that means we created orphan nodes, remove them + int transferred = 0; + for(std::map::iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter) + { + if(posesOut.find(iter->first) == posesOut.end()) + { + memory.deleteLocation(iter->first, 0, keepLinked); + if(keepLinked) { + printf("Transferred %d to LTM (--keep_linked).\n", iter->first); + } + else { + printf("Removed %d from WM/LTM.\n", iter->first); + } + wm.erase(iter->first); + ++transferred; + } + } + if(transferred>0) { + memory.emptyTrash(); + memory.joinTrashThread(); + } + } + else { + printf("[Warning] After graph reduction, the global graph is not all " + "connected anymore while it was before (%ld -> reduced to %ld -> %ld connected globally). " + "Add %s option to remove the %ld orphan nodes from WM and LTM.\n", + totalBefore, ids.size(), posesOut.size(), + removeAllOrphanNodes?"--remove_all_orphan_nodes":"--remove_orphan_nodes", + poses.size()-posesOut.size()); + } + } + else + { + printf("Whole optimized graph is still all connected after graph reduction (%ld/%ld)\n", posesOut.size(), ids.size()); + } + } // else: we cannot detect orphan nodes if the original graph was not all connected. if(!optimizedPoses.empty()) { @@ -251,9 +407,13 @@ int main(int argc, char * argv[]) ++iter; } } - printf("Updated optimized graph from %ld poses to %ld poses\n", optimizedPoses.size()+removed, optimizedPoses.size()); + printf("Updated local optimized graph from %ld poses to %ld poses\n", optimizedPoses.size()+removed, optimizedPoses.size()); + printf("Saving back %ld optimized poses to database.\n", optimizedPoses.size()); memory.saveOptimizedPoses(optimizedPoses, lastLocalizationPose); } + + float xMin, yMin, cellSize; + bool hasOptimizedMap = !memory.load2DMap(xMin, yMin, cellSize).empty(); if(hasOptimizedMap) { printf("The database has a global occupancy grid, regenerating one with the remaining nodes of the optimized graph!\n"); @@ -280,6 +440,37 @@ int main(int argc, char * argv[]) // Restore original parameters before saving back the database memory.parseParameters(originalParameters); + + // Restore Working Memory (Mem/InitWMWithAllNodes is used above): + // When memory is closing, it updates the Info table with current time, + // then move to trash the nodes afterwards so that nodes's update time + // in the database is greater than last info entry. This is how rtabmap + // knows which nodes are in working memory. The idea here is the move to + // trash nodes that were not in original WM before closing Memory. We can + // use deleteLocation with keepLinkedInDb=true to achieve what Memory::clear() does. + ids = memory.getAllSignatureIds(); // LTM ids + // Count number of nodes in WM that were reduced (directly/indirectly) + int wmReduced = 0; + for(auto id:wm) + { + if(ids.find(id) == ids.end()) { + ++wmReduced; + } + } + printf("Restoring Working Memory (org:%ld -> reduced:%ld)...\n", wmOrgSize, wm.size() - wmReduced); + int transferred = 0; + for(auto id:ids) + { + if(wm.find(id) == wm.end() && memory.getWorkingMem().find(id) != memory.getWorkingMem().end()) { + memory.deleteLocation(id, 0, /*keepLinkedInDb*/ true); + ++transferred; + } + } + if(transferred>0) { + memory.emptyTrash(); + memory.joinTrashThread(); + } + printf("Restoring Working Memory... done! Transferred %d nodes.\n", transferred); printf("Saving all changes to database...\n"); memory.close(true);