Added RGBD/ProximityGlobalScanMap for proximity detection using the whole scan map in localization mode. Added Icp RMS statistics (CCCorelib). Updated how localization is corrected with gravity when available (simply use roll and pitch of current gravity constraint).

corelib/include/rtabmap/core/LaserScan.h

@@ -136,8 +136,19 @@ public:
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
 	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
 
+	float & field(unsigned int pointIndex, unsigned int channelOffset);
+
 	void clear() {data_ = cv::Mat();}
 
+	/**
+	 * Concatenate scan's data, localTransform is ignored.
+	 */
+	LaserScan & operator+=(const LaserScan &);
+	/**
+	 * Concatenate scan's data, localTransform is ignored.
+	 */
+	LaserScan operator+(const LaserScan &);
+
 private:
 	void init(const cv::Mat & data,
 			Format format,

corelib/include/rtabmap/core/Memory.h

@@ -242,6 +242,7 @@ public:
 
 	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
 	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
+	Transform computeIcpTransform(const Signature & fromS, const Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
 	Transform computeIcpTransformMulti(
 			int newId,
 			int oldId,

corelib/include/rtabmap/core/Parameters.h

@@ -385,6 +385,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
     RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for one-to-one proximity detection.");
     RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityGlobalScanMap,       bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str(), kRGBDProximityPathRawPosesUsed().c_str()));
 
     // Graph optimization
 #ifdef RTABMAP_GTSAM

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -37,6 +37,7 @@ public:
 	RegistrationInfo() :
 		totalTime(0.0),
 		inliers(0),
+		inliersRatio(0),
 		inliersMeanDistance(0.0f),
 		inliersDistribution(0.0f),
 		matches(0),
@@ -45,7 +46,8 @@ public:
 		icpRotation(0.0f),
 		icpStructuralComplexity(0.0f),
 		icpStructuralDistribution(0.0f),
-		icpCorrespondences(0)
+		icpCorrespondences(0),
+		icpRMS(0)
 
 	{
 	}
@@ -90,6 +92,7 @@ public:
 	float icpStructuralComplexity;
 	float icpStructuralDistribution;
 	int   icpCorrespondences;
+	float icpRMS;
 };
 
 }

corelib/include/rtabmap/core/Rtabmap.h

@@ -248,6 +248,8 @@ private:
 	void updateGoalIndex();
 	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
 
+	void createGlobalScanMap();
+
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();
 
@@ -306,6 +308,7 @@ private:
 	bool _loopCovLimited;
 	bool _loopGPS;
 	int _maxOdomCacheSize;
+	bool _createGlobalScanMap;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
@@ -341,6 +344,8 @@ private:
 	int _lastLocalizationNodeId; // for localization mode
 	std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
 	bool _currentSessionHasGPS;
+	LaserScan _globalScanMap;
+	std::map<int, Transform> _globalScanMapPoses;
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
 	std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link

corelib/include/rtabmap/core/Statistics.h

@@ -122,7 +122,8 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Proximity, Space_visual_paths_checked,);
 	RTABMAP_STATS(Proximity, Space_scan_paths_checked,);
 	RTABMAP_STATS(Proximity, Space_detections_added_visually,);
-	RTABMAP_STATS(Proximity, Space_detections_added_icp_only,);
+	RTABMAP_STATS(Proximity, Space_detections_added_icp_multi,);
+	RTABMAP_STATS(Proximity, Space_detections_added_icp_global,);
 
 	RTABMAP_STATS(NeighborLinkRefining, Accepted,);
 	RTABMAP_STATS(NeighborLinkRefining, Inliers,);

corelib/include/rtabmap/core/Transform.h

@@ -103,6 +103,7 @@ public:
 	Transform rotation() const;
 	Transform translation() const;
 	Transform to3DoF() const;
+	Transform to4DoF() const;
 
 	cv::Mat rotationMatrix() const;
 	cv::Mat translationMatrix() const;

corelib/include/rtabmap/core/util3d_surface.h

@@ -437,6 +437,12 @@ void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::vector<int> & rawCameraIndices,
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
 
+void RTABMAP_EXP adjustNormalsToViewPoints(
+		const std::map<int, Transform> & viewpoints,
+		const LaserScan & rawScan,
+		const std::vector<int> & viewpointIds,
+		LaserScan & scan);
+
 pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor);
 
 template<typename pointT>

corelib/src/LaserScan.cpp

@@ -366,4 +366,42 @@ LaserScan LaserScan::clone() const
 	return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone());
 }
 
+float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset)
+{
+	UASSERT(pointIndex < data_.cols);
+	UASSERT(channelOffset < data_.channels());
+	return data_.ptr<float>(0, pointIndex)[channelOffset];
+}
+
+LaserScan & LaserScan::operator+=(const LaserScan & scan)
+{
+	*this = *this+scan;
+	return *this;
+}
+
+LaserScan LaserScan::operator+(const LaserScan & scan)
+{
+	UASSERT(this->empty() || scan.empty() || this->format() == scan.format());
+	LaserScan dest;
+	if(!scan.empty())
+	{
+		if(this->empty())
+		{
+			dest = LaserScan(scan.data().clone(), 0, 0, this->format());
+		}
+		else
+		{
+			cv::Mat destData(1, data_.cols + scan.data().cols, data_.type());
+			data_.copyTo(destData(cv::Range::all(), cv::Range(0,data_.cols)));
+			scan.data().copyTo(destData(cv::Range::all(), cv::Range(data_.cols, data_.cols+scan.data().cols)));
+			dest = LaserScan(destData, 0, 0, this->format());
+		}
+	}
+	else
+	{
+		dest = this->clone();
+	}
+	return dest;
+}
+
 }

corelib/src/Memory.cpp

@@ -3017,6 +3017,19 @@ Transform Memory::computeTransform(
 	return transform;
 }
 
+Transform Memory::computeIcpTransform(
+		const Signature & fromS,
+		const Signature & toS,
+		Transform guess,
+		RegistrationInfo * info) const
+{
+	UDEBUG("%d -> %d, Guess=%s", fromS.id(), toS.id(), guess.prettyPrint().c_str());
+
+	Signature tmpFrom = fromS;
+	Signature tmpTo = toS;
+	return _registrationIcpMulti->computeTransformation(tmpFrom.sensorData(), tmpTo.sensorData(), guess, info);
+}
+
 // compute transform fromId -> multiple toId
 Transform Memory::computeIcpTransformMulti(
 		int fromId,
@@ -3042,7 +3055,7 @@ Transform Memory::computeIcpTransformMulti(
 	}
 
 	// make sure that all laser scans are loaded
-	std::list<Signature*> depthToLoad;
+	std::list<Signature*> scansToLoad;
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
 		Signature * s = _getSignature(iter->first);
@@ -3051,12 +3064,12 @@ Transform Memory::computeIcpTransformMulti(
 		if(s->sensorData().imageCompressed().empty() &&
 		   s->sensorData().laserScanCompressed().isEmpty())
 		{
-			depthToLoad.push_back(s);
+			scansToLoad.push_back(s);
 		}
 	}
-	if(depthToLoad.size() && _dbDriver)
+	if(scansToLoad.size() && _dbDriver)
 	{
-		_dbDriver->loadNodeData(depthToLoad, false, true, false, false);
+		_dbDriver->loadNodeData(scansToLoad, false, true, false, false);
 	}
 
 	Signature * fromS = _getSignature(fromId);
@@ -3081,7 +3094,6 @@ Transform Memory::computeIcpTransformMulti(
 		}
 
 		// Create a fake signature with all scans merged in oldId referential
-		SensorData assembledData;
 		Transform toPoseInv = poses.at(toId).inverse();
 		std::string msg;
 		int maxPoints = fromScan.size();
@@ -3203,6 +3215,7 @@ Transform Memory::computeIcpTransformMulti(
 		UDEBUG("assembledScan=%d points", assembledScan.size());
 
 		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
+		SensorData assembledData;
 		assembledData.setLaserScan(
 				LaserScan(assembledScan,
 					maxPoints,

corelib/src/RegistrationIcp.cpp

@@ -701,6 +701,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 								 _outlierRatio,
 								 _ccFilterOutFarthestPoints,
 								 _ccMaxFinalRMS,
+								 &info.icpRMS,
 								 &msg);
 						hasConverged = !icpT.isNull();
 					}

corelib/src/Rtabmap.cpp

@@ -34,6 +34,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/EpipolarGeometry.h"
 
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_surface.h"
+
 #include "rtabmap/core/DBDriver.h"
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/VWDictionary.h"
@@ -130,6 +135,7 @@ Rtabmap::Rtabmap() :
 	_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
 	_loopGPS(Parameters::defaultRtabmapLoopGPS()),
 	_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
+	_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -333,14 +339,27 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 		_memory->init(_databasePath, false, allParameters, true);
 	}
 
+	_optimizedPoses.clear();
+	_constraints.clear();
+	_globalScanMap.clear();
+	_globalScanMapPoses.clear();
+
 	// Parse all parameters
 	this->parseParameters(allParameters);
 
 	Transform lastPose;
-	_optimizedPoses.clear();
 	if(!_memory->isIncremental())
 	{
 		_optimizedPoses = _memory->loadOptimizedPoses(&lastPose);
+		if(_optimizedPoses.empty() &&
+			_memory->getWorkingMem().size()>1 &&
+			_memory->getWorkingMem().lower_bound(1)!=_memory->getWorkingMem().end())
+		{
+			cv::Mat cov;
+			this->optimizeCurrentMap(
+					_optimizeFromGraphEnd?_memory->getWorkingMem().lower_bound(1)->first:_memory->getWorkingMem().rbegin()->first,
+					false, _optimizedPoses, cov, &_constraints);
+		}
 		if(!_optimizedPoses.empty())
 		{
 			if(_restartAtOrigin)
@@ -352,9 +371,12 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 
 			UINFO("Loaded optimizedPoses=%d lastPose=%s", _optimizedPoses.size(), _lastLocalizationPose.prettyPrint().c_str());
 
-			std::map<int, Transform> tmp;
+			if(_constraints.empty())
-			// Get just the links
+			{
-			_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
+				std::map<int, Transform> tmp;
+				// Get just the links
+				_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
+			}
 
 			// Initialize Bayes' prediction matrix
 			UTimer time;
@@ -369,6 +391,9 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 			}
 			_bayesFilter->computePosterior(_memory, likelihood);
 			UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
+
+			if(_createGlobalScanMap)
+				createGlobalScanMap();
 		}
 		else
 		{
@@ -419,6 +444,9 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_gpsGeocentricCache.clear();
 	_currentSessionHasGPS = false;
 
+	_globalScanMap.clear();
+	_globalScanMapPoses.clear();
+
 	flushStatisticLogs();
 	if(_foutFloat)
 	{
@@ -554,6 +582,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
 	Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
+	Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
 
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -590,9 +619,26 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		_graphOptimizer = Optimizer::create(optimizerType, parameters);
 	}
 
+	if(!_createGlobalScanMap)
+	{
+		_globalScanMap.clear();
+		_globalScanMapPoses.clear();
+	}
+
 	if(_memory)
 	{
 		_memory->parseParameters(parameters);
+		if(_memory->isIncremental() && !_globalScanMap.empty())
+		{
+			UWARN("Map is now incremental, clearing global scan map...");
+			_globalScanMap.clear();
+			_globalScanMapPoses.clear();
+		}
+
+		if(_createGlobalScanMap && !_memory->isIncremental() && _globalScanMap.empty() && !_optimizedPoses.empty())
+		{
+			this->createGlobalScanMap();
+		}
 	}
 
 	if(!_epipolarGeometry)
@@ -757,9 +803,9 @@ int Rtabmap::triggerNewMap()
 
 		if(!_memory->isIncremental())
 		{
+			_mapCorrection.setIdentity();
 			if(_restartAtOrigin)
 			{
-				_mapCorrection.setIdentity();
 				_lastLocalizationPose.setIdentity();
 			}
 			return mapId;
@@ -922,6 +968,8 @@ void Rtabmap::resetMemory()
 	_distanceTravelled = 0.0f;
 	_distanceTravelledSinceLastLocalization = 0.0f;
 	_optimizeFromGraphEndChanged = false;
+	_globalScanMap.clear();
+	_globalScanMapPoses.clear();
 	this->clearPath(0);
 
 	if(_memory)
@@ -1117,6 +1165,14 @@ bool Rtabmap::process(
 			}
 		}
 
+		UDEBUG("incremental=%d odomPose=%s optimizedPoses=%d mapCorrection=%s lastLocalizationPose=%s lastLocalizationNodeId=%d",
+				_memory->isIncremental()?1:0,
+				odomPose.prettyPrint().c_str(),
+				(int)_optimizedPoses.size(),
+				_mapCorrection.prettyPrint().c_str(),
+				_lastLocalizationPose.prettyPrint().c_str(),
+				_lastLocalizationNodeId);
+
 		if(!_memory->isIncremental() &&
 			!odomPose.isNull() &&
 			_optimizedPoses.size() &&
@@ -1128,7 +1184,18 @@ bool Rtabmap::process(
 			if(!_optimizeFromGraphEnd)
 			{
 				//set map->odom so that odom is moved back to last saved localization
-				_mapCorrection = _lastLocalizationPose * odomPose.inverse();
+				if(_graphOptimizer->isSlam2d())
+				{
+					_mapCorrection = _lastLocalizationPose.to3DoF() * odomPose.to3DoF().inverse();
+				}
+				else if((!data.imu().empty() || _memory->isOdomGravityUsed()) && _graphOptimizer->gravitySigma()>0.0f)
+				{
+					_mapCorrection = _lastLocalizationPose.to4DoF() * odomPose.to4DoF().inverse();
+				}
+				else
+				{
+					_mapCorrection = _lastLocalizationPose * odomPose.inverse();
+				}
 				std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
 				_lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose);
 				UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
@@ -1140,7 +1207,19 @@ bool Rtabmap::process(
 			else
 			{
 				//move optimized poses accordingly to last saved localization
-				Transform mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
+				Transform mapCorrectionInv;
+				if(_graphOptimizer->isSlam2d())
+				{
+					mapCorrectionInv = odomPose.to3DoF() * _lastLocalizationPose.to3DoF().inverse();
+				}
+				else if((!data.imu().empty() || _memory->isOdomGravityUsed()) && _graphOptimizer->gravitySigma()>0.0f)
+				{
+					mapCorrectionInv = odomPose.to4DoF() * _lastLocalizationPose.to4DoF().inverse();
+				}
+				else
+				{
+					mapCorrectionInv = odomPose * _lastLocalizationPose.inverse();
+				}
 				for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 				{
 					iter->second = mapCorrectionInv * iter->second;
@@ -2224,6 +2303,13 @@ bool Rtabmap::process(
 
 			// Immunize just retrieved signatures
 			immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
+
+			if(!signaturesRetrieved.empty() && !_globalScanMap.empty())
+			{
+				UWARN("Some signatures have been retrieved from memory management, clearing global scan map...");
+				_globalScanMap.clear();
+				_globalScanMapPoses.clear();
+			}
 		}
 		timeReactivations = timer.ticks();
 		ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
@@ -2263,7 +2349,8 @@ bool Rtabmap::process(
 	float loopClosureVisualInliersDistribution = 0;
 
 	int proximityDetectionsAddedVisually = 0;
-	int proximityDetectionsAddedByICPOnly = 0;
+	int proximityDetectionsAddedByICPMulti = 0;
+	int proximityDetectionsAddedByICPGlobal = 0;
 	int lastProximitySpaceClosureId = 0;
 	int proximitySpacePaths = 0;
 	int localVisualPathsChecked = 0;
@@ -2544,7 +2631,32 @@ bool Rtabmap::process(
 								{
 									++localScanPathsChecked;
 									RegistrationInfo info;
-									Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
+									Transform transform;
+									bool icpMulti = true;
+									if(_globalScanMap.empty())
+									{
+										transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, filteredPath, &info);
+									}
+									else
+									{
+										icpMulti = false;
+										// use pre-assembled scan map
+										SensorData assembledData;
+										assembledData.setId(nearestId);
+										assembledData.setLaserScan(
+												LaserScan(_globalScanMap,
+													signature->sensorData().laserScanCompressed().maxPoints(),
+													signature->sensorData().laserScanCompressed().rangeMax(),
+													_globalScanMapPoses.at(nearestId).inverse() * (signature->sensorData().laserScanCompressed().is2d()?Transform(0,0,signature->sensorData().laserScanCompressed().localTransform().z(),0,0,0):Transform::getIdentity())));
+										Signature nearestNode(assembledData);
+										Transform guess = filteredPath.at(nearestId).inverse() * filteredPath.at(signature->id());
+										transform = _memory->computeIcpTransform(nearestNode, *signature, guess, &info);
+										if(!transform.isNull())
+										{
+											transform = transform.inverse();
+										}
+									}
+
 									if(!transform.isNull())
 									{
 										UINFO("[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
@@ -2575,7 +2687,14 @@ bool Rtabmap::process(
 										_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/100.0, scanMatchingIds));
 										loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
 
-										++proximityDetectionsAddedByICPOnly;
+										if(icpMulti)
+										{
+											++proximityDetectionsAddedByICPMulti;
+										}
+										else
+										{
+											++proximityDetectionsAddedByICPGlobal;
+										}
 
 										// no local loop closure added visually
 										if(proximityDetectionsAddedVisually == 0)
@@ -2587,6 +2706,10 @@ bool Rtabmap::process(
 									{
 										UINFO("Local scan matching rejected: %s", info.rejectedMsg.c_str());
 									}
+									if(!_globalScanMap.empty())
+									{
+										break;
+									}
 								}
 							}
 						}
@@ -2925,40 +3048,20 @@ bool Rtabmap::process(
 					else if(_graphOptimizer->gravitySigma() > 0)
 					{
 						// Adjust transform with gravity
-						Transform transform = localizationLinks.begin()->second.transform();
-						int loopId = localizationLinks.begin()->first;
-						if(loopId < 0)
-						{
-							//For landmarks, use transform against other node looking the landmark
-							// (because we don't assume that landmarks are aligned with gravity)
-							int landmarkId = loopId;
-							UASSERT(!landmarkDetectedNodesRef.empty());
-							loopId = *landmarkDetectedNodesRef.begin();
-							const Signature * loopS = _memory->getSignature(loopId);
-                            transform = transform * _optimizedPoses.at(landmarkId).inverse()*_optimizedPoses.at(loopS->id());
-						}
-
-						const Signature * loopS = _memory->getSignature(loopId);
-						UASSERT(loopS !=0);
-						std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
 						std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
-						if(iterGravityLoop!=loopS->getLinks().end() &&
+						if(iterGravitySign!=signature->getLinks().end())
-						   iterGravitySign!=signature->getLinks().end())
 						{
 							float roll,pitch,yaw;
-							iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
+							float tmp1,tmp2;
-							Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
+							UDEBUG("Gravity link = %s", iterGravitySign->second.transform().prettyPrint().c_str());
-							targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
+							iterGravitySign->second.transform().getEulerAngles(roll, pitch, tmp1);
-							Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
+							newPose.getEulerAngles(tmp1, tmp2, yaw);
-							transform *= error;
+							newPose = Transform(newPose.x(), newPose.y(), newPose.z(), roll, pitch, yaw);
-
-							newPose  = _optimizedPoses.at(loopId) * transform.inverse();
 							UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str());
 						}
-						else if(iterGravityLoop!=loopS->getLinks().end() ||
+						else if(iterGravitySign!=signature->getLinks().end())
-								iterGravitySign!=signature->getLinks().end())
 						{
-							UWARN("Gravity link not found for %d or %d, localization won't be corrected with gravity.", loopId, signature->id());
+							UWARN("Gravity link not found for %d, localization won't be corrected with gravity.", signature->id());
 						}
 					}
 					_optimizedPoses.at(signature->id()) = newPose;
@@ -3225,7 +3328,8 @@ bool Rtabmap::process(
 
 			statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
 			statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
-			statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_only(), proximityDetectionsAddedByICPOnly);
+			statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_multi(), proximityDetectionsAddedByICPMulti);
+			statistics_.addStatistic(Statistics::kProximitySpace_detections_added_icp_global(), proximityDetectionsAddedByICPGlobal);
 			statistics_.addStatistic(Statistics::kProximitySpace_paths(), proximitySpacePaths);
 			statistics_.addStatistic(Statistics::kProximitySpace_visual_paths_checked(), localVisualPathsChecked);
 			statistics_.addStatistic(Statistics::kProximitySpace_scan_paths_checked(), localScanPathsChecked);
@@ -3582,6 +3686,13 @@ bool Rtabmap::process(
 						UDEBUG("Removed %d from local map", iter->first);
 						UASSERT(iter->first != _lastLocalizationNodeId);
 						_optimizedPoses.erase(iter++);
+
+						if(!_globalScanMap.empty())
+						{
+							UWARN("optimized poses have been modified, clearing global scan map...");
+							_globalScanMap.clear();
+							_globalScanMapPoses.clear();
+						}
 					}
 					else
 					{
@@ -3603,6 +3714,8 @@ bool Rtabmap::process(
 		}
 		else
 		{
+			if(!_optimizedPoses.empty())
+				UDEBUG("Optimized poses cleared!");
 			_optimizedPoses.clear();
 			_constraints.clear();
 		}
@@ -5862,4 +5975,87 @@ void Rtabmap::updateGoalIndex()
 	}
 }
 
+void Rtabmap::createGlobalScanMap()
+{
+	UDEBUG("Creating global scan map (if scans are available)");
+	_globalScanMap.clear();
+	_globalScanMapPoses.clear();
+	std::vector<int> scanIndices;
+	std::map<int, Transform> scanViewpoints;
+	for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+	{
+		SensorData data = _memory->getNodeData(iter->first, false, true, false, false);
+		if(!data.laserScanCompressed().empty())
+		{
+			LaserScan scan;
+			data.uncompressDataConst(0, 0, &scan, 0, 0, 0, 0);
+			if(!scan.empty())
+			{
+				scan = util3d::transformLaserScan(scan, iter->second*scan.localTransform());
+				if(_globalScanMap.empty() || _globalScanMap.format() == scan.format())
+				{
+					_globalScanMap += scan;
+					_globalScanMapPoses.insert(*iter);
+					scanViewpoints.insert(std::make_pair(iter->first, iter->second * scan.localTransform()));
+					scanIndices.resize(_globalScanMap.size(), iter->first);
+				}
+				else
+				{
+					UWARN("Incompatible scan formats (%s vs %s), cannot create global scan map.",
+							_globalScanMap.formatName().c_str(),
+							scan.formatName().c_str());
+					_globalScanMap.clear();
+					_globalScanMapPoses.clear();
+					break;
+				}
+			}
+		}
+	}
+	if(_globalScanMap.size() > 3)
+	{
+		float voxelSize = 0.0f;
+		int normalK = 0;
+		float normalRadius = 0.0f;
+		Parameters::parse(_parameters, Parameters::kMemLaserScanVoxelSize(), voxelSize);
+		Parameters::parse(_parameters, Parameters::kMemLaserScanNormalK(), normalK);
+		Parameters::parse(_parameters, Parameters::kMemLaserScanNormalRadius(), normalRadius);
+
+		if(voxelSize > 0.0f)
+		{
+			LaserScan voxelScan = util3d::commonFiltering(_globalScanMap, 1, 0, 0, voxelSize, normalK, normalRadius);
+			if(voxelScan.hasNormals())
+			{
+				// adjust with point of views
+				util3d::adjustNormalsToViewPoints(
+						scanViewpoints,
+						_globalScanMap,
+						scanIndices,
+						voxelScan);
+			}
+			_globalScanMap = voxelScan;
+		}
+
+		UINFO("Global scan map has been assembled (size=%d points, %d poses) "
+				"for proximity detection (only in localization mode %s=false and with %s=false)",
+				(int)_globalScanMap.size(),
+				(int)_globalScanMapPoses.size(),
+				Parameters::kMemIncrementalMemory().c_str(),
+				Parameters::kRGBDProximityPathRawPosesUsed().c_str());
+
+		//for debugging...
+		if(!_globalScanMap.empty() && ULogger::level() == ULogger::kDebug)
+		{
+			UWARN("Saving rtabmap_global_scan_map.pcd (only saved when logger level is debug)");
+			pcl::PCLPointCloud2::Ptr cloud2 = util3d::laserScanToPointCloud2(_globalScanMap);
+			pcl::io::savePCDFile("rtabmap_global_scan_map.pcd", *cloud2);
+		}
+	}
+	if(!_globalScanMap.empty() && _globalScanMap.size()<100)
+	{
+		UWARN("Ignoring global scan map because it is too small (%d points).", (int)_globalScanMap.size());
+		_globalScanMap.clear();
+		_globalScanMapPoses.clear();
+	}
+}
+
 } // namespace rtabmap

corelib/src/Transform.cpp

@@ -214,6 +214,13 @@ Transform Transform::to3DoF() const
 	return Transform(x,y,0, 0,0,yaw);
 }
 
+Transform Transform::to4DoF() const
+{
+	float x,y,z,roll,pitch,yaw;
+	this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+	return Transform(x,y,z, 0,0,yaw);
+}
+
 cv::Mat Transform::rotationMatrix() const
 {
 	return data_.colRange(0, 3).clone();

corelib/src/icp/cccorelib.h

@@ -43,6 +43,7 @@ rtabmap::Transform icpCC(
 		double finalOverlapRatio = 0.85,
 		bool filterOutFarthestPoints = false,
 		double maxFinalRMS = 0.2,
+		float * finalRMS = 0,
 		std::string * errorMsg = 0)
 {
 	UDEBUG("maxIterations=%d", maxIterations);
@@ -119,6 +120,11 @@ rtabmap::Transform icpCC(
 	UDEBUG("CC Final error: %f . Finall Pointcount: %d", finalError, finalPointCount);
 	UDEBUG("CC ICP success Trans: %f %f %f", transform.T.x,transform.T.y,transform.T.z);
 
+	if(finalRMS)
+	{
+		*finalRMS = (float)finalError;
+	}
+
 	if(result != 1)
 	{
 		std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as result %d !=1", result);
@@ -131,9 +137,9 @@ rtabmap::Transform icpCC(
 		icpTransformation.setNull();
 		return icpTransformation;
 	}
-	else if(finalPointCount <10)
+	else if(finalPointCount < 50)
 	{
-		std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as finalPointCount %d < 10 ", finalPointCount);
+		std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as finalPointCount %d < 50 ", finalPointCount);
 		UDEBUG(msg.c_str());
 		if(errorMsg)
 		{

corelib/src/util3d_surface.cpp

@@ -3572,6 +3572,55 @@ void adjustNormalsToViewPoints(
 	}
 }
 
+void adjustNormalsToViewPoints(
+		const std::map<int, Transform> & viewpoints,
+		const LaserScan & rawScan,
+		const std::vector<int> & viewpointIds,
+		LaserScan & scan)
+{
+	UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)viewpoints.size(), (int)rawScan.size(), (int)viewpointIds.size(), (int)scan.size());
+	if(viewpoints.size() && rawScan.size() && rawScan.size() == (int)viewpointIds.size() && scan.size() && scan.hasNormals())
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr rawCloud = util3d::laserScanToPointCloud(rawScan);
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
+		rawTree->setInputCloud (rawCloud);
+		for(int i=0; i<scan.size(); ++i)
+		{
+			pcl::PointNormal point = util3d::laserScanToPointNormal(scan, i);
+			pcl::PointXYZ normal(point.normal_x, point.normal_y, point.normal_z);
+			if(pcl::isFinite(normal))
+			{
+				std::vector<int> indices;
+				std::vector<float> dist;
+				rawTree->nearestKSearch(pcl::PointXYZ(point.x, point.y, point.z), 1, indices, dist);
+				if(indices.size() && indices[0]>=0)
+				{
+					UASSERT_MSG(indices[0]<(int)viewpointIds.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)viewpointIds.size()).c_str());
+					UASSERT(uContains(viewpoints, viewpointIds[indices[0]]));
+					Transform p = viewpoints.at(viewpointIds[indices[0]]);
+					pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
+					Eigen::Vector3f v = viewpoint.getVector3fMap() - point.getVector3fMap();
+
+					Eigen::Vector3f n(normal.x, normal.y, normal.z);
+
+					float result = v.dot(n);
+					if(result < 0)
+					{
+						//reverse normal
+						scan.field(i, scan.getNormalsOffset()) *= -1.0f;
+						scan.field(i, scan.getNormalsOffset()+1) *= -1.0f;
+						scan.field(i, scan.getNormalsOffset()+2) *= -1.0f;
+					}
+				}
+				else
+				{
+					UWARN("Not found camera viewpoint for point %d!?", i);
+				}
+			}
+		}
+	}
+}
+
 pcl::PolygonMesh::Ptr meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor)
 {
 	pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);

guilib/src/MainWindow.cpp

@@ -604,6 +604,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false);
 	_ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", false);
 	_ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", false);
+	_ui->statsToolBox->updateStat("Odometry/ICPRMS/", false);
 	_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
 	_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
 	_ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
@@ -1441,33 +1442,32 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 						}
 					}
 				}
-
+			}
-				if(  _preferencesDialog->isIMUGravityShown(1) &&
+			if(  _preferencesDialog->isIMUGravityShown(1) &&
-					(data->imu().orientation().val[0]!=0 ||
+				(data->imu().orientation().val[0]!=0 ||
-					data->imu().orientation().val[1]!=0 ||
+				data->imu().orientation().val[1]!=0 ||
-					data->imu().orientation().val[2]!=0 ||
+				data->imu().orientation().val[2]!=0 ||
-					data->imu().orientation().val[3]!=0))
+				data->imu().orientation().val[3]!=0))
-				{
+			{
-					Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(1));
+				Eigen::Vector3f gravity(0,0,-_preferencesDialog->getIMUGravityLength(1));
-					Transform orientation(0,0,0, data->imu().orientation()[0], data->imu().orientation()[1], data->imu().orientation()[2], data->imu().orientation()[3]);
+				Transform orientation(0,0,0, data->imu().orientation()[0], data->imu().orientation()[1], data->imu().orientation()[2], data->imu().orientation()[3]);
-					gravity = (orientation* data->imu().localTransform().inverse()*(_odometryCorrection*pose).rotation().inverse()).toEigen3f()*gravity;
+				gravity = (orientation* data->imu().localTransform().inverse()*(_odometryCorrection*pose).rotation().inverse()).toEigen3f()*gravity;
-					_cloudViewer->addOrUpdateLine("odom_imu_orientation", _odometryCorrection*pose, (_odometryCorrection*pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, true);
+				_cloudViewer->addOrUpdateLine("odom_imu_orientation", _odometryCorrection*pose, (_odometryCorrection*pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, true);
-					filteredGravityUpdated = true;
+				filteredGravityUpdated = true;
-				}
+			}
-				if( _preferencesDialog->isIMUAccShown() &&
+			if( _preferencesDialog->isIMUAccShown() &&
-					(data->imu().linearAcceleration().val[0]!=0 ||
+				(data->imu().linearAcceleration().val[0]!=0 ||
-					data->imu().linearAcceleration().val[1]!=0 ||
+				data->imu().linearAcceleration().val[1]!=0 ||
-					data->imu().linearAcceleration().val[2]!=0))
+				data->imu().linearAcceleration().val[2]!=0))
-				{
+			{
-					Eigen::Vector3f gravity(
+				Eigen::Vector3f gravity(
-							-data->imu().linearAcceleration().val[0],
+						-data->imu().linearAcceleration().val[0],
-							-data->imu().linearAcceleration().val[1],
+						-data->imu().linearAcceleration().val[1],
-							-data->imu().linearAcceleration().val[2]);
+						-data->imu().linearAcceleration().val[2]);
-					gravity = gravity.normalized() * _preferencesDialog->getIMUGravityLength(1);
+				gravity = gravity.normalized() * _preferencesDialog->getIMUGravityLength(1);
-					gravity = data->imu().localTransform().toEigen3f()*gravity;
+				gravity = data->imu().localTransform().toEigen3f()*gravity;
-					_cloudViewer->addOrUpdateLine("odom_imu_acc", _odometryCorrection*pose, _odometryCorrection*pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::red, true, true);
+				_cloudViewer->addOrUpdateLine("odom_imu_acc", _odometryCorrection*pose, _odometryCorrection*pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::red, true, true);
-					accelerationUpdated = true;
+				accelerationUpdated = true;
-				}
 			}
 		}
 		if(!dataIgnored)
@@ -1713,6 +1713,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 		_ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpStructuralDistribution, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpCorrespondences, _preferencesDialog->isCacheSavedInFigures());
+		_ui->statsToolBox->updateStat("Odometry/ICPRMS/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRMS, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
 		_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());

guilib/src/PreferencesDialog.cpp

@@ -906,6 +906,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_createMapLabels->setObjectName(Parameters::kMemMapLabelsAdded().c_str());
 	_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
 	_ui->checkBox_localSpaceScanMatchingIDsSaved->setObjectName(Parameters::kRGBDScanMatchingIdsSavedInLinks().c_str());
+	_ui->checkBox_localSpaceCreateGlobalScanMap->setObjectName(Parameters::kRGBDProximityGlobalScanMap().c_str());
 	_ui->spinBox_imagePreDecimation->setObjectName(Parameters::kMemImagePreDecimation().c_str());
 	_ui->spinBox_imagePostDecimation->setObjectName(Parameters::kMemImagePostDecimation().c_str());
 	_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-132</y>
             <width>686</width>
             <height>3905</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>16</number>
+                <number>13</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -11334,26 +11334,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </item>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
-                         <item row="5" column="0">
+                         <item row="1" column="1">
-                          <widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
+                          <widget class="QLabel" name="label_space3_4">
-                           <property name="suffix">
-                            <string> m</string>
-                           </property>
-                           <property name="decimals">
-                            <number>2</number>
-                           </property>
-                           <property name="singleStep">
-                            <double>0.100000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>1.000000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="6" column="1">
-                          <widget class="QLabel" name="label_scanMatching_4">
                            <property name="text">
-                            <string>When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.</string>
+                            <string>Maximum paths compared (from the most recent) for proximity detection. 0 means no limit.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -11363,10 +11347,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="1">
+                         <item row="4" column="1">
-                          <widget class="QLabel" name="label_space3_3">
+                          <widget class="QLabel" name="label_scanMatching_13">
                            <property name="text">
-                            <string>Path filtering radius to reduce the number of nodes to compare in a path in one-to-many proximity detection. The nearest node in a path should be inside that radius to be considered for one-to-one proximity detection.</string>
+                            <string>Use odometry as motion guess for one-to-one proximity detection.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -11376,13 +11360,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="6" column="0">
-                          <widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="0" column="0">
                           <widget class="QSpinBox" name="localDetection_maxDiffID">
                            <property name="maximum">
@@ -11393,52 +11370,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="0" column="1">
-                          <widget class="QLabel" name="label_space3_2">
-                           <property name="text">
-                            <string>Maximum graph depth between the current/last loop closure location and the proximity hypotheses. Set 0 to ignore.</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="1">
-                          <widget class="QLabel" name="label_scanMatching_6">
-                           <property name="text">
-                            <string>Save scan matching IDs from one-to-many proximity detection in link's user data.</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">
-                          <widget class="QCheckBox" name="checkBox_localSpaceScanMatchingIDsSaved">
-                           <property name="text">
-                            <string/>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="3" column="1">
-                          <widget class="QLabel" name="label_scanMatching_8">
-                           <property name="text">
-                            <string>Maximum angle (degrees) for one-to-one proximity detection.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="3" column="0">
                           <widget class="QDoubleSpinBox" name="localDetection_angle">
                            <property name="suffix">
@@ -11455,10 +11386,30 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="1">
+                         <item row="4" column="0">
-                          <widget class="QLabel" name="label_space3_4">
+                          <widget class="QCheckBox" name="checkBox_localSpaceOdomGuess">
                            <property name="text">
-                            <string>Maximum paths compared (from the most recent) for proximity detection. 0 means no limit.</string>
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="5" column="1">
+                          <widget class="QLabel" name="label_space3_3">
+                           <property name="text">
+                            <string>Path filtering radius to reduce the number of nodes to compare in a path in one-to-many proximity detection. The nearest node in a path should be inside that radius to be considered for one-to-one proximity detection.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="0" column="1">
+                          <widget class="QLabel" name="label_space3_2">
+                           <property name="text">
+                            <string>Maximum graph depth between the current/last loop closure location and the proximity hypotheses. Set 0 to ignore.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -11478,6 +11429,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="6" column="1">
+                          <widget class="QLabel" name="label_scanMatching_4">
+                           <property name="text">
+                            <string>When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="2" column="1">
                           <widget class="QLabel" name="label_space3_8">
                            <property name="text">
@@ -11491,6 +11455,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="5" column="0">
+                          <widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
+                           <property name="suffix">
+                            <string> m</string>
+                           </property>
+                           <property name="decimals">
+                            <number>2</number>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.100000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>1.000000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="2" column="0">
                           <widget class="QSpinBox" name="localDetection_maxNeighbors">
                            <property name="maximum">
@@ -11501,10 +11481,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="1">
+                         <item row="6" column="0">
-                          <widget class="QLabel" name="label_scanMatching_13">
+                          <widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
                            <property name="text">
-                            <string>Use odometry as motion guess for one-to-one proximity detection.</string>
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="8" column="1">
+                          <widget class="QLabel" name="label_scanMatching_6">
+                           <property name="text">
+                            <string>Save scan matching IDs from one-to-many proximity detection in link's user data.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -11514,8 +11501,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="0">
+                         <item row="3" column="1">
-                          <widget class="QCheckBox" name="checkBox_localSpaceOdomGuess">
+                          <widget class="QLabel" name="label_scanMatching_8">
+                           <property name="text">
+                            <string>Maximum angle (degrees) for one-to-one proximity detection.</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">
+                          <widget class="QCheckBox" name="checkBox_localSpaceScanMatchingIDsSaved">
+                           <property name="text">
+                            <string/>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="9" column="1">
+                          <widget class="QLabel" name="label_scanMatching_15">
+                           <property name="text">
+                            <string>Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode, otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.</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="QCheckBox" name="checkBox_localSpaceCreateGlobalScanMap">
                            <property name="text">
                             <string/>
                            </property>
@@ -13315,7 +13335,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <number>4</number>
                         </property>
                         <property name="minimum">
-                         <double>-1.0</double>
+                         <double>-1.000000000000000</double>
                         </property>
                         <property name="singleStep">
                          <double>0.010000000000000</double>