Added RGB/IR calibration

corelib/include/rtabmap/core/CameraModel.h

@@ -92,28 +92,24 @@ public:
 	StereoCameraModel() {}
 	StereoCameraModel(const std::string & name, const cv::Size & imageSize,
 			const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
-			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2) :
+			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
+			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F) :
 		left_(name+"_left", imageSize, K1, D1, R1, P1),
 		right_(name+"_right", imageSize, K2, D2, R2, P2),
-		name_(name)
+		name_(name),
+		R_(R),
+		T_(T),
+		E_(E),
+		F_(F)
 	{
 	}
 	virtual ~StereoCameraModel() {}
 
-	bool isValid() const {return left_.isValid() && right_.isValid();}
+	bool isValid() const {return left_.isValid() && right_.isValid() && !R_.empty() && !T_.empty() && !E_.empty() && !F_.empty();}
 	const std::string & name() const {return name_;}
 
-	bool load(const std::string & directory, const std::string & cameraName)
-	{
-		name_ = cameraName;
-		return left_.load(directory+"/"+cameraName+"_left.yaml") &&
-				right_.load(directory+"/"+cameraName+"_right.yaml");
-	}
-	bool save(const std::string & directory, const std::string & cameraName)
-	{
-		return left_.save(directory+"/"+cameraName+"_left.yaml") &&
-				right_.save(directory+"/"+cameraName+"_right.yaml");
-	}
+	bool load(const std::string & directory, const std::string & cameraName);
+	bool save(const std::string & directory, const std::string & cameraName);
 	double baseline() const {return -right_.Tx()/right_.fx();}
 
 	const CameraModel & left() const {return left_;}
@@ -123,6 +119,10 @@ private:
 	CameraModel left_;
 	CameraModel right_;
 	std::string name_;
+	cv::Mat R_;
+	cv::Mat T_;
+	cv::Mat E_;
+	cv::Mat F_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/CameraRGBD.h

@@ -270,9 +270,16 @@ class RTABMAP_EXP CameraFreenect2 :
 public:
 	static bool available();
 
+	enum Type{
+		kTypeRGBDepthSD,
+		kTypeRGBDepthHD,
+		kTypeRGBIR
+	};
+
 public:
 	// default local transform z in, x right, y down));
 	CameraFreenect2(int deviceId= 0,
+					Type type = kTypeRGBDepthSD,
 					float imageRate=0.0f,
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect2();
@@ -286,6 +293,7 @@ protected:
 
 private:
 	int deviceId_;
+	Type type_;
 	libfreenect2::Freenect2 * freenect2_;
 	libfreenect2::Freenect2Device *dev_;
 	libfreenect2::SyncMultiFrameListener * listener_;

corelib/src/CameraModel.cpp

@@ -186,4 +186,115 @@ cv::Mat CameraModel::rectifyImage(const cv::Mat & raw) const
 	}
 }
 
+//
+//StereoCameraModel
+//
+bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName)
+{
+	name_ = cameraName;
+	if(left_.load(directory+"/"+cameraName+"_left.yaml") && right_.load(directory+"/"+cameraName+"_right.yaml"))
+	{
+		//load rotation, translation
+		R_ = cv::Mat();
+		T_ = cv::Mat();
+
+		std::string filePath = directory+"/"+cameraName+"_pose.yaml";
+		if(UFile::exists(filePath))
+		{
+			UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
+			cv::FileStorage fs(filePath, cv::FileStorage::READ);
+
+			name_ = (int)fs["camera_name"];
+
+			// import from ROS calibration format
+			cv::FileNode n = fs["rotation_matrix"];
+			int rows = (int)n["rows"];
+			int cols = (int)n["cols"];
+			std::vector<double> data;
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 3);
+			R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			n = fs["translation_matrix"];
+			rows = (int)n["rows"];
+			cols = (int)n["cols"];
+			data.clear();
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 1);
+			T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			n = fs["essential_matrix"];
+			rows = (int)n["rows"];
+			cols = (int)n["cols"];
+			data.clear();
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 3);
+			E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			n = fs["fundamental_matrix"];
+			rows = (int)n["rows"];
+			cols = (int)n["cols"];
+			data.clear();
+			n["data"] >> data;
+			UASSERT(rows*cols == (int)data.size());
+			UASSERT(rows == 3 && cols == 3);
+			F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+
+			fs.release();
+
+			return true;
+		}
+	}
+	return false;
+}
+bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName)
+{
+	if(left_.save(directory+"/"+cameraName+"_left.yaml") && right_.save(directory+"/"+cameraName+"_right.yaml"))
+	{
+		std::string filePath = directory+"/"+cameraName+"_pose.yaml";
+		if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
+		{
+			UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
+			cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
+
+			// export in ROS calibration format
+
+			fs << "camera_name" << name_;
+			fs << "rotation_matrix" << "{";
+			fs << "rows" << R_.rows;
+			fs << "cols" << R_.cols;
+			fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
+			fs << "}";
+
+			fs << "translation_matrix" << "{";
+			fs << "rows" << T_.rows;
+			fs << "cols" << T_.cols;
+			fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
+			fs << "}";
+
+			fs << "camera_name" << name_;
+			fs << "essential_matrix" << "{";
+			fs << "rows" << E_.rows;
+			fs << "cols" << E_.cols;
+			fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
+			fs << "}";
+
+			fs << "camera_name" << name_;
+			fs << "fundamental_matrix" << "{";
+			fs << "rows" << F_.rows;
+			fs << "cols" << F_.cols;
+			fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
+			fs << "}";
+
+			fs.release();
+
+			return true;
+		}
+	}
+	return false;
+}
+
 } /* namespace rtabmap */

corelib/src/CameraRGBD.cpp

@@ -1038,17 +1038,27 @@ bool CameraFreenect2::available()
 #endif
 }
 
-CameraFreenect2::CameraFreenect2(int deviceId, float imageRate, const Transform & localTransform) :
+CameraFreenect2::CameraFreenect2(int deviceId, Type type, float imageRate, const Transform & localTransform) :
 		CameraRGBD(imageRate, localTransform),
 		deviceId_(deviceId),
+		type_(type),
 		freenect2_(0),
 		dev_(0),
 		listener_(0)
 {
 #ifdef WITH_FREENECT2
 	freenect2_ = new libfreenect2::Freenect2();
-	//listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
-	listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
+	switch(type_)
+	{
+	case kTypeRGBIR:
+		listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
+		break;
+	case kTypeRGBDepthSD:
+	case kTypeRGBDepthHD:
+	default:
+		listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color  | libfreenect2::Frame::Depth);
+		break;
+	}
 	UWARN("CameraFreenect2: Images are not yet registered!");
 #endif
 }
@@ -1141,22 +1151,42 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 		libfreenect2::FrameMap frames;
 		if(listener_->waitForNewFrame(frames, 1000))
 		{
-			libfreenect2::Frame *rgbFrame = frames[libfreenect2::Frame::Color];
-			//libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
-			libfreenect2::Frame *depthFrame = frames[libfreenect2::Frame::Depth];
+			libfreenect2::Frame *rgbFrame = 0;
+			libfreenect2::Frame *irFrame = 0;
+			libfreenect2::Frame *depthFrame = 0;
 
-			if(rgbFrame && depthFrame)
+			switch(type_)
 			{
-				cv::flip(cv::Mat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data), rgb, 1);
-				cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
-				cv::flip(depth, depth, 1);
-				libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
-				fx = params.fx;
-				fy = params.fy;
-				cx = params.cx;
-				cy = params.cy;
+			case kTypeRGBIR:
+				rgbFrame = frames[libfreenect2::Frame::Color];
+				irFrame = frames[libfreenect2::Frame::Ir];
+				break;
+			case kTypeRGBDepthSD:
+			case kTypeRGBDepthHD:
+			default:
+				rgbFrame = frames[libfreenect2::Frame::Color];
+				depthFrame = frames[libfreenect2::Frame::Depth];
+				break;
 			}
 
+			cv::flip(cv::Mat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data), rgb, 1);
+			if(irFrame)
+			{
+				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);
+			}
+			cv::flip(depth, depth, 1);
+
+			//libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
+			libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
+			fx = params.fx;
+			fy = params.fy;
+			cx = params.cx;
+			cy = params.cy;
+
 			listener_->release(frames);
 		}
 		else

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -102,6 +102,8 @@ private:
 	std::vector<cv::Size > imageSize_;
 	std::vector<rtabmap::CameraModel> models_;
 	rtabmap::StereoCameraModel stereoModel_;
+	std::vector<unsigned short> minIrs_;
+	std::vector<unsigned short> maxIrs_;
 
 	Ui_calibrationDialog * ui_;
 };

guilib/src/CalibrationDialog.cpp

@@ -59,6 +59,13 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 	stereoImagePoints_.resize(2);
 	models_.resize(2);
 
+	minIrs_.resize(2);
+	maxIrs_.resize(2);
+	minIrs_[0] = 0x0000;
+	maxIrs_[0] = 0x7fff;
+	minIrs_[1] = 0x0000;
+	maxIrs_[1] = 0x7fff;
+
 	qRegisterMetaType<cv::Mat>("cv::Mat");
 
 	ui_ = new Ui_calibrationDialog();
@@ -67,6 +74,7 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 	connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
 	connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
 	connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
+	connect(ui_->checkBox_switchImages, SIGNAL(stateChanged(int)), this, SLOT(restart()));
 
 	connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
 	connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
@@ -81,6 +89,8 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 	ui_->progressBar_count_2->setMaximum(COUNT_MIN);
 	ui_->progressBar_count_2->setFormat("%v");
 
+	ui_->radioButton_raw->setChecked(true);
+
 	this->setStereoMode(stereo_);
 }
 
@@ -136,6 +146,7 @@ void CalibrationDialog::setStereoMode(bool stereo)
 	ui_->progressBar_size_2->setVisible(stereo_);
 	ui_->progressBar_skew_2->setVisible(stereo_);
 	ui_->progressBar_count_2->setVisible(stereo_);
+	ui_->label_right->setVisible(stereo_);
 	ui_->image_view_2->setVisible(stereo_);
 	ui_->label_fx_2->setVisible(stereo_);
 	ui_->label_fy_2->setVisible(stereo_);
@@ -148,6 +159,7 @@ void CalibrationDialog::setStereoMode(bool stereo)
 	ui_->lineEdit_D_2->setVisible(stereo_);
 	ui_->lineEdit_R_2->setVisible(stereo_);
 	ui_->lineEdit_P_2->setVisible(stereo_);
+	ui_->radioButton_stereoRectified->setVisible(stereo_);
 }
 
 void CalibrationDialog::setBoardWidth(int width)
@@ -244,10 +256,21 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 		ui_->label_serial->setText(cameraName_);
 
 	}
+	std::vector<cv::Mat> inputRawImages(2);
+	if(ui_->checkBox_switchImages->isChecked())
+	{
+		inputRawImages[0] = imageRight;
+		inputRawImages[1] = imageLeft;
+	}
+	else
+	{
+		inputRawImages[0] = imageLeft;
+		inputRawImages[1] = imageRight;
+	}
+
 	std::vector<cv::Mat> images(2);
-	cv::resize(imageLeft, images[0], cv::Size(), 2.0, 2.0, CV_INTER_CUBIC);
-	//images[0] = imageLeft;
-	images[1] = imageRight;
+	images[0] = inputRawImages[0];
+	images[1] = inputRawImages[1];
 	imageSize_[0] = images[0].size();
 	imageSize_[1] = images[1].size();
 
@@ -262,7 +285,21 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 		cv::Mat viewGray;
 		if(!images[id].empty())
 		{
-			if(images[id].channels() == 3)
+			if(images[id].type() == CV_16UC1)
+			{
+				//assume IR image: convert to gray scaled
+				const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
+				viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1);
+				for(int i=0; i<images[id].rows; ++i)
+				{
+					for(int j=0; j<images[id].cols; ++j)
+					{
+						viewGray.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(images[id].at<unsigned short>(i,j) - minIrs_[id], 0)) * factor, 255.0f);
+					}
+				}
+				cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
+			}
+			else if(images[id].channels() == 3)
 			{
 				cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
 			}
@@ -277,6 +314,9 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 			UERROR("Image %d is empty!! Should not!", id);
 		}
 
+		minIrs_[id] = 0;
+		maxIrs_[id] = 0x7FFF;
+
 		//Dot it only if not yet calibrated
 		if(!ui_->pushButton_save->isEnabled())
 		{
@@ -284,7 +324,29 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 			if(!viewGray.empty())
 			{
 				int flags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;
-				boardFound[id] = cv::findChessboardCorners(viewGray, boardSize, pointBuf[id], flags);
+
+				if(!viewGray.empty())
+				{
+					int maxScale = viewGray.cols < 640?2:1;
+					for( int scale = 1; scale <= maxScale; scale++ )
+					{
+						cv::Mat timg;
+						if( scale == 1 )
+							timg = viewGray;
+						else
+							cv::resize(viewGray, timg, cv::Size(), scale, scale, CV_INTER_CUBIC);
+						boardFound[id] = cv::findChessboardCorners(timg, boardSize, pointBuf[id], flags);
+						if(boardFound[id])
+						{
+							if( scale > 1 )
+							{
+								cv::Mat cornersMat(pointBuf[id]);
+								cornersMat *= 1./scale;
+							}
+							break;
+						}
+					}
+				}
 			}
 
 			if(boardFound[id]) // If done with success,
@@ -388,11 +450,38 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 				{
 					readyToCalibrate[id] = true;
 				}
+
+				//update IR values
+				if(inputRawImages[id].type() == CV_16UC1)
+				{
+					//update min max IR if the chessboard was found
+					minIrs_[id] = 0xFFFF;
+					maxIrs_[id] = 0;
+					for(size_t i = 0; i < pointBuf[id].size(); ++i)
+					{
+						const cv::Point2f &p = pointBuf[id][i];
+						cv::Rect roi(std::max(0, (int)p.x - 3), std::max(0, (int)p.y - 3), 6, 6);
+
+						roi.width = std::min(roi.width, inputRawImages[id].cols - roi.x);
+						roi.height = std::min(roi.height, inputRawImages[id].rows - roi.y);
+
+						//find minMax in the roi
+						double min, max;
+						cv::minMaxLoc(inputRawImages[id](roi), &min, &max);
+						if(min < minIrs_[id])
+						{
+							minIrs_[id] = min;
+						}
+						if(max > maxIrs_[id])
+						{
+							maxIrs_[id] = max;
+						}
+					}
+				}
 			}
 		}
 	}
 
-
 	if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0])))
 	{
 		stereoImagePoints_[0].push_back(pointBuf[0]);
@@ -409,18 +498,22 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 		ui_->pushButton_calibrate->setEnabled(true);
 	}
 
-	if(models_[0].isValid() && ui_->checkBox_rectified->isChecked())
+	if(ui_->radioButton_rectified->isChecked())
 	{
-		if(!stereo_)
+		if(models_[0].isValid())
 		{
 			images[0] = models_[0].rectifyImage(images[0]);
 		}
-		else
+		if(models_[1].isValid())
 		{
-			images[0] = stereoModel_.left().rectifyImage(images[0]);
-			images[1] = stereoModel_.right().rectifyImage(images[1]);
+			images[1] = models_[1].rectifyImage(images[1]);
 		}
 	}
+	else if(ui_->radioButton_stereoRectified->isChecked() && stereoModel_.isValid())
+	{
+		images[0] = stereoModel_.left().rectifyImage(images[0]);
+		images[1] = stereoModel_.right().rectifyImage(images[1]);
+	}
 
 	if(ui_->checkBox_showHorizontalLines->isChecked())
 	{
@@ -460,10 +553,16 @@ void CalibrationDialog::restart()
 	models_[1] = CameraModel();
 	stereoModel_ = StereoCameraModel();
 	cameraName_.clear();
+	minIrs_[0] = 0x0000;
+	maxIrs_[0] = 0x7fff;
+	minIrs_[1] = 0x0000;
+	maxIrs_[1] = 0x7fff;
 
 	ui_->pushButton_calibrate->setEnabled(false);
 	ui_->pushButton_save->setEnabled(false);
-	ui_->checkBox_rectified->setEnabled(false);
+	ui_->radioButton_raw->setChecked(true);
+	ui_->radioButton_rectified->setEnabled(false);
+	ui_->radioButton_stereoRectified->setEnabled(false);
 
 	ui_->progressBar_count->reset();
 	ui_->progressBar_count->setMaximum(COUNT_MIN);
@@ -617,7 +716,7 @@ void CalibrationDialog::calibrate()
 	if(stereo_ && models_[0].isValid() && models_[1].isValid())
 	{
 		UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
-		cv::Size imageSize = imageSize_[0];
+		cv::Size imageSize = imageSize_[0].width > imageSize_[1].width?imageSize_[0]:imageSize_[1];
 		cv::Mat R, T, E, F;
 
 		std::vector<std::vector<cv::Point3f> > objectPoints(1);
@@ -683,7 +782,8 @@ void CalibrationDialog::calibrate()
 				cameraName_.toStdString(),
 				imageSize,
 				models_[0].K(), models_[0].D(), R1, P1,
-				models_[1].K(), models_[1].D(), R2, P2);
+				models_[1].K(), models_[1].D(), R2, P2,
+				R, T, E, F);
 
 		std::stringstream strR1, strP1, strR2, strP2;
 		strR1 << stereoModel_.left().R();
@@ -699,14 +799,19 @@ void CalibrationDialog::calibrate()
 		//ui_->label_error_stereo->setNum(totalAvgErr);
 	}
 
-	if((!stereo_ && models_[0].isValid()) ||
-		(stereo_ && stereoModel_.isValid()))
+	if(stereo_ && stereoModel_.isValid())
 	{
-		ui_->checkBox_rectified->setEnabled(true);
-		ui_->checkBox_rectified->setChecked(true);
-
+		ui_->radioButton_rectified->setEnabled(true);
+		ui_->radioButton_stereoRectified->setEnabled(true);
+		ui_->radioButton_stereoRectified->setChecked(true);
 		ui_->pushButton_save->setEnabled(true);
 	}
+	else if(models_[0].isValid())
+	{
+		ui_->radioButton_rectified->setEnabled(true);
+		ui_->radioButton_rectified->setChecked(true);
+		ui_->pushButton_save->setEnabled(!stereo_);
+	}
 
 	UINFO("End calibration");
 	processingData_ = false;
@@ -749,10 +854,11 @@ bool CalibrationDialog::save()
 			std::string base = (dir+QDir::separator()+name).toStdString();
 			std::string leftPath = base+"_left.yaml";
 			std::string rightPath = base+"_right.yaml";
+			std::string posePath = base+"_pose.yaml";
 			if(stereoModel_.save(dir.toStdString(), name.toStdString()))
 			{
-				QMessageBox::information(this, tr("Export"), tr("Calibration files saved to \"%1\" and \"%2\".").
-						arg(leftPath.c_str()).arg(rightPath.c_str()));
+				QMessageBox::information(this, tr("Export"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\"\n  \"%3\".").
+						arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
 				UINFO("Saved \"%s\" and \"%s\"!", leftPath.c_str(), rightPath.c_str());
 				savedCalibration_ = true;
 				saved = true;

guilib/src/PreferencesDialog.cpp

@@ -3124,6 +3124,7 @@ CameraRGBD * PreferencesDialog::createCameraRGBD() const
 	{
 		return new CameraFreenect2(
 			this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()),
+			CameraFreenect2::kTypeRGBDepthSD,
 			this->getGeneralInputRate(),
 			this->getSourceOpenniLocalTransform());
 	}

guilib/src/ui/calibrationDialog.ui

@@ -17,16 +17,7 @@
    <property name="spacing">
     <number>0</number>
    </property>
-   <property name="leftMargin">
-    <number>0</number>
-   </property>
-   <property name="topMargin">
-    <number>0</number>
-   </property>
-   <property name="rightMargin">
-    <number>0</number>
-   </property>
-   <property name="bottomMargin">
+   <property name="margin">
     <number>0</number>
    </property>
    <item>
@@ -86,9 +77,30 @@
        <item>
         <layout class="QHBoxLayout" name="horizontalLayout_2">
          <item>
-          <widget class="QCheckBox" name="checkBox_rectified">
+          <widget class="QRadioButton" name="radioButton_raw">
            <property name="text">
-            <string>Show rectified</string>
+            <string>Raw</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QRadioButton" name="radioButton_rectified">
+           <property name="text">
+            <string>Rectified</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QRadioButton" name="radioButton_stereoRectified">
+           <property name="text">
+            <string>Stereo Rectified</string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="Line" name="line">
+           <property name="orientation">
+            <enum>Qt::Vertical</enum>
            </property>
           </widget>
          </item>
@@ -381,6 +393,13 @@
             </property>
            </widget>
           </item>
+          <item row="0" column="2">
+           <widget class="QCheckBox" name="checkBox_switchImages">
+            <property name="text">
+             <string>Switch images</string>
+            </property>
+           </widget>
+          </item>
          </layout>
         </widget>
        </item>

tools/Calibration/main.cpp

@@ -181,7 +181,7 @@ int main(int argc, char * argv[])
 			UERROR("Not built with Freenect2 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraFreenect2();
+		camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBIR);
 	}
 	else if(driver == 6)
 	{

tools/OdometryViewer/main.cpp

@@ -755,7 +755,7 @@ int main (int argc, char * argv[])
 				UERROR("Not built with Freenect2 support...");
 				exit(-1);
 			}
-			camera = new rtabmap::CameraFreenect2(0, rate, t);
+			camera = new rtabmap::CameraFreenect2(0, rtabmap::CameraFreenect2::kTypeRGBDepthSD, rate, t);
 		}
 		else if(driver == 6)
 		{