GridMap: fixing eigen error with for downstream consumers

* Parallelize cloud generation in export/view clouds dialog and rtabmap-export

* Simplified NodeExportData by holding Signature directly. Added --threads option (default max cores) for CLI and UI.

* Parallelized texturing phase the same way than cloud generation

* Fixed not cancelable (right away) cloud generation in UI

---------

Co-authored-by: matlabbe <matlabbe@gmail.com>

corelib/include/rtabmap/core/Graph.h

@@ -490,6 +490,29 @@ std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
 			int to,
 			bool updateNewCosts = false);
 
+/**
+ * @brief Single-source graph depth via BFS.
+ *
+ * Runs one breadth-first search from @p from and returns, for every reached
+ * node, its depth from @p from (i.e., the number of links on the shortest
+ * path, @p from itself having depth 0).
+ *
+ * @note The depth is a number of hops, not a distance: it counts links, and the poses
+ *       of the nodes play no part in it. The path it stands for is thus not the one
+ *       @ref computePath() returns, which minimizes the Euclidean length instead and
+ *       can walk more links to save meters.
+ *
+ * @param links Directed edges (`from` → `to`) keyed by source id.
+ * @param from Start node id.
+ * @param maxDepth If &gt; 0, only nodes with depth ≤ this value are returned (the
+ *                 frontier is not expanded further); `0` explores the whole component.
+ * @return Node id → depth mapping.
+ */
+std::map<int, int> RTABMAP_CORE_EXPORT computePathDepths(
+			const std::multimap<int, int> & links,
+			int from,
+			int maxDepth = 0);
+
 /**
  * @brief Dijkstra shortest path on link constraints.
  *

corelib/include/rtabmap/core/util3d_surface.h

@@ -150,7 +150,8 @@ pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh(
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0, // For each point, we have a list of cameras with corresponding pixel in it. Beware that the camera ids don't correspond to pose ids, they are indexes from 0 to total camera models and texture's materials.
-		bool distanceToCamPolicy = false);
+		bool distanceToCamPolicy = false,
+		int numThreads = 1); // number of threads used to compute the visible faces of the cameras (1=sequential)
 pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh(
 		const pcl::PolygonMesh::Ptr & mesh,
 		const std::map<int, Transform> & poses,
@@ -163,7 +164,8 @@ pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh(
 		const std::vector<float> & roiRatios = std::vector<float>(), // [left, right, top, bottom] region of interest (in ratios) of the image projected.
 		const ProgressState * state = 0,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0, // For each point, we have a list of cameras with corresponding pixel in it. Beware that the camera ids don't correspond to pose ids, they are indexes from 0 to total camera models and texture's materials.
-		bool distanceToCamPolicy = false);
+		bool distanceToCamPolicy = false,
+		int numThreads = 1); // number of threads used to compute the visible faces of the cameras (1=sequential)
 
 /**
  * Remove not textured polygon clusters. If minClusterSize<0, only the largest cluster is kept.

corelib/src/CMakeLists.txt

@@ -689,16 +689,42 @@ IF(grid_map_core_FOUND)
             ${LIBRARIES}
             grid_map_core::grid_map_core
         )
+        # ${grid_map_core_INCLUDE_DIRS} is only ${EIGEN3_INCLUDE_DIR} on an
+        # ament install; the path to grid_map's own headers lives solely on
+        # the imported target.
+        GET_TARGET_PROPERTY(grid_map_core_PUBLIC_INCLUDE_DIRS
+                            grid_map_core::grid_map_core
+                            INTERFACE_INCLUDE_DIRECTORIES)
     ELSE()
-        SET(INCLUDE_DIRS
-            ${INCLUDE_DIRS}
-            ${grid_map_core_INCLUDE_DIRS}
-        )
+        SET(grid_map_core_PUBLIC_INCLUDE_DIRS ${grid_map_core_INCLUDE_DIRS})
         SET(LIBRARIES
             ${LIBRARIES}
             ${grid_map_core_LIBRARIES}
         )
     ENDIF()
+
+    # grid_map_core links PRIVATE (only global_map/GridMap.cpp includes it),
+    # but its Eigen plugins aren't private: FIND_PACKAGE(grid_map_core) injects
+    # -DEIGEN_FUNCTORS_PLUGIN / -DEIGEN_DENSEBASE_PLUGIN with a directory-scope
+    # ADD_DEFINITIONS, adding members to Eigen::MatrixBase and DenseBase in
+    # every translation unit. grid_map never pairs those with the include
+    # directory holding the headers they name (still true on master), so any
+    # target inheriting them without linking grid_map_core -- corelib/test, for
+    # one -- fails on its first Eigen include.
+    #
+    # Keep the two halves together on the public interface: everything here
+    # reaches Eigen through rtabmap_core, and installed consumers then get an
+    # Eigen matching the one rtabmap_core was built with.
+    SET(PUBLIC_INCLUDE_DIRS
+        ${PUBLIC_INCLUDE_DIRS}
+        ${grid_map_core_PUBLIC_INCLUDE_DIRS}
+    )
+    SET(PUBLIC_DEFINITIONS
+        ${PUBLIC_DEFINITIONS}
+        "EIGEN_FUNCTORS_PLUGIN=\"${EIGEN_FUNCTORS_PLUGIN_PATH}\""
+        "EIGEN_DENSEBASE_PLUGIN=\"${EIGEN_DENSEBASE_PLUGIN_PATH}\""
+    )
+
 	SET(SRC_FILES
     	${SRC_FILES}
 		global_map/GridMap.cpp
@@ -898,6 +924,12 @@ target_include_directories(rtabmap_core SYSTEM PUBLIC
   "$<BUILD_INTERFACE:${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
   "$<INSTALL_INTERFACE:${PUBLIC_INCLUDE_DIRS}>")
 
+# Definitions that change how a dependency's headers compile, so consumers of
+# rtabmap_core's headers have to see them too (see grid_map_core above).
+IF(PUBLIC_DEFINITIONS)
+  target_compile_definitions(rtabmap_core PUBLIC ${PUBLIC_DEFINITIONS})
+ENDIF()
+
 # GCC 12 false positives from PCL/Eigen template instantiations (SSE codepath
 # unaligned-loads 16 bytes from a 3-element Eigen vector). Eigen knows the
 # over-read is safe; GCC 12 doesn't. Fixed in GCC 13. PCL itself doesn't

corelib/src/Graph.cpp

@@ -1904,6 +1904,39 @@ std::list<std::pair<int, Transform> > computePath(
 	return path;
 }
 
+std::map<int, int> computePathDepths(
+			const std::multimap<int, int> & links,
+			int from,
+			int maxDepth)
+{
+	std::map<int, int> pathDepths;
+	pathDepths.insert(std::make_pair(from, 0));
+	std::list<int> frontier;
+	frontier.push_back(from);
+	while(!frontier.empty())
+	{
+		int currentId = frontier.front();
+		frontier.pop_front();
+		int currentDepth = pathDepths.at(currentId);
+		if(maxDepth > 0 && currentDepth >= maxDepth)
+		{
+			continue;
+		}
+		for(std::multimap<int, int>::const_iterator iter = links.find(currentId);
+			iter!=links.end() && iter->first == currentId;
+			++iter)
+		{
+			int nextId = iter->second;
+			if(pathDepths.find(nextId) == pathDepths.end())
+			{
+				pathDepths.insert(std::make_pair(nextId, currentDepth+1));
+				frontier.push_back(nextId);
+			}
+		}
+	}
+	return pathDepths;
+}
+
 // Dijksta
 std::list<int> computePath(
 			const std::multimap<int, Link> & links,

corelib/src/Rtabmap.cpp

@@ -2746,7 +2746,6 @@ bool Rtabmap::process(
 				std::map<int, Transform> nearestPoses;
 				std::map<int, Transform> optimizedPosesWithOdomCache;
 				std::multimap<int, int> links;
-				std::map<int, Transform> * refPoses = &_optimizedPoses;
 				if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
 				{
 					// get bidirectional links
@@ -2765,7 +2764,6 @@ bool Rtabmap::process(
 						// mapping mode while being localized on the previous session.
 						optimizedPosesWithOdomCache = _optimizedPoses;
 						optimizedPosesWithOdomCache.insert(_odomCachePoses.begin(), _odomCachePoses.end());
-						refPoses = &optimizedPosesWithOdomCache;
 						for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter)
 						{
 							if(uContains(optimizedPosesWithOdomCache, iter->second.from()) && 
@@ -2778,18 +2776,23 @@ bool Rtabmap::process(
 						}
 					}
 				}
+				std::map<int, int> proximityPathDepths;
+				if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
+				{
+					proximityPathDepths = graph::computePathDepths(links, signature->id(), _proximityMaxGraphDepth);
+				}
 				for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
 				{
 					if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
 					{
 						if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
 						{
-							std::list<std::pair<int, Transform> > path = graph::computePath(*refPoses, links, signature->id(), iter->first);
-							UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
-							if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
+							std::map<int, int>::const_iterator depthIter = proximityPathDepths.find(iter->first);
+							if(depthIter == proximityPathDepths.end())
 							{
-								nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+								continue;
 							}
+							nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
 						}
 						else
 						{

corelib/src/pcl18/surface/impl/texture_mapping.hpp

@@ -42,6 +42,9 @@
 #include <pcl18/surface/texture_mapping.h>
 #include <pcl/search/octree.h>
 #include <pcl/common/common.h> // for getAngle3D
+#ifdef _OPENMP
+#include <omp.h>
+#endif
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
@@ -1051,7 +1054,8 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		const pcl::texture_mapping::CameraVector &cameras,
 		const rtabmap::ProgressState * state,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
-		bool distanceToCamPolicy)
+		bool distanceToCamPolicy,
+		int numThreads)
 {
 
 	if (mesh.tex_polygons.size () != 1)
@@ -1081,7 +1085,17 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		UWARN("Texturing cancelled!");
 		return false;
 	}
-	for (unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
+	// Visible faces of a camera don't depend on the other cameras, so they are computed by batch
+	// in parallel below, then merged sequentially (in camera order) to keep the same output.
+	struct CameraVisibility
+	{
+		std::vector<int> keptFaces; // faces kept for that camera, in increasing face index
+		int occludedFaces = 0;
+		int spuriousFaces = 0;
+		int projectedFaces = 0;
+	};
+
+	auto computeVisibleFaces = [&](unsigned int current_cam, CameraVisibility & out)
 	{
 		UDEBUG("Texture camera %d...", current_cam);
 
@@ -1259,7 +1273,6 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 			for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
 			{
 				polygonsKept.insert(polygon_to_face_index[*jter]);
-				faceCameras[polygon_to_face_index[*jter]].push_back(current_cam);
 			}
 		}
 
@@ -1276,14 +1289,55 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 			}
 		}
 
-		msg = uFormat("Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (int)current_cam+1, (int)cameras.size(), (int)occludedFaces.size(), clusterFaces, (int)visibilityIndices.size());
-		UINFO("%s", msg.c_str());
-		if(state && !state->callback(msg))
+		out.keptFaces = std::vector<int>(polygonsKept.begin(), polygonsKept.end());
+		out.occludedFaces = (int)occludedFaces.size();
+		out.spuriousFaces = clusterFaces;
+		out.projectedFaces = (int)visibilityIndices.size();
+	};
+
+#ifdef _OPENMP
+	const int usedThreads = numThreads>1?numThreads:1;
+#else
+	const int usedThreads = 1;
+#endif
+	// More cameras than threads are batched so that a thread getting a cheap camera can pick up
+	// more work. With a single thread, cameras are processed one by one.
+	const size_t chunkSize = usedThreads>1?usedThreads*4:1;
+	std::vector<CameraVisibility> chunkVisibility;
+	bool canceled = false;
+	for(size_t chunkStart=0; chunkStart<cameras.size() && !canceled; chunkStart+=chunkSize)
+	{
+		size_t chunkCams = std::min(chunkSize, cameras.size()-chunkStart);
+		chunkVisibility.assign(chunkCams, CameraVisibility());
+
+		#pragma omp parallel for schedule(dynamic) num_threads(usedThreads)
+		for(int i=0; i<(int)chunkCams; ++i)
 		{
-			//cancelled!
-			UWARN("Texturing cancelled!");
-			return false;
+			computeVisibleFaces((unsigned int)(chunkStart+i), chunkVisibility[i]);
 		}
+
+		for(size_t i=0; i<chunkCams && !canceled; ++i)
+		{
+			unsigned int current_cam = (unsigned int)(chunkStart+i);
+			const CameraVisibility & visibility = chunkVisibility[i];
+			for(size_t j=0; j<visibility.keptFaces.size(); ++j)
+			{
+				faceCameras[visibility.keptFaces[j]].push_back(current_cam);
+			}
+
+			msg = uFormat("Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (int)current_cam+1, (int)cameras.size(), visibility.occludedFaces, visibility.spuriousFaces, visibility.projectedFaces);
+			UINFO("%s", msg.c_str());
+			if(state && !state->callback(msg))
+			{
+				//cancelled!
+				canceled = true;
+			}
+		}
+	}
+	if(canceled)
+	{
+		UWARN("Texturing cancelled!");
+		return false;
 	}
 
 	msg = uFormat("Texturing %d polygons...", (int)faces.size());

corelib/src/pcl18/surface/texture_mapping.h

@@ -368,7 +368,8 @@ namespace pcl
                                       const pcl::texture_mapping::CameraVector &cameras,
 									  const rtabmap::ProgressState * callback = 0,
 									  std::vector<std::map<int, pcl::PointXY> > * vertexToPixels = 0,
-									  bool distanceToCamPolicy = false);
+									  bool distanceToCamPolicy = false,
+									  int numThreads = 1); // number of threads used to compute the visible faces of the cameras (1=sequential)
 
     protected:
       /** \brief mesh scale control. */

corelib/src/util3d_surface.cpp

@@ -738,7 +738,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
-		bool distanceToCamPolicy)
+		bool distanceToCamPolicy,
+		int numThreads)
 {
 	std::map<int, std::vector<CameraModel> > cameraSubModels;
 	for(std::map<int, CameraModel>::const_iterator iter=cameraModels.begin(); iter!=cameraModels.end(); ++iter)
@@ -760,7 +761,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
 			roiRatios,
 			state,
 			vertexToPixels,
-			distanceToCamPolicy);
+			distanceToCamPolicy,
+			numThreads);
 }
 
 pcl::TextureMesh::Ptr createTextureMesh(
@@ -775,7 +777,8 @@ pcl::TextureMesh::Ptr createTextureMesh(
 		const std::vector<float> & roiRatios,
 		const ProgressState * state,
 		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
-		bool distanceToCamPolicy)
+		bool distanceToCamPolicy,
+		int numThreads)
 {
 	UASSERT(mesh->polygons.size());
 	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
@@ -837,7 +840,7 @@ pcl::TextureMesh::Ptr createTextureMesh(
 	tm.setMaxAngle(maxAngle);
 	tm.setMaxDepthError(maxDepthError);
 	tm.setMinClusterSize(minClusterSize);
-	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy))
+	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy, numThreads))
 	{
 		// compute normals for the mesh if not already here
 		bool hasNormals = false;

corelib/test/CMakeLists.txt

@@ -137,6 +137,20 @@ IF(BUILD_PERF_TESTS)
 	set_tests_properties(test_bayesfilter_perf PROPERTIES
 			TIMEOUT ${_perf_timeout}
 			LABELS  "performance")
+
+	# Comparison of the two ways of getting the graph depth of every node of a map, which
+	# is what the RGBD/ProximityMaxGraphDepth filtering needs: one graph::computePath()
+	# (A*) per candidate against one graph::computePathDepths() (BFS) for all of them,
+	# over spiral graphs, where the straight line to the goal tells A* nothing:
+	#   bin/test_graph_perf
+	#   bin/test_graph_perf --gtest_filter=*ProximityLinks*
+	add_executable(test_graph_perf perf_graph.cpp)
+	target_link_libraries(test_graph_perf gtest_main rtabmap_core)
+
+	add_test(NAME test_graph_perf COMMAND test_graph_perf)
+	set_tests_properties(test_graph_perf PROPERTIES
+			TIMEOUT ${_perf_timeout}
+			LABELS  "performance")
 ENDIF(BUILD_PERF_TESTS)
 
 # Rtabmap end-to-end replay of sample DBs (test data fetched by

corelib/test/perf_graph.cpp

@@ -0,0 +1,315 @@
+// Comparison of the two ways of getting the graph depth of every node of a map,
+// which is what Rtabmap::process() needs to reject proximity candidates that are
+// too far in the graph (Parameters::kRGBDProximityMaxGraphDepth()):
+//
+//   - one graph::computePath() (A*) per candidate, which is what it did before, each
+//     search paying for the whole graph again;
+//   - one graph::computePathDepths() (BFS) for all of them, which is what it does now.
+//
+// The graph is a spiral walked inward, a pose every 30 cm: the shape a robot draws
+// covering a room, and the worst case for the A* heuristic. Two nodes on neighboring
+// turns are ~50 cm apart in space but a whole turn apart in the graph, so the straight
+// line to the goal says nothing about the path to it and each A* expands nearly the
+// whole graph. That is exactly the situation proximity detection is called for.
+//
+// Its own executable, run by ctest under the "performance" label, so that its seconds
+// of benchmarking stay out of the unit test shards:
+//   ctest -L performance    to run them
+//   ctest -LE performance   to skip them
+//   bin/test_graph_perf --gtest_filter=*Spiral*
+//
+// Each spiral it builds is written to the temp directory as a g2o file (the path is
+// printed with the results), so that the graph a number was measured on can be looked at
+// with rtabmap-graphViewer or g2o_viewer, or replayed by another tool.
+//
+// The times are reported rather than asserted on, as they depend on the machine. What
+// is asserted is that both approaches answer the same thing on the spiral, so that the
+// numbers below compare two ways of computing the same depths.
+#include <gtest/gtest.h>
+#include "TestUtils.h"
+#include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <algorithm>
+#include <cmath>
+#include <cstdio>
+#include <iostream>
+#include <list>
+#include <map>
+#include <vector>
+
+using namespace rtabmap;
+
+namespace {
+
+// All the spirals have their turns PITCH apart and a pose every SPACING meters, walked
+// from their own radius in to RADIUS_END.
+static const float RADIUS_END = 1.0f;
+static const float PITCH = 0.5f;
+static const float SPACING = 0.3f;
+
+// An Archimedean spiral r(theta) = radiusStart - pitch*theta/(2*pi), walked from
+// radiusStart inward to radiusEnd with one pose every `spacing` meters of arc length,
+// linked as a chain in the order it was walked. Ids are 1..n, so the last id is the
+// innermost pose: the one a session ends on, and the one the depths are computed from.
+struct Spiral
+{
+	std::map<int, Transform> poses;
+	std::multimap<int, int> links;   // bidirectional, as Rtabmap builds them
+	std::vector<int> ids;            // in the order they were walked
+	float length = 0.0f;             // walked arc length, meters
+};
+
+Spiral makeSpiral(float radiusStart, float radiusEnd, float pitch, float spacing)
+{
+	UASSERT(radiusStart > radiusEnd && pitch > 0.0f && spacing > 0.0f);
+	Spiral spiral;
+	const float b = pitch/(2.0f*M_PI);          // -dr/dtheta
+	float theta = 0.0f;
+	float r = radiusStart;
+	while(r >= radiusEnd)
+	{
+		const int id = (int)spiral.ids.size()+1;
+		// Heading along the tangent, so that the poses are what a robot would have.
+		const float tangent = theta + M_PI_2 - std::atan2(b, r);
+		spiral.poses.insert(std::make_pair(id,
+				Transform(r*std::cos(theta), r*std::sin(theta), 0.0f, 0.0f, 0.0f, tangent)));
+		spiral.ids.push_back(id);
+		if(id > 1)
+		{
+			spiral.links.insert(std::make_pair(id-1, id));
+			spiral.links.insert(std::make_pair(id, id-1));
+			spiral.length += spacing;
+		}
+		// Arc length ds = sqrt(r^2 + (dr/dtheta)^2) dtheta, stepped by `spacing`.
+		theta += spacing/std::sqrt(r*r + b*b);
+		r = radiusStart - b*theta;
+	}
+	return spiral;
+}
+
+// The links between two poses closer than `maxDistance` in space but more than
+// `minTrajectoryGap` meters apart along the trajectory: the proximity links a session
+// would have added between neighboring turns, and the shortcuts that make the
+// fewest-links path and the shortest-in-meters path two different paths. The gap is what
+// makes them proximity links rather than trajectory ones: two poses a few steps apart are
+// within `maxDistance` of each other as well, but linking them adds no shortcut, it just
+// short-circuits the chain.
+std::multimap<int, int> proximityLinks(
+		const Spiral & spiral,
+		float maxDistance,
+		float minTrajectoryGap = 2.0f,
+		int * added = 0)
+{
+	std::multimap<int, int> links = spiral.links;
+	const size_t minStep = (size_t)std::ceil(minTrajectoryGap/SPACING);
+	int count = 0;
+	for(size_t i=0; i<spiral.ids.size(); ++i)
+	{
+		const Transform & a = spiral.poses.at(spiral.ids[i]);
+		for(size_t j=i+minStep; j<spiral.ids.size(); ++j)
+		{
+			const Transform & b = spiral.poses.at(spiral.ids[j]);
+			if(a.getDistance(b) <= maxDistance)
+			{
+				links.insert(std::make_pair(spiral.ids[i], spiral.ids[j]));
+				links.insert(std::make_pair(spiral.ids[j], spiral.ids[i]));
+				++count;
+			}
+		}
+	}
+	if(added)
+	{
+		*added = count;
+	}
+	return links;
+}
+
+// The links as constraints, one per pair (the multimap above holds both directions),
+// with the transform the poses give between the two nodes. Only needed to write the
+// graph to disk: exportPoses() needs Link objects, the searches only need the ids.
+std::multimap<int, Link> constraints(const Spiral & spiral, const std::multimap<int, int> & links)
+{
+	std::multimap<int, Link> constraints;
+	const cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1);
+	for(std::multimap<int, int>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		const int from = iter->first, to = iter->second;
+		if(from > to)
+		{
+			continue;   // the other direction of a pair already written
+		}
+		const Transform & a = spiral.poses.at(from);
+		const Transform & b = spiral.poses.at(to);
+		// Consecutive poses are the trajectory, the rest are the proximity detections.
+		const Link::Type type = (to == from+1) ? Link::kNeighbor : Link::kLocalSpaceClosure;
+		constraints.insert(std::make_pair(from, Link(from, to, type, a.inverse()*b, information)));
+	}
+	return constraints;
+}
+
+// The graph these numbers were measured on, written next to the results so that it can be
+// looked at (rtabmap-graphViewer, g2o_viewer) or replayed by another tool. Overwritten on
+// every run, under a stable name rather than a pid-suffixed one: the file is there to be
+// opened, and makeSpiral() builds the same graph every time anyway.
+void saveG2o(const Spiral & spiral, const std::multimap<int, int> & links, const std::string & name)
+{
+	const std::string path = test::tempPath(uFormat("rtabmap_spiral_%s.g2o", name.c_str()));
+	if(graph::exportPoses(path, /*format=*/4, spiral.poses, constraints(spiral, links)))
+	{
+		std::cout << "[          ]   graph saved to " << path << std::endl;
+	}
+	else
+	{
+		std::cout << "[          ]   could not save the graph to " << path << std::endl;
+	}
+}
+
+// What Rtabmap::process() did before: one A* per candidate, from the last node, and the
+// candidate is kept when the path it found is short enough. Returns the ids each path
+// walks through, `from` first: its node count is what the old code compared against
+// RGBD/ProximityMaxGraphDepth, one more than the depth graph::computePathDepths() gives.
+std::map<int, std::vector<std::pair<int, Transform> > > pathsWithAStar(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, int> & links,
+		int from,
+		const std::vector<int> & targets)
+{
+	std::map<int, std::vector<std::pair<int, Transform> > > paths;
+	for(size_t i=0; i<targets.size(); ++i)
+	{
+		const std::list<std::pair<int, Transform> > path =
+				graph::computePath(poses, links, from, targets[i]);
+		if(!path.empty())
+		{
+			// As a vector, which is what the checks below (and graph::computePathLength()) take.
+			paths.insert(std::make_pair(targets[i],
+					std::vector<std::pair<int, Transform> >(path.begin(), path.end())));
+		}
+	}
+	return paths;
+}
+
+// A* needs one search per node, BFS answers for every node in the one search.
+void report(size_t nodes, double aStarTime, double bfsTime)
+{
+	printf("[          ]   A* %8.2f ms (%ld searches), BFS %6.2f ms (1 search), speedup x%.0f\n",
+			aStarTime*1000.0, (long)nodes, bfsTime*1000.0,
+			bfsTime > 0.0 ? aStarTime/bfsTime : 0.0);
+}
+
+// The spirals compared. All of them have a pose every 30 cm and turns 50 cm apart; what
+// changes is how far out they start, and so how many nodes they hold.
+struct SpiralSize
+{
+	float radiusStart;
+	const char * name;
+	const char * fileName;
+};
+static const SpiralSize SPIRAL_SIZES[] = {
+	{2.0f,  "2 m to 1 m",  "2m_to_1m"},
+	{5.0f,  "5 m to 1 m",  "5m_to_1m"},
+	{10.0f, "10 m to 1 m", "10m_to_1m"},
+};
+static const size_t SPIRAL_COUNT = sizeof(SPIRAL_SIZES)/sizeof(SPIRAL_SIZES[0]);
+
+}
+
+// The depths of every node of the spiral, from its last node, both ways. The spiral is a
+// chain, so there is only one path between two of its nodes and both approaches have to
+// agree: the A* path holds one more node than the BFS depth, the start node itself.
+TEST(GraphPerfTest, PathDepthsOnSpiral)
+{
+	for(size_t s=0; s<SPIRAL_COUNT; ++s)
+	{
+		const Spiral spiral = makeSpiral(SPIRAL_SIZES[s].radiusStart, RADIUS_END, PITCH, SPACING);
+		const int from = spiral.ids.back();
+		std::cout << "[          ] spiral " << SPIRAL_SIZES[s].name << ", turns "
+				<< PITCH << " m apart, a pose every " << SPACING << " m: "
+				<< spiral.ids.size() << " nodes, " << spiral.length << " m walked, depths from "
+				<< from << " (the innermost pose) to all of them" << std::endl;
+		saveG2o(spiral, spiral.links, SPIRAL_SIZES[s].fileName);
+
+		UTimer timer;
+		const std::map<int, std::vector<std::pair<int, Transform> > > aStarPaths =
+				pathsWithAStar(spiral.poses, spiral.links, from, spiral.ids);
+		const double aStarTime = timer.ticks();
+
+		const std::map<int, int> depths = graph::computePathDepths(spiral.links, from);
+		const double bfsTime = timer.ticks();
+
+		report(spiral.ids.size(), aStarTime, bfsTime);
+
+		ASSERT_EQ(depths.size(), spiral.ids.size());
+		ASSERT_EQ(aStarPaths.size(), spiral.ids.size());
+		EXPECT_EQ(depths.at(from), 0);
+		for(size_t i=0; i<spiral.ids.size(); ++i)
+		{
+			const int id = spiral.ids[i];
+			// The chain gives the depth in closed form: the number of links back to `from`.
+			EXPECT_EQ(depths.at(id), from-id) << "node " << id;
+
+			// Same path, not only the same count: the only way from `from` to `id` walks the
+			// chain, and A* walks it node by node, each step one deeper than the one before.
+			const std::vector<std::pair<int, Transform> > & path = aStarPaths.at(id);
+			ASSERT_EQ((int)path.size(), depths.at(id)+1) << "node " << id;
+			for(size_t j=0; j<path.size(); ++j)
+			{
+				ASSERT_EQ(path[j].first, from-(int)j) << "node " << id << ", step " << j;
+				ASSERT_EQ(depths.at(path[j].first), (int)j) << "node " << id << ", step " << j;
+			}
+		}
+	}
+}
+
+// The same spiral once the proximity links between neighboring turns are added, which is
+// what the graph looks like after a session closed on itself. Beyond the timings, this is
+// where the two approaches stop answering the same thing: A* minimizes meters, so the path
+// it returns is not always the one with the fewest links, and the node count it reports is
+// then larger than the depth. Rejecting candidates on it rejected some that were within
+// RGBD/ProximityMaxGraphDepth links of the current node.
+TEST(GraphPerfTest, PathDepthsOnSpiralWithProximityLinks)
+{
+	for(size_t s=0; s<SPIRAL_COUNT; ++s)
+	{
+		const Spiral spiral = makeSpiral(SPIRAL_SIZES[s].radiusStart, RADIUS_END, PITCH, SPACING);
+		int added = 0;
+		const std::multimap<int, int> links = proximityLinks(spiral, /*maxDistance=*/0.6f,
+				/*minTrajectoryGap=*/2.0f, &added);
+		const int from = spiral.ids.back();
+		std::cout << "[          ] spiral " << SPIRAL_SIZES[s].name << ": " << spiral.ids.size()
+				<< " nodes, " << added << " proximity links added between turns" << std::endl;
+		saveG2o(spiral, links, uFormat("%s_proximity", SPIRAL_SIZES[s].fileName));
+
+		UTimer timer;
+		const std::map<int, std::vector<std::pair<int, Transform> > > aStarPaths =
+				pathsWithAStar(spiral.poses, links, from, spiral.ids);
+		const double aStarTime = timer.ticks();
+
+		const std::map<int, int> depths = graph::computePathDepths(links, from);
+		const double bfsTime = timer.ticks();
+
+		report(spiral.ids.size(), aStarTime, bfsTime);
+
+		ASSERT_EQ(depths.size(), spiral.ids.size());
+		ASSERT_EQ(aStarPaths.size(), spiral.ids.size());
+		int overestimated = 0, maxOverestimation = 0;
+		for(size_t i=0; i<spiral.ids.size(); ++i)
+		{
+			const int id = spiral.ids[i];
+			const int over = (int)aStarPaths.at(id).size() - (depths.at(id)+1);
+			// A* cannot beat the BFS depth, it can only walk more links to save meters.
+			EXPECT_GE(over, 0) << "node " << id;
+			if(over > 0)
+			{
+				++overestimated;
+				maxOverestimation = std::max(maxOverestimation, over);
+			}
+		}
+		printf("[          ]   A* counted more links than the depth on %d of the %ld nodes"
+				" (up to %d more)\n", overestimated, (long)spiral.ids.size(), maxOverestimation);
+	}
+}

guilib/include/rtabmap/gui/ExportCloudsDialog.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <QDialog>
 #include <QMap>
+#include <QColor>
 #include <QtCore/QSettings>
 
 #include <rtabmap/core/Signature.h>
@@ -46,6 +47,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 class Ui_ExportCloudsDialog;
 class QAbstractButton;
 
+namespace clams {
+class DiscreteDepthDistortionModel;
+}
+
 namespace rtabmap {
 class ProgressDialog;
 class GainCompensator;
@@ -132,7 +137,30 @@ private Q_SLOTS:
 	void cancel();
 
 private:
+	int numThreads() const; // resolves the "Auto" value of the threads spin box
 	std::map<int, Transform> filterNodes(const std::map<int, Transform> & poses);
+	struct CloudGenResult
+	{
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud;
+		pcl::IndicesPtr indices;
+		bool hasScan = false;
+		bool scanHasRGB = false;
+		std::vector<std::pair<QString, QColor> > messages;
+	};
+	CloudGenResult generateCloudForNode(
+			int nodeId,
+			const Transform & pose,
+			int index,
+			int totalPoses,
+			const std::vector<float> & roiRatios,
+			const clams::DiscreteDepthDistortionModel * model,
+			const QMap<int, Signature> & cachedSignatures,
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
+			const std::map<int, LaserScan> & cachedScans,
+			const ParametersMap & parameters,
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr * previousCloud,
+			pcl::IndicesPtr * previousIndices,
+			Transform * previousPose) const;
 	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > getClouds(
 			const std::map<int, Transform> & poses,
 			const QMap<int, Signature> & cachedSignatures,

guilib/src/ui/exportCloudsDialog.ui

@@ -31,7 +31,7 @@
       <layout class="QVBoxLayout" name="verticalLayout_13">
        <item>
         <layout class="QGridLayout" name="gridLayout_8" columnstretch="0,1">
-         <item row="17" column="0">
+         <item row="18" column="0">
           <widget class="QCheckBox" name="checkBox_cameraProjection">
            <property name="text">
             <string/>
@@ -45,7 +45,7 @@
            </property>
           </widget>
          </item>
-         <item row="14" column="0">
+         <item row="15" column="0">
           <widget class="QCheckBox" name="checkBox_filtering">
            <property name="text">
             <string/>
@@ -59,7 +59,7 @@
            </property>
           </widget>
          </item>
-         <item row="18" column="1">
+         <item row="19" column="1">
           <widget class="QLabel" name="label_binaryFile_12">
            <property name="text">
             <string>Meshing.</string>
@@ -69,7 +69,7 @@
            </property>
           </widget>
          </item>
-         <item row="14" column="1">
+         <item row="15" column="1">
           <widget class="QLabel" name="label_binaryFile_9">
            <property name="text">
             <string>Cloud filtering.</string>
@@ -89,14 +89,14 @@
            </property>
           </widget>
          </item>
-         <item row="18" column="0">
+         <item row="19" column="0">
           <widget class="QCheckBox" name="checkBox_meshing">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="16" column="1">
+         <item row="17" column="1">
           <widget class="QLabel" name="label_gainCompensation">
            <property name="text">
             <string>Gain compensation. Normalize brightness of images.</string>
@@ -208,7 +208,7 @@
            </property>
           </widget>
          </item>
-         <item row="15" column="0">
+         <item row="16" column="0">
           <widget class="QCheckBox" name="checkBox_smoothing">
            <property name="text">
             <string/>
@@ -229,7 +229,7 @@
            </item>
           </widget>
          </item>
-         <item row="15" column="1">
+         <item row="16" column="1">
           <widget class="QLabel" name="label_smoothing">
            <property name="text">
             <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
@@ -278,7 +278,7 @@
            </item>
           </widget>
          </item>
-         <item row="17" column="1">
+         <item row="18" column="1">
           <widget class="QLabel" name="label_cameraProjection">
            <property name="text">
             <string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string>
@@ -329,7 +329,7 @@
            </property>
           </widget>
          </item>
-         <item row="16" column="0">
+         <item row="17" column="0">
           <widget class="QCheckBox" name="checkBox_gainCompensation">
            <property name="text">
             <string/>
@@ -403,6 +403,32 @@
            </property>
           </widget>
          </item>
+         <item row="14" column="0">
+          <widget class="QSpinBox" name="spinBox_numThreads">
+           <property name="specialValueText">
+            <string>Auto</string>
+           </property>
+           <property name="minimum">
+            <number>0</number>
+           </property>
+           <property name="maximum">
+            <number>128</number>
+           </property>
+           <property name="value">
+            <number>0</number>
+           </property>
+          </widget>
+         </item>
+         <item row="14" column="1">
+          <widget class="QLabel" name="label_numThreads">
+           <property name="text">
+            <string>Number of threads used to generate the clouds and to texture the mesh (Auto=one per core, 1=process them one by one).</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
         </layout>
        </item>
        <item>

tools/Export/main.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_mapping.h>
 #include <rtabmap/core/optimizer/OptimizerG2O.h>
 #include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Signature.h>
 #include <rtabmap/core/global_map/OccupancyGrid.h>
 #ifdef RTABMAP_OCTOMAP
 #include <rtabmap/core/global_map/OctoMap.h>
@@ -49,8 +50,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/common/common.h>
 #include <pcl/surface/poisson.h>
 #include <stdio.h>
+#include <algorithm>
 #include <fstream>
 
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
 #ifdef RTABMAP_PDAL
 #include <rtabmap/core/PDALWriter.h>
 #endif
@@ -184,6 +190,8 @@ void showUsage()
 			"    --density_angle #     Filter poses up to angle (deg) in the --density_radius.\n"
 			"    --filter_ceiling #    Filter points over a custom height (default 0 m, 0=disabled).\n"
 			"    --filter_floor #      Filter points below a custom height (default 0 m, 0=disabled).\n"
+			"    --threads       #     Number of threads used to generate the clouds and to texture the mesh\n"
+			"                              (default 0=one per core, 1=process them sequentially).\n"
 
 			"\n%s", Parameters::showUsage());
 	;
@@ -256,6 +264,7 @@ int main(int argc, char * argv[])
 	float poissonSize = 0.03;
 	int maxPolygons = 300000;
 	int decimation = -1;
+	int numThreads = 0;
 	float depthEdgeBleedingFilterError = 0.0f;
 	unsigned char depthConfidenceThr = 0;
 	float minRange = 0.0f;
@@ -819,6 +828,23 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--threads") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				numThreads = uStr2Int(argv[i]);
+				if(numThreads < 0)
+				{
+					printf("--threads cannot be negative!\n");
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--decimation") == 0)
 		{
 			++i;
@@ -1493,6 +1519,9 @@ int main(int argc, char * argv[])
 	}
 	int processedNodes = 0;
 	int lastPercent = 0;
+
+	std::vector<std::pair<int, Transform> > nodes;
+	nodes.reserve(optimizedPoses.size());
 	for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
 	{
 		if(iter->first<0)
@@ -1503,26 +1532,42 @@ int main(int argc, char * argv[])
 
 			landmarkPoses.insert(*iter);
 			landmarkStamps.insert(std::make_pair(iter->first, 0));
-			continue;
 		}
-		
+		else
+		{
+			nodes.push_back(*iter);
+		}
+	}
+
+	struct NodeExportData
+	{
+		// node info, calibration, compressed data, uncompressed local occupancy grid
+		// and uncompressed depth image (only if texturing)
+		Signature node;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
+	};
+
+	auto loadNode = [&](int nodeId, const Transform & pose, NodeExportData & out)
+	{
 		Transform p, gt;
 		int m;
 		std::string l;
 		GPS gps;
 		std::vector<float> v;
 		EnvSensors s;
-		int weight = -1;
-		double stamp = 0.0;
-		dbDriver->getNodeInfo(iter->first, p, m, weight, l, stamp, gt, v, gps, s);
+		int weight;
+		double stamp;
+		dbDriver->getNodeInfo(nodeId, p, m, weight, l, stamp, gt, v, gps, s);
 
-		SensorData data;
+		out.node = Signature(nodeId, m, weight, stamp, l, pose, gt);
+		SensorData & data = out.node.sensorData();
 		bool loadImages = ((exportCloud || exportMesh) && (!cloudFromScan || texture || camProjection)) || exportImages;
 		bool loadScan = ((exportCloud || exportMesh) && cloudFromScan) || exportPosesScan;
 		if(loadImages || loadScan || export2DMap || exportOctomap)
 		{
 			dbDriver->getNodeData(
-				iter->first, 
+				nodeId, 
 				data, 
 				loadImages, 
 				loadScan,
@@ -1530,23 +1575,29 @@ int main(int argc, char * argv[])
 				export2DMap || exportOctomap);
 		}
 
+		data.setGPS(gps); // getNodeData() above overwrites the whole sensor data
+
 		// uncompress data
-		std::vector<CameraModel> models;
-		std::vector<StereoCameraModel> stereoModels;
 		if(loadImages || exportPosesCamera)
 		{
-			dbDriver->getCalibration(iter->first, models, stereoModels);
+			std::vector<CameraModel> models;
+			std::vector<StereoCameraModel> stereoModels;
+			dbDriver->getCalibration(nodeId, models, stereoModels);
+			data.setCameraModels(models);
+			data.setStereoCameraModels(stereoModels);
 		}
+		const std::vector<CameraModel> & models = data.cameraModels();
+		const std::vector<StereoCameraModel> & stereoModels = data.stereoCameraModels();
 
+		cv::Mat depth;
 		if(exportCloud || exportMesh || exportImages)
 		{
-			bool densityFiltered = !densityPoses.empty() && densityPoses.find(iter->first) == densityPoses.end();
+			bool densityFiltered = !densityPoses.empty() && densityPoses.find(nodeId) == densityPoses.end();
 			cv::Mat rgb;
-			cv::Mat depth;
 			cv::Mat confidence;
 			pcl::IndicesPtr indices(new std::vector<int>);
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-			pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud = out.cloud;
+			pcl::PointCloud<pcl::PointXYZI>::Ptr & cloudI = out.cloudI;
 			if(weight != -1)
 			{
 				if(!densityFiltered && cloudFromScan && (exportCloud || exportMesh))
@@ -1555,7 +1606,7 @@ int main(int argc, char * argv[])
 					data.uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!data.depthOrRightCompressed().empty()?&depth:0, &scan, 0, 0, 0, 0, exportImages?&confidence:0);
 					if(scan.empty())
 					{
-						printf("Node %d doesn't have scan data, empty cloud is created.\n", iter->first);
+						printf("Node %d doesn't have scan data, empty cloud is created.\n", nodeId);
 					}
 					if(decimation>1 || minRange>0.0f || maxRange)
 					{
@@ -1586,7 +1637,7 @@ int main(int argc, char * argv[])
 						if(depth.empty())
 						{
 							printf("Node %d doesn't have depth or stereo data, empty cloud is "
-									"created (if you want to create point cloud from scan, use --scan option).\n", iter->first);
+									"created (if you want to create point cloud from scan, use --scan option).\n", nodeId);
 						}
 						else if(!data.depthRaw().empty() && depthEdgeBleedingFilterError>0.0f)
 						{
@@ -1617,8 +1668,9 @@ int main(int argc, char * argv[])
 				if(!UDirectory::exists(dir)) {
 					UDirectory::makeDir(dir);
 				}
-				std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+".jpg";
+				std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp))+".jpg";
 				cv::imwrite(outputPath, rgb);
+				#pragma omp atomic
 				++imagesExported;
 				if(!depth.empty())
 				{
@@ -1642,7 +1694,7 @@ int main(int argc, char * argv[])
 						UDirectory::makeDir(dir);
 					}
 
-					outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+ext;
+					outputPath=dir+"/"+(exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp))+ext;
 					cv::imwrite(outputPath, depthExported);
 				}
 				if(!confidence.empty())
@@ -1652,7 +1704,7 @@ int main(int argc, char * argv[])
 						UDirectory::makeDir(dir);
 					}
 
-					outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp))+".png";
+					outputPath=dir+"/"+(exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp))+".png";
 					cv::imwrite(outputPath, confidence);
 				}
 
@@ -1660,7 +1712,7 @@ int main(int argc, char * argv[])
 				for(size_t i=0; i<models.size(); ++i)
 				{
 					CameraModel model = models[i];
-					std::string modelName = (exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp));
+					std::string modelName = (exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp));
 					if(models.size() > 1) {
 						modelName += "_" + uNumber2Str((int)i);
 					}
@@ -1674,7 +1726,7 @@ int main(int argc, char * argv[])
 				for(size_t i=0; i<stereoModels.size(); ++i)
 				{
 					StereoCameraModel model = stereoModels[i];
-					std::string modelName = (exportImagesId?uNumber2Str(iter->first):uFormat("%f", stamp));
+					std::string modelName = (exportImagesId?uNumber2Str(nodeId):uFormat("%f", stamp));
 					if(stereoModels.size() > 1) {
 						modelName += "_" + uNumber2Str((int)i);
 					}
@@ -1694,20 +1746,20 @@ int main(int argc, char * argv[])
 					if(cloud.get() && !cloud->empty()) {
 						cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
 						if(!cloud->empty())
-							cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
+							cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
 					}
 					else if(cloudI.get() && !cloudI->empty()) {
 						cloudI = rtabmap::util3d::voxelize(cloudI, indices, voxelSize);
 						if(!cloudI->empty())
-							cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
+							cloudI = rtabmap::util3d::transformPointCloud(cloudI, pose);
 					}
 				}
 				else
 				{
 					if(cloud.get() && !cloud->empty())
-						cloud = rtabmap::util3d::transformPointCloud(cloud, indices, iter->second);
+						cloud = rtabmap::util3d::transformPointCloud(cloud, indices, pose);
 					else if(cloudI.get() && !cloudI->empty())
-						cloudI = rtabmap::util3d::transformPointCloud(cloudI, indices, iter->second);
+						cloudI = rtabmap::util3d::transformPointCloud(cloudI, indices, pose);
 				}
 
 				if(filter_ceiling != 0.0 || filter_floor != 0.0f)
@@ -1722,54 +1774,90 @@ int main(int argc, char * argv[])
 					}
 				}
 
-				if(cloudFromScan)
+			}
+		}
+
+		if(weight != -1 && (export2DMap || exportOctomap))
+		{
+			cv::Mat ground, obstacles, empty;
+			data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
+		}
+
+		data.clearRawData(true, true, true, false); // keep uncompressed occupancy grid
+		if(texture && !depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1))
+		{
+			// Keep uncompressed depth for texturing, the compressed one is not needed anymore.
+			// The compressed image is passed back as is (rows==1), as flushNode() uses it to
+			// know if the node has an image.
+			data.setRGBDImage(data.imageCompressed(), depth, cv::Mat(), data.cameraModels());
+		}
+	};
+
+	auto flushNode = [&](NodeExportData & out)
+	{
+		const Signature & node = out.node;
+		const SensorData & data = node.sensorData();
+		const int nodeId = node.id();
+		const Transform & pose = node.getPose();
+		std::vector<CameraModel> models = data.cameraModels();
+		const std::vector<StereoCameraModel> & stereoModels = data.stereoCameraModels();
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud = out.cloud;
+		pcl::PointCloud<pcl::PointXYZI>::Ptr & cloudI = out.cloudI;
+		const cv::Mat & depth = data.depthOrRightRaw();
+		double stamp = node.getStamp();
+		int weight = node.getWeight();
+		const GPS & gps = data.gps();
+		const Transform & gt = node.getGroundTruthPose();
+
+		if(exportCloud || exportMesh)
+		{
+			if(cloudFromScan)
+			{
+				Transform lidarViewpoint = pose * data.laserScanCompressed().localTransform();
+				rawViewpoints.insert(std::make_pair(nodeId, lidarViewpoint));
+			}
+			else if(!models.empty() && !models[0].localTransform().isNull())
+			{
+				Transform cameraViewpoint = pose * models[0].localTransform(); // take the first camera
+				rawViewpoints.insert(std::make_pair(nodeId, cameraViewpoint));
+			}
+			else if(!stereoModels.empty() && !stereoModels[0].localTransform().isNull())
+			{
+				Transform cameraViewpoint = pose * stereoModels[0].localTransform();
+				rawViewpoints.insert(std::make_pair(nodeId, cameraViewpoint));
+			}
+			else
+			{
+				rawViewpoints.insert(std::make_pair(nodeId, pose));
+			}
+
+			if(cloud.get() && !cloud->empty())
+			{
+				if(assembledCloud->empty())
 				{
-					Transform lidarViewpoint = iter->second * data.laserScanRaw().localTransform();
-					rawViewpoints.insert(std::make_pair(iter->first, lidarViewpoint));
-				}
-				else if(!models.empty() && !models[0].localTransform().isNull())
-				{
-					Transform cameraViewpoint = iter->second * models[0].localTransform(); // take the first camera
-					rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
-				}
-				else if(!stereoModels.empty() && !stereoModels[0].localTransform().isNull())
-				{
-					Transform cameraViewpoint = iter->second * stereoModels[0].localTransform();
-					rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
+					*assembledCloud = *cloud;
 				}
 				else
 				{
-					rawViewpoints.insert(*iter);
+					*assembledCloud += *cloud;
 				}
-
-				if(cloud.get() && !cloud->empty())
+				rawViewpointIndices.resize(assembledCloud->size(), nodeId);
+			}
+			else if(cloudI.get() && !cloudI->empty())
+			{
+				if(assembledCloudI->empty())
 				{
-					if(assembledCloud->empty())
-					{
-						*assembledCloud = *cloud;
-					}
-					else
-					{
-						*assembledCloud += *cloud;
-					}
-					rawViewpointIndices.resize(assembledCloud->size(), iter->first);
+					*assembledCloudI = *cloudI;
 				}
-				else if(cloudI.get() && !cloudI->empty())
+				else
 				{
-					if(assembledCloudI->empty())
-					{
-						*assembledCloudI = *cloudI;
-					}
-					else
-					{
-						*assembledCloudI += *cloudI;
-					}
-					rawViewpointIndices.resize(assembledCloudI->size(), iter->first);
-				}
-				if(texture && !depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1))
-				{
-					cameraDepths.insert(std::make_pair(iter->first, depth));
+					*assembledCloudI += *cloudI;
 				}
+				rawViewpointIndices.resize(assembledCloudI->size(), nodeId);
+			}
+			if(!depth.empty()) // depth is set only when texturing (see loadNode)
+			{
+				cameraDepths.insert(std::make_pair(nodeId, depth));
 			}
 		}
 
@@ -1781,8 +1869,8 @@ int main(int argc, char * argv[])
 			}
 		}
 
-		robotPoses.insert(std::make_pair(iter->first, iter->second));
-		robotStamps.insert(std::make_pair(iter->first, stamp));
+		robotPoses.insert(std::make_pair(nodeId, pose));
+		robotStamps.insert(std::make_pair(nodeId, stamp));
 		if(models.empty() && weight == -1 && !cameraModels.empty())
 		{
 			// For intermediate nodes, use latest models
@@ -1792,7 +1880,7 @@ int main(int argc, char * argv[])
 		{
 			if(!data.imageCompressed().empty())
 			{
-				cameraModels.insert(std::make_pair(iter->first, models));
+				cameraModels.insert(std::make_pair(nodeId, models));
 			}
 			if(exportPosesCamera)
 			{
@@ -1804,15 +1892,15 @@ int main(int argc, char * argv[])
 				UASSERT_MSG(models.size() == cameraPoses.size(), "Not all nodes have same number of cameras to export camera poses.");
 				for(size_t i=0; i<models.size(); ++i)
 				{
-					cameraPoses[i].insert(std::make_pair(iter->first, iter->second*models[i].localTransform()));
-					cameraStamps[i].insert(std::make_pair(iter->first, stamp));
+					cameraPoses[i].insert(std::make_pair(nodeId, pose*models[i].localTransform()));
+					cameraStamps[i].insert(std::make_pair(nodeId, stamp));
 				}
 			}
 		}
 		if(exportPosesScan && !data.laserScanCompressed().empty())
 		{
-			scanPoses.insert(std::make_pair(iter->first, iter->second*data.laserScanCompressed().localTransform()));
-			scanStamps.insert(std::make_pair(iter->first, stamp));
+			scanPoses.insert(std::make_pair(nodeId, pose*data.laserScanCompressed().localTransform()));
+			scanStamps.insert(std::make_pair(nodeId, stamp));
 		}
 
 		if(exportPosesGps || exportGps>=0)
@@ -1832,56 +1920,85 @@ int main(int argc, char * argv[])
 						gpsOrigin = gps;
 					}
 					Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
-					gpsPoses.insert(std::make_pair(iter->first, pose));
+					gpsPoses.insert(std::make_pair(nodeId, pose));
 				}
 				if(exportGps>=0)
 				{
-					gpsValues.insert(std::make_pair(iter->first, gps));
+					gpsValues.insert(std::make_pair(nodeId, gps));
 				}
-				gpsStamps.insert(std::make_pair(iter->first, gps.stamp()));
+				gpsStamps.insert(std::make_pair(nodeId, gps.stamp()));
 			}
 		}
 
 		if(exportPosesGt && !gt.isNull())
 		{
-			gtPoses.insert(std::make_pair(iter->first, gt));
-			gtStamps.insert(std::make_pair(iter->first, stamp));
+			gtPoses.insert(std::make_pair(nodeId, gt));
+			gtStamps.insert(std::make_pair(nodeId, stamp));
 		}
 
 		if(weight != -1 && (export2DMap || exportOctomap)) {
-			cv::Mat ground;
-			cv::Mat obstacles;
-			cv::Mat empty;
-			data.uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
+			const cv::Mat & ground = data.gridGroundCellsRaw();
+			const cv::Mat & obstacles = data.gridObstacleCellsRaw();
+			const cv::Mat & empty = data.gridEmptyCellsRaw();
 			if(ground.empty() && obstacles.empty() && empty.empty()) {
-				printf("Node %d doesn't have local occupancy grid, ignored!\n", iter->first);
+				printf("Node %d doesn't have local occupancy grid, ignored!\n", nodeId);
 			}
 			else {
-				addedPosesToMap.insert(*iter);
-				localGridCache.add(iter->first, ground, obstacles, empty, data.gridCellSize(), data.gridViewPoint());
+				addedPosesToMap.insert(std::make_pair(nodeId, pose));
+				localGridCache.add(nodeId, ground, obstacles, empty, data.gridCellSize(), data.gridViewPoint());
 				if(export2DMap && !grid.update(addedPosesToMap)) {
-					printf("Failed to assemble local grid %d to global occupancy grid!\n", iter->first);
+					printf("Failed to assemble local grid %d to global occupancy grid!\n", nodeId);
 				}
 #ifdef RTABMAP_OCTOMAP
 				if(exportOctomap && !octomap.update(addedPosesToMap)) {
-					printf("Failed to assemble local grid %d to OctoMap!\n", iter->first);
+					printf("Failed to assemble local grid %d to OctoMap!\n", nodeId);
 				}
 #endif
 				localGridCache.clear();
 			}
 		}
 
-		if(optimizedPoses.size() >= 500)
+	};
+
+#ifdef _OPENMP
+	const int usedThreads = numThreads>0?numThreads:omp_get_max_threads();
+#else
+	const int usedThreads = 1;
+#endif
+	// Nodes are loaded by batch, a batch is generated in parallel then assembled sequentially
+	// (in node order) to keep the output independent of the thread count. More nodes than
+	// threads are batched so that a thread getting cheap nodes can pick up more work. With a
+	// single thread, nodes are processed one by one, keeping only one node in memory.
+	const size_t chunkSize = usedThreads>1?usedThreads*4:1;
+	std::vector<NodeExportData> chunkData;
+	for(size_t chunkStart=0; chunkStart<nodes.size(); chunkStart+=chunkSize)
+	{
+		size_t chunkNodes = std::min(chunkSize, nodes.size()-chunkStart);
+		chunkData.assign(chunkNodes, NodeExportData());
+
+		#pragma omp parallel for schedule(dynamic) num_threads(usedThreads)
+		for(int i=0; i<(int)chunkNodes; ++i)
 		{
-			++processedNodes;
-			int percent = processedNodes*100/(int)optimizedPoses.size();
-			if(percent != lastPercent)
+			loadNode(nodes[chunkStart+i].first, nodes[chunkStart+i].second, chunkData[i]);
+		}
+
+		for(size_t i=0; i<chunkNodes; ++i)
+		{
+			flushNode(chunkData[i]);
+			chunkData[i] = NodeExportData();
+
+			if(optimizedPoses.size() >= 500)
 			{
-				printf("Processed %d/%d (%d%%) nodes...\n",
-					processedNodes,
-					(int)optimizedPoses.size(),
-					percent);
-				lastPercent = percent;
+				++processedNodes;
+				int percent = processedNodes*100/(int)optimizedPoses.size();
+				if(percent != lastPercent)
+				{
+					printf("Processed %d/%d (%d%%) nodes...\n",
+						processedNodes,
+						(int)optimizedPoses.size(),
+						percent);
+					lastPercent = percent;
+				}
 			}
 		}
 	}
@@ -2658,7 +2775,8 @@ int main(int argc, char * argv[])
 							textureRoiRatios,
 							&progressState,
 							&vertexToPixels,
-							distanceToCamPolicy);
+							distanceToCamPolicy,
+							usedThreads);
 					printf("Texturing... done (%fs).\n", timer.ticks());
 
 					// Remove occluded polygons (polygons with no texture)