Added check to make sure input odometry poses are invertible. Source/DB: added stereo to depth option.

corelib/include/rtabmap/core/Transform.h

@@ -98,6 +98,7 @@ public:
 
 	float theta() const;
 
+	bool isInvertible() const;
 	Transform inverse() const;
 	Transform rotation() const;
 	Transform translation() const;

corelib/src/Memory.cpp

@@ -886,7 +886,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 	// add signature on top of the short-term memory
 	if(signature)
 	{
-		UDEBUG("adding %d", signature->id());
+		UDEBUG("adding %d (pose=%s)", signature->id(), signature->getPose().prettyPrint().c_str());
 		// Update neighbors
 		if(_stMem.size())
 		{

corelib/src/Rtabmap.cpp

@@ -1071,6 +1071,43 @@ bool Rtabmap::process(
 	bool fakeOdom = false;
 	if(_rgbdSlamMode)
 	{
+		if(!odomPose.isNull())
+		{
+			// this will make sure that all inverse operations will work!
+			if(!odomPose.isInvertible())
+			{
+				UWARN("Input odometry is not invertible! pose = %s\n"
+						"[%f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" 0 0 0 1]\n"
+						"Trying to normalize rotation to see if it makes it invertible...",
+						odomPose.prettyPrint().c_str(),
+						odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
+						odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
+						odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
+				odomPose.normalizeRotation();
+				UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible!\n"
+						"[%f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" 0 0 0 1]", odomPose.prettyPrint().c_str(),
+						odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
+						odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
+						odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34()).c_str());
+				UWARN("Normalizing rotation succeeded! fixed pose = %s\n"
+						"[%f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" %f %f %f %f;\n"
+						" 0 0 0 1]\n"
+						"If the resulting rotation is very different from original one, try to fix the odometry or TF.",
+						odomPose.prettyPrint().c_str(),
+						odomPose.r11(), odomPose.r12(), odomPose.r13(), odomPose.o14(),
+						odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
+						odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
+			}
+		}
+
 		if(!_memory->isIncremental() &&
 			!odomPose.isNull() &&
 			_optimizedPoses.size() &&
@@ -1238,6 +1275,7 @@ bool Rtabmap::process(
 							// This will disable global loop closure detection, only retrieval will be done.
 							// The location will also be deleted at the end.
 							smallDisplacement = true;
+							UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
 						}
 					}
 				}

corelib/src/Transform.cpp

@@ -166,9 +166,30 @@ float Transform::theta() const
 	return yaw;
 }
 
+bool Transform::isInvertible() const
+{
+	bool invertible = false;
+	Eigen::Matrix4f inverse;
+	Eigen::Matrix4f::RealScalar det;
+	toEigen4f().computeInverseAndDetWithCheck(inverse, det, invertible);
+	return invertible;
+}
+
 Transform Transform::inverse() const
 {
-	return fromEigen4f(toEigen4f().inverse());
+	bool invertible = false;
+	Eigen::Matrix4f inverse;
+	Eigen::Matrix4f::RealScalar det;
+	toEigen4f().computeInverseAndDetWithCheck(inverse, det, invertible);
+	UASSERT_MSG(invertible, uFormat("This transform is not invertible! %s \n"
+			"[%f %f %f %f;\n"
+			" %f %f %f %f;\n"
+			" %f %f %f %f;\n"
+			" 0 0 0 1]", prettyPrint().c_str(),
+			r11(), r12(), r13(), o14(),
+			r21(), r22(), r23(), o24(),
+			r31(), r32(), r33(), o34()).c_str());
+	return fromEigen4f(inverse);
 }
 
 Transform Transform::rotation() const

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -249,6 +249,7 @@ public:
 	QString getSourceDevice() const;
 
 	bool isSourceDatabaseStampsUsed() const;
+	bool isSourceDatabaseStereoToDepth() const;
 	bool isSourceRGBDColorOnly() const;
 	int getIMUFilteringStrategy() const;
 	bool isDepthFilteringAvailable() const;

guilib/src/PreferencesDialog.cpp

@@ -645,6 +645,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->source_spinBox_databaseStopId, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_checkBox_useDbStamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->source_spinBox_database_cameraIndex, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->source_checkBox_stereoToDepthDB, SIGNAL(toggled(bool)), _ui->checkbox_stereo_depthGenerated, SLOT(setChecked(bool)));
+	connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(toggled(bool)), _ui->source_checkBox_stereoToDepthDB, SLOT(setChecked(bool)));
 
 	//openni group
 	_ui->stackedWidget_rgbd->setCurrentIndex(_ui->comboBox_cameraRGBD->currentIndex());

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-27</y>
+            <y>-498</y>
             <width>686</width>
             <height>3236</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>9</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3109,7 +3109,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>0</number>
+                       <number>3</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -5877,6 +5877,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
+                           <item row="1" column="0">
+                            <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
                            <item row="0" column="2">
                             <widget class="QToolButton" name="toolButton_dbViewer">
                              <property name="toolTip">
@@ -5891,13 +5898,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
-                           <item row="1" column="0">
-                            <widget class="QCheckBox" name="source_checkBox_ignoreOdometry">
-                             <property name="text">
-                              <string/>
-                             </property>
-                            </widget>
-                           </item>
                            <item row="0" column="0">
                             <widget class="QToolButton" name="source_database_toolButton_selectSource">
                              <property name="text">
@@ -5908,7 +5908,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item row="0" column="1">
                             <widget class="QLineEdit" name="source_database_lineEdit_path"/>
                            </item>
-                           <item row="8" column="1">
+                           <item row="9" column="1">
                             <spacer name="verticalSpacer_35">
                              <property name="orientation">
                               <enum>Qt::Vertical</enum>
@@ -6040,6 +6040,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </property>
                             </widget>
                            </item>
+                           <item row="8" column="1">
+                            <widget class="QLabel" name="label_557">
+                             <property name="text">
+                              <string>If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab.</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="source_checkBox_stereoToDepthDB">
+                             <property name="text">
+                              <string/>
+                             </property>
+                            </widget>
+                           </item>
                           </layout>
                          </widget>
                         </item>