New parameters: max accepted ICP translation error, new map on large odometry error

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1126 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Parameters.h

@@ -204,6 +204,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ScanMatchingSize,  int, 0, 		"Laser scan matching history for odometry correction (laser scans are required). Set to 0 to disable odometry correction.");
 	RTABMAP_PARAM(RGBD, LinearUpdate,      float, 0.0, 	"Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 1, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
 	RTABMAP_PARAM(RGBD, ToroIterations,    int, 100,    "TORO graph optimization iterations")
 
 	// Local loop closure detection
@@ -238,6 +239,7 @@ class RTABMAP_EXP Parameters
 
 	// Loop closure constraint
 	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");
+	RTABMAP_PARAM(LccIcp, MaxDistance,     float, 0.2,     "Maximum ICP correction distance accepted (m).");
 
 	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
 	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.01, 	"Maximum distance for visual word correspondences.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -141,7 +141,9 @@ private:
 	bool _rgbdSlamMode;
 	float _rgbdLinearUpdate;
 	float _rgbdAngularUpdate;
+	float _newMapOdomChangeDistance;
 	int _globalLoopClosureIcpType;
+	float _globalLoopClosureIcpMaxDistance;
 	int _scanMatchingSize;
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;

corelib/include/rtabmap/core/Transform.h

@@ -52,6 +52,9 @@ public:
 	Transform translation() const;
 
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
+	void getTranslation(float & x, float & y, float & z) const;
+	float getNorm() const;
+	float getNormSquared() const;
 	std::string prettyPrint() const;
 
 	Transform operator*(const Transform & t) const;

corelib/src/Rtabmap.cpp

@@ -79,7 +79,9 @@ Rtabmap::Rtabmap() :
 	_rgbdSlamMode(Parameters::defaultRGBDEnabled()),
 	_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
 	_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
+	_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
 	_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
+	_globalLoopClosureIcpMaxDistance(Parameters::defaultLccIcpMaxDistance()),
 	_scanMatchingSize(Parameters::defaultRGBDScanMatchingSize()),
 	_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
 	_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
@@ -327,7 +329,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), _rgbdSlamMode);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
+	Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
 	Parameters::parse(parameters, Parameters::kRGBDScanMatchingSize(), _scanMatchingSize);
+	Parameters::parse(parameters, Parameters::kLccIcpMaxDistance(), _globalLoopClosureIcpMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionRadius(), _localDetectRadius);
@@ -739,11 +743,7 @@ bool Rtabmap::process(const Image & image)
 			// Detect if the odometry is reset. If yes, trigger a new map.
 			if(_memory->getLastWorkingSignature())
 			{
-				Transform lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
+				const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
-				Transform lastPoseToNewPose = lastPose.inverse() * image.pose();
-				float x,y,z, roll,pitch,yaw;
-				lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-				// TODO Increment map id also if there is a big position change
 				if(!lastPose.isIdentity() && image.pose().isIdentity())
 				{
 					int mapId = _memory->incrementMapId();
@@ -751,6 +751,23 @@ bool Rtabmap::process(const Image & image)
 					_optimizedPoses.clear();
 					_constraints.clear();
 				}
+				else
+				{
+					Transform lastPoseToNewPose = lastPose.inverse() * image.pose();
+					float x,y,z, roll,pitch,yaw;
+					lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+					if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
+					{
+						int mapId = _memory->incrementMapId();
+						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(),
+								image.pose().prettyPrint().c_str());
+						_optimizedPoses.clear();
+						_constraints.clear();
+					}
+				}
 			}
 		}
 	}
@@ -892,7 +909,20 @@ bool Rtabmap::process(const Image & image)
 					Transform transform = _memory->computeVisualTransform(*iter, signature->id());
 					if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 					{
-						transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1);
+						Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1);
+						float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
+						if(!icpTransform.isNull() &&
+							_globalLoopClosureIcpMaxDistance>0.0f &&
+							squaredNorm > _globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance)
+						{
+							UWARN("Local loop closure rejected (%d->%d) (ICP correction too large %f > %f [squared norm])",
+									signature->id(),
+									*iter,
+									squaredNorm,
+									_globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance);
+							icpTransform.setNull();
+						}
+						transform = icpTransform;
 					}
 					if(!transform.isNull())
 					{
@@ -1218,7 +1248,20 @@ bool Rtabmap::process(const Image & image)
 			transform = _memory->computeVisualTransform(_lcHypothesisId, signature->id());
 			if(!transform.isNull() && _globalLoopClosureIcpType > 0)
 			{
-				transform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1);
+				Transform icpTransform  = _memory->computeIcpTransform(_lcHypothesisId, signature->id(), transform, _globalLoopClosureIcpType == 1);
+				float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
+				if(!icpTransform.isNull() &&
+					_globalLoopClosureIcpMaxDistance>0.0f &&
+					squaredNorm > _globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance)
+				{
+					UWARN("Global loop closure rejected (%d->%d) (ICP correction too large %f > %f [squared norm])",
+							signature->id(),
+							_lcHypothesisId,
+							squaredNorm,
+							_globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance);
+					icpTransform.setNull();
+				}
+				transform = icpTransform;
 			}
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)

corelib/src/Transform.cpp

@@ -143,6 +143,23 @@ void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, fl
 	pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(*this), x, y, z, roll, pitch, yaw);
 }
 
+void Transform::getTranslation(float & x, float & y, float & z) const
+{
+	x = this->x();
+	y = this->y();
+	z = this->z();
+}
+
+float Transform::getNorm() const
+{
+	return std::sqrt(this->getNorm());
+}
+
+float Transform::getNormSquared() const
+{
+	return this->x()*this->x() + this->y()*this->y() + this->z()*this->z();
+}
+
 std::string Transform::prettyPrint() const
 {
 	float x,y,z,roll,pitch,yaw;

guilib/src/MainWindow.cpp

@@ -1003,7 +1003,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		_ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
 	}
 	float elapsedTime = static_cast<float>(totalTime.elapsed());
-	UINFO("Processing statistics time = %fs", elapsedTime/1000.0f);
+	UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
 	_ui->statsToolBox->updateStat("/Gui refresh stats/ms", stat.refImageId(), elapsedTime);
 	if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
 	{

guilib/src/PreferencesDialog.cpp

@@ -326,6 +326,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_activateRGBD->setObjectName(Parameters::kRGBDEnabled().c_str());
 	_ui->rgdb_linearUpdate->setObjectName(Parameters::kRGBDLinearUpdate().c_str());
 	_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
+	_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
 	_ui->odomScanHistory->setObjectName(Parameters::kRGBDScanMatchingSize().c_str());
 	_ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str());
 
@@ -341,6 +342,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kLccBowMaxDepth().c_str());
 
 	_ui->globalDetection_icpType->setObjectName(Parameters::kLccIcpType().c_str());
+	_ui->globalDetection_icpMaxDistance->setObjectName(Parameters::kLccIcpMaxDistance().c_str());
 
 	_ui->loopClosure_icpDecimation->setObjectName(Parameters::kLccIcp3Decimation().c_str());
 	_ui->loopClosure_icpMaxDepth->setObjectName(Parameters::kLccIcp3MaxDepth().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>777</width>
+            <width>761</width>
-            <height>826</height>
+            <height>981</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>12</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -3733,6 +3733,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_163">
+                         <property name="text">
+                          <string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="rgdb_newMapOdomChange">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <double>99.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -3821,14 +3850,14 @@ 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="2" column="1">
                           <widget class="QLabel" name="label_31">
                            <property name="text">
                             <string>TORO graph optimization iterations</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="1" column="0">
+                         <item row="2" column="0">
                           <widget class="QSpinBox" name="globalDetection_toroIterations">
                            <property name="minimum">
                             <number>1</number>
@@ -3841,6 +3870,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_51">
+                           <property name="text">
+                            <string>Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="0">
+                          <widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
+                           <property name="suffix">
+                            <string> m</string>
+                           </property>
+                           <property name="decimals">
+                            <number>2</number>
+                           </property>
+                           <property name="maximum">
+                            <double>1.000000000000000</double>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.200000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>