Added parameter Rtabmap/LoopGPS (default true) to enabled/disable the use of GPS in likelihood computation

corelib/include/rtabmap/core/Parameters.h

@@ -193,6 +193,7 @@ class RTABMAP_EXP Parameters
     // Hypotheses selection
     RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
     RTABMAP_PARAM(Rtabmap, LoopRatio,         float, 0,         "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
+    RTABMAP_PARAM(Rtabmap, LoopGPS,           bool,  true,      uFormat("Use GPS to filter likelihood (if GPS is recorded). Only locations inside the local radius \"%s\" of the current GPS location are considered for loop closure detection.", kRGBDLocalRadius().c_str()));
 
     // Memory
     RTABMAP_PARAM(Mem, RehearsalSimilarity,         float, 0.6,     "Rehearsal similarity.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -261,6 +261,7 @@ private:
 	float _pathAngularVelocity;
 	bool _savedLocalizationIgnored;
 	bool _loopCovLimited;
+	bool _loopGPS;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;

corelib/src/Rtabmap.cpp

@@ -124,6 +124,7 @@ Rtabmap::Rtabmap() :
 	_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
 	_savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
 	_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
+	_loopGPS(Parameters::defaultRtabmapLoopGPS()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -469,6 +470,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
 	Parameters::parse(parameters, Parameters::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
 	Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
+	Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
 
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1399,37 +1401,41 @@ bool Rtabmap::process(
 			ULOGGER_INFO("computing likelihood...");
 
 			std::list<int> signaturesToCompare;
-			GPS originGPS = signature->sensorData().gps();
+			GPS originGPS;
 			Transform originOffsetENU = Transform::getIdentity();
-			if(originGPS.stamp() == 0.0 && _currentSessionHasGPS)
+			if(_loopGPS)
 			{
-				UTimer tmpT;
+				originGPS = signature->sensorData().gps();
-				if(_optimizedPoses.size() && _memory->isIncremental())
+				if(originGPS.stamp() == 0.0 && _currentSessionHasGPS)
 				{
-					//Search for latest node having GPS linked to current signature not too far.
+					UTimer tmpT;
-					std::map<int, float> nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
+					if(_optimizedPoses.size() && _memory->isIncremental())
-					for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend(); ++iter)
 					{
-						const Signature * s = _memory->getSignature(iter->first);
+						//Search for latest node having GPS linked to current signature not too far.
-						UASSERT(s!=0);
+						std::map<int, float> nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
-						if(s->sensorData().gps().stamp() > 0.0)
+						for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend(); ++iter)
 						{
-							originGPS = s->sensorData().gps();
+							const Signature * s = _memory->getSignature(iter->first);
-							const Transform & sPose = _optimizedPoses.at(s->id());
+							UASSERT(s!=0);
-							Transform localToENU(0,0,(float)((-(originGPS.bearing()-90))*M_PI/180.0) - sPose.theta());
+							if(s->sensorData().gps().stamp() > 0.0)
-							originOffsetENU = localToENU * (sPose.rotation()*(sPose.inverse()*_optimizedPoses.at(signature->id())));
+							{
-							break;
+								originGPS = s->sensorData().gps();
+								const Transform & sPose = _optimizedPoses.at(s->id());
+								Transform localToENU(0,0,(float)((-(originGPS.bearing()-90))*M_PI/180.0) - sPose.theta());
+								originOffsetENU = localToENU * (sPose.rotation()*(sPose.inverse()*_optimizedPoses.at(signature->id())));
+								break;
+							}
 						}
 					}
+					//else if(!_memory->isIncremental()) // TODO, how can we estimate current GPS position in localization?
+					//{
+					//}
+				}
+				if(originGPS.stamp() > 0.0)
+				{
+					// no need to save it if it is in localization mode
+					_gpsGeocentricCache.insert(std::make_pair(signature->id(), std::make_pair(originGPS.toGeodeticCoords().toGeocentric_WGS84(), originOffsetENU)));
 				}
-				//else if(!_memory->isIncremental()) // TODO, how can we estimate current GPS position in localization?
-				//{
-				//}
-			}
-			if(originGPS.stamp() > 0.0)
-			{
-				// no need to save it if it is in localization mode
-				_gpsGeocentricCache.insert(std::make_pair(signature->id(), std::make_pair(originGPS.toGeodeticCoords().toGeocentric_WGS84(), originOffsetENU)));
 			}
 
 			for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();

guilib/src/PreferencesDialog.cpp

@@ -709,6 +709,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_startNewMapOnGoodSignature->setObjectName(Parameters::kRtabmapStartNewMapOnGoodSignature().c_str());
 	_ui->general_checkBox_imagesAlreadyRectified->setObjectName(Parameters::kRtabmapImagesAlreadyRectified().c_str());
 	_ui->general_checkBox_rectifyOnlyFeatures->setObjectName(Parameters::kRtabmapRectifyOnlyFeatures().c_str());
+	_ui->checkBox_rtabmap_loopGPS->setObjectName(Parameters::kRtabmapLoopGPS().c_str());
 	_ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str());
 	connect(_ui->toolButton_workingDirectory, SIGNAL(clicked()), this, SLOT(changeWorkingDirectory()));
 

guilib/src/ui/preferencesDialog.ui

@@ -94,7 +94,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-57</y>
             <width>681</width>
             <height>2983</height>
            </rect>
@@ -126,7 +126,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>24</number>
+                <number>11</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -8704,6 +8704,26 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
                         </property>
                        </widget>
                       </item>
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_502">
+                        <property name="text">
+                         <string>Use GPS to filter likelihood (if GPS is recorded). Only locations inside the Local Radius (see RGBD-SLAM panel) of the current GPS location are considered for loop closure 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="QCheckBox" name="checkBox_rtabmap_loopGPS">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>