Merge branch 'master' of github.com:introlab/rtabmap

corelib/include/rtabmap/core/CameraRGBD.h

@@ -278,6 +278,7 @@ public:
 	enum Type{
 		kTypeRGBDepthSD,
 		kTypeRGBDepthHD,
+		kTypeIRDepth,
 		kTypeRGBIR
 	};
 

corelib/src/CameraRGBD.cpp

@@ -1126,6 +1126,9 @@ CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const
 	case kTypeRGBIR:
 		listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
 		break;
+	case kTypeIRDepth:
+		listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
+		break;
 	case kTypeRGBDepthSD:
 	case kTypeRGBDepthHD:
 	default:
@@ -1318,10 +1321,14 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 
 			switch(type_)
 			{
-			case kTypeRGBIR:
+			case kTypeRGBIR: //used for calibration
 				rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
 				irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
 				break;
+			case kTypeIRDepth:
+				irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
+				depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
+				break;
 			case kTypeRGBDepthSD:
 			case kTypeRGBDepthHD:
 			default:
@@ -1330,6 +1337,48 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 				break;
 			}
 
+			if(irFrame && depthFrame)
+			{
+				cv::Mat irMat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data);
+				//convert to gray scaled
+				float maxIr_ = 0x7FFF;
+				float minIr_ = 0x0;
+				const float factor = 255.0f / float((maxIr_ - minIr_));
+				rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
+				for(int i=0; i<irMat.rows; ++i)
+				{
+					for(int j=0; j<irMat.cols; ++j)
+					{
+						rgb.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
+					}
+				}
+
+				cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+				cv::flip(rgb, rgb, 1);
+				cv::flip(depth, depth, 1);
+				if(stereoModel_.isValid())
+				{
+					//rectify
+					rgb = stereoModel_.left().rectifyImage(rgb);
+					depth = stereoModel_.left().rectifyImage(depth);
+					fx = stereoModel_.left().fx();
+					fy = stereoModel_.left().fy();
+					cx = stereoModel_.left().cx();
+					cy = stereoModel_.left().cy();
+				}
+				else
+				{
+					libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
+					fx = params.fx;
+					fy = params.fy;
+					cx = params.cx;
+					cy = params.cy;
+				}
+			}
+			else
+			{
+				//rgb + ir or rgb + depth
+
 				cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data);
 				cv::flip(rgbMat, rgb, 1);
 
@@ -1453,6 +1502,7 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 					}
 					cv::flip(depth, depth, 1);
 				}
+			}
 			listener_->release(frames);
 		}
 		else

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -47,7 +47,7 @@ class RTABMAPGUI_EXP CalibrationDialog  : public QDialog, public UEventsHandler
 	Q_OBJECT;
 
 public:
-	CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", QWidget * parent = 0);
+	CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", bool switchImages = false, QWidget * parent = 0);
 	virtual ~CalibrationDialog();
 
 	bool isCalibrated() const {return models_[0].isValid() && (stereo_?models_[1].isValid():true);}
@@ -58,6 +58,7 @@ public:
 	void saveSettings(QSettings & settings, const QString & group = "") const;
 	void loadSettings(QSettings & settings, const QString & group = "");
 
+	void setSwitchedImages(bool switched);
 	void setStereoMode(bool stereo);
 	void setSavingDirectory(const QString & savingDirectory) {savingDirectory_ = savingDirectory;}
 

guilib/src/CalibrationDialog.cpp

@@ -46,7 +46,7 @@ namespace rtabmap {
 
 #define COUNT_MIN 40
 
-CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, QWidget * parent) :
+CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, bool switchImages, QWidget * parent) :
 	QDialog(parent),
 	stereo_(stereo),
 	savingDirectory_(savingDirectory),
@@ -91,6 +91,8 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 
 	ui_->radioButton_raw->setChecked(true);
 
+	ui_->checkBox_switchImages->setChecked(switchImages);
+
 	this->setStereoMode(stereo_);
 }
 
@@ -136,6 +138,11 @@ void CalibrationDialog::loadSettings(QSettings & settings, const QString & group
 	}
 }
 
+void CalibrationDialog::setSwitchedImages(bool switched)
+{
+	ui_->checkBox_switchImages->setChecked(switched);
+}
+
 void CalibrationDialog::setStereoMode(bool stereo)
 {
 	this->restart();

guilib/src/PreferencesDialog.cpp

@@ -3523,6 +3523,7 @@ void PreferencesDialog::calibrate()
 		}
 	}
 	_calibrationDialog->setStereoMode(true);
+	_calibrationDialog->setSwitchedImages(dynamic_cast<CameraFreenect2*>(camera) != 0);
 	_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 	_calibrationDialog->registerToEventsManager();
 

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-485</y>
+            <y>-591</y>
             <width>760</width>
-            <height>1132</height>
+            <height>1502</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -1974,6 +1974,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               <string>RGB+Depth HD</string>
                              </property>
                             </item>
+                            <item>
+                             <property name="text">
+                              <string>IR+Depth</string>
+                             </property>
+                            </item>
                            </widget>
                           </item>
                          </layout>

tools/Calibration/main.cpp

@@ -128,6 +128,8 @@ int main(int argc, char * argv[])
 	UINFO("Using device %d", device);
 	UINFO("Stereo: %s", stereo?"true":"false");
 
+	bool switchImages = false;
+
 	rtabmap::Camera * cameraUsb = 0;
 	rtabmap::CameraRGBD * camera = 0;
 	if(driver == -1)
@@ -181,6 +183,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Freenect2 support...");
 			exit(-1);
 		}
+		switchImages = true;
 		camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBIR);
 	}
 	else if(driver == 6)
@@ -230,7 +233,7 @@ int main(int argc, char * argv[])
 	}
 
 	QApplication app(argc, argv);
-	rtabmap::CalibrationDialog dialog(stereo);
+	rtabmap::CalibrationDialog dialog(stereo, ".", switchImages);
 	dialog.registerToEventsManager();
 
 	dialog.show();