New rtabmap-reduceGraph CLI tool (#1655)

* New rtabmap-reduceGraph CLI tool

* fixed some edge cases

* Regenerating optimized map if there was one before reducing the graph

* addMoreLoopClosures: refactored how ctrl-c is handled to stop faster when no loop closures are added

* Added kilted status

* Make offline tool always propagate neighbor merged links

* removed a parameter

* fixed disconnected graph

* fixed --help

* Added error log on Kp/NNStrategy not compatible with huge vocabulary. ReduceGraph/DetectMoreLoopClosures: Make sure original parameters are saved back on closing. g2o: fixing optimizer to Levenberg for SBA to avoid [SetJac] infinite jac fatal error.

* exposing neighbor merged ratio parameter to the tool

* show param in log

* refactored detectMoreLoopClosures to ignore too close nodes in terms of neighbor links based on Mem/STMSize parameter. Reduce graph: added direction parameter.

* Simplified: removed ratio parameter, removed recursive reduction. Just don't reduce if a NM link is longer than maxDistance.

* Removed NNStrategy override, as it was still done on closing when we changed back to original params

* DBViewer: show missing links when showing OptimizedPoses in GraphView, fixed clicking on landmark links

* DetectMoreLoopClosures: Added support for min graph distance option in MainWindow and DbViewer

* slight renaming of ROS jobs

* reprocess: added option --params_last

tools/CMakeLists.txt

@@ -16,6 +16,7 @@ ADD_SUBDIRECTORY( Report )
 ADD_SUBDIRECTORY( Info )
 ADD_SUBDIRECTORY( CleanupLocalGrids )
 ADD_SUBDIRECTORY( GlobalBundleAdjustment )
+ADD_SUBDIRECTORY( ReduceGraph )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/DetectMoreLoopClosures/main.cpp

@@ -63,14 +63,7 @@ void showUsage()
 	exit(1);
 }
 
-// catch ctrl-c
 bool g_loopForever = true;
-void sighandler(int sig)
-{
-	printf("\nSignal %d caught...\n", sig);
-	g_loopForever = false;
-}
-
 class PrintProgressState : public ProgressState
 {
 public:
@@ -86,6 +79,15 @@ public:
 private:
 	double stamp_;
 };
+PrintProgressState progress;
+
+// catch ctrl-c
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_loopForever = false;
+	progress.setCanceled(true);
+}
 
 int main(int argc, char * argv[])
 {
@@ -94,7 +96,7 @@ int main(int argc, char * argv[])
 	signal(SIGINT, &sighandler);
 
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kError);
+	ULogger::setLevel(ULogger::kWarning);
 
 	if(argc < 2)
 	{
@@ -195,7 +197,7 @@ int main(int argc, char * argv[])
 
 	// Add some optimizations (soft set, can be overriden by arguments)
 	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
-	inputParams.insert(ParametersPair(Parameters::kKpNNStrategy(), "3")); // don't need flann index
+	inputParams.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // should be incremental to update links
 
 	std::string dbPath = argv[argc-1];
 	if(!UFile::exists(dbPath))
@@ -245,6 +247,7 @@ int main(int argc, char * argv[])
 	Rtabmap rtabmap;
 	printf("Initialization...\n");
 	UTimer timer;
+	ParametersMap originalParameters = parameters;
 	uInsert(parameters, inputParams);
 	rtabmap.init(parameters, dbPath);
 	printf("Initialization... done! (%f sec)\n", timer.ticks());
@@ -267,7 +270,6 @@ int main(int argc, char * argv[])
 		printf("From/To Session ID = %d%s\n", fromToMapId, last?" (last session)":"");
 	}
 
-	PrintProgressState progress;
 	printf("Detecting...\n");
 	int detected = rtabmap.detectMoreLoopClosures(clusterRadiusMax, clusterAngle, iterations, intraSession, interSession, &progress, clusterRadiusMin, fromToMapId);
 	if(detected < 0)
@@ -306,6 +308,9 @@ int main(int argc, char * argv[])
 		}
 	}
 
+	// Restore original parameters before saving back the database
+	rtabmap.parseParameters(originalParameters);
+
 	rtabmap.close();
 
 	return 0;

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)
+
+

tools/ReduceGraph/main.cpp

@@ -0,0 +1,281 @@
+/*
+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 <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/global_map/OccupancyGrid.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UStl.h>
+#include <pcl/filters/filter.h>
+#include <pcl/io/ply_io.h>
+#include <pcl/io/obj_io.h>
+#include <pcl/common/common.h>
+#include <pcl/surface/poisson.h>
+#include <stdio.h>
+#include <signal.h>
+
+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"
+			"    --pre_cleanup  Remove all user loop closures linking nodes closer than %s 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"
+			"    --udebug/--uinfo/--warn can also be used to change verbosity.\n"
+			"\n", exec, Parameters::kMemSTMSize().c_str());
+	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;
+	float radius = 1.0f;
+	bool preCleanup = false;
+	for(int i=1; i<argc; ++i)
+	{
+		if(std::strcmp(argv[i], "--help") == 0)
+		{
+			showUsage(argv[0]);
+		}
+		else if(std::strcmp(argv[i], "--keep_latest") == 0)
+		{
+			keepLatest = true;
+		}
+		else if(std::strcmp(argv[i], "--keep_linked") == 0)
+		{
+			keepLinked = true;
+		}
+		else if(std::strcmp(argv[i], "--pre_cleanup") == 0)
+		{
+			preCleanup = true;
+		}
+		else if(std::strcmp(argv[i], "--radius") == 0)
+		{
+			++i;
+			if(i < argc-1)
+			{
+				radius = uStr2Float(argv[i]);
+				if(radius <= 0.0f)
+				{
+					printf("--radius should be > 0, parsed %f\n", radius);
+					showUsage(argv[0]);
+				}
+			}
+			else {
+				showUsage(argv[0]);
+			}
+		}
+	}
+	printf("Parameters:\n");
+	printf("  radius = %f m\n", radius);
+	printf("  keep_latest = %s\n", keepLatest?"true":"false");
+	printf("  keep_linked = %s\n", keepLinked?"true":"false");
+	printf("  pre_cleanup = %s\n", preCleanup?"true":"false");
+	
+	// Just parse logging options
+	Parameters::parseArguments(argc, argv);
+
+	// Add some optimizations
+	ParametersMap inputParams;
+	inputParams.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true")); // load the whole map in RAM
+	inputParams.insert(ParametersPair(Parameters::kMemLoadVisualLocalFeaturesOnInit(), "false")); // don't need features already loaded in RAM
+
+	std::string dbPath = argv[argc-1];
+	if(!UFile::exists(dbPath))
+	{
+		printf("Database %s doesn't exist!\n", dbPath.c_str());
+		return 1;
+	}
+
+	printf("Database: %s\n", dbPath.c_str());
+
+	// Get parameters
+	ParametersMap parameters;
+	DBDriver * driver = DBDriver::create();
+	if(driver->openConnection(dbPath))
+	{
+		parameters = driver->getLastParameters();
+		driver->closeConnection(false);
+	}
+	else
+	{
+		UERROR("Cannot open database %s!", dbPath.c_str());
+	}
+	delete driver;
+
+	Memory memory;
+	printf("Initialization...\n");
+	UTimer timer;
+	ParametersMap originalParameters = parameters;
+	uInsert(parameters, inputParams);
+	if(!memory.init(dbPath, false, parameters))
+	{
+		printf("Initialization... failed! Aborting!\n");
+		return 1;
+	}
+	std::set<int> 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;
+	std::map<int, Transform> optimizedPoses = memory.loadOptimizedPoses(&lastLocalizationPose);
+	float xMin, yMin, cellSize;
+	bool hasOptimizedMap = !memory.load2DMap(xMin, yMin, cellSize).empty();
+
+	int totalNodesReduced = 0;
+	std::vector<int> vids;
+	vids.reserve(ids.size());
+	if(keepLatest)
+	{
+		// we process older to newer nodes, merging to new nodes
+		vids.insert(vids.end(), ids.begin(), ids.end());
+	}
+	else
+	{
+		// we process newer to older nodes, merging to old nodes
+		vids.insert(vids.end(), ids.rbegin(), ids.rend());
+	}
+
+	if(preCleanup)
+	{
+		if(memory.getMaxStMemSize() <= 1)
+		{
+			printf("--pre_cleanup is used but %s <= 1, skipping pre cleanup...\n", Parameters::kMemSTMSize().c_str());
+		}
+		else
+		{
+			int totalRemoved = 0;
+			for(auto id: vids)
+			{
+				auto nids = memory.getNeighborsId(id, memory.getMaxStMemSize(), -1, true, true, true);
+				auto links = memory.getLinks(id, true, false);
+				for(auto link:links)
+				{
+					if( link.second.type() == Link::kUserClosure &&
+						nids.find(link.first)!=nids.end())
+					{
+						memory.removeLink(id, link.first);
+						++totalRemoved;
+					}
+				}
+			}
+			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());
+		}
+	}
+
+	for(auto id: vids)
+	{
+		// Nodes can be already reduced by other nodes, check if they are still there
+		if(memory.getSignature(id) != 0)
+		{
+			int reducedId = memory.reduceNode(id, radius, keepLinked, keepLatest?1:-1);
+			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());
+
+	if(!optimizedPoses.empty())
+	{
+		size_t removed = 0;
+		// cleanup reduced nodes from the optimized poses
+		for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(0); iter!=optimizedPoses.end();)
+		{
+			if(memory.getSignature(iter->first) == 0)
+			{
+				iter = optimizedPoses.erase(iter);
+				++removed;
+			}
+			else
+			{
+				++iter;
+			}
+		}
+		printf("Updated optimized graph from %ld poses to %ld poses\n", optimizedPoses.size()+removed, optimizedPoses.size());
+		memory.saveOptimizedPoses(optimizedPoses, lastLocalizationPose);
+	}
+	if(hasOptimizedMap)
+	{
+		printf("The database has a global occupancy grid, regenerating one with the remaining nodes of the optimized graph!\n");
+		LocalGridCache cache;
+		OccupancyGrid grid(&cache, parameters);
+		for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(0); iter!=optimizedPoses.end(); ++iter)
+		{
+			SensorData data = memory.getNodeData(iter->first, false, false, false, true);
+			data.uncompressData();
+			cache.add(iter->first, data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint());
+		}
+		grid.update(optimizedPoses);
+		cv::Mat map = grid.getMap(xMin, yMin);
+		if(map.empty())
+		{
+			printf("Could not regenerate the global occupancy grid! The grid is not updated.\n");
+		}
+		else
+		{
+			memory.save2DMap(map, xMin, yMin, grid.getCellSize());
+			printf("Saved the new global occupancy grid!\n");
+		}
+	}
+
+	// Restore original parameters before saving back the database
+	memory.parseParameters(originalParameters);
+	
+	printf("Saving all changes to database...\n");
+	memory.close(true);
+
+	return 0;
+}

tools/Reprocess/main.cpp

@@ -57,7 +57,7 @@ void showUsage()
 			"   rtabmap-reprocess [options] \"input.db\" \"output.db\"\n"
 			"   rtabmap-reprocess [options] \"input1.db;input2.db;input3.db\" \"output.db\"\n"
 			"\n"
-			"   For the second example, only parameters from the first database are used.\n"
+			"   For the second example, only parameters from the first database are used (unless -params_last or -default are used).\n"
 			"   If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database,\n"
 			"   then localization-only is done with next databases against the first one.\n"
 			"   To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n"
@@ -71,6 +71,7 @@ void showUsage()
 			"                       from the database. If custom parameters are also set as \n"
 			"                       arguments, they overwrite those in config file and the database.\n"
 			"     -default    Input database's parameters are ignored, using default ones instead.\n"
+			"     -params_last  Parameters of the last database is used instead of the first one (ignored if -default is also used).\n"
 			"     -odom       Recompute odometry. See \"Odom/\" parameters with --params. If -skip option\n"
 			"                 is used, it will be applied to odometry frames, not rtabmap frames. Multi-session\n"
 			"                 may not be detected correctly if the input covariance between sessions doesn't have 9999.\n"
@@ -260,6 +261,7 @@ int main(int argc, char * argv[])
 	bool assemble3dOctoMap = false;
 	bool useDatabaseRate = false;
 	bool useDefaultParameters = false;
+	bool useLastDatabaseParameters = false;
 	bool recomputeOdometry = false;
 	bool useInputOdometryAsGuess = false;
 	double odomLinVarOverride = 0.0;
@@ -318,6 +320,10 @@ int main(int argc, char * argv[])
 			useDefaultParameters = true;
 			printf("Using default parameters.\n");
 		}
+		else if(strcmp(argv[i], "-params_last") == 0 || strcmp(argv[i], "--params_last") == 0)
+		{
+			useLastDatabaseParameters = true;
+		}
 		else if(strcmp(argv[i], "-odom") == 0 || strcmp(argv[i], "--odom") == 0)
 		{
 			recomputeOdometry = true;
@@ -683,11 +689,10 @@ int main(int argc, char * argv[])
 	}
 
 	// Get parameters of the first database
-	DBDriver * dbDriver = DBDriver::create();
+	std::shared_ptr<DBDriver> dbDriver(DBDriver::create());
 	if(!dbDriver->openConnection(databases.front(), false))
 	{
-		printf("Failed opening input database!\n");
-		delete dbDriver;
+		printf("Failed opening the input database!\n");
 		return 1;
 	}
 
@@ -695,13 +700,28 @@ int main(int argc, char * argv[])
 	std::string targetVersion;
 	if(!useDefaultParameters)
 	{
-		parameters = dbDriver->getLastParameters();
-		targetVersion = dbDriver->getDatabaseVersion();
-		parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
+		if(databases.size() > 1 && useLastDatabaseParameters)
+		{
+			printf("Using last database's parameters.\n");
+			std::shared_ptr<DBDriver> lastDbDriver(DBDriver::create());
+			if(!lastDbDriver->openConnection(databases.back(), true))
+			{
+				printf("Failed opening the last input database!\n");
+				return 1;
+			}
+			parameters = lastDbDriver->getLastParameters();
+			targetVersion = lastDbDriver->getDatabaseVersion();
+		}
+		else
+		{
+			parameters = dbDriver->getLastParameters();
+			targetVersion = dbDriver->getDatabaseVersion();
+		}
 		if(parameters.empty())
 		{
-			printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", dbDriver->getDatabaseVersion().c_str());
+			printf("WARNING: Failed getting parameters from database, reprocessing will be done with default parameters! Database version may be too old (%s).\n", targetVersion.c_str());
 		}
+		parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), targetVersion));
 	}
 
 	if(customParameters.size())
@@ -800,7 +820,6 @@ int main(int argc, char * argv[])
 	{
 		printf("Input database doesn't have any nodes saved in it.\n");
 		dbDriver->closeConnection(false);
-		delete dbDriver;
 		return 1;
 	}
 	if(!((!incrementalMemory || appendMode) && databases.size() > 1))
@@ -822,7 +841,6 @@ int main(int argc, char * argv[])
 		if (!dbDriver->openConnection(*iter, false))
 		{
 			printf("Failed opening input database!\n");
-			delete dbDriver;
 			return 1;
 		}
 		ids.clear();
@@ -830,8 +848,7 @@ int main(int argc, char * argv[])
 		totalIds += ids.size();
 		dbDriver->closeConnection(false);
 	}
-	delete dbDriver;
-	dbDriver = 0;
+	dbDriver.reset();
 
 	std::string workingDirectory = UDirectory::getDir(outputDatabasePath);
 	printf("Set working directory to \"%s\".\n", workingDirectory.c_str());
@@ -1386,13 +1403,12 @@ int main(int argc, char * argv[])
 		{
 			if(save2DMap)
 			{
-				DBDriver * driver = DBDriver::create();
+				std::shared_ptr<DBDriver> driver(DBDriver::create());
 				if(driver->openConnection(outputDatabasePath))
 				{
 					driver->save2DMap(map, xMin, yMin, grid.getCellSize());
 					printf("Saving occupancy grid to database... done!\n");
 				}
-				delete driver;
 			}
 			else
 			{
@@ -1481,13 +1497,12 @@ int main(int argc, char * argv[])
 		{
 			if(save2DMap)
 			{
-				DBDriver * driver = DBDriver::create();
+				std::shared_ptr<DBDriver> driver(DBDriver::create());
 				if(driver->openConnection(outputDatabasePath))
 				{
 					driver->save2DMap(map, xMin, yMin, cellSize);
 					printf("Saving occupancy grid to database... done!\n");
 				}
-				delete driver;
 			}
 			else
 			{