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,128 +1337,171 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 				break;
 			}
 
-			cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data);
+			if(irFrame && depthFrame)
-			cv::flip(rgbMat, rgb, 1);
-
-			if(stereoModel_.isValid())
 			{
-				//rectify color
+				cv::Mat irMat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data);
-				rgb = stereoModel_.right().rectifyImage(rgb);
+				//convert to gray scaled
-				if(irFrame)
+				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)
 				{
-					//rectify IR
+					for(int j=0; j<irMat.cols; ++j)
-					cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
+					{
-					cv::flip(depth, depth, 1);
+						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
 				{
-					//rectify depth
+					libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
-					cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+					fx = params.fx;
-					cv::flip(depth, depth, 1);
+					fy = params.fy;
-					depth = stereoModel_.left().rectifyDepth(depth);
+					cx = params.cx;
-
+					cy = params.cy;
-					bool registered = true;
-					if(registered)
-					{
-						depth = util3d::registerDepth(
-								depth,
-								stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
-								stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
-								stereoModel_.transform());
-						util3d::fillRegisteredDepthHoles(depth, true, false);
-						fx = stereoModel_.right().fx();
-						fy = stereoModel_.right().fy();
-						cx = stereoModel_.right().cx();
-						cy = stereoModel_.right().cy();
-					}
-					else
-					{
-						fx = stereoModel_.left().fx();
-						fy = stereoModel_.left().fy();
-						cx = stereoModel_.left().cx();
-						cy = stereoModel_.left().cy();
-					}
 				}
 			}
 			else
 			{
-				//use data from libfreenect2
+				//rgb + ir or rgb + depth
-				if(irFrame)
+
+				cv::Mat rgbMat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data);
+				cv::flip(rgbMat, rgb, 1);
+
+				if(stereoModel_.isValid())
 				{
-					cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
+					//rectify color
+					rgb = stereoModel_.right().rectifyImage(rgb);
+					if(irFrame)
+					{
+						//rectify IR
+						cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
+						cv::flip(depth, depth, 1);
+						depth = stereoModel_.left().rectifyImage(depth);
+					}
+					else
+					{
+						//rectify depth
+						cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+						cv::flip(depth, depth, 1);
+						depth = stereoModel_.left().rectifyDepth(depth);
+
+						bool registered = true;
+						if(registered)
+						{
+							depth = util3d::registerDepth(
+									depth,
+									stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
+									stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
+									stereoModel_.transform());
+							util3d::fillRegisteredDepthHoles(depth, true, false);
+							fx = stereoModel_.right().fx();
+							fy = stereoModel_.right().fy();
+							cx = stereoModel_.right().cx();
+							cy = stereoModel_.right().cy();
+						}
+						else
+						{
+							fx = stereoModel_.left().fx();
+							fy = stereoModel_.left().fy();
+							cx = stereoModel_.left().cx();
+							cy = stereoModel_.left().cy();
+						}
+					}
 				}
 				else
 				{
-					cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+					//use data from libfreenect2
-
+					if(irFrame)
-					//registration of the depth
-					if(reg_)
 					{
-						if(type_ == kTypeRGBDepthSD)
+						cv::Mat(irFrame->height, irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
+					}
+					else
+					{
+						cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+
+						//registration of the depth
+						if(reg_)
 						{
-							cv::Mat tmp;
+							if(type_ == kTypeRGBDepthSD)
-							cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
-							rgb = tmp;
-						}
-						cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data);
-						depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U);
-						for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
-						{
-							for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
 							{
-								float dz = depthFrameMat.at<float>(dy,dx);
+								cv::Mat tmp;
-								float dz1 = depthFrameMat.at<float>(dy,dx+1);
+								cv::resize(rgb, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
-								float dz2 = depthFrameMat.at<float>(dy+1,dx);
+								rgb = tmp;
-								float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
+							}
-								if(dz && dz1 && dz2 && dz3)
+							cv::Mat depthFrameMat = cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data);
+							depth = cv::Mat::zeros(rgb.rows, rgb.cols, CV_16U);
+							for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
+							{
+								for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
 								{
-									float avg = (dz + dz1 + dz2 + dz3) / 4;
+									float dz = depthFrameMat.at<float>(dy,dx);
-									float thres = 0.01 * avg;
+									float dz1 = depthFrameMat.at<float>(dy,dx+1);
-									if( fabs(dz - avg) < thres &&
+									float dz2 = depthFrameMat.at<float>(dy+1,dx);
-										fabs(dz1 - avg) < thres &&
+									float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
-										fabs(dz2 - avg) < thres &&
+									if(dz && dz1 && dz2 && dz3)
-										fabs(dz3 - avg) < thres)
 									{
-										float cx=-1,cy=-1;
+										float avg = (dz + dz1 + dz2 + dz3) / 4;
-										reg_->apply(dx, dy, dz, cx, cy);
+										float thres = 0.01 * avg;
-										if(type_==kTypeRGBDepthSD)
+										if( fabs(dz - avg) < thres &&
+											fabs(dz1 - avg) < thres &&
+											fabs(dz2 - avg) < thres &&
+											fabs(dz3 - avg) < thres)
 										{
-											cx/=2.0f;
+											float cx=-1,cy=-1;
-											cy/=2.0f;
+											reg_->apply(dx, dy, dz, cx, cy);
-										}
+											if(type_==kTypeRGBDepthSD)
-										int rcx = cvRound(cx);
-										int rcy = cvRound(cy);
-										if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
-										{
-											unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
-											if(zReg == 0 || zReg > (unsigned short)dz)
 											{
-												zReg = (unsigned short)dz;
+												cx/=2.0f;
+												cy/=2.0f;
+											}
+											int rcx = cvRound(cx);
+											int rcy = cvRound(cy);
+											if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
+											{
+												unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
+												if(zReg == 0 || zReg > (unsigned short)dz)
+												{
+													zReg = (unsigned short)dz;
+												}
 											}
 										}
 									}
 								}
 							}
+							util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD);
+							util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);//second pass
+							libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
+							fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f);
+							fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f);
+							cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f);
+							cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f);
+						}
+						else
+						{
+							libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
+							fx = params.fx;
+							fy = params.fy;
+							cx = params.cx;
+							cy = params.cy;
 						}
-						util3d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeRGBDepthHD);
-						util3d::fillRegisteredDepthHoles(depth, type_==kTypeRGBDepthSD, type_==kTypeRGBDepthHD);//second pass
-						libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
-						fx = params.fx*(type_==kTypeRGBDepthSD?0.5:1.0f);
-						fy = params.fy*(type_==kTypeRGBDepthSD?0.5:1.0f);
-						cx = params.cx*(type_==kTypeRGBDepthSD?0.5:1.0f);
-						cy = params.cy*(type_==kTypeRGBDepthSD?0.5:1.0f);
-					}
-					else
-					{
-						libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
-						fx = params.fx;
-						fy = params.fy;
-						cx = params.cx;
-						cy = params.cy;
 					}
+					cv::flip(depth, depth, 1);
 				}
-				cv::flip(depth, depth, 1);
 			}
 			listener_->release(frames);
 		}

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();