From 25b687c36951503eed58f74e8755953b24c3259f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 15 Feb 2026 16:19:53 -0800 Subject: [PATCH] New rtabmap-reduceGraph CLI tool --- corelib/include/rtabmap/core/Memory.h | 1 + corelib/src/DBDriverSqlite3.cpp | 15 +- corelib/src/Memory.cpp | 211 ++++++++++++++++---------- corelib/src/Optimizer.cpp | 6 +- tools/CMakeLists.txt | 1 + tools/ReduceGraph/CMakeLists.txt | 13 ++ tools/ReduceGraph/main.cpp | 190 +++++++++++++++++++++++ 7 files changed, 347 insertions(+), 90 deletions(-) create mode 100644 tools/ReduceGraph/CMakeLists.txt create mode 100644 tools/ReduceGraph/main.cpp diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 5de7c741bc..9d0f6fce29 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -144,6 +144,7 @@ class RTABMAP_CORE_EXPORT Memory void saveLocationData(int locationId); void removeLink(int idA, int idB); void removeRawData(int id, bool image = true, bool scan = true, bool userData = true); + int reduceNode(int id, float maxProximityDistance = 0.0f, bool keepLinkedInDb = false); //getters const std::map & getWorkingMem() const {return _workingMem;} diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index d2118f9bdc..66986051eb 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -4419,14 +4419,7 @@ void DBDriverSqlite3::updateQuery(const std::list & nodes, bool upd ULOGGER_DEBUG("Update Node table, Time=%fs", timer.ticks()); // Update links part1 - if(uStrNumCmp(_version, "0.18.3") >= 0) - { - query = uFormat("DELETE FROM Link WHERE from_id=? and type!=%d;", (int)Link::kLandmark); - } - else - { - query = uFormat("DELETE FROM Link WHERE from_id=?;"); - } + query = uFormat("DELETE FROM Link WHERE from_id=?;"); rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); for(std::list::const_iterator j=nodes.begin(); j!=nodes.end(); ++j) @@ -4461,6 +4454,12 @@ void DBDriverSqlite3::updateQuery(const std::list & nodes, bool upd { stepLink(ppStmt, i->second); } + // Save landmarks + const std::map & landmarks = (*j)->getLandmarks(); + for(std::map::const_iterator i=landmarks.begin(); i!=landmarks.end(); ++i) + { + stepLink(ppStmt, i->second); + } } } // Finalize (delete) the statement diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 6d5aa8ad68..5ccb815d3c 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -1185,114 +1185,162 @@ void Memory::addSignatureToWmFromLTM(Signature * signature) } } -void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo) +int Memory::reduceNode(int id, float maxProximityDistance, bool keepLinkedInDb) { - UDEBUG("Inserting node %d from STM in WM...", id); - UASSERT(_stMem.find(id) != _stMem.end()); Signature * s = this->_getSignature(id); UASSERT(s!=0); - if(_reduceGraph) + if(!s->getLabel().empty()) { - bool merge = false; - const std::multimap & links = s->getLinks(); - std::map neighbors; - for(std::multimap::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.to() != iter->second.from() && - 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(); - } - } + // We currently not remove nodes with labels + return 0; + } - } - if(iter->second.type() == Link::kNeighbor) + int reducedTo = 0; + bool merge = false; + const std::multimap & links = s->getLinks(); + std::map neighbors; + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + if(!merge || iter->second.type() == Link::kGlobalClosure) // prioritize reducing to global loop closures + { + merge = iter->second.to() != iter->second.from() && + iter->second.type() != Link::kNeighbor && + iter->second.type() != Link::kNeighborMerged && + iter->second.userDataCompressed().empty() && + iter->second.type() != Link::kUndef && + iter->second.type() != Link::kVirtualClosure && + (maxProximityDistance==0 || + (iter->second.type() == Link::kLocalSpaceClosure && + iter->second.transform().getNormSquared() < maxProximityDistance*maxProximityDistance)); // in case of far lidar proximity links + if(merge) { - neighbors.insert(*iter); + reducedTo = iter->second.to(); + UDEBUG("Reduce %d to %d", s->id(), reducedTo); } } - if(merge) + if(iter->second.type() == Link::kNeighbor) + { + neighbors.insert(*iter); + } + } + if(merge) + { + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { - if(s->getLabel().empty()) + Signature * sTo = this->_getSignature(iter->first); + if(sTo->id()!=s->id()) // Not Prior/Gravity links... { - for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str()); + sTo->removeLink(s->id()); + if(iter->second.type() != Link::kNeighbor && + iter->second.type() != Link::kNeighborMerged && + iter->second.type() != Link::kUndef) { - Signature * sTo = this->_getSignature(iter->first); - if(sTo->id()!=s->id()) // Not Prior/Gravity links... + // link to all neighbors + for(std::map::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter) { - UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str()); - sTo->removeLink(s->id()); - if(iter->second.type() != Link::kNeighbor && - iter->second.type() != Link::kNeighborMerged && - iter->second.type() != Link::kUndef) + if(!sTo->hasLink(jter->second.to())) { - // link to all neighbors - for(std::map::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter) + UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)", + iter->second.from(), iter->second.to(), iter->second.type(), + jter->second.from(), jter->second.to(), jter->second.type()); + 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_MSG(!sB->hasLink(l.from()), uFormat("%d->%d type=%d", sB->id(), l.to(), l.type()).c_str()); + sB->addLink(l.inverse()); + } + } + // link to all landmarks + for(std::map::const_iterator jter=s->getLandmarks().begin(); jter!=s->getLandmarks().end(); ++jter) + { + if(!uContains(sTo->getLandmarks(), jter->first)) + { + UDEBUG("Move landmark observation %d from %d to %d", + jter->first, s->id(), sTo->id()); + Link l = iter->second.inverse().merge( + jter->second, + jter->second.type()); + sTo->addLandmark(l); + // Update landmark index + std::map >::iterator nter = _landmarksIndex.find(jter->first); + if(nter!=_landmarksIndex.end()) { - if(!sTo->hasLink(jter->second.to())) - { - UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)", - iter->second.from(), iter->second.to(), iter->second.type(), - jter->second.from(), jter->second.to(), jter->second.type()); - 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_MSG(!sB->hasLink(l.from()), uFormat("%d->%d", sB->id(), l.to()).c_str()); - sB->addLink(l.inverse()); - } + nter->second.insert(sTo->id()); + } + else + { + std::set tmp; + tmp.insert(sTo->id()); + _landmarksIndex.insert(std::make_pair(jter->first, tmp)); } } } } + } + } - //remove neighbor links - std::multimap linksCopy = links; - for(std::multimap::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter) + //remove neighbor links + std::multimap linksCopy = links; + for(std::multimap::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter) + { + if(iter->second.type() == Link::kNeighborMerged) + { + // Removing only merged neighbor links, we keep original neighbor + // links to be able to reprocess databases with correct odometry covariance. + s->removeLink(iter->first); + } + } + + this->moveToTrash(s, keepLinkedInDb); + s = 0; + _linksChanged = true; + _memoryChanged = true; + } + return reducedTo; +} + +void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut) +{ + UDEBUG("Inserting node %d from STM in WM...", id); + UASSERT(_stMem.find(id) != _stMem.end()); + int reducedId = 0; + if(_reduceGraph) + { + Signature * s = this->_getSignature(id); + UASSERT(s!=0); + std::multimap links = s->getLinks(); + // Setting 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); + if(reducedToOut) { + *reducedToOut = reducedId; + } + if(reducedId>0) + { + for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) + { + if(iter->second.type() == Link::kNeighbor) { - if(iter->second.type() == Link::kNeighborMerged) + if(_lastGlobalLoopClosureId == s->id()) { - // Removing only merged neighbor links, we keep original neighbor - // links to be able to reprocess databases with correct odometry covariance. - s->removeLink(iter->first); - } - if(iter->second.type() == Link::kNeighbor) - { - if(_lastGlobalLoopClosureId == s->id()) - { - _lastGlobalLoopClosureId = iter->first; - } + _lastGlobalLoopClosureId = iter->first; } } - - // Setting true to make sure we save all visual - // words that could be referenced in a previously - // transferred node in LTM (#979) - this->moveToTrash(s, true); - s = 0; } } } - if(s != 0) + if(reducedId == 0) { _workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now())); _stMem.erase(*_stMem.begin()); } - // else already removed from STM/WM in moveToTrash() + // else already removed from STM/WM in reduceNode() } const Signature * Memory::getSignature(int id) const @@ -2507,9 +2555,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * // If not saved to database if(!keepLinkedToGraph) { - UASSERT_MSG(this->isInSTM(s->id()), + UASSERT_MSG(this->isInSTM(s->id()) || this->isInWM(s->id()), uFormat("Deleting location (%d) outside the " - "STM is not implemented!", s->id()).c_str()); + "WM/STM is not implemented! STM size=%ld WM size=%ld", + s->id(), this->getStMem().size(), this->getWorkingMem().size()).c_str()); const std::multimap & links = s->getLinks(); for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { @@ -2520,7 +2569,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * 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()); + "outside the WM/STM?", iter->first, s->id()).c_str()); if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id())) { @@ -2530,7 +2579,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list * } // child - if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id() && s->getWeight()>0) + if(iter->second.type() == Link::kGlobalClosure && s->getWeight()>0) { sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight } diff --git a/corelib/src/Optimizer.cpp b/corelib/src/Optimizer.cpp index 4f2197b2a3..86be75dc3e 100644 --- a/corelib/src/Optimizer.cpp +++ b/corelib/src/Optimizer.cpp @@ -217,6 +217,10 @@ LinkIdKey(int id, Link::Type type) : { return true; } + else if(k.type_ == Link::kNeighborMerged && type_ != Link::kNeighbor && type_ != Link::kNeighborMerged) + { + return false; + } else { // normal link, sort by smallest to largest id @@ -256,7 +260,7 @@ void Optimizer::getConnectedGraph( } } - while(nextPoses.size()) + while(!nextPoses.empty()) { // Fill up all nodes before landmarks // For nodes, fill up all neightbor nodes before loop closure ones diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index d50724b43b..a8d9c4ada0 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -15,6 +15,7 @@ ADD_SUBDIRECTORY( Report ) ADD_SUBDIRECTORY( Info ) ADD_SUBDIRECTORY( CleanupLocalGrids ) ADD_SUBDIRECTORY( GlobalBundleAdjustment ) +ADD_SUBDIRECTORY( ReduceGraph ) IF(OPENCV_NONFREE_FOUND) ADD_SUBDIRECTORY( VocabularyComparison ) diff --git a/tools/ReduceGraph/CMakeLists.txt b/tools/ReduceGraph/CMakeLists.txt new file mode 100644 index 0000000000..dfa61deb51 --- /dev/null +++ b/tools/ReduceGraph/CMakeLists.txt @@ -0,0 +1,13 @@ + +ADD_EXECUTABLE(reduceGraph main.cpp) + +TARGET_LINK_LIBRARIES(reduceGraph rtabmap_core) + +SET_TARGET_PROPERTIES( reduceGraph + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-reduceGraph) + +INSTALL(TARGETS reduceGraph + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime + BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime) + + diff --git a/tools/ReduceGraph/main.cpp b/tools/ReduceGraph/main.cpp new file mode 100644 index 0000000000..43f0d7f25a --- /dev/null +++ b/tools/ReduceGraph/main.cpp @@ -0,0 +1,190 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace rtabmap; + +void showUsage(const char * exec) +{ + printf("\nUsage:\n" + "%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" + "\n%s", exec, Parameters::showUsage()); + exit(1); +} + +int main(int argc, char * argv[]) +{ + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); + + if(argc < 2) + { + showUsage(argv[0]); + } + + bool keepLatest = false; + bool keepLinked = false; + for(int i=1; iopenConnection(dbPath)) + { + parameters = driver->getLastParameters(); + driver->closeConnection(false); + } + else + { + UERROR("Cannot open database %s!", dbPath.c_str()); + } + delete driver; + + for(ParametersMap::iterator iter=inputParams.begin(); iter!=inputParams.end(); ++iter) + { + printf(" Using parameter \"%s=%s\" from arguments\n", iter->first.c_str(), iter->second.c_str()); + } + + Memory memory; + printf("Initialization...\n"); + UTimer timer; + uInsert(parameters, inputParams); + if(!memory.init(dbPath, false, parameters)) + { + printf("Initialization... failed! Aborting!\n"); + return 1; + } + std::set ids = memory.getAllSignatureIds(); + printf("Initialization... done! %ld nodes loaded. (%f sec)\n", ids.size(), timer.ticks()); + + if(ids.empty()) + { + printf("IDs are empty?! Aborting.\n"); + return 1; + } + + Transform lastLocalizationPose; + bool hasOptimizedPoses = !memory.loadOptimizedPoses(&lastLocalizationPose).empty(); + + float proximityMaxRadius = Parameters::defaultRGBDProximityPathFilteringRadius(); + Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityMaxRadius); + int totalNodesReduced = 0; + if(keepLatest) + { + // we process older to newer nodes, merging to new nodes + for(std::set::iterator iter = ids.begin(); iter!=ids.end(); ++iter) + { + int id = *iter; + int reducedId = memory.reduceNode(id, proximityMaxRadius, keepLinked); + if(reducedId > 0) + { + printf("Reduced node %d to node %d!\n", id, reducedId); + ++totalNodesReduced; + } + } + } + else + { + // we process newer to older nodes, merging to old nodes + for(std::set::reverse_iterator iter = ids.rbegin(); iter!=ids.rend(); ++iter) + { + int id = *iter; + int reducedId = memory.reduceNode(id, proximityMaxRadius, keepLinked); + if(reducedId > 0) + { + printf("Reduced node %d to node %d!\n", id, reducedId); + ++totalNodesReduced; + } + } + } + printf("Reduced a total of %d nodes out of %ld nodes\n", totalNodesReduced, ids.size()); + + // Clear the optimized poses, this will force rtabmap to re-optimize the graph on initialization + if(hasOptimizedPoses) + { + printf("Note that there were optimized poses/map saved in the database, " + "clearing them so that they are regenerated next time rtabmap loads that map.\n"); + memory.saveOptimizedPoses(std::map(), lastLocalizationPose); + } + // This will force rtabmap_ros to regenerate the global occupancy grid if there was one + memory.save2DMap(cv::Mat(), 0, 0, 0); + memory.saveOptimizedMesh(cv::Mat()); + + memory.close(true); + + return 0; +}