Save intermediate node input features (#1698)

* Avoid removing nodes from STM when using CreateIntermediateNodes

* Clean up the data of the intermediate node when IntermediateNodeDataKept is false

* Save intermediate node input features

* Don't delete intermediate nodes

* comment typo

* Updated parameter description, bump patch for DBReader API chang

* When ID is not udpated to new one, don't disable words because that signature is still used in the bayes filter

* updated comment

* rtabmap-reprocess: support Rtabmap/DetectionRate when intermediate nodes are created and -skip option is not used

* Fixed smallMovement when rehearsal is not trigdered at the same time

* GUI: visualize rehearsal darkblue when intermediate nodes are used

* Fixed RtabmapThread not transfering input features on intermediate nodes

* Do not override smallMovement and fastMovement when converting to intermediate nodes to keep GUI visualization color working

---------

Co-authored-by: Borong Yuan <yuanborong@hotmail.com>

CMakeLists.txt

@@ -22,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 23)
-SET(RTABMAP_PATCH_VERSION 5)
+SET(RTABMAP_PATCH_VERSION 6)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 

corelib/include/rtabmap/core/DBReader.h

@@ -60,6 +60,7 @@ public:
 			 int stopMapId = -1,
 			 bool priorsIgnored = false,
 			 bool imuIgnored = false,
+			 bool intermediateNodesAreNormalNodes = false,
 			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
@@ -76,6 +77,7 @@ public:
 			 int stopMapId = -1,
 			 bool priorsIgnored = false,
 			 bool imuIgnored = false,
+			 bool intermediateNodesAreNormalNodes = false,
 			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
 	virtual ~DBReader();
 
@@ -106,6 +108,7 @@ private:
 	int _stopId;
 	std::vector<unsigned int> _cameraIndices;
 	bool _intermediateNodesIgnored;
+	bool _intermediateNodesAreNormalNodes;
 	bool _landmarksIgnored;
 	bool _featuresIgnored;
 	bool _priorsIgnored;

corelib/include/rtabmap/core/Memory.h

@@ -140,6 +140,7 @@ public:
 			float radius,
 			const std::map<int, Transform> & optimizedPoses,
 			int maxGraphDepth) const;
+	void convertToIntermediate(int locationId);
 	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
 	void saveLocationData(int locationId);
 	void removeLink(int idA, int idB);

corelib/include/rtabmap/core/Parameters.h

@@ -218,7 +218,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Mem, ReduceGraph,                 bool, false,    uFormat("Reduce graph. Merge nodes when loop closures are added (ignoring those with user data). Note that this approach assumes that 100%% of the loop closures accepted are good, so it is highly recommended to enable \"%s\" at the same time.", kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(Mem, RecentWmRatio,               float, 0.2,     "Ratio of locations after the last loop closure in WM that cannot be transferred.");
     RTABMAP_PARAM(Mem, TransferSortingByWeightId,   bool, false,    "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
-    RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,  bool, false,    "On merge, update to new id. When false, no copy.");
+    RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,  bool, false,    uFormat("On merge, update to new id. When false, no copy. Keep this disable if %s=true.", kRtabmapCreateIntermediateNodes().c_str()));
     RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
     RTABMAP_PARAM(Mem, GenerateIds,                 bool, true,     "True=Generate location IDs, False=use input image IDs.");
     RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");

corelib/include/rtabmap/core/SensorData.h

@@ -212,6 +212,12 @@ public:
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
 			_descriptors.empty() &&
+			_groundCellsRaw.empty() &&
+			_groundCellsCompressed.empty() &&
+			_obstacleCellsRaw.empty() &&
+			_obstacleCellsCompressed.empty() &&
+			_emptyCellsRaw.empty() &&
+			_emptyCellsCompressed.empty() &&
 			imu_.empty());
 	}
 
@@ -309,8 +315,6 @@ public:
 			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewPoint);
-	// remove raw occupancy grids
-	void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();}
 	const cv::Mat & gridGroundCellsRaw() const {return _groundCellsRaw;}
 	const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
 	const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
@@ -355,12 +359,12 @@ public:
 	 * Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
 	 * Raw data are kept is set.
 	 */
-	void clearCompressedData(bool images = true, bool scan = true, bool userData = true);
+	void clearCompressedData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
 	/**
 	 * Clear raw rgb/depth (left/right) images, raw laser scan and raw user data.
 	 * Compressed data are kept is set.
 	 */
-	void clearRawData(bool images = true, bool scan = true, bool userData = true);
+	void clearRawData(bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
 
 	bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
 

corelib/src/DBReader.cpp

@@ -57,6 +57,7 @@ DBReader::DBReader(const std::string & databasePath,
 				   int stopMapId,
 				   bool priorsIgnored,
 				   bool imuIgnored,
+				   bool intermediateNodesAreNormalNodes,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
@@ -67,6 +68,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_stopId(stopId),
 	_cameraIndices(cameraIndices),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
+	_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
@@ -99,6 +101,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int stopMapId,
 				   bool priorsIgnored,
 				   bool imuIgnored,
+				   bool intermediateNodesAreNormalNodes,
 				   const std::vector<Transform> & cameraLocalTransformOverrides) :
 	Camera(frameRate),
    _paths(databasePaths),
@@ -109,6 +112,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_stopId(stopId),
 	_cameraIndices(cameraIndices),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
+	_intermediateNodesAreNormalNodes(intermediateNodesAreNormalNodes),
 	_landmarksIgnored(landmarksIgnored),
 	_featuresIgnored(featuresIgnored),
 	_priorsIgnored(priorsIgnored),
@@ -749,7 +753,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 					data.setStereoCameraModels(combinedStereoModels);
 				}
 			}
-			data.setId(s->getWeight()==-1 ? -1 : seq);
+			data.setId(!_intermediateNodesAreNormalNodes && s->getWeight()==-1 ? -1 : seq);
 			data.setStamp(s->getStamp());
 			data.setGroundTruth(s->getGroundTruthPose());
 			if(!globalPose.isNull())

corelib/src/Memory.cpp

@@ -2413,7 +2413,9 @@ int Memory::cleanup()
 	int signatureRemoved = 0;
 
 	// bad signature
-	if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory))
+	if(_lastSignature && 
+		((_lastSignature->isBadSignature() && _badSignaturesIgnored && _lastSignature->getWeight()!=-1) || 
+		 !_incrementalMemory))
 	{
 		if(_lastSignature->isBadSignature())
 		{
@@ -2744,7 +2746,11 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 		}
 
 		// it is a bad signature (not saved), remove links!
-		if(keepLinkedToGraph && (!s->isSaved() && s->isBadSignature() && _badSignaturesIgnored))
+		if(keepLinkedToGraph && 
+		   !s->isSaved() && 
+		   s->isBadSignature() &&
+		   _badSignaturesIgnored && 
+		   s->getWeight()!=-1)
 		{
 			keepLinkedToGraph = false;
 		}
@@ -3042,6 +3048,29 @@ bool Memory::setUserData(int id, const cv::Mat & data)
 	return false;
 }
 
+void Memory::convertToIntermediate(int locationId)
+{
+	UDEBUG("Converting location %d to intermediate node", locationId);
+	Signature * location = _getSignature(locationId);
+	if(location)
+	{
+		location->setWeight(-1);
+		location->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
+		this->disableWordsRef(locationId); // won't be used for loop closure detection anymore
+		if(!_saveIntermediateNodeData)
+		{
+			location->removeAllWords();
+			location->sensorData().clearGlobalDescriptors();
+		}
+
+		location->sensorData().clearRawData();
+		if(!_saveIntermediateNodeData || !this->isBinDataKept())
+		{
+			location->sensorData().clearCompressedData();
+		}
+	}
+}
+
 void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
 {
 	UDEBUG("Deleting location %d", locationId);
@@ -4317,18 +4346,46 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				// just update weight
 				int w = oldS->getWeight()>=0?oldS->getWeight():0;
 				newS->setWeight(w + newS->getWeight() + 1);
-				oldS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
+				oldS->setWeight(0);
 
 				if(_lastGlobalLoopClosureId == oldS->id())
 				{
 					_lastGlobalLoopClosureId = newS->id();
 				}
+				if(intermediateMerge)
+				{
+					static bool warned = false;
+					if(!warned)
+					{
+						UWARN("A rehearsal was accepted (%d->%d) while not moving but "
+							"there are intermediate nodes between them in the graph. "
+							"Because %s=true, the node %d cannot be converted "
+							"into an intermediate node so it will be kept in the graph "
+							"even if we are not moving. Set %s=false to handle intermediate "
+							"nodes with rehearsal enabled so that loop closure hypotheses "
+							"are propagated correctly. This message is only "
+							"printed once.",
+							oldS->id(),
+							newS->id(),
+							Parameters::kMemRehearsalIdUpdatedToNewOne().c_str(),
+							oldS->id(),
+							Parameters::kMemRehearsalIdUpdatedToNewOne().c_str());
+						warned = true;
+					}
+				}
 			}
 			else // !_idUpdatedToNewOneRehearsal
 			{
 				int w = newS->getWeight()>=0?newS->getWeight():0;
 				oldS->setWeight(w + oldS->getWeight() + 1);
-				newS->setWeight(intermediateMerge?-1:0); // convert to intermediate node
+				if(intermediateMerge)
+				{
+					this->convertToIntermediate(newS->id());
+				}
+				else
+				{
+					newS->setWeight(0);
+				}
 			}
 		}
 	}
@@ -5584,7 +5641,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			UDEBUG("Intermediate node detected, don't extract features!");
 		}
 	}
-	else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
+	else
 	{
 		_receivingOdometryFeatures = true;
 		UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
@@ -5600,133 +5657,75 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
 		UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
 
-		int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
+		if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
-		bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
-		if((int)keypoints.size() > maxFeatures)
 		{
-			if(data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
+			int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
-				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()), ssc);
+			if((int)keypoints.size() > maxFeatures)
-			else
-				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
-		}
-		t = timer.ticks();
-		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
-		UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
-
-		if(descriptors.empty())
-		{
-			cv::Mat imageMono;
-			if(data.imageRaw().channels() == 3)
 			{
-				cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
+				bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
-			}
+				if(data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
-			else
+					_feature2D->limitKeypoints(keypoints,
-			{
+						keypoints3D,
-				imageMono = data.imageRaw();
+						descriptors,
-			}
+						maxFeatures,
-
+						data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(),
-			UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
+						data.cameraModels()[0].imageHeight()):cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(),
-			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
+						data.stereoCameraModels()[0].left().imageHeight()),
-		}
+						ssc);
-		else if(!imagesRectified && !data.cameraModels().empty())
-		{
-			std::vector<cv::KeyPoint> keypointsValid;
-			keypointsValid.reserve(keypoints.size());
-			cv::Mat descriptorsValid;
-			descriptorsValid.reserve(descriptors.rows);
-			std::vector<cv::Point3f> keypoints3DValid;
-			keypoints3DValid.reserve(keypoints3D.size());
-
-			//undistort keypoints before projection (RGB-D)
-			if(data.cameraModels().size() == 1)
-			{
-				std::vector<cv::Point2f> pointsIn, pointsOut;
-				cv::KeyPoint::convert(keypoints,pointsIn);
-				if(data.cameraModels()[0].D_raw().cols == 6)
-				{
-#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
-					// Equidistant / FishEye
-					// get only k parameters (k1,k2,p1,p2,k3,k4)
-					cv::Mat D(1, 4, CV_64FC1);
-					D.at<double>(0,0) = data.cameraModels()[0].D_raw().at<double>(0,0);
-					D.at<double>(0,1) = data.cameraModels()[0].D_raw().at<double>(0,1);
-					D.at<double>(0,2) = data.cameraModels()[0].D_raw().at<double>(0,4);
-					D.at<double>(0,3) = data.cameraModels()[0].D_raw().at<double>(0,5);
-					cv::fisheye::undistortPoints(pointsIn, pointsOut,
-							data.cameraModels()[0].K_raw(),
-							D,
-							data.cameraModels()[0].R(),
-							data.cameraModels()[0].P());
-				}
 				else
-#else
+					_feature2D->limitKeypoints(keypoints,
-					UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+						keypoints3D,
-							CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+						descriptors,
-				}
+						maxFeatures);
-#endif
-				{
-					//RadialTangential
-					cv::undistortPoints(pointsIn, pointsOut,
-							data.cameraModels()[0].K_raw(),
-							data.cameraModels()[0].D_raw(),
-							data.cameraModels()[0].R(),
-							data.cameraModels()[0].P());
-				}
-				UASSERT(pointsOut.size() == keypoints.size());
-				for(unsigned int i=0; i<pointsOut.size(); ++i)
-				{
-					if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<data.cameraModels()[0].imageWidth() &&
-					   pointsOut.at(i).y>=0 && pointsOut.at(i).y<data.cameraModels()[0].imageHeight())
-					{
-						keypointsValid.push_back(keypoints.at(i));
-						keypointsValid.back().pt.x = pointsOut.at(i).x;
-						keypointsValid.back().pt.y = pointsOut.at(i).y;
-						descriptorsValid.push_back(descriptors.row(i));
-						if(!keypoints3D.empty())
-						{
-							keypoints3DValid.push_back(keypoints3D.at(i));
-						}
-					}
-				}
 			}
-			else
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
+			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
+
+			if(descriptors.empty())
 			{
-				float subImageWidth;
+				cv::Mat imageMono;
-				if(!data.imageRaw().empty())
+				if(data.imageRaw().channels() == 3)
 				{
-					UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
+					cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
-					subImageWidth = data.imageRaw().cols/data.cameraModels().size();
 				}
 				else
 				{
-					UASSERT(data.cameraModels()[0].imageWidth()>0);
+					imageMono = data.imageRaw();
-					subImageWidth = data.cameraModels()[0].imageWidth();
 				}
 
-				for(unsigned int i=0; i<keypoints.size(); ++i)
+				UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
-				{
+				descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-					int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+			}
-					UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)data.cameraModels().size(),
+			else if(!imagesRectified && !data.cameraModels().empty())
-							uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
+			{
-									cameraIndex, (int)data.cameraModels().size(), keypoints[i].pt.x, subImageWidth, data.cameraModels()[0].imageWidth()).c_str());
+				std::vector<cv::KeyPoint> keypointsValid;
+				keypointsValid.reserve(keypoints.size());
+				cv::Mat descriptorsValid;
+				descriptorsValid.reserve(descriptors.rows);
+				std::vector<cv::Point3f> keypoints3DValid;
+				keypoints3DValid.reserve(keypoints3D.size());
 
+				//undistort keypoints before projection (RGB-D)
+				if(data.cameraModels().size() == 1)
+				{
 					std::vector<cv::Point2f> pointsIn, pointsOut;
-					pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
+					cv::KeyPoint::convert(keypoints,pointsIn);
-					if(data.cameraModels()[cameraIndex].D_raw().cols == 6)
+					if(data.cameraModels()[0].D_raw().cols == 6)
 					{
 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
 						// Equidistant / FishEye
 						// get only k parameters (k1,k2,p1,p2,k3,k4)
 						cv::Mat D(1, 4, CV_64FC1);
-						D.at<double>(0,0) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
+						D.at<double>(0,0) = data.cameraModels()[0].D_raw().at<double>(0,0);
-						D.at<double>(0,1) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+						D.at<double>(0,1) = data.cameraModels()[0].D_raw().at<double>(0,1);
-						D.at<double>(0,2) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
+						D.at<double>(0,2) = data.cameraModels()[0].D_raw().at<double>(0,4);
-						D.at<double>(0,3) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
+						D.at<double>(0,3) = data.cameraModels()[0].D_raw().at<double>(0,5);
 						cv::fisheye::undistortPoints(pointsIn, pointsOut,
-								data.cameraModels()[cameraIndex].K_raw(),
+								data.cameraModels()[0].K_raw(),
 								D,
-								data.cameraModels()[cameraIndex].R(),
+								data.cameraModels()[0].R(),
-								data.cameraModels()[cameraIndex].P());
+								data.cameraModels()[0].P());
 					}
 					else
 #else
@@ -5737,57 +5736,128 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					{
 						//RadialTangential
 						cv::undistortPoints(pointsIn, pointsOut,
-								data.cameraModels()[cameraIndex].K_raw(),
+								data.cameraModels()[0].K_raw(),
-								data.cameraModels()[cameraIndex].D_raw(),
+								data.cameraModels()[0].D_raw(),
-								data.cameraModels()[cameraIndex].R(),
+								data.cameraModels()[0].R(),
-								data.cameraModels()[cameraIndex].P());
+								data.cameraModels()[0].P());
 					}
-
+					UASSERT(pointsOut.size() == keypoints.size());
-					if(pointsOut[0].x>=0 && pointsOut[0].x<data.cameraModels()[cameraIndex].imageWidth() &&
+					for(unsigned int i=0; i<pointsOut.size(); ++i)
-					   pointsOut[0].y>=0 && pointsOut[0].y<data.cameraModels()[cameraIndex].imageHeight())
 					{
-						keypointsValid.push_back(keypoints.at(i));
+						if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<data.cameraModels()[0].imageWidth() &&
-						keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
+						pointsOut.at(i).y>=0 && pointsOut.at(i).y<data.cameraModels()[0].imageHeight())
-						keypointsValid.back().pt.y = pointsOut[0].y;
-						descriptorsValid.push_back(descriptors.row(i));
-						if(!keypoints3D.empty())
 						{
-							keypoints3DValid.push_back(keypoints3D.at(i));
+							keypointsValid.push_back(keypoints.at(i));
+							keypointsValid.back().pt.x = pointsOut.at(i).x;
+							keypointsValid.back().pt.y = pointsOut.at(i).y;
+							descriptorsValid.push_back(descriptors.row(i));
+							if(!keypoints3D.empty())
+							{
+								keypoints3DValid.push_back(keypoints3D.at(i));
+							}
 						}
 					}
 				}
+				else
+				{
+					float subImageWidth;
+					if(!data.imageRaw().empty())
+					{
+						UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
+						subImageWidth = data.imageRaw().cols/data.cameraModels().size();
+					}
+					else
+					{
+						UASSERT(data.cameraModels()[0].imageWidth()>0);
+						subImageWidth = data.cameraModels()[0].imageWidth();
+					}
+
+					for(unsigned int i=0; i<keypoints.size(); ++i)
+					{
+						int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+						UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)data.cameraModels().size(),
+								uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
+										cameraIndex, (int)data.cameraModels().size(), keypoints[i].pt.x, subImageWidth, data.cameraModels()[0].imageWidth()).c_str());
+
+						std::vector<cv::Point2f> pointsIn, pointsOut;
+						pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
+						if(data.cameraModels()[cameraIndex].D_raw().cols == 6)
+						{
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+							// Equidistant / FishEye
+							// get only k parameters (k1,k2,p1,p2,k3,k4)
+							cv::Mat D(1, 4, CV_64FC1);
+							D.at<double>(0,0) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
+							D.at<double>(0,1) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+							D.at<double>(0,2) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
+							D.at<double>(0,3) = data.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
+							cv::fisheye::undistortPoints(pointsIn, pointsOut,
+									data.cameraModels()[cameraIndex].K_raw(),
+									D,
+									data.cameraModels()[cameraIndex].R(),
+									data.cameraModels()[cameraIndex].P());
+						}
+						else
+#else
+							UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+									CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+						}
+#endif
+						{
+							//RadialTangential
+							cv::undistortPoints(pointsIn, pointsOut,
+									data.cameraModels()[cameraIndex].K_raw(),
+									data.cameraModels()[cameraIndex].D_raw(),
+									data.cameraModels()[cameraIndex].R(),
+									data.cameraModels()[cameraIndex].P());
+						}
+
+						if(pointsOut[0].x>=0 && pointsOut[0].x<data.cameraModels()[cameraIndex].imageWidth() &&
+						pointsOut[0].y>=0 && pointsOut[0].y<data.cameraModels()[cameraIndex].imageHeight())
+						{
+							keypointsValid.push_back(keypoints.at(i));
+							keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
+							keypointsValid.back().pt.y = pointsOut[0].y;
+							descriptorsValid.push_back(descriptors.row(i));
+							if(!keypoints3D.empty())
+							{
+								keypoints3DValid.push_back(keypoints3D.at(i));
+							}
+						}
+					}
+				}
+
+				keypoints = keypointsValid;
+				descriptors = descriptorsValid;
+				keypoints3D = keypoints3DValid;
+
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
+				UDEBUG("time rectification = %fs", t);
 			}
-
-			keypoints = keypointsValid;
-			descriptors = descriptorsValid;
-			keypoints3D = keypoints3DValid;
-
 			t = timer.ticks();
-			if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
+			if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
-			UDEBUG("time rectification = %fs", t);
+			UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
-		}
-		t = timer.ticks();
-		if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
-		UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 
-		if(keypoints3D.empty() &&
+			if(keypoints3D.empty() &&
-			((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
+				((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
-		   (!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection())))
+			(!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection())))
-		{
+			{
-			keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
+				keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
-		}
+			}
-		if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
+			if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
-		{
+			{
-			_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
+				_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
-		}
+			}
-		t = timer.ticks();
+			t = timer.ticks();
-		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-		UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
 
-		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
+			UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
-		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
+			if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
-		{
+			{
-			descriptors = cv::Mat();
+				descriptors = cv::Mat();
+			}
 		}
 	}
 
@@ -5810,107 +5880,117 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	}
 
 	std::list<int> wordIds;
-	if(descriptors.rows)
+	bool addedToDictionary = false;
+	if(!keypoints.empty())
 	{
-		// In case the number of features we want to do quantization is lower
+		if(descriptors.rows && !isIntermediateNode)
-		// than extracted ones (that would be used for transform estimation)
-		std::vector<bool> inliers;
-		cv::Mat descriptorsForQuantization = descriptors;
-		std::vector<int> quantizedToRawIndices;
-		if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
 		{
-			UASSERT((int)keypoints.size() == descriptors.rows);
+			// In case the number of features we want to do quantization is lower
-			int inliersCount = 0;
+			// than extracted ones (that would be used for transform estimation)
-			if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) &&
+			std::vector<bool> inliers;
-				(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
+			cv::Mat descriptorsForQuantization = descriptors;
-					data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
+			std::vector<int> quantizedToRawIndices;
+			if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
 			{
-				Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
+				UASSERT((int)keypoints.size() == descriptors.rows);
-					decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
+				int inliersCount = 0;
-					decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
+				if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) &&
-					data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
+					(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
-					_feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC());
+						data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
-			}
-			else
-			{
-				if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1)
 				{
-					UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
+					Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
-							Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
+						decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
-				}
+						decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
-				if(decimatedData.cameraModels().size()>=1 || decimatedData.stereoCameraModels().size()>=1 ||
+						data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
-					data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
+						_feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC());
-				{
-					Feature2D::limitKeypoints(
-						keypoints,
-						inliers,
-						_feature2D->getMaxFeatures(),
-						decimatedData.cameraModels().size()?cv::Size(decimatedData.cameraModels()[0].imageWidth()*decimatedData.cameraModels().size(), decimatedData.cameraModels()[0].imageHeight()):
-						decimatedData.stereoCameraModels().size()?cv::Size(decimatedData.stereoCameraModels()[0].left().imageWidth()*decimatedData.stereoCameraModels().size(), decimatedData.stereoCameraModels()[0].left().imageWidth()):
-						data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):
-						cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()),
-						_feature2D->getSSC());
 				}
 				else
 				{
-					Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
+					if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1)
-				}
-			}
-			for(size_t i=0; i<inliers.size(); ++i)
-			{
-				if(inliers[i])
-					++inliersCount;
-			}
-
-			descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
-			quantizedToRawIndices.resize(inliersCount);
-			unsigned int oi=0;
-			UASSERT((int)inliers.size() == descriptors.rows);
-			for(int k=0; k < descriptors.rows; ++k)
-			{
-				if(inliers[k])
-				{
-					UASSERT(oi < quantizedToRawIndices.size());
-					if(descriptors.type() == CV_32FC1)
 					{
-						memcpy(descriptorsForQuantization.ptr<float>(oi), descriptors.ptr<float>(k), descriptors.cols*sizeof(float));
+						UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
+								Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
+					}
+					if(decimatedData.cameraModels().size()>=1 || decimatedData.stereoCameraModels().size()>=1 ||
+						data.cameraModels().size()>=1 || data.stereoCameraModels().size()>=1)
+					{
+						Feature2D::limitKeypoints(
+							keypoints,
+							inliers,
+							_feature2D->getMaxFeatures(),
+							decimatedData.cameraModels().size()?cv::Size(decimatedData.cameraModels()[0].imageWidth()*decimatedData.cameraModels().size(), decimatedData.cameraModels()[0].imageHeight()):
+							decimatedData.stereoCameraModels().size()?cv::Size(decimatedData.stereoCameraModels()[0].left().imageWidth()*decimatedData.stereoCameraModels().size(), decimatedData.stereoCameraModels()[0].left().imageWidth()):
+							data.cameraModels().size()?cv::Size(data.cameraModels()[0].imageWidth()*data.cameraModels().size(), data.cameraModels()[0].imageHeight()):
+							cv::Size(data.stereoCameraModels()[0].left().imageWidth()*data.stereoCameraModels().size(), data.stereoCameraModels()[0].left().imageHeight()),
+							_feature2D->getSSC());
 					}
 					else
 					{
-						memcpy(descriptorsForQuantization.ptr<char>(oi), descriptors.ptr<char>(k), descriptors.cols*sizeof(char));
+						Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
 					}
-					quantizedToRawIndices[oi] = k;
-					++oi;
 				}
-			}
+				for(size_t i=0; i<inliers.size(); ++i)
-			UASSERT_MSG((int)oi == inliersCount,
-					uFormat("oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)",
-							oi, inliersCount, _feature2D->getMaxFeatures(), _feature2D->getGridCols(), _feature2D->getGridRows()).c_str());
-		}
-
-		// Quantization to vocabulary
-		wordIds = _vwd->addNewWords(descriptorsForQuantization, id);
-
-		// Set ID -1 to features not used for quantization
-		if(wordIds.size() < keypoints.size())
-		{
-			std::vector<int> allWordIds;
-			allWordIds.resize(keypoints.size(),-1);
-			int i=0;
-			for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
-			{
-				allWordIds[quantizedToRawIndices[i]] = *iter;
-				++i;
-			}
-			int negIndex = -1;
-			for(i=0; i<(int)allWordIds.size(); ++i)
-			{
-				if(allWordIds[i] < 0)
 				{
-					allWordIds[i] = negIndex--;
+					if(inliers[i])
+						++inliersCount;
 				}
+
+				descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
+				quantizedToRawIndices.resize(inliersCount);
+				unsigned int oi=0;
+				UASSERT((int)inliers.size() == descriptors.rows);
+				for(int k=0; k < descriptors.rows; ++k)
+				{
+					if(inliers[k])
+					{
+						UASSERT(oi < quantizedToRawIndices.size());
+						if(descriptors.type() == CV_32FC1)
+						{
+							memcpy(descriptorsForQuantization.ptr<float>(oi), descriptors.ptr<float>(k), descriptors.cols*sizeof(float));
+						}
+						else
+						{
+							memcpy(descriptorsForQuantization.ptr<char>(oi), descriptors.ptr<char>(k), descriptors.cols*sizeof(char));
+						}
+						quantizedToRawIndices[oi] = k;
+						++oi;
+					}
+				}
+				UASSERT_MSG((int)oi == inliersCount,
+						uFormat("oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)",
+								oi, inliersCount, _feature2D->getMaxFeatures(), _feature2D->getGridCols(), _feature2D->getGridRows()).c_str());
 			}
-			wordIds = uVectorToList(allWordIds);
+
+			// Quantization to vocabulary
+			wordIds = _vwd->addNewWords(descriptorsForQuantization, id);
+			addedToDictionary = true;
+
+			// Set ID -1 to features not used for quantization
+			if(wordIds.size() < keypoints.size())
+			{
+				std::vector<int> allWordIds;
+				allWordIds.resize(keypoints.size(),-1);
+				int i=0;
+				for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
+				{
+					allWordIds[quantizedToRawIndices[i]] = *iter;
+					++i;
+				}
+				int negIndex = -1;
+				for(i=0; i<(int)allWordIds.size(); ++i)
+				{
+					if(allWordIds[i] < 0)
+					{
+						allWordIds[i] = negIndex--;
+					}
+				}
+				wordIds = uVectorToList(allWordIds);
+			}
+		}
+		else
+		{
+			// Set all words as not used in dictionary
+			wordIds.resize(keypoints.size(),-1);
 		}
 
 		t = timer.ticks();
@@ -5930,6 +6010,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	if(wordIds.size() > 0)
 	{
 		UASSERT(wordIds.size() == keypoints.size());
+		UASSERT(descriptors.rows == 0 || descriptors.rows == (int)wordIds.size());
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
 		float decimationRatio = float(preDecimation) / float(_imagePostDecimation);
@@ -5937,7 +6018,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
 			cv::KeyPoint kpt = keypoints[i];
-			if(preDecimation != _imagePostDecimation)
+			if(preDecimation != _imagePostDecimation && !isIntermediateNode)
 			{
 				// remap keypoints to final image size
 				kpt.pt.x *= decimationRatio;
@@ -5956,7 +6037,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					++words3DValid;
 				}
 			}
-			if(_rawDescriptorsKept)
+			if(!descriptors.empty() && _rawDescriptorsKept)
 			{
 				wordsDescriptors.push_back(descriptors.row(i));
 			}
@@ -5964,12 +6045,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	}
 
 	Landmarks landmarks = data.landmarks();
-	if(!landmarks.empty() && isIntermediateNode)
+	if(_detectMarkers && !isIntermediateNode && !data.imageRaw().empty())
-	{
-		UDEBUG("Landmarks provided (size=%ld) are ignored because this signature is set as intermediate.", landmarks.size());
-		landmarks.clear();
-	}
-	else if(_detectMarkers && !isIntermediateNode && !data.imageRaw().empty())
 	{
 		UDEBUG("Detecting markers...");
 		if(landmarks.empty())
@@ -6101,7 +6177,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UDEBUG("time post-decimation = %fs", t);
 	}
 
-	if(_stereoFromMotion &&
+	if(!isIntermediateNode &&
+		_stereoFromMotion &&
 		!pose.isNull() &&
 		cameraModels.size() == 1 &&
 		words.size() &&
@@ -6514,9 +6591,15 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						compressedUserData));
 	}
 
-	s->setWords(words, wordsKpts,
+	if(!isIntermediateNode || _saveIntermediateNodeData)
-			_reextractLoopClosureFeatures?std::vector<cv::Point3f>():words3D,
+	{
-			_reextractLoopClosureFeatures?cv::Mat():wordsDescriptors);
+		s->setWords(words, wordsKpts,
+				_reextractLoopClosureFeatures?std::vector<cv::Point3f>():words3D,
+				_reextractLoopClosureFeatures?cv::Mat():wordsDescriptors);
+
+		s->sensorData().setLaserScan(laserScan, false);
+		s->sensorData().setUserData(data.userDataRaw(), false);
+	}
 
 	// set raw data
 	if(!cameraModels.empty())
@@ -6527,8 +6610,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	{
 		s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModels, false);
 	}
-	s->sensorData().setLaserScan(laserScan, false);
-	s->sensorData().setUserData(data.userDataRaw(), false);
 
 	UDEBUG("data.groundTruth()      =%s", data.groundTruth().prettyPrint().c_str());
 	UDEBUG("data.gps()              =%s", data.gps().stamp()?"true":"false");
@@ -6538,28 +6619,33 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	s->sensorData().setGPS(data.gps());
 	s->sensorData().setEnvSensors(data.envSensors());
 
-	if(!isIntermediateNode)
+	std::vector<GlobalDescriptor> globalDescriptors = data.globalDescriptors();
+	if(!isIntermediateNode && _globalDescriptorExtractor)
 	{
-		std::vector<GlobalDescriptor> globalDescriptors = data.globalDescriptors();
+		GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData);
-		if(_globalDescriptorExtractor)
+		if(!gdescriptor.data().empty())
 		{
-			GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData);
+			globalDescriptors.push_back(gdescriptor);
-			if(!gdescriptor.data().empty())
-			{
-				globalDescriptors.push_back(gdescriptor);
-			}
 		}
-		s->sensorData().setGlobalDescriptors(globalDescriptors);
 	}
-	else if(!data.globalDescriptors().empty())
+	if(!globalDescriptors.empty())
 	{
-		UDEBUG("Global descriptors provided (size=%ld) are ignored because this signature is set as intermediate.", data.globalDescriptors().size());
+		if(!isIntermediateNode || _saveIntermediateNodeData)
+		{
+			s->sensorData().setGlobalDescriptors(globalDescriptors);
+		}
+		else
+		{
+			UDEBUG("Global descriptors provided (size=%ld) are ignored because this signature is set as intermediate and %s=false.",
+				globalDescriptors.size(),
+				Parameters::kMemIntermediateNodeDataKept().c_str());
+		}
 	}
 
 	t = timer.ticks();
 	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
 	UDEBUG("time compressing data (id=%d) %fs", id, t);
-	if(words.size())
+	if(words.size() && addedToDictionary)
 	{
 		s->setEnabled(true); // All references are already activated in the dictionary at this point (see _vwd->addNewWords())
 	}
@@ -6699,16 +6785,19 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			s->addLandmark(landmark);
 
 			// Update landmark index
-			std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
+			if(!isIntermediateNode)
-			if(nter!=_landmarksIndex.end())
 			{
-				nter->second.insert(s->id());
+				std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
-			}
+				if(nter!=_landmarksIndex.end())
-			else
+				{
-			{
+					nter->second.insert(s->id());
-				std::set<int> tmp;
+				}
-				tmp.insert(s->id());
+				else
-				_landmarksIndex.insert(std::make_pair(landmarkId, tmp));
+				{
+					std::set<int> tmp;
+					tmp.insert(s->id());
+					_landmarksIndex.insert(std::make_pair(landmarkId, tmp));
+				}
 			}
 		}
 		else

corelib/src/Rtabmap.cpp

@@ -1518,31 +1518,48 @@ bool Rtabmap::process(
 		}
 		else
 		{
+			bool linkedToIntermediateNode = false;
+			Transform t;
+			if(_memory->isIncremental())
+			{
+				// Check small motion if current node is not an intermediate node already
+				if(signature->getWeight() >= 0)
+				{
+					// It should contain only the query and its first (non-intermediate) neighbor (smaller id)
+					std::map<int, int> neighbors = _memory->getNeighborsId(signature->id(), 2, 0, true, true, true, true);
+					if(neighbors.size() == 2)
+					{
+						int nid = neighbors.begin()->first;
+						const std::multimap<int, Link> & links = signature->getLinks();
+						if(links.find(nid) != links.end())
+						{
+							// direct neighbor
+							t = links.find(nid)->second.transform();
+						}
+						else
+						{
+							// Use optimized poses to check how far it is from the latest non-intermediate node
+							std::map<int, Transform>::iterator niter = _optimizedPoses.find(nid);
+							if(niter != _optimizedPoses.end())
+							{
+								t = niter->second.inverse() * _mapCorrection * signature->getPose();
+							}
+							// not direct link, it means there are intermediate nodes
+							linkedToIntermediateNode = true; 
+						}
+					}
+				}
+			}
+			else if(!_odomCachePoses.empty())
+			{
+				t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
+			}
+			
 			if(_rgbdLinearUpdate > 0.0f || _rgbdAngularUpdate > 0.0f)
 			{
 				//============================================================
 				// Minimum displacement required to add to Memory
 				//============================================================
-				Transform t;
-
-				if(_memory->isIncremental())
-				{
-					const std::multimap<int, Link> & links = signature->getLinks();
-					if(links.size() && links.begin()->second.type() == Link::kNeighbor)
-					{
-						const Signature * s = _memory->getSignature(links.begin()->second.to());
-						UASSERT(s!=0);
-						// Check small motion if previous node is not an intermediate node (consecutive intermediate nodes are merged later)
-						if(s->getWeight() >= 0)
-						{
-							t = links.begin()->second.transform();
-						}
-					}
-				}
-				else if(!_odomCachePoses.empty())
-				{
-					t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
-				}
 				if(!t.isNull())
 				{
 					float x,y,z, roll,pitch,yaw;
@@ -1564,7 +1581,7 @@ bool Rtabmap::process(
 					}
 				}
 			}
-			if(odomVelocity.size() == 6)
+			if(odomVelocity.size() == 6 && signature->getWeight() != -1)
 			{
 				// This will disable global loop closure detection, only retrieval will be done.
 				// The location will also be deleted at the end.
@@ -1572,6 +1589,10 @@ bool Rtabmap::process(
 						(_rgbdLinearSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[0]), fabs(odomVelocity[1]), fabs(odomVelocity[2])) > _rgbdLinearSpeedUpdate) ||
 						(_rgbdAngularSpeedUpdate>0.0f && uMax3(fabs(odomVelocity[3]), fabs(odomVelocity[4]), fabs(odomVelocity[5])) > _rgbdAngularSpeedUpdate);
 			}
+			if(linkedToIntermediateNode && (smallDisplacement || tooFastMovement))
+			{
+				_memory->convertToIntermediate(signature->id());
+			}
 		}
 
 		// Update optimizedPoses with the newly added node
@@ -2207,7 +2228,7 @@ bool Rtabmap::process(
 			}
 		} // if(_memory->getWorkingMemSize())
 	}// !isBadSignature
-	else if(!signature->isBadSignature() && (smallDisplacement || tooFastMovement))
+	else if(!signature->isBadSignature() && signature->getWeight()>=0 && (smallDisplacement || tooFastMovement))
 	{
 		_highestHypothesis = lastHighestHypothesis;
 		UDEBUG("smallDisplacement=%d tooFastMovement=%d", smallDisplacement?1:0, tooFastMovement?1:0);
@@ -3166,7 +3187,7 @@ bool Rtabmap::process(
 	// Landmark
 	//============================================================
 	std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
-	if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored())
+	if(!signature->getLandmarks().empty() && !_graphOptimizer->landmarksIgnored() && signature->getWeight()!=-1)
 	{
 		bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0;
 		UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
@@ -4446,7 +4467,8 @@ bool Rtabmap::process(
 			signaturesRemoved.push_back(signature->id());
 			_memory->deleteLocation(signature->id());
 		}
-		else if((smallDisplacement || tooFastMovement) &&
+		else if((!_memory->isIncremental() || signature->getWeight()>=0) &&
+				(smallDisplacement || tooFastMovement) &&
 				_loopClosureHypothesis.first == 0 &&
 				lastProximitySpaceClosureId == 0 &&
 				(rejectedLoopClosure || landmarksDetected.empty()) &&

corelib/src/RtabmapThread.cpp

@@ -556,7 +556,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			// set negative id so rtabmap will detect it as an intermediate node
 			SensorData tmp = odomEvent.data();
 			tmp.setId(-1);
-			tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
 			_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), odomInfo));
 		}
 		else

corelib/src/SensorData.cpp

@@ -976,7 +976,7 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			(_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
 }
 
-void SensorData::clearCompressedData(bool images, bool scan, bool userData)
+void SensorData::clearCompressedData(bool images, bool scan, bool userData, bool occupancyGrid)
 {
 	if(images)
 	{
@@ -992,14 +992,24 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
 	{
 		_userDataCompressed=cv::Mat();
 	}
+	if(occupancyGrid)
+	{
+		_groundCellsCompressed=cv::Mat();
+		_emptyCellsCompressed=cv::Mat();
+		_obstacleCellsCompressed=cv::Mat();
+	}
 }
-void SensorData::clearRawData(bool images, bool scan, bool userData)
+void SensorData::clearRawData(bool images, bool scan, bool userData, bool occupancyGrid)
 {
 	if(images)
 	{
 		_imageRaw=cv::Mat();
 		_depthOrRightRaw=cv::Mat();
 		_depthConfidenceRaw=cv::Mat();
+#ifdef HAVE_OPENCV_CUDEV
+		_imageRawGpu = cv::cuda::GpuMat();
+		_depthOrRightRawGpu = cv::cuda::GpuMat();
+#endif
 	}
 	if(scan)
 	{
@@ -1009,6 +1019,12 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
 	{
 		_userDataRaw=cv::Mat();
 	}
+	if(occupancyGrid)
+	{
+		_groundCellsRaw=cv::Mat();
+		_emptyCellsRaw=cv::Mat();
+		_obstacleCellsRaw=cv::Mat();
+	}
 }
 
 

guilib/src/MainWindow.cpp

@@ -2107,6 +2107,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		}
 
 		// For intermediate empty nodes, keep latest image shown
+		bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
 		if(signature.getWeight() >= 0)
 		{
 			_ui->imageView_source->clear();
@@ -2131,7 +2132,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			_ui->label_matchId->clear();
 
-			bool rehearsedSimilarity = (float)uValue(stat.data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0f;
 			int proximityTimeDetections = (int)uValue(stat.data(), Statistics::kProximityTime_detections(), 0.0f);
 			bool scanMatchingSuccess = (bool)uValue(stat.data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
 			_ui->label_stats_imageNumber->setText(QString("%1 [%2]").arg(stat.refImageId()).arg(refMapId));
@@ -2455,6 +2455,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				UDEBUG("time= %d ms (update loop closure viewer)", time.restart());
 			}
 		}
+		else if(rehearsedSimilarity)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::darkBlue);
+		}
+		else if(smallMovement)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::gray);
+		}
+		else if(fastMovement)
+		{
+			_ui->imageView_source->setBackgroundColor(Qt::magenta);
+		}
 
 		// PDF AND LIKELIHOOD
 		if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
@@ -2712,7 +2724,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			Signature & s = *_cachedSignatures.find(stat.refImageId());
 			_cachedMemoryUsage -= s.sensorData().getMemoryUsed();
 			s.sensorData().clearRawData();
-			s.sensorData().clearOccupancyGridRaw();
 			_cachedMemoryUsage += s.sensorData().getMemoryUsed();
 		}
 

guilib/src/PreferencesDialog.cpp

@@ -761,6 +761,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_checkBox_ignoreFeatures, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignorePriors, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_ignoreIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_intermediateNodesAreNormalNodes, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStartId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -2229,6 +2230,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->source_checkBox_ignoreFeatures->setChecked(true);
 		_ui->source_checkBox_ignorePriors->setChecked(false);
 		_ui->source_checkBox_ignoreIMU->setChecked(false);
+		_ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(false);
 		_ui->source_spinBox_databaseStartId->setValue(0);
 		_ui->source_spinBox_databaseStopId->setValue(0);
 		_ui->source_lineEdit_databaseCameraIndex->setText("");
@@ -2991,6 +2993,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->source_checkBox_ignoreFeatures->setChecked(settings.value("ignoreFeatures", _ui->source_checkBox_ignoreFeatures->isChecked()).toBool());
 	_ui->source_checkBox_ignorePriors->setChecked(settings.value("ignorePriors", _ui->source_checkBox_ignorePriors->isChecked()).toBool());
 	_ui->source_checkBox_ignoreIMU->setChecked(settings.value("ignoreImu", _ui->source_checkBox_ignoreIMU->isChecked()).toBool());
+	_ui->source_checkBox_intermediateNodesAreNormalNodes->setChecked(settings.value("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked()).toBool());
 	
 	_ui->source_spinBox_databaseStartId->setValue(settings.value("startId", _ui->source_spinBox_databaseStartId->value()).toInt());
 	_ui->source_spinBox_databaseStopId->setValue(settings.value("stopId", _ui->source_spinBox_databaseStopId->value()).toInt());
@@ -3611,6 +3614,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("ignoreFeatures",  _ui->source_checkBox_ignoreFeatures->isChecked());
 	settings.setValue("ignorePriors",  _ui->source_checkBox_ignorePriors->isChecked());
 	settings.setValue("ignoreImu",  _ui->source_checkBox_ignoreIMU->isChecked());
+	settings.setValue("intermediateNodesAreNormalNodes", _ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked());
 	settings.setValue("startId",          _ui->source_spinBox_databaseStartId->value());
 	settings.setValue("stopId",          _ui->source_spinBox_databaseStopId->value());
 	settings.setValue("cameraIndices",       _ui->source_lineEdit_databaseCameraIndex->text());
@@ -7303,13 +7307,14 @@ Camera * PreferencesDialog::createCamera(
 				_ui->source_spinBox_databaseStartId->value(),
 				cameraIndices,
 				_ui->source_spinBox_databaseStopId->value(),
-				!_ui->general_checkBox_createIntermediateNodes->isChecked(),
+				!_ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked() && !_ui->general_checkBox_createIntermediateNodes->isChecked(),
 				_ui->source_checkBox_ignoreLandmarks->isChecked(),
 				_ui->source_checkBox_ignoreFeatures->isChecked(),
 				0,
 				-1,
 				_ui->source_checkBox_ignorePriors->isChecked(),
 				_ui->source_checkBox_ignoreIMU->isChecked(),
+				_ui->source_checkBox_intermediateNodesAreNormalNodes->isChecked(),
 				localTransformOverrides);
 	}
 	else

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-700</y>
             <width>684</width>
             <height>5218</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>14</number>
+                <number>8</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -3469,7 +3469,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <item>
                         <widget class="QStackedWidget" name="stackedWidget_src">
                          <property name="currentIndex">
-                          <number>0</number>
+                          <number>3</number>
                          </property>
                          <widget class="QWidget" name="page_41">
                           <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -7180,37 +7180,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               <bool>false</bool>
                              </property>
                              <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1,0">
-                              <item row="10" column="0">
+                              <item row="8" column="1">
-                               <widget class="QSpinBox" name="source_spinBox_databaseStopId">
+                               <widget class="QLabel" name="label_786">
-                                <property name="minimum">
-                                 <number>0</number>
-                                </property>
-                                <property name="maximum">
-                                 <number>99999</number>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="0" column="1">
-                               <widget class="QLineEdit" name="source_database_lineEdit_path"/>
-                              </item>
-                              <item row="2" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_useDbStamps">
                                 <property name="text">
-                                 <string/>
+                                 <string>Ignore IMU (i.e., gravity links).</string>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="3" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_ignoreGoals">
-                                <property name="text">
-                                 <string/>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="6" column="1">
-                               <widget class="QLabel" name="label_644">
-                                <property name="text">
-                                 <string>Ignore features.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -7220,6 +7193,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                 </property>
                                </widget>
                               </item>
+                              <item row="4" column="0">
+                               <widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
                               <item row="0" column="0">
                                <widget class="QToolButton" name="source_database_toolButton_selectSource">
                                 <property name="text">
@@ -7227,123 +7207,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                 </property>
                                </widget>
                               </item>
-                              <item row="12" column="1">
+                              <item row="10" column="1">
-                               <widget class="QLabel" name="label_784">
+                               <widget class="QLabel" name="label_58">
                                 <property name="text">
-                                 <string>Override camera local transform(s) with local transform(s) set above. For multi-cameras, use a &quot;;&quot; between each transform.</string>
+                                 <string>Start position (node ID).</string>
-                                </property>
-                                <property name="wordWrap">
-                                 <bool>true</bool>
                                 </property>
                                 <property name="textInteractionFlags">
                                  <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                                 </property>
                                </widget>
                               </item>
-                              <item row="1" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
-                                <property name="text">
-                                 <string/>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="11" column="0">
-                               <widget class="QLineEdit" name="source_lineEdit_databaseCameraIndex"/>
-                              </item>
-                              <item row="4" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_ignoreGoalDelay">
-                                <property name="text">
-                                 <string/>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="13" column="0">
-                               <widget class="QLineEdit" name="source_lineEdit_databaseLocalTransformOffset"/>
-                              </item>
-                              <item row="0" column="2">
-                               <widget class="QToolButton" name="toolButton_dbViewer">
-                                <property name="toolTip">
-                                 <string>Open database viewer</string>
-                                </property>
-                                <property name="text">
-                                 <string/>
-                                </property>
-                                <property name="icon">
-                                 <iconset resource="../GuiLib.qrc">
-                                  <normaloff>:/images/mag_glass.png</normaloff>:/images/mag_glass.png</iconset>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="12" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_overrideLocalTransforms">
-                                <property name="text">
-                                 <string/>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="6" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_ignoreFeatures">
-                                <property name="text">
-                                 <string/>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="1" column="1">
-                               <widget class="QLabel" name="label_72">
-                                <property name="text">
-                                 <string>Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed.</string>
-                                </property>
-                                <property name="wordWrap">
-                                 <bool>true</bool>
-                                </property>
-                                <property name="textInteractionFlags">
-                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="11" column="1">
-                               <widget class="QLabel" name="label_315">
-                                <property name="text">
-                                 <string>Camera index. If the database contains multi-camera data, you can choose which camera to use. Leave empty to use all cameras. Can also be multiple indices split by spaces in a string like &quot;0 2&quot; to stream cameras 0 and 2 only.</string>
-                                </property>
-                                <property name="wordWrap">
-                                 <bool>true</bool>
-                                </property>
-                                <property name="textInteractionFlags">
-                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="7" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_ignorePriors">
-                                <property name="text">
-                                 <string/>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="13" column="1">
-                               <widget class="QLabel" name="label_785">
-                                <property name="text">
-                                 <string>Add an y-axis offset before optical rotation on the overriden local transform(s). For multi-cameras,explicitly enumerate offsets if they are different (e.g., &quot;0.05 0.075&quot; for two cameras setup), or set single number to apply to all camera transforms.</string>
-                                </property>
-                                <property name="wordWrap">
-                                 <bool>true</bool>
-                                </property>
-                                <property name="textInteractionFlags">
-                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="9" column="0">
-                               <widget class="QSpinBox" name="source_spinBox_databaseStartId">
-                                <property name="minimum">
-                                 <number>0</number>
-                                </property>
-                                <property name="maximum">
-                                 <number>99999</number>
-                                </property>
-                               </widget>
-                              </item>
                               <item row="2" column="1">
                                <widget class="QLabel" name="label_90">
                                 <property name="text">
@@ -7357,25 +7230,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                 </property>
                                </widget>
                               </item>
-                              <item row="5" column="0">
+                              <item row="8" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_ignoreLandmarks">
+                               <widget class="QCheckBox" name="source_checkBox_ignoreIMU">
-                                <property name="text">
-                                 <string/>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="9" column="1">
-                               <widget class="QLabel" name="label_58">
-                                <property name="text">
-                                 <string>Start position (node ID).</string>
-                                </property>
-                                <property name="textInteractionFlags">
-                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                </property>
-                               </widget>
-                              </item>
-                              <item row="14" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_stereoToDepthDB">
                                 <property name="text">
                                  <string/>
                                 </property>
@@ -7394,6 +7250,139 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                 </property>
                                </widget>
                               </item>
+                              <item row="0" column="1">
+                               <widget class="QLineEdit" name="source_database_lineEdit_path"/>
+                              </item>
+                              <item row="15" column="0">
+                               <widget class="QCheckBox" name="source_checkBox_stereoToDepthDB">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="6" column="1">
+                               <widget class="QLabel" name="label_644">
+                                <property name="text">
+                                 <string>Ignore features.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="6" column="0">
+                               <widget class="QCheckBox" name="source_checkBox_ignoreFeatures">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="12" column="1">
+                               <widget class="QLabel" name="label_315">
+                                <property name="text">
+                                 <string>Camera index. If the database contains multi-camera data, you can choose which camera to use. Leave empty to use all cameras. Can also be multiple indices split by spaces in a string like &quot;0 2&quot; to stream cameras 0 and 2 only.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="5" column="0">
+                               <widget class="QCheckBox" name="source_checkBox_ignoreLandmarks">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="0" column="2">
+                               <widget class="QToolButton" name="toolButton_dbViewer">
+                                <property name="toolTip">
+                                 <string>Open database viewer</string>
+                                </property>
+                                <property name="text">
+                                 <string/>
+                                </property>
+                                <property name="icon">
+                                 <iconset resource="../GuiLib.qrc">
+                                  <normaloff>:/images/mag_glass.png</normaloff>:/images/mag_glass.png</iconset>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="3" column="1">
+                               <widget class="QLabel" name="label_263">
+                                <property name="text">
+                                 <string>Ignore goals saved in the database.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="9" column="1">
+                               <widget class="QLabel" name="label_797">
+                                <property name="text">
+                                 <string>Publish intermediate nodes as normal nodes.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="15" column="1">
+                               <widget class="QLabel" name="label_557">
+                                <property name="text">
+                                 <string>If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="11" column="0">
+                               <widget class="QSpinBox" name="source_spinBox_databaseStopId">
+                                <property name="minimum">
+                                 <number>0</number>
+                                </property>
+                                <property name="maximum">
+                                 <number>99999</number>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="11" column="1">
+                               <widget class="QLabel" name="label_529">
+                                <property name="text">
+                                 <string>Stop position (node ID) is the last node to process. If 0, all nodes after start position are published.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="1" column="0">
+                               <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
                               <item row="5" column="1">
                                <widget class="QLabel" name="label_546">
                                 <property name="text">
@@ -7407,6 +7396,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                 </property>
                                </widget>
                               </item>
+                              <item row="14" column="0">
+                               <widget class="QLineEdit" name="source_lineEdit_databaseLocalTransformOffset"/>
+                              </item>
+                              <item row="10" column="0">
+                               <widget class="QSpinBox" name="source_spinBox_databaseStartId">
+                                <property name="minimum">
+                                 <number>0</number>
+                                </property>
+                                <property name="maximum">
+                                 <number>99999</number>
+                                </property>
+                               </widget>
+                              </item>
                               <item row="4" column="1">
                                <widget class="QLabel" name="label_80">
                                 <property name="text">
@@ -7421,9 +7423,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                </widget>
                               </item>
                               <item row="14" column="1">
-                               <widget class="QLabel" name="label_557">
+                               <widget class="QLabel" name="label_785">
                                 <property name="text">
-                                 <string>If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab.</string>
+                                 <string>Add an y-axis offset before optical rotation on the overriden local transform(s). For multi-cameras,explicitly enumerate offsets if they are different (e.g., &quot;0.05 0.075&quot; for two cameras setup), or set single number to apply to all camera transforms.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -7433,10 +7435,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                 </property>
                                </widget>
                               </item>
-                              <item row="10" column="1">
+                              <item row="13" column="1">
-                               <widget class="QLabel" name="label_529">
+                               <widget class="QLabel" name="label_784">
                                 <property name="text">
-                                 <string>Stop position (node ID) is the last node to process. If 0, all nodes after start position are published.</string>
+                                 <string>Override camera local transform(s) with local transform(s) set above. For multi-cameras, use a &quot;;&quot; between each transform.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -7446,10 +7448,34 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                 </property>
                                </widget>
                               </item>
-                              <item row="3" column="1">
+                              <item row="13" column="0">
-                               <widget class="QLabel" name="label_263">
+                               <widget class="QCheckBox" name="source_checkBox_overrideLocalTransforms">
                                 <property name="text">
-                                 <string>Ignore goals saved in the database.</string>
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="9" column="0">
+                               <widget class="QCheckBox" name="source_checkBox_intermediateNodesAreNormalNodes">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="7" column="0">
+                               <widget class="QCheckBox" name="source_checkBox_ignorePriors">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="12" column="0">
+                               <widget class="QLineEdit" name="source_lineEdit_databaseCameraIndex"/>
+                              </item>
+                              <item row="1" column="1">
+                               <widget class="QLabel" name="label_72">
+                                <property name="text">
+                                 <string>Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed.</string>
                                 </property>
                                 <property name="wordWrap">
                                  <bool>true</bool>
@@ -7459,21 +7485,15 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                 </property>
                                </widget>
                               </item>
-                              <item row="8" column="1">
+                              <item row="3" column="0">
-                               <widget class="QLabel" name="label_786">
+                               <widget class="QCheckBox" name="source_checkBox_ignoreGoals">
                                 <property name="text">
-                                 <string>Ignore IMU (i.e., gravity links).</string>
+                                 <string/>
-                                </property>
-                                <property name="wordWrap">
-                                 <bool>true</bool>
-                                </property>
-                                <property name="textInteractionFlags">
-                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                                 </property>
                                </widget>
                               </item>
-                              <item row="8" column="0">
+                              <item row="2" column="0">
-                               <widget class="QCheckBox" name="source_checkBox_ignoreIMU">
+                               <widget class="QCheckBox" name="source_checkBox_useDbStamps">
                                 <property name="text">
                                  <string/>
                                 </property>
@@ -11102,7 +11122,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item row="2" column="1">
                         <widget class="QLabel" name="label_rehearsalIdUpdate">
                          <property name="text">
-                          <string>On merging, update to new id.</string>
+                          <string>On merging, update to new id. Keep this unchecked if intermediate nodes are created.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>

tools/Reprocess/main.cpp

@@ -96,6 +96,7 @@ void showUsage()
 			"     -nopriors   Don't republish priors contained in input database.\n"
 			"     -noimu      Don't republish IMU contained in input database.\n"
 			"     -pub_loops  Republish loop closures contained in input database.\n"
+			"     -pub_inter_as_normal Republish intermediate nodes as normal nodes.\n"
 			"     -loc_null   On localization mode, reset localization pose to null and map correction to identity between sessions.\n"
 			"     -gt         When reprocessing a single database, load its original optimized graph, then \n"
 			"                 set it as ground truth for output database. If there was a ground truth in the input database, it will be ignored.\n"
@@ -279,6 +280,7 @@ int main(int argc, char * argv[])
 	bool ignorePriors = false;
 	bool ignoreImu = false;
 	bool republishLoopClosures = false;
+	bool pubInterNodesAsNormalNodes = false;
 	bool locNull = false;
 	bool originalGraphAsGT = false;
 	bool scanFromDepth = false;
@@ -510,6 +512,11 @@ int main(int argc, char * argv[])
 			republishLoopClosures = true;
 			printf("Republish loop closures from input database (-pub_loops option).\n");
 		}
+		else if(strcmp(argv[i], "-pub_inter_as_normal") == 0 || strcmp(argv[i], "--pub_inter_as_normal") == 0)
+		{
+			pubInterNodesAsNormalNodes = true;
+			printf("Republish intermdiate nodes as normal nodes (-pub_inter_as_normal option).\n");
+		}
 		else if(strcmp(argv[i], "-loc_null") == 0 || strcmp(argv[i], "--loc_null") == 0)
 		{
 			locNull = true;
@@ -815,7 +822,7 @@ int main(int argc, char * argv[])
 
 	int totalIds = 0;
 	std::set<int> ids;
-	dbDriver->getAllNodeIds(ids, false, false, !intermediateNodes);
+	dbDriver->getAllNodeIds(ids, false, false, !pubInterNodesAsNormalNodes && !intermediateNodes);
 	if(ids.empty())
 	{
 		printf("Input database doesn't have any nodes saved in it.\n");
@@ -844,7 +851,7 @@ int main(int argc, char * argv[])
 			return 1;
 		}
 		ids.clear();
-		dbDriver->getAllNodeIds(ids, false, false, !intermediateNodes);
+		dbDriver->getAllNodeIds(ids, false, false, !pubInterNodesAsNormalNodes && !intermediateNodes);
 		totalIds += ids.size();
 		dbDriver->closeConnection(false);
 	}
@@ -920,13 +927,14 @@ int main(int argc, char * argv[])
 			startId,
 			cameraIndices,
 			stopId,
-			!intermediateNodes,
+			!pubInterNodesAsNormalNodes && !intermediateNodes,
 			ignoreLandmarks,
 			!useOdomFeatures,
 			startMapId,
 			stopMapId,
 			ignorePriors,
 			ignoreImu,
+			pubInterNodesAsNormalNodes,
 			cameraLocalTransformOverrides);
 
 	dbReader->init();
@@ -945,6 +953,11 @@ int main(int argc, char * argv[])
 
 	Odometry * odometry = 0;
 	float rtabmapUpdateRate = Parameters::defaultRtabmapDetectionRate();
+	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
+	if(rtabmapUpdateRate!=0)
+	{
+		rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
+	}
 	double lastUpdateStamp = 0;
 	if(recomputeOdometry)
 	{
@@ -957,14 +970,17 @@ int main(int argc, char * argv[])
 		{
 			printf("Odometry will be recomputed (\"odom\" option is set)%s.\n",
 				useInputOdometryAsGuess?" with input odometry guess (\"odom_guess_input\" option is set)":"");
-			Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), rtabmapUpdateRate);
-			if(rtabmapUpdateRate!=0)
-			{
-				rtabmapUpdateRate = 1.0f/rtabmapUpdateRate;
-			}
 			odometry = Odometry::create(parameters);
 		}
 	}
+	else if(!intermediateNodes && framesToSkip == 0 &&
+		(configParameters.find(Parameters::kRtabmapDetectionRate())!=configParameters.end() ||
+	     customParameters.find(Parameters::kRtabmapDetectionRate())!=customParameters.end()))
+	{
+		printf("[Warning] Parameter %s is ignored because parameter %s=false.\n",
+				Parameters::kRtabmapDetectionRate().c_str(),
+				Parameters::kRtabmapCreateIntermediateNodes().c_str());
+	}
 
 	printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
 	std::map<std::string, float> globalMapStats;
@@ -1071,11 +1087,16 @@ int main(int argc, char * argv[])
 			info.odomPose = pose;
 			info.odomCovariance = odomCovariance;
 			odomCovariance = cv::Mat();
-			if(data.id() != -1)
-				lastUpdateStamp = data.stamp();
 
 			uInsert(globalMapStats, odomInfo.statistics(pose));
 		}
+		else if(framesToSkip==0 && intermediateNodes && lastUpdateStamp > 0.0 && (data.stamp() < lastUpdateStamp + rtabmapUpdateRate))
+		{
+			data.setId(-1); // intermediate node
+		}
+
+		if(data.id() != -1)
+			lastUpdateStamp = data.stamp();
 
 		UTimer iterationTime;
 		std::string status;
@@ -1266,15 +1287,19 @@ int main(int argc, char * argv[])
 			{
 				++loopIntra;
 			}
-			printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
+			printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms %s on %d [%d]\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
 		}
 		else if(landmarkId != 0)
 		{
-			printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Loop on landmark %d\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f),  int(iterationTime.ticks() * 1000), landmarkId);
+			printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Loop on landmark %d\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f),  int(iterationTime.ticks() * 1000), landmarkId);
+		}
+		else if(data.id() == -1)
+		{
+			printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms Intermediate node\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f),  int(iterationTime.ticks() * 1000));
 		}
 		else
 		{
-			printf("Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms\n", ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000));
+			printf("[%f] Processed %d/%d nodes [id=%d map=%d graph=%d hyp=%d]... %dms\n", data.stamp(), ++processed, totalIds, refId, refMapId, int(stats.poses().size()), int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value())*100.0f), int(iterationTime.ticks() * 1000));
 		}
 
 		// Here we accumulate statistics about distance from last localization