New parameter: Mem/ImageDecimation (save downscaled images in database), Updated default parameter RGBD/LocalLoopDetectionMaxDiffID to 50

corelib/include/rtabmap/core/Memory.h

@@ -123,8 +123,7 @@ public:
 	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
 	bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
 	bool isIDsGenerated() const {return _generateIds;}
-	int getLastGlobalLoopClosureParentId() const {return _lastGlobalLoopClosureParentId;}
-	int getLastGlobalLoopClosureChildId() const {return _lastGlobalLoopClosureChildId;}
+	int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
 	const Feature2D * getFeature2D() const {return _feature2D;}
 
 	void setRoi(const std::string & roi);
@@ -209,12 +208,12 @@ private:
 	bool _idUpdatedToNewOneRehearsal;
 	bool _generateIds;
 	bool _badSignaturesIgnored;
+	int _imageDecimation;
 
 	int _idCount;
 	int _idMapCount;
 	Signature * _lastSignature;
-	int _lastGlobalLoopClosureParentId;
-	int _lastGlobalLoopClosureChildId;
+	int _lastGlobalLoopClosureId;
 	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;

corelib/include/rtabmap/core/Parameters.h

@@ -192,7 +192,9 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
 	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.");
-	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.")
+	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
+	RTABMAP_PARAM(Mem, ImageDecimation,         int, 1,          "Image decimation (>=1).");
+
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM_COND(Kp, NNStrategy,       int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -294,7 +296,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius,   float, 15, 		"Maximum radius for space detection.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 0,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 50,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");

corelib/include/rtabmap/core/Rtabmap.h

@@ -97,7 +97,7 @@ public:
 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
 
-	void triggerNewMap();
+	int triggerNewMap();
 	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
 	void generateTOROGraph(const std::string & path, bool optimized, bool global);
 	void resetMemory();
@@ -186,6 +186,8 @@ private:
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
 	double _lastProcessTime;
+	int _lastLocalLoopClosureParentId;
+	int _lastLocalLoopClosureChildId;
 
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.

corelib/include/rtabmap/core/Statistics.h

@@ -60,8 +60,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
 	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
 	RTABMAP_STATS(Loop, VisualInliers,);
-	RTABMAP_STATS(Loop, Last_loop_closure_parent,);
-	RTABMAP_STATS(Loop, Last_loop_closure_child,);
+	RTABMAP_STATS(Loop, Last_id,);
 
 	RTABMAP_STATS(LocalLoop, Odom_corrected,);
 	RTABMAP_STATS(LocalLoop, Time_closures,);
@@ -69,6 +68,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(LocalLoop, Space_nearest_id,);
 	RTABMAP_STATS(LocalLoop, Space_neighbors,);
 	RTABMAP_STATS(LocalLoop, Space_diff_id,);
+	RTABMAP_STATS(LocalLoop, Space_last_parent,);
+	RTABMAP_STATS(LocalLoop, Space_last_child,);
 
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);

corelib/include/rtabmap/core/util3d.h

@@ -401,6 +401,8 @@ pcl::IndicesPtr RTABMAP_EXP concatenate(
 		const pcl::IndicesPtr & indicesA,
 		const pcl::IndicesPtr & indicesB);
 
+cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
+
 ///////////////////
 // Templated PCL methods
 ///////////////////

corelib/src/Memory.cpp

@@ -66,11 +66,11 @@ Memory::Memory(const ParametersMap & parameters) :
 	_idUpdatedToNewOneRehearsal(Parameters::defaultMemRehearsalIdUpdatedToNewOne()),
 	_generateIds(Parameters::defaultMemGenerateIds()),
 	_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
+	_imageDecimation(Parameters::defaultMemImageDecimation()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
-	_lastGlobalLoopClosureParentId(0),
-	_lastGlobalLoopClosureChildId(0),
+	_lastGlobalLoopClosureId(0),
 	_memoryChanged(false),
 	_linksChanged(false),
 	_signaturesAdded(0),
@@ -384,10 +384,12 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
 	Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
 	Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
+	Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
 
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
 	UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
+	UASSERT(_imageDecimation >= 1);
 
 	// SLAM mode vs Localization mode
 	iter = parameters.find(Parameters::kMemIncrementalMemory());
@@ -590,9 +592,6 @@ bool Memory::update(const SensorData & data, Statistics * stats)
 
 	UDEBUG("totalTimer = %fs", totalTimer.ticks());
 
-	if(stats) stats->addStatistic(Statistics::kLoopLast_loop_closure_parent(), _lastGlobalLoopClosureParentId);
-	if(stats) stats->addStatistic(Statistics::kLoopLast_loop_closure_child(), _lastGlobalLoopClosureChildId);
-
 	return true;
 }
 
@@ -1003,8 +1002,7 @@ void Memory::clear()
 	}
 	UDEBUG("");
 	_lastSignature = 0;
-	_lastGlobalLoopClosureParentId = 0;
-	_lastGlobalLoopClosureChildId = 0;
+	_lastGlobalLoopClosureId = 0;
 	_idCount = kIdStart;
 	_idMapCount = kIdStart;
 	_memoryChanged = false;
@@ -1303,10 +1301,10 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 		bool recentWmImmunized = false;
 		// look for the position of the lastLoopClosureId in WM
 		int currentRecentWmSize = 0;
-		if(_lastGlobalLoopClosureParentId > 0 && _stMem.find(_lastGlobalLoopClosureParentId) == _stMem.end())
+		if(_lastGlobalLoopClosureId > 0 && _stMem.find(_lastGlobalLoopClosureId) == _stMem.end())
 		{
 			// If set, it must be in WM
-			std::set<int>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureParentId);
+			std::set<int>::const_iterator iter = _workingMem.find(_lastGlobalLoopClosureId);
 			while(iter != _workingMem.end())
 			{
 				++currentRecentWmSize;
@@ -1318,9 +1316,9 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 			}
 			else if(currentRecentWmSize == 0 && _workingMem.size() > 1)
 			{
-				UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureParentId);
+				UERROR("Last loop closure id not found in WM (%d)", _lastGlobalLoopClosureId);
 			}
-			UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureParentId);
+			UDEBUG("currentRecentWmSize=%d, recentWmMaxSize=%d, _recentWmRatio=%f, end recent wM = %d", currentRecentWmSize, recentWmMaxSize, _recentWmRatio, _lastGlobalLoopClosureId);
 		}
 
 		// Ignore neighbor of the last location in STM (for neighbor links redirection issue during Rehearsal).
@@ -1332,8 +1330,8 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 
 		for(std::set<int>::const_iterator memIter = wm.begin(); memIter != wm.end(); ++memIter)
 		{
-			if( (recentWmImmunized && *memIter > _lastGlobalLoopClosureParentId) ||
-				*memIter == _lastGlobalLoopClosureParentId)
+			if( (recentWmImmunized && *memIter > _lastGlobalLoopClosureId) ||
+				*memIter == _lastGlobalLoopClosureId)
 			{
 				// ignore recent memory
 			}
@@ -1390,7 +1388,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 					removableSignatures.push_back(iter->second);
 					addedSignatures.insert(iter->second->id());
 
-					if(iter->second->id() > _lastGlobalLoopClosureParentId)
+					if(iter->second->id() > _lastGlobalLoopClosureId)
 					{
 						++recentWmCount;
 						if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
@@ -1400,7 +1398,7 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 						}
 					}
 				}
-				else if(iter->second->id() < _lastGlobalLoopClosureParentId)
+				else if(iter->second->id() < _lastGlobalLoopClosureId)
 				{
 					UDEBUG("weight=%d, id=%d",
 							iter->first.weight,
@@ -1579,10 +1577,9 @@ void Memory::removeLink(int oldId, int newId)
 					break;
 				}
 			}
-			if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureParentId)
+			if(noChildrenAnymore && newS->id() == _lastGlobalLoopClosureId)
 			{
-				_lastGlobalLoopClosureParentId = 0;
-				_lastGlobalLoopClosureChildId = 0;
+				_lastGlobalLoopClosureId = 0;
 			}
 		}
 		else
@@ -2181,8 +2178,7 @@ bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Ty
 
 		if(_incrementalMemory && type == Link::kGlobalClosure)
 		{
-			_lastGlobalLoopClosureParentId = newS->id()>oldS->id()?newS->id():oldS->id();
-			_lastGlobalLoopClosureChildId = newS->id()>oldS->id()?oldS->id():newS->id();
+			_lastGlobalLoopClosureId = newS->id()>oldS->id()?newS->id():oldS->id();
 
 			// udpate weights only if the memory is incremental
 			if(newS->id() > oldS->id())
@@ -2470,9 +2466,9 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 			// update weight
 			newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
 
-			if(_lastGlobalLoopClosureParentId == oldS->id())
+			if(_lastGlobalLoopClosureId == oldS->id())
 			{
-				_lastGlobalLoopClosureParentId = newS->id();
+				_lastGlobalLoopClosureId = newS->id();
 			}
 		}
 		else
@@ -2978,8 +2974,8 @@ private:
 Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 {
 	UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
-	UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1);
-	UASSERT(data.rightImage().empty() || data.rightImage().type() == CV_8UC1);
+	UASSERT(data.depth().empty() || ((data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1) && data.depth().rows == data.image().rows && data.depth().cols == data.image().cols));
+	UASSERT(data.rightImage().empty() || (data.rightImage().type() == CV_8UC1 && data.rightImage().rows == data.image().rows && data.rightImage().cols == data.image().cols));
 	UASSERT(data.laserScan().empty() || data.laserScan().type() == CV_32FC2);
 	UASSERT(_feature2D != 0);
 
@@ -3372,7 +3368,18 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 		unsigned int i=0;
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
-			words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
+			if(_imageDecimation > 1)
+			{
+				cv::KeyPoint kpt = keypoints[i];
+				kpt.pt.x /= float(_imageDecimation);
+				kpt.pt.y /= float(_imageDecimation);
+				kpt.size /= float(_imageDecimation);
+				words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
+			}
+			else
+			{
+				words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
+			}
 			if(keypoints3D->size())
 			{
 				words3D.insert(std::pair<int, pcl::PointXYZ>(*iter, keypoints3D->at(i)));
@@ -3380,25 +3387,44 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 		}
 	}
 
+	cv::Mat image = data.image();
+	cv::Mat depthOrRightImage = data.depthOrRightImage();
+	float fx = data.fx();
+	float fyOrBaseline = data.fyOrBaseline();
+	float cx = data.cx();
+	float cy = data.cy();
+
+	// apply decimation?
+	if((this->isBinDataKept() || this->isRawDataKept()) && _imageDecimation > 1)
+	{
+		image = util3d::decimate(image, _imageDecimation);
+		depthOrRightImage = util3d::decimate(depthOrRightImage, _imageDecimation);
+		cx/=float(_imageDecimation);
+		cy/=float(_imageDecimation);
+		if(data.baseline() != 0.0f)
+		{
+			fx/=float(_imageDecimation);
+		}
+		else
+		{
+			fx/=float(_imageDecimation);
+			fyOrBaseline/=float(_imageDecimation);
+		}
+	}
+
 	Signature * s;
 	if(this->isBinDataKept())
 	{
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
-		if(data.depth().type() == CV_32FC1)
+
+		if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
 		{
 			UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion.");
+			depthOrRightImage = util3d::cvtDepthFromFloat(depthOrRightImage);
 		}
-		cv::Mat depthOrRightImage;
-		if(!data.depth().empty())
-		{
-			depthOrRightImage = data.depth().type() == CV_32FC1?util3d::cvtDepthFromFloat(data.depth()):data.depth();
-		}
-		else if(!data.rightImage().empty())
-		{
-			depthOrRightImage = data.rightImage();
-		}
-		rtabmap::CompressionThread ctImage(data.image(), std::string(".jpg"));
+
+		rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
 		rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
 		rtabmap::CompressionThread ctDepth2d(data.laserScan());
 		ctImage.start();
@@ -3416,10 +3442,10 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 			ctDepth2d.getCompressedData(),
 			ctImage.getCompressedData(),
 			ctDepth.getCompressedData(),
-			data.fx(),
-			data.fy()>0.0f?data.fy():data.baseline(),
-			data.cx(),
-			data.cy(),
+			fx,
+			fyOrBaseline,
+			cx,
+			cy,
 			data.localTransform());
 	}
 	else
@@ -3433,8 +3459,8 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 	}
 	if(this->isRawDataKept())
 	{
-		s->setImageRaw(data.image());
-		s->setDepthRaw(data.depth());
+		s->setImageRaw(image);
+		s->setDepthRaw(depthOrRightImage);
 		s->setLaserScanRaw(data.laserScan());
 	}
 

corelib/src/Rtabmap.cpp

@@ -112,6 +112,8 @@ Rtabmap::Rtabmap() :
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
+	_lastLocalLoopClosureParentId(0),
+	_lastLocalLoopClosureChildId(0),
 	_epipolarGeometry(0),
 	_bayesFilter(0),
 	_memory(0),
@@ -584,15 +586,19 @@ Transform Rtabmap::getPose(int locationId) const
 	return Transform();
 }
 
-void Rtabmap::triggerNewMap()
+int Rtabmap::triggerNewMap()
 {
+	int mapId = -1;
 	if(_memory)
 	{
-		int mapId = _memory->incrementMapId();
-		UINFO("New map triggerred, new map = %d", mapId);
+		mapId = _memory->incrementMapId();
+		UINFO("New map triggered, new map = %d", mapId);
 		_optimizedPoses.clear();
 		_constraints.clear();
+		_lastLocalLoopClosureParentId = 0;
+		_lastLocalLoopClosureChildId = 0;
 	}
+	return mapId;
 }
 
 void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
@@ -653,6 +659,8 @@ void Rtabmap::resetMemory()
 	_highestHypothesis = std::make_pair(0,0.0f);
 	_loopClosureHypothesis = std::make_pair(0,0.0f);
 	_lastProcessTime = 0.0;
+	_lastLocalLoopClosureParentId = 0;
+	_lastLocalLoopClosureChildId = 0;
 	_optimizedPoses.clear();
 	_constraints.clear();
 	_mapCorrection.setIdentity();
@@ -773,14 +781,12 @@ bool Rtabmap::process(const SensorData & data)
 				lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
 				if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
 				{
-					int mapId = _memory->incrementMapId();
+					int mapId = triggerNewMap();
 					UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
 							_newMapOdomChangeDistance,
 							mapId,
 							lastPose.prettyPrint().c_str(),
 							data.pose().prettyPrint().c_str());
-					_optimizedPoses.clear();
-					_constraints.clear();
 				}
 			}
 		}
@@ -1372,6 +1378,11 @@ bool Rtabmap::process(const SensorData & data)
 		{
 			// Make the new one the parent of the old one
 			rejectedHypothesis = !_memory->addLink(_loopClosureHypothesis.first, signature->id(), transform, Link::kGlobalClosure, variance);
+			if(!rejectedHypothesis)
+			{
+				_lastLocalLoopClosureParentId = signature->id();
+				_lastLocalLoopClosureChildId = _loopClosureHypothesis.first;
+			}
 		}
 
 		if(rejectedHypothesis)
@@ -1444,6 +1455,9 @@ bool Rtabmap::process(const SensorData & data)
 							t.prettyPrint().c_str());
 					_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance);
 
+					_lastLocalLoopClosureParentId = signature->id();
+					_lastLocalLoopClosureChildId = localSpaceNearestId;
+
 					// Old map -> new map, used for localization correction on loop closure
 					const Signature * oldS = _memory->getSignature(localSpaceNearestId);
 					UASSERT(oldS != 0);
@@ -1555,25 +1569,26 @@ bool Rtabmap::process(const SensorData & data)
 			statistics_.addStatistic(Statistics::kLoopReactivateId(), retrievalId);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			statistics_.addStatistic(Statistics::kLoopVisualInliers(), loopClosureVisualInliers);
+			statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
 
 			statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
 			statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_neighbors(), localSpaceDetectionPosesCount);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_closure_id(), localSpaceClosureId);
 			statistics_.addStatistic(Statistics::kLocalLoopSpace_nearest_id(), localSpaceNearestId);
-			if(localSpaceClosureId)
-			{
-				statistics_.setLocalLoopClosureId(localSpaceClosureId);
-			}
+			statistics_.setLocalLoopClosureId(localSpaceClosureId);
 			if(localSpaceNearestId)
 			{
 				int d1 = abs(signature->id() - localSpaceNearestId);
-				int d2 = abs(localSpaceNearestId - _memory->getLastGlobalLoopClosureChildId());
-				int d3 = abs(signature->id() - _memory->getLastGlobalLoopClosureParentId());
+				int d2 = abs(localSpaceNearestId - _lastLocalLoopClosureChildId);
+				int d3 = abs(signature->id() - _lastLocalLoopClosureParentId);
 				int d = d1<=d2?d1:d2;
 				d = d <= d3?d:d3;
 				statistics_.addStatistic(Statistics::kLocalLoopSpace_diff_id(), d);
 			}
+			statistics_.addStatistic(Statistics::kLocalLoopSpace_last_parent(), _lastLocalLoopClosureParentId);
+			statistics_.addStatistic(Statistics::kLocalLoopSpace_last_child(), _lastLocalLoopClosureChildId);
+
 			if(_loopClosureHypothesis.first || localSpaceClosureId)
 			{
 				UASSERT(uContains(sLoop->getLinks(), signature->id()));
@@ -1919,8 +1934,8 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 		// Only locations in Working Memory with ID not too far from the last loop closure child id
 		bool diffIdOk = maxDiffID == 0 ||
 				abs(fromId - iter->first) <= maxDiffID ||
-				(abs(iter->first - _memory->getLastGlobalLoopClosureChildId()) <= maxDiffID &&
-				 abs(fromId - _memory->getLastGlobalLoopClosureParentId()) <= maxDiffID);
+				(abs(iter->first - _lastLocalLoopClosureChildId) <= maxDiffID &&
+				 abs(fromId - _lastLocalLoopClosureParentId) <= maxDiffID);
 		if(stm.find(iter->first) == stm.end() && diffIdOk)
 		{
 			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());

corelib/src/util3d.cpp

@@ -827,7 +827,7 @@ cv::Mat disparityFromStereoImages(
 			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
 			leftImage.cols == rightImage.cols &&
 			leftImage.rows == rightImage.rows);
-	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 160, 15);
+	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 0, 15);
 	cv::Mat disparity;
 	stereo(leftImage, rightImage, disparity, CV_16SC1);
 	cv::filterSpeckles(disparity, 0, 1000, 16);
@@ -2562,6 +2562,53 @@ pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::Indices
 	return ind;
 }
 
+cv::Mat decimate(const cv::Mat & image, int decimation)
+{
+	UASSERT(decimation >= 1);
+	cv::Mat out;
+	if(!image.empty())
+	{
+		if(decimation > 1)
+		{
+			if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
+			{
+				UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0, "Decimation of depth images should be exact!");
+
+				out = cv::Mat(image.rows/decimation, image.cols/decimation, image.type());
+				if(image.type() == CV_32FC1)
+				{
+					for(int j=0; j<out.rows; ++j)
+					{
+						for(int i=0; i<out.cols; ++i)
+						{
+							out.at<float>(j, i) = image.at<float>(j*decimation, i*decimation);
+						}
+					}
+				}
+				else // CV_16UC1
+				{
+					for(int j=0; j<out.rows; ++j)
+					{
+						for(int i=0; i<out.cols; ++i)
+						{
+							out.at<unsigned short>(j, i) = image.at<unsigned short>(j*decimation, i*decimation);
+						}
+					}
+				}
+			}
+			else
+			{
+				cv::resize(image, out, cv::Size(), 1.0f/float(decimation), 1.0f/float(decimation), cv::INTER_AREA);
+			}
+		}
+		else
+		{
+			out = image;
+		}
+	}
+	return out;
+}
+
 }
 
 }

guilib/src/PreferencesDialog.cpp

@@ -339,6 +339,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_generateIds->setObjectName(Parameters::kMemGenerateIds().c_str());
 	_ui->general_checkBox_badSignaturesIgnored->setObjectName(Parameters::kMemBadSignaturesIgnored().c_str());
 	_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
+	_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
+
 
 	// Database
 	_ui->checkBox_dbInMemory->setObjectName(Parameters::kDbSqlite3InMemory().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>7</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -2725,16 +2725,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_10">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_42" columnstretch="0,1">
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_retrieved_4">
-                        <property name="text">
-                         <string>Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="0" column="0">
                        <widget class="QSpinBox" name="general_spinBox_maxStMemSize">
                         <property name="minimum">
@@ -2748,6 +2738,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_retrieved_4">
+                        <property name="text">
+                         <string>Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="1">
                        <widget class="QLabel" name="label_29">
                         <property name="text">
@@ -2880,6 +2880,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_retrieved_6">
+                        <property name="text">
+                         <string>Image decimation. This feature can be used to save images in lower resolution (size/decimation).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QSpinBox" name="spinBox_imageDecimation">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <number>16</number>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>