Rtabmap::detectMoreLoopClosures: added clusterRadiusMin parameter and update optimized poses after each accepted loop closure (also in MainWindow) like in DbViewer. Added graph::computeMinMax(poses). OdometryInfo: added guess transform. Export: added min/max axis ranges to filter nodes before expoting clouds.

tools/DetectMoreLoopClosures/main.cpp

@@ -51,6 +51,7 @@ void showUsage()
 			"rtabmap-detectMoreLoopClosures [options] database.db\n"
 			"Options:\n"
 			"    -r #          Cluster radius (default 1 m).\n"
+			"    -rx #         Cluster radius min (default 0 m).\n"
 			"    -a #          Cluster angle (default 30 deg).\n"
 			"    -i #          Iterations (default 1).\n"
 			"    --intra       Add only intra-session loop closures.\n"
@@ -92,7 +93,8 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
-	float clusterRadius = 1.0f;
+	float clusterRadiusMin = 0.0f;
+	float clusterRadiusMax = 1.0f;
 	float clusterAngle = CV_PI/6.0f;
 	int iterations = 1;
 	bool intraSession = false;
@@ -124,7 +126,19 @@ int main(int argc, char * argv[])
 			++i;
 			if(i<argc-1)
 			{
-				clusterRadius = uStr2Float(argv[i]);
+				clusterRadiusMax = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "-rx") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				clusterRadiusMin = uStr2Float(argv[i]);
 			}
 			else
 			{
@@ -165,7 +179,8 @@ int main(int argc, char * argv[])
 	}
 
 	printf("\nDatabase: %s\n", dbPath.c_str());
-	printf("Cluster radius = %f m\n", clusterRadius);
+	printf("Cluster radius min = %f m\n", clusterRadiusMin);
+	printf("Cluster radius max = %f m\n", clusterRadiusMax);
 	printf("Cluster angle = %f deg\n", clusterAngle*180.0f/CV_PI);
 	if(intraSession)
 	{
@@ -204,7 +219,7 @@ int main(int argc, char * argv[])
 
 	PrintProgressState progress;
 	printf("Detecting...\n");
-	int detected = rtabmap.detectMoreLoopClosures(clusterRadius, clusterAngle, iterations, intraSession, interSession, &progress);
+	int detected = rtabmap.detectMoreLoopClosures(clusterRadiusMax, clusterAngle, iterations, intraSession, interSession, &progress, clusterRadiusMin);
 	if(detected < 0)
 	{
 		if(!g_loopForever)

tools/Export/main.cpp

@@ -83,6 +83,12 @@ void showUsage()
 			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
 			"    --scan                Use laser scan for the point cloud.\n"
 			"    --save_in_db          Save resulting assembled point cloud or mesh in the database.\n"
+			"    --xmin #              Minimum range on X axis to keep nodes to export.\n"
+			"    --xmax #              Maximum range on X axis to keep nodes to export.\n"
+			"    --ymin #              Minimum range on Y axis to keep nodes to export.\n"
+			"    --ymax #              Maximum range on Y axis to keep nodes to export.\n"
+			"    --zmin #              Minimum range on Z axis to keep nodes to export.\n"
+			"    --zmax #              Maximum range on Z axis to keep nodes to export.\n"
 			"\n%s", Parameters::showUsage());
 	;
 	exit(1);
@@ -125,6 +131,7 @@ int main(int argc, char * argv[])
 	bool camProjection = false;
 	bool exportPoses = false;
 	bool exportImages = false;
+	cv::Vec3f min, max;
 	for(int i=1; i<argc; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
@@ -346,6 +353,78 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--xmin") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				min[0] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--xmax") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				max[0] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--ymin") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				min[1] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--ymax") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				max[1] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--zmin") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				min[2] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--zmax") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				max[2] = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 	}
 
 	if(decimation < 1)
@@ -432,6 +511,43 @@ int main(int argc, char * argv[])
 		return -1;
 	}
 
+	if(min[0] != max[0] || min[1] != max[1] || min[2] != max[2])
+	{
+		cv::Vec3f minP,maxP;
+		graph::computeMinMax(optimizedPoses, minP, maxP);
+		printf("Filtering poses (range: x=%f<->%f, y=%f<->%f, z=%f<->%f, map size=%f x %f x %f)...\n",
+				min[0],max[0],min[1],max[1],min[2],max[2],
+				maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]);
+		std::map<int, Transform> posesFiltered;
+		for(std::map<int, Transform>::const_iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+		{
+			bool ignore = false;
+			if(min[0] != max[0] && (iter->second.x() < min[0] || iter->second.x() > max[0]))
+			{
+				ignore = true;
+			}
+			if(min[1] != max[1] && (iter->second.y() < min[1] || iter->second.y() > max[1]))
+			{
+				ignore = true;
+			}
+			if(min[2] != max[2] && (iter->second.z() < min[2] || iter->second.z() > max[2]))
+			{
+				ignore = true;
+			}
+			if(!ignore)
+			{
+				posesFiltered.insert(*iter);
+			}
+		}
+		graph::computeMinMax(posesFiltered, minP, maxP);
+		printf("Filtering poses... done! %d/%d remaining (new map size=%f x %f x %f).\n", (int)posesFiltered.size(), (int)optimizedPoses.size(), maxP[0]-minP[0],maxP[1]-minP[1],maxP[2]-minP[2]);
+		optimizedPoses = posesFiltered;
+		if(optimizedPoses.empty())
+		{
+			return -1;
+		}
+	}
+
 	std::string outputDirectory = UDirectory::getDir(dbPath);
 	std::string baseName = uSplit(UFile::getName(dbPath), '.').front();
 

tools/Info/main.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/DBDriver.h>
 #include <rtabmap/core/VisualWord.h>
+#include <rtabmap/core/Graph.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UStl.h"
@@ -231,6 +232,11 @@ int main(int argc, char * argv[])
 		driver->getAllNodeIds(ids);
 		Transform lastLocalization;
 		std::map<int, Transform> optimizedPoses = driver->loadOptimizedPoses(&lastLocalization);
+		cv::Vec3f min, max;
+		if(!optimizedPoses.empty())
+		{
+			graph::computeMinMax(optimizedPoses, min, max);
+		}
 		std::multimap<int, int> mapIdsLinkedToLastGraph;
 		int lastMapId=0;
 		double previousStamp = 0.0f;
@@ -353,7 +359,7 @@ int main(int argc, char * argv[])
 		std::cout << (uFormat("%s%d nodes and %d words (dim=%d type=%s)\n", pad("LTM:").c_str(), (int)ids.size(), driver->getTotalDictionarySize(), wordsDim, wordsType==CV_8UC1?"8U":wordsType==CV_32FC1?"32F":uNumber2Str(wordsType).c_str()));
 		std::cout << (uFormat("%s%d nodes and %d words\n", pad("WM:").c_str(), driver->getLastNodesSize(), driver->getLastDictionarySize()));
 		std::cout << (uFormat("%s%d poses and %d links\n", pad("Global graph:").c_str(), odomPoses, links.size()));
-		std::cout << (uFormat("%s%d poses\n", pad("Optimized graph:").c_str(), (int)optimizedPoses.size(), links.size()));
+		std::cout << (uFormat("%s%d poses (x=%d->%d, y=%d->%d, z=%d->%d)\n", pad("Optimized graph:").c_str(), (int)optimizedPoses.size(), links.size(), (int)min[0], (int)max[0], (int)min[1], (int)max[1], min[2], (int)max[2]));
 		std::cout << (uFormat("%s%d/%d [%s]\n", pad("Maps in graph:").c_str(), (int)mapsLinkedToLastGraph.size(), sessions, sessionsInOptGraphStr.str().c_str()));
 		std::cout << (uFormat("%s%d poses\n", pad("Ground truth:").c_str(), gtPoses));
 		std::cout << (uFormat("%s%d poses\n", pad("GPS:").c_str(), gpsValues));