New parameter "RGBD/MaxOdomCacheSize" used in localization mode to reject similar locations (default disabled)

app/android/jni/RTABMapApp.cpp

@@ -130,7 +130,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpEpsilon(), std::string("0.001")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxRotation(), std::string("0.17"))); // 10 degrees
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxTranslation(), std::string("0.05")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.5")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.49")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
 
 	parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kKpMaxFeatures()));

corelib/include/rtabmap/core/Parameters.h

@@ -357,6 +357,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
     RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Aruco\" group for parameters.");
     RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
+    RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  0,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false) and when %s!=0. This is used to verify localization transforms to make sure we don't teleport to a location very similar to one we previously localized on. When the cache is full, the whole cache is cleared and the next localization is automatically accepted without verification. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
 
     // Local/Proximity loop closure detection
     RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -268,6 +268,7 @@ private:
 	bool _savedLocalizationIgnored;
 	bool _loopCovLimited;
 	bool _loopGPS;
+	int _maxOdomCacheSize;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
@@ -301,6 +302,8 @@ private:
 	int _lastLocalizationNodeId; // for localization mode
 	std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
 	bool _currentSessionHasGPS;
+	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
 
 	// Planning stuff
 	int _pathStatus;

corelib/src/Graph.cpp

@@ -842,6 +842,7 @@ void computeMaxGraphErrors(
 	maxLinearError = -1;
 	maxAngularError = -1;
 
+	UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
 		// ignore links with high variance, priors and landmarks

corelib/src/Rtabmap.cpp

@@ -125,6 +125,7 @@ Rtabmap::Rtabmap() :
 	_savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
 	_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
 	_loopGPS(Parameters::defaultRtabmapLoopGPS()),
+	_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -361,6 +362,8 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_mapCorrectionBackup.setNull();
 
 	_lastLocalizationNodeId = 0;
+	_odomCachePoses.clear();
+	_odomCacheConstraints.clear();
 	_distanceTravelled = 0.0f;
 	this->clearPath(0);
 	_gpsGeocentricCache.clear();
@@ -482,6 +485,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
 	Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
+	Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
 
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -682,6 +686,8 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
 		{
 			_lastLocalizationPose = initialPose;
 			_lastLocalizationNodeId = 0;
+			_odomCachePoses.clear();
+			_odomCacheConstraints.clear();
 			_mapCorrection.setIdentity();
 			_mapCorrectionBackup.setNull();
 
@@ -717,6 +723,8 @@ int Rtabmap::triggerNewMap()
 		_optimizedPoses.clear();
 		_constraints.clear();
 		_lastLocalizationNodeId = 0;
+		_odomCachePoses.clear();
+		_odomCacheConstraints.clear();
 
 		if(_bayesFilter)
 		{
@@ -864,6 +872,8 @@ void Rtabmap::resetMemory()
 	_mapCorrectionBackup.setNull();
 	_lastLocalizationPose.setNull();
 	_lastLocalizationNodeId = 0;
+	_odomCachePoses.clear();
+	_odomCacheConstraints.clear();
 	_distanceTravelled = 0.0f;
 	this->clearPath(0);
 
@@ -1318,7 +1328,6 @@ bool Rtabmap::process(
 				_constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
 			}
 		}
-		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
 		if(signature->getLinks().size() &&
 		   signature->getLinks().begin()->second.type() == Link::kNeighbor)
 		{
@@ -1345,6 +1354,42 @@ bool Rtabmap::process(
 			}
 			_constraints.insert(std::make_pair(tmp.from(), tmp));
 		}
+		// Localization mode stuff
+		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
+		if(!_memory->isIncremental())
+		{
+			if(_optimizationMaxError <= 0.0f || _maxOdomCacheSize <= 0)
+			{
+				_odomCachePoses.clear();
+				_odomCacheConstraints.clear();
+			}
+			else if(!_odomCachePoses.empty())
+			{
+				if((int)_odomCachePoses.size() > _maxOdomCacheSize)
+				{
+					UWARN("Odometry poses cached for localization verification reached the "
+						  "maximum numbers of %d, clearing the buffer. The next localization "
+						  "won't be verified! Set %s to 0 if you want to disable the localization verification.",
+						  _maxOdomCacheSize,
+						  Parameters::kRGBDMaxOdomCacheSize().c_str());
+					_odomCachePoses.clear();
+					_odomCacheConstraints.clear();
+				}
+				else
+				{
+					_odomCacheConstraints.insert(
+							std::make_pair(signature->id(),
+									Link(signature->id(),
+										 _odomCacheConstraints.rbegin()->first,
+										 Link::kNeighbor,
+										 signature->getPose().inverse() * _odomCachePoses.rbegin()->second,
+										 odomCovariance.inv())));
+					_odomCachePoses.insert(std::make_pair(signature->id(), signature->getPose())); // keep odometry poses
+				}
+
+			}
+		}
+
 		//============================================================
 		// Reduced graph
 		//============================================================
@@ -2513,6 +2558,115 @@ bool Rtabmap::process(
 		   signaturesRetrieved.size() == 0 &&
 		   localizationLinks.size() &&
 		   uContains(_optimizedPoses, localizationLinks.begin()->first))
+		{
+			bool rejectLocalization = false;
+			if(!_odomCachePoses.empty() && _optimizationMaxError > 0.0f)
+			{
+				// Verify if the new localization is valid by checking if there is
+				// not too much deformation in odometry poses since previous localization
+				std::map<int, Transform>::iterator iter = _optimizedPoses.find(_odomCacheConstraints.find(_odomCachePoses.begin()->first)->second.to());
+				Transform optPoseRefA;
+				Transform optPoseRefB;
+				if(iter != _optimizedPoses.end());
+				{
+					optPoseRefA = iter->second * _odomCacheConstraints.find(_odomCachePoses.begin()->first)->second.transform().inverse();
+				}
+				iter = _optimizedPoses.find(localizationLinks.begin()->first);
+				if(iter != _optimizedPoses.end())
+				{
+					optPoseRefB = iter->second * localizationLinks.begin()->second.transform().inverse();
+				}
+				if(optPoseRefA.isNull() || optPoseRefB.isNull())
+				{
+					UWARN("Both optimized pose references are null! Flushing cached odometry poses. Localization won't be verified.");
+					_odomCachePoses.clear();
+					_constraints.clear();
+				}
+				else
+				{
+					std::multimap<int, Link> constraints = _odomCacheConstraints;
+					constraints.erase(_odomCachePoses.begin()->first);
+					constraints.insert(std::make_pair(_odomCachePoses.begin()->first,
+							Link(_odomCachePoses.begin()->first, signature->id(), Link::kVirtualClosure,
+									optPoseRefA.inverse() * optPoseRefB, cv::Mat::eye(6,6,CV_64FC1)*100)));
+
+					std::map<int, Transform> optPoses = _graphOptimizer->optimize(signature->id(), _odomCachePoses, constraints);
+
+					if(optPoses.empty())
+					{
+						UWARN("Optimization failed, rejecting localization!");
+						rejectLocalization = true;
+					}
+					else
+					{
+						UINFO("Compute max graph errors...");
+						const Link * maxLinearLink = 0;
+						const Link * maxAngularLink = 0;
+						graph::computeMaxGraphErrors(
+								optPoses,
+								constraints,
+								maxLinearErrorRatio,
+								maxAngularErrorRatio,
+								maxLinearError,
+								maxAngularError,
+								&maxLinearLink,
+								&maxAngularLink);
+						if(maxLinearLink == 0 && maxAngularLink==0)
+						{
+							UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
+						}
+
+						if(maxLinearLink)
+						{
+							UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
+							if(maxLinearErrorRatio > _optimizationMaxError)
+							{
+								UWARN("Rejecting localization (%d <-> %d) in this "
+									  "iteration because a wrong loop closure has been "
+									  "detected after graph optimization, resulting in "
+									  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
+									  "maximum error ratio parameter \"%s\" is %f of std deviation.",
+									  loopClosureLinksAdded.front().first,
+									  loopClosureLinksAdded.front().second,
+									  maxLinearErrorRatio,
+									  maxLinearLink->from(),
+									  maxLinearLink->to(),
+									  maxLinearLink->type(),
+									  maxLinearError,
+									  sqrt(maxLinearLink->transVariance()),
+									  Parameters::kRGBDOptimizeMaxError().c_str(),
+									  _optimizationMaxError);
+								rejectLocalization = true;
+							}
+						}
+						if(maxAngularLink)
+						{
+							UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
+							if(maxAngularErrorRatio > _optimizationMaxError)
+							{
+								UWARN("Rejecting localization (%d <-> %d) in this "
+									  "iteration because a wrong loop closure has been "
+									  "detected after graph optimization, resulting in "
+									  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
+									  "maximum error ratio parameter \"%s\" is %f of std deviation.",
+									  loopClosureLinksAdded.front().first,
+									  loopClosureLinksAdded.front().second,
+									  maxAngularErrorRatio,
+									  maxAngularLink->from(),
+									  maxAngularLink->to(),
+									  maxAngularLink->type(),
+									  maxAngularError*180.0f/CV_PI,
+									  sqrt(maxAngularLink->rotVariance()),
+									  Parameters::kRGBDOptimizeMaxError().c_str(),
+									  _optimizationMaxError);
+								rejectLocalization = true;
+							}
+						}
+					}
+				}
+			}
+
+			if(!rejectLocalization)
 			{
 				// If there are no signatures retrieved, we don't
 				// need to re-optimize the graph. Just update the last
@@ -2599,6 +2753,21 @@ bool Rtabmap::process(
 					_optimizedPoses.at(signature->id()) = newPose;
 				}
 				localizationCovariance = localizationLinks.begin()->second.infMatrix().inv();
+
+				_odomCachePoses.clear();
+				_odomCacheConstraints.clear();
+				if(_optimizationMaxError > 0.0f && _maxOdomCacheSize > 0)
+				{
+					_odomCachePoses.insert(std::make_pair(signature->id(), signature->getPose()));
+					_odomCacheConstraints.insert(std::make_pair(signature->id(), localizationLinks.begin()->second));
+				}
+			}
+			else
+			{
+				_loopClosureHypothesis.first = 0;
+				lastProximitySpaceClosureId = 0;
+				rejectedHypothesis = true;
+			}
 		}
 		else
 		{
@@ -2989,6 +3158,14 @@ bool Rtabmap::process(
 			_memory->saveLocationData(signature->id());
 		}
 	}
+	else if(!_memory->isIncremental() &&
+			(smallDisplacement || tooFastMovement) &&
+			_loopClosureHypothesis.first == 0 &&
+			lastProximitySpaceClosureId == 0)
+	{
+		_odomCachePoses.erase(signatureRemoved);
+		_odomCacheConstraints.erase(signatureRemoved);
+	}
 
 	// Pass this point signature should not be used, since it could have been transferred...
 	signature = 0;

guilib/src/PreferencesDialog.cpp

@@ -929,6 +929,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_bunlde->setObjectName(Parameters::kRGBDLocalBundleOnLoopClosure().c_str());
 	_ui->checkbox_rgbd_createOccupancyGrid->setObjectName(Parameters::kRGBDCreateOccupancyGrid().c_str());
 	_ui->RGBDMarkerDetection->setObjectName(Parameters::kRGBDMarkerDetection().c_str());
+	_ui->spinBox_maxOdomCacheSize->setObjectName(Parameters::kRGBDMaxOdomCacheSize().c_str());
 
 	// Registration
 	_ui->reg_repeatOnce->setObjectName(Parameters::kRegRepeatOnce().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -94,7 +94,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-184</y>
             <width>680</width>
             <height>3082</height>
            </rect>
@@ -126,7 +126,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>14</number>
+                <number>12</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -9165,6 +9165,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="9" column="0">
+                        <widget class="QCheckBox" name="memCovOffDiagIgnored">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="8" column="1">
                         <widget class="QLabel" name="label_scanMatching_10">
                          <property name="text">
@@ -9191,13 +9198,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="0">
-                        <widget class="QCheckBox" name="memCovOffDiagIgnored">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="0" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
                          <property name="suffix">
@@ -9224,6 +9224,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="8" column="0">
+                        <widget class="QCheckBox" name="loopClosure_bunlde">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="1" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_angularUpdate">
                          <property name="suffix">
@@ -9253,13 +9260,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="0">
-                        <widget class="QCheckBox" name="loopClosure_bunlde">
-                         <property name="text">
-                          <string/>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="13" column="1">
                         <widget class="QLabel" name="label_space2">
                          <property name="text">
@@ -9286,6 +9286,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="10" column="0">
+                        <widget class="QCheckBox" name="rgbd_loopCovLimited">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                        <item row="3" column="0">
                         <widget class="QDoubleSpinBox" name="rgdb_angularSpeedUpdate">
                          <property name="suffix">
@@ -9315,10 +9322,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="0">
+                       <item row="14" column="1">
-                        <widget class="QCheckBox" name="rgbd_loopCovLimited">
+                        <widget class="QLabel" name="label_space2_5">
                          <property name="text">
-                          <string/>
+                          <string>Maximum odometry cache size. Used only in localization mode and when maximum graph error parameter is not null. This is used to verify localization transforms to make sure we don't teleport to a location very similar to one we previously localized on. When the cache is full, the whole cache is cleared and the next localization is automatically accepted without verification. Set 0 to disable caching.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="14" column="0">
+                        <widget class="QSpinBox" name="spinBox_maxOdomCacheSize">
+                         <property name="maximum">
+                          <number>9999</number>
                          </property>
                         </widget>
                        </item>