Added T265 support (stereo-only yet)

corelib/include/rtabmap/core/camera/CameraRealSense2.h

@@ -69,6 +69,7 @@ public:
 	// parameters are set during initialization
 	void setEmitterEnabled(bool enabled);
 	void setIRDepthFormat(bool enabled);
+	void setImagesRectified(bool enabled);
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
@@ -86,9 +87,11 @@ private:
 	cv::Mat depthBuffer_;
 	cv::Mat rgbBuffer_;
 	CameraModel model_;
+	StereoCameraModel stereoModel_;
 
 	bool emitterEnabled_;
 	bool irDepth_;
+	bool rectifyImages_;
 #endif
 };
 

corelib/src/camera/CameraRealSense2.cpp

@@ -65,7 +65,8 @@ CameraRealSense2::CameraRealSense2(
 	rgbIntrinsics_(new rs2_intrinsics),
 	depthToRGBExtrinsics_(new rs2_extrinsics),
 	emitterEnabled_(true),
-	irDepth_(false)
+	irDepth_(false),
+	rectifyImages_(true)
 #endif
 {
 	UDEBUG("");
@@ -75,10 +76,17 @@ CameraRealSense2::~CameraRealSense2()
 {
 #ifdef RTABMAP_REALSENSE2
 	for(rs2::sensor _sensor : dev_->query_sensors())
+	{
+		try
 		{
 			_sensor.stop();
 			_sensor.close();
 		}
+		catch(const rs2::wrong_api_call_sequence_error & error)
+		{
+			UINFO("%s", error.what());
+		}
+	}
 	delete ctx_;
 	delete dev_;
 	delete syncer_;
@@ -224,6 +232,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 
 	UINFO("Device Sensors: ");
 	std::vector<rs2::sensor> sensors(2); //0=rgb 1=depth
+	bool stereo = false;
 	for(auto&& elem : dev_sensors)
 	{
 		std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
@@ -238,6 +247,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		}
 		else if ("Coded-Light Depth Sensor" == module_name)
 		{
+			UINFO("Found \"%s\"", module_name.c_str());
 		}
 		else if ("RGB Camera" == module_name)
 		{
@@ -252,9 +262,15 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		else if ("Motion Module" == module_name)
 		{
 		}
+		else if ("Tracking Module" == module_name)
+		{
+			sensors.resize(1);
+			sensors[0] = elem;
+			stereo = true;
+		}
 		else
 		{
-			UERROR("Module Name \"%s\" isn't supported by LibRealSense!", module_name.c_str());
+			UERROR("Module Name \"%s\" isn't supported!", module_name.c_str());
 			return false;
 		}
 		UINFO("%s was found.", elem.get_info(RS2_CAMERA_INFO_NAME));
@@ -265,16 +281,25 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	model_ = CameraModel();
 	rs2::stream_profile depthStreamProfile;
 	rs2::stream_profile rgbStreamProfile;
-	std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(2);
+	std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(sensors.size());
 	 for (unsigned int i=0; i<sensors.size(); ++i)
 	 {
-		 UDEBUG("i=%d", (int)i);
+		UINFO("Sensor %d \"%s\"", (int)i, sensors[i].get_info(RS2_CAMERA_INFO_NAME));
 		auto profiles = sensors[i].get_stream_profiles();
 		bool added = false;
-		UDEBUG("profiles=%d", (int)profiles.size());
+		UINFO("profiles=%d", (int)profiles.size());
+		int pi = 0;
 		for (auto& profile : profiles)
 		{
 			auto video_profile = profile.as<rs2::video_stream_profile>();
+			UINFO("%s %d %d %d", rs2_format_to_string(
+					video_profile.format()),
+					video_profile.width(),
+					video_profile.height(),
+					video_profile.fps());
+
+			if(!stereo)
+			{
 				if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) &&
 					video_profile.width()  == 640 &&
 					video_profile.height() == 480 &&
@@ -299,6 +324,43 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					break;
 				}
 			}
+			else if(stereo)
+			{
+				if(video_profile.format() == RS2_FORMAT_Y8 &&
+					video_profile.width()  == 848 &&
+					video_profile.height() == 800 &&
+					video_profile.fps()    == 30)
+				{
+					UASSERT(i<2);
+					profilesPerSensor[0].push_back(profile);
+					auto intrinsic = video_profile.get_intrinsics();
+					if(pi==0)
+					{
+						// RIGHT FISHEYE
+						depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
+						depthStreamProfile = profile;
+						*depthIntrinsics_ = intrinsic;
+					}
+					else
+					{
+						// LEFT FISHEYE
+						rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
+						rgbStreamProfile = profile;
+						*rgbIntrinsics_ = intrinsic;
+					}
+					added = true;
+				}
+				//MOTION_XYZ32F 0 0 200
+				//[ INFO] (2019-05-08 15:11:04.824) CameraRealSense2.cpp:299::init() MOTION_XYZ32F 0 0 62
+				//[ INFO] (2019-05-08 15:11:04.824) CameraRealSense2.cpp:299::init() 6DOF 0 0 200
+				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
+				{
+					//profilesPerSensor[0].push_back(profile);
+					//added = true;
+				}
+			}
+			++pi;
+		}
 		if (!added)
 		{
 			UERROR("Given stream configuration is not supported by the device! "
@@ -306,13 +368,49 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			return false;
 		}
 	 }
-
+	 UDEBUG("");
+	 if(!stereo)
+	 {
 		 if(!model_.isValidForProjection())
 		 {
 			 UERROR("Calibration info not valid!");
 			 return false;
 		 }
 		 *depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
+	 }
+	 else
+	 {
+
+		// look for calibration files
+		 std::string serial = sn;
+		 if(!cameraName.empty())
+		 {
+			 serial = cameraName;
+		 }
+		if(!calibrationFolder.empty() && !serial.empty())
+		{
+			if(!stereoModel_.load(calibrationFolder, serial, false))
+			{
+				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+						serial.c_str(), calibrationFolder.c_str());
+			}
+			else
+			{
+				UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
+						stereoModel_.left().fx(),
+						stereoModel_.left().cx(),
+						stereoModel_.left().cy(),
+						stereoModel_.baseline());
+			}
+		}
+
+		stereoModel_.setLocalTransform(this->getLocalTransform());
+		if(rectifyImages_ && !stereoModel_.isValidForRectification())
+		{
+			UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
+			return false;
+		}
+	 }
 
 	 for (unsigned int i=0; i<sensors.size(); ++i)
 	 {
@@ -320,7 +418,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 {
 			 UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
 			 sensors[i].open(profilesPerSensor[i]);
-			 if(i ==1)
+			 if(sensors[i].is<rs2::depth_sensor>())
 			 {
 				 auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
 				 depth_scale_meters_ = depth_sensor.get_depth_scale();
@@ -342,7 +440,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 
 bool CameraRealSense2::isCalibrated() const
 {
-	return true;
+	return model_.isValidForProjection() || stereoModel_.isValidForRectification();
 }
 
 std::string CameraRealSense2::getSerial() const
@@ -367,6 +465,13 @@ void CameraRealSense2::setIRDepthFormat(bool enabled)
 #endif
 }
 
+void CameraRealSense2::setImagesRectified(bool enabled)
+{
+#ifdef RTABMAP_REALSENSE2
+	rectifyImages_ = enabled;
+#endif
+}
+
 SensorData CameraRealSense2::captureImage(CameraInfo * info)
 {
 	SensorData data;
@@ -386,6 +491,8 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 			UDEBUG("Frameset arrived.");
 			bool is_rgb_arrived = false;
 			bool is_depth_arrived = false;
+			bool is_left_fisheye_arrived = false;
+			bool is_right_fisheye_arrived = false;
 			rs2::frame rgb_frame;
 			rs2::frame depth_frame;
 			for (auto it = frameset.begin(); it != frameset.end(); ++it)
@@ -402,6 +509,19 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					depth_frame = f;
 					is_depth_arrived = true;
 				}
+				else if (stream_type == RS2_STREAM_FISHEYE)
+				{
+					if(!is_right_fisheye_arrived)
+					{
+						depth_frame = f;
+						is_right_fisheye_arrived = true;
+					}
+					else
+					{
+						rgb_frame = f;
+						is_left_fisheye_arrived = true;
+					}
+				}
 			}
 
 			if(is_rgb_arrived && is_depth_arrived)
@@ -433,6 +553,28 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 
 				data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
 			}
+			else if(is_left_fisheye_arrived && is_right_fisheye_arrived)
+			{
+				auto from_image_frame = depth_frame.as<rs2::video_frame>();
+				cv::Mat left,right;
+				if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+				{
+					left = stereoModel_.left().rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
+					right = stereoModel_.right().rectifyImage(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()));
+				}
+				else
+				{
+					left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
+					right = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
+				}
+
+				if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
+				{
+					stereoModel_.setImageSize(left.size());
+				}
+
+				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
+			}
 			else
 			{
 				UERROR("Not received depth and rgb");

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -65,6 +65,7 @@ public:
 	void setCameraName(const QString & name);
 	void setProgressVisibility(bool visible);
 	void setSwitchedImages(bool switched);
+	void setFisheyeImages(bool enabled);
 	void setStereoMode(bool stereo, const QString & leftSuffix = "left", const QString & rightSuffix = "right");
 	void setSavingDirectory(const QString & savingDirectory) {savingDirectory_ = savingDirectory;}
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -174,6 +174,7 @@ protected Q_SLOTS:
 	void selectK4W2();
 	void selectRealSense();
 	void selectRealSense2();
+	void selectRealSense2Stereo();
 	void selectStereoDC1394();
 	void selectStereoFlyCapture2();
 	void selectStereoZed();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -102,6 +102,7 @@ public:
 		kSrcStereoZed      = 104,
 		kSrcStereoUsb      = 105,
 		kSrcStereoTara 	   = 106,
+		kSrcRealSense2Stereo = 107,
 
 		kSrcRGB            = 200,
 		kSrcUsbDevice      = 200,

guilib/src/CalibrationDialog.cpp

@@ -36,6 +36,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/calib3d/calib3d_c.h>
 #endif
 #include <opencv2/highgui/highgui.hpp>
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+#include "opencv/stereoRectifyFisheye.h"
+#endif
 
 #include <QFileDialog>
 #include <QMessageBox>
@@ -173,6 +176,11 @@ void CalibrationDialog::setSwitchedImages(bool switched)
 	ui_->checkBox_switchImages->setChecked(switched);
 }
 
+void CalibrationDialog::setFisheyeImages(bool enabled)
+{
+	ui_->checkBox_fisheye->setChecked(enabled);
+}
+
 void CalibrationDialog::setStereoMode(bool stereo, const QString & leftSuffix, const QString & rightSuffix)
 {
 	leftSuffix_ = leftSuffix;
@@ -296,15 +304,13 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 {
 	UDEBUG("Processing images");
 	processingData_ = true;
-	if(cameraName_.isEmpty())
-	{
 	cameraName_ = "0000";
 	if(!cameraName.isEmpty())
 	{
 		cameraName_ = cameraName;
 	}
-	}
+
-	if(ui_->label_serial->text().isEmpty())
+	if(ui_->label_serial->text().compare(cameraName_)!=0)
 	{
 		ui_->label_serial->setText(cameraName_);
 
@@ -847,7 +853,6 @@ void CalibrationDialog::calibrate()
 	if(stereo_ && models_[0].isValidForRectification() && models_[1].isValidForRectification())
 	{
 		stereoModel_ = stereoCalibration(models_[0], models_[1], false);
-
 		std::stringstream strR1, strP1, strR2, strP2;
 		strR1 << stereoModel_.left().R();
 		strP1 << stereoModel_.left().P();
@@ -862,23 +867,38 @@ void CalibrationDialog::calibrate()
 		//ui_->label_error_stereo->setNum(totalAvgErr);
 	}
 
-	if(stereo_ &&
+	if(stereo_)
-		stereoModel_.isValidForRectification())
+	{
+		if(models_[0].isValidForRectification())
+		{
+			models_[0].initRectificationMap();
+		}
+		if(models_[1].isValidForRectification())
+		{
+			models_[1].initRectificationMap();
+		}
+		if(models_[0].isValidForRectification() || models_[1].isValidForRectification())
+		{
+			ui_->radioButton_rectified->setEnabled(true);
+		}
+		if(stereoModel_.isValidForRectification())
 		{
 			stereoModel_.initRectificationMap();
-		models_[0].initRectificationMap();
-		models_[1].initRectificationMap();
-		ui_->radioButton_rectified->setEnabled(true);
 			ui_->radioButton_stereoRectified->setEnabled(true);
 			ui_->radioButton_stereoRectified->setChecked(true);
 			ui_->pushButton_save->setEnabled(true);
 		}
+		else
+		{
+			ui_->radioButton_rectified->setChecked(ui_->radioButton_rectified->isEnabled());
+		}
+	}
 	else if(models_[0].isValidForRectification())
 	{
 		models_[0].initRectificationMap();
 		ui_->radioButton_rectified->setEnabled(true);
 		ui_->radioButton_rectified->setChecked(true);
-		ui_->pushButton_save->setEnabled(!stereo_);
+		ui_->pushButton_save->setEnabled(true);
 	}
 
 	UINFO("End calibration");
@@ -924,14 +944,8 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 	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);
 	cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
 	float squareSize = ui_->doubleSpinBox_squareSize->value();
-	// compute board corner positions
-	for (int i = 0; i < boardSize.height; ++i)
-		for (int j = 0; j < boardSize.width; ++j)
-			objectPoints[0].push_back(cv::Point3f(float(j*squareSize), float(i*squareSize), 0));
-	objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
 
 	double rms = 0.0;
 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
@@ -939,31 +953,133 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 	// calibrate extrinsic
 	if(fishEye)
 	{
-		cv::Mat D_left(1,4,CV_64FC1);
+		// compute board corner positions
-		D_left.at<double>(0,0) = left.D_raw().at<double>(0,0);
+		std::vector<std::vector<cv::Point3d> > objectPoints(1);
-		D_left.at<double>(0,1) = left.D_raw().at<double>(0,1);
+		for (int i = 0; i < boardSize.height; ++i)
-		D_left.at<double>(0,2) = left.D_raw().at<double>(0,4);
+			for (int j = 0; j < boardSize.width; ++j)
-		D_left.at<double>(0,3) = left.D_raw().at<double>(0,5);
+				objectPoints[0].push_back(cv::Point3d(double(j*squareSize), double(i*squareSize), 0));
-		cv::Mat D_right(1,4,CV_64FC1);
+		objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
-		UASSERT(right.D_raw().cols == 6);
-		D_right.at<double>(0,0) = right.D_raw().at<double>(0,0);
-		D_right.at<double>(0,1) = right.D_raw().at<double>(0,1);
-		D_right.at<double>(0,2) = right.D_raw().at<double>(0,4);
-		D_right.at<double>(0,3) = right.D_raw().at<double>(0,5);
 
+		cv::Vec3d Tvec;
+		cv::Vec4d D_left(left.D_raw().at<double>(0,0), left.D_raw().at<double>(0,1), left.D_raw().at<double>(0,4), left.D_raw().at<double>(0,5));
+		cv::Vec4d D_right(right.D_raw().at<double>(0,0), right.D_raw().at<double>(0,1), right.D_raw().at<double>(0,4), right.D_raw().at<double>(0,5));
+
+		UASSERT(stereoImagePoints_[0].size() == stereoImagePoints_[1].size());
+		std::vector<std::vector<cv::Point2d> > leftPoints(stereoImagePoints_[0].size());
+		std::vector<std::vector<cv::Point2d> > rightPoints(stereoImagePoints_[1].size());
+		for(unsigned int i =0; i<stereoImagePoints_[0].size(); ++i)
+		{
+			UASSERT(stereoImagePoints_[0][i].size() == stereoImagePoints_[1][i].size());
+			leftPoints[i].resize(stereoImagePoints_[0][i].size());
+			rightPoints[i].resize(stereoImagePoints_[1][i].size());
+			for(unsigned int j =0; j<stereoImagePoints_[0][i].size(); ++j)
+			{
+				leftPoints[i][j].x = stereoImagePoints_[0][i][j].x;
+				leftPoints[i][j].y = stereoImagePoints_[0][i][j].y;
+				rightPoints[i][j].x = stereoImagePoints_[1][i][j].x;
+				rightPoints[i][j].y = stereoImagePoints_[1][i][j].y;
+			}
+		}
+
+		try
+		{
 			rms = cv::fisheye::stereoCalibrate(
 					objectPoints,
-				stereoImagePoints_[0],
+					leftPoints,
-				stereoImagePoints_[1],
+					rightPoints,
+					left.K_raw(), D_left, right.K_raw(), D_right,
+					imageSize, R, Tvec,
+					cv::fisheye::CALIB_FIX_INTRINSIC,
+					cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+			UINFO("stereo calibration... done with RMS error=%f", rms);
+		}
+		catch(const cv::Exception & e)
+		{
+			UERROR("Error: %s (try restarting the calibration)", e.what());
+			return output;
+		}
+
+		std::cout << "R = " << R << std::endl;
+		std::cout << "T = " << Tvec << std::endl;
+
+		if(imageSize_[0] == imageSize_[1] && !ignoreStereoRectification)
+		{
+			UINFO("Compute stereo rectification");
+
+			cv::Mat R1, R2, P1, P2, Q;
+			stereoRectifyFisheye(
 					left.K_raw(), D_left,
 					right.K_raw(), D_right,
-				imageSize, R, T,
+					imageSize, R, Tvec, R1, R2, P1, P2, Q,
-				cv::CALIB_FIX_INTRINSIC,
+					cv::CALIB_ZERO_DISPARITY, 0, imageSize);
-				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+
+			// Very hard to get good results with this one:
+			/*double balance = 0.0, fov_scale = 1.0;
+			cv::fisheye::stereoRectify(
+					left.K_raw(), D_left,
+					right.K_raw(), D_right,
+					imageSize, R, Tvec, R1, R2, P1, P2, Q,
+					cv::CALIB_ZERO_DISPARITY, imageSize, balance, fov_scale);*/
+
+			std::cout << "R1 = " << R1 << std::endl;
+			std::cout << "R2 = " << R2 << std::endl;
+			std::cout << "P1 = " << P1 << std::endl;
+			std::cout << "P2 = " << P2 << std::endl;
+
+			// Re-zoom to original focal distance
+			if(P1.at<double>(0,0) < 0)
+			{
+				P1.at<double>(0,0) *= -1;
+				P1.at<double>(1,1) *= -1;
+			}
+			if(P2.at<double>(0,0) < 0)
+			{
+				P2.at<double>(0,0) *= -1;
+				P2.at<double>(1,1) *= -1;
+			}
+			if(P2.at<double>(0,3) > 0)
+			{
+				P2.at<double>(0,3) *= -1;
+			}
+			P2.at<double>(0,3) = P2.at<double>(0,3) * left.K_raw().at<double>(0,0) / P2.at<double>(0,0);
+			P1.at<double>(0,0) = P1.at<double>(1,1) = left.K_raw().at<double>(0,0);
+			P2.at<double>(0,0) = P2.at<double>(1,1) = left.K_raw().at<double>(0,0);
+
+			std::cout << "P1n = " << P1 << std::endl;
+			std::cout << "P2n = " << P2 << std::endl;
+
+
+			cv::Mat T(3,1,CV_64FC1);
+			T.at <double>(0,0) = Tvec[0];
+			T.at <double>(1,0) = Tvec[1];
+			T.at <double>(2,0) = Tvec[2];
+			output = StereoCameraModel(
+							cameraName_.toStdString(),
+							imageSize_[0], left.K_raw(), left.D_raw(), R1, P1,
+							imageSize_[1], right.K_raw(), right.D_raw(), R2, P2,
+							R, T, E, F);
+		}
+		else
+		{
+			UDEBUG("%s", cameraName_.toStdString().c_str());
+			//Kinect, ignore the stereo rectification
+			output = StereoCameraModel(
+							cameraName_.toStdString(),
+							imageSize_[0], left.K_raw(), left.D_raw(), left.R(), left.P(),
+							imageSize_[1], right.K_raw(), right.D_raw(), right.R(), right.P(),
+							R, T, E, F);
+		}
 	}
 	else
 #endif
 	{
+		// compute board corner positions
+		std::vector<std::vector<cv::Point3f> > objectPoints(1);
+		for (int i = 0; i < boardSize.height; ++i)
+			for (int j = 0; j < boardSize.width; ++j)
+				objectPoints[0].push_back(cv::Point3f(float(j*squareSize), float(i*squareSize), 0));
+		objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
+
 #if CV_MAJOR_VERSION < 3
 		rms = cv::stereoCalibrate(
 				objectPoints,
@@ -985,7 +1101,6 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 				cv::CALIB_FIX_INTRINSIC,
 				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
 #endif
-	}
 		UINFO("stereo calibration... done with RMS error=%f", rms);
 
 		std::cout << "R = " << R << std::endl;
@@ -1003,6 +1118,11 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 							imageSize, R, T, R1, R2, P1, P2, Q,
 							cv::CALIB_ZERO_DISPARITY, 0, imageSize);
 
+			std::cout << "R1 = " << R1 << std::endl;
+			std::cout << "P1 = " << P1 << std::endl;
+			std::cout << "R2 = " << R2 << std::endl;
+			std::cout << "P2 = " << P2 << std::endl;
+
 			double err = 0;
 			int npoints = 0;
 			std::vector<cv::Vec3f> lines[2];
@@ -1047,6 +1167,7 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 							imageSize_[1], right.K_raw(), right.D_raw(), right.R(), right.P(),
 							R, T, E, F);
 		}
+	}
 	return output;
 }
 

guilib/src/GuiLib.qrc

@@ -36,5 +36,6 @@
         <file>images/d435.png</file>
         <file>images/d415.png</file>
         <file>images/tara.png</file>
+        <file>images/t265.png</file>
     </qresource>
 </RCC>

guilib/src/MainWindow.cpp

@@ -440,6 +440,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed()));
      connect(_ui->actionStereoTara, SIGNAL(triggered()), this, SLOT(selectStereoTara()));
 	connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb()));
+	connect(_ui->actionRealSense2_T265, SIGNAL(triggered()), this, SLOT(selectRealSense2Stereo()));
 	_ui->actionFreenect->setEnabled(CameraFreenect::available());
 	_ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
 	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
@@ -452,6 +453,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
 	_ui->actionRealSense2_D415->setEnabled(CameraRealSense2::available());
 	_ui->actionRealSense2_D435->setEnabled(CameraRealSense2::available());
+	_ui->actionRealSense2_T265->setEnabled(CameraRealSense2::available());
 	_ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
 	_ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
 	_ui->actionStereoZed->setEnabled(CameraStereoZed::available());
@@ -4452,6 +4454,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed);
     _ui->actionStereoTara->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoTara);
 	_ui->actionStereoUsb->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoUsb);
+	_ui->actionRealSense2_T265->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2Stereo);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -6187,6 +6190,11 @@ void MainWindow::selectRealSense2()
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense2);
 }
 
+void MainWindow::selectRealSense2Stereo()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense2Stereo);
+}
+
 void MainWindow::selectStereoDC1394()
 {
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcDC1394);

guilib/src/PreferencesDialog.cpp

@@ -303,6 +303,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	if (!CameraRealSense2::available())
 	{
 		_ui->comboBox_cameraRGBD->setItemData(kSrcRealSense2 - kSrcRGBD, 0, Qt::UserRole - 1);
+		_ui->comboBox_cameraRGBD->setItemData(kSrcRealSense2Stereo - kSrcStereo, 0, Qt::UserRole - 1);
 	}
 	if(!CameraStereoDC1394::available())
 	{
@@ -4334,7 +4335,8 @@ void PreferencesDialog::updateStereoDisparityVisibility()
 			 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoUsb - kSrcStereo ||
               _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoTara - kSrcStereo ||
 			 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo - kSrcStereo ||
-			 _ui->comboBox_cameraStereo->currentIndex() == kSrcDC1394 - kSrcStereo);
+			 _ui->comboBox_cameraStereo->currentIndex() == kSrcDC1394 - kSrcStereo ||
+			 _ui->comboBox_cameraStereo->currentIndex() == kSrcRealSense2Stereo - kSrcStereo);
 	_ui->checkBox_stereo_rectify->setVisible(_ui->checkBox_stereo_rectify->isEnabled());
 	_ui->label_stereo_rectify->setVisible(_ui->checkBox_stereo_rectify->isEnabled());
 }
@@ -5202,9 +5204,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			((CameraRealSense*)camera)->setRGBSource((CameraRealSense::RGBSource)_ui->comboBox_realsenseRGBSource->currentIndex());
 		}
 	}
-	else if (driver == kSrcRealSense2)
+	else if (driver == kSrcRealSense2 || driver == kSrcRealSense2Stereo)
 	{
-		if(useRawImages)
+		if(driver == kSrcRealSense2 && useRawImages)
 		{
 			QMessageBox::warning(this, tr("Calibration"),
 					tr("Using raw images for \"RealSense\" driver is not yet supported. "
@@ -5219,6 +5221,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				this->getSourceLocalTransform());
 			((CameraRealSense2*)camera)->setEmitterEnabled(_ui->checkbox_rs2_emitter->isChecked());
 			((CameraRealSense2*)camera)->setIRDepthFormat(_ui->checkbox_rs2_irDepth->isChecked());
+			((CameraRealSense2*)camera)->setImagesRectified(_ui->checkBox_stereo_rectify->isChecked() && !useRawImages);
 		}
 	}
 	else if(driver == kSrcRGBDImages)
@@ -5999,8 +6002,10 @@ void PreferencesDialog::calibrate()
 		}
 
 		bool freenect2 = driver == kSrcFreenect2;
+		bool fisheye = driver == kSrcRealSense2Stereo;
 		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB && driver != kSrcRealSense, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
 		_calibrationDialog->setSwitchedImages(freenect2);
+		_calibrationDialog->setFisheyeImages(fisheye);
 		_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 		_calibrationDialog->registerToEventsManager();
 

guilib/src/opencv/stereoRectifyFisheye.h

@@ -0,0 +1,341 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+This code is the same has cv::stereoRectify() but accepting fisheye distortion model:
+All cvUndistortPoints() have been replaced by cv::fisheye::undistortPoints()
+See https://github.com/opencv/opencv/blob/master/modules/calib3d/src/calibration.cpp
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef GUILIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_
+#define GUILIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_
+
+#include <opencv2/calib3d/calib3d.hpp>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/calib3d/calib3d_c.h>
+#endif
+
+namespace rtabmap
+{
+
+void
+icvGetRectanglesFisheye( const CvMat* cameraMatrix, const CvMat* distCoeffs,
+                 const CvMat* R, const CvMat* newCameraMatrix, CvSize imgSize,
+                 cv::Rect_<float>& inner, cv::Rect_<float>& outer )
+{
+    const int N = 9;
+    int x, y, k;
+    cv::Mat _pts(1, N*N, CV_32FC2);
+    CvPoint2D32f* pts = (CvPoint2D32f*)(_pts.data);
+
+    for( y = k = 0; y < N; y++ )
+        for( x = 0; x < N; x++ )
+            pts[k++] = cvPoint2D32f((float)x*imgSize.width/(N-1),
+                                    (float)y*imgSize.height/(N-1));
+
+    cv::Mat cameraMatrixM(cameraMatrix->rows, cameraMatrix->cols, cameraMatrix->type, cameraMatrix->data.ptr);
+    cv::Mat distCoeffsM(distCoeffs->rows, distCoeffs->cols, distCoeffs->type, distCoeffs->data.ptr);
+    cv::Mat RM(R->rows, R->cols, R->type, R->data.ptr);
+    cv::Mat newCameraMatrixM(newCameraMatrix->rows, newCameraMatrix->cols, newCameraMatrix->type, newCameraMatrix->data.ptr);
+    cv::fisheye::undistortPoints(_pts, _pts, cameraMatrixM, distCoeffsM, RM, newCameraMatrixM);
+    float iX0=-FLT_MAX, iX1=FLT_MAX, iY0=-FLT_MAX, iY1=FLT_MAX;
+    float oX0=FLT_MAX, oX1=-FLT_MAX, oY0=FLT_MAX, oY1=-FLT_MAX;
+    // find the inscribed rectangle.
+    // the code will likely not work with extreme rotation matrices (R) (>45%)
+    for( y = k = 0; y < N; y++ )
+        for( x = 0; x < N; x++ )
+        {
+            CvPoint2D32f p = pts[k++];
+            oX0 = MIN(oX0, p.x);
+            oX1 = MAX(oX1, p.x);
+            oY0 = MIN(oY0, p.y);
+            oY1 = MAX(oY1, p.y);
+
+            if( x == 0 )
+                iX0 = MAX(iX0, p.x);
+            if( x == N-1 )
+                iX1 = MIN(iX1, p.x);
+            if( y == 0 )
+                iY0 = MAX(iY0, p.y);
+            if( y == N-1 )
+                iY1 = MIN(iY1, p.y);
+        }
+    inner = cv::Rect_<float>(iX0, iY0, iX1-iX0, iY1-iY0);
+    outer = cv::Rect_<float>(oX0, oY0, oX1-oX0, oY1-oY0);
+}
+
+void cvStereoRectifyFisheye( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
+                      const CvMat* _distCoeffs1, const CvMat* _distCoeffs2,
+                      CvSize imageSize, const CvMat* matR, const CvMat* matT,
+                      CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,
+                      CvMat* matQ, int flags, double alpha, CvSize newImgSize )
+{
+    double _om[3], _t[3] = {0}, _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4];
+    double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3], _w3[3];
+    cv::Rect_<float> inner1, inner2, outer1, outer2;
+
+    CvMat om  = cvMat(3, 1, CV_64F, _om);
+    CvMat t   = cvMat(3, 1, CV_64F, _t);
+    CvMat uu  = cvMat(3, 1, CV_64F, _uu);
+    CvMat r_r = cvMat(3, 3, CV_64F, _r_r);
+    CvMat pp  = cvMat(3, 4, CV_64F, _pp);
+    CvMat ww  = cvMat(3, 1, CV_64F, _ww); // temps
+    CvMat w3  = cvMat(3, 1, CV_64F, _w3); // temps
+    CvMat wR  = cvMat(3, 3, CV_64F, _wr);
+    CvMat Z   = cvMat(3, 1, CV_64F, _z);
+    CvMat Ri  = cvMat(3, 3, CV_64F, _ri);
+    double nx = imageSize.width, ny = imageSize.height;
+    int i, k;
+    double nt, nw;
+
+    if( matR->rows == 3 && matR->cols == 3 )
+        cvRodrigues2(matR, &om);          // get vector rotation
+    else
+        cvConvert(matR, &om); // it's already a rotation vector
+    cvConvertScale(&om, &om, -0.5); // get average rotation
+    cvRodrigues2(&om, &r_r);        // rotate cameras to same orientation by averaging
+
+    cvMatMul(&r_r, matT, &t);
+    int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;
+    // if idx == 0
+    //   e1 = T / ||T||
+    //   e2 = e1 x [0,0,1]
+
+    // if idx == 1
+    //   e2 = T / ||T||
+    //   e1 = e2 x [0,0,1]
+
+    // e3 = e1 x e2
+    _uu[2] = 1;
+    cvCrossProduct(&uu, &t, &ww);
+    nt = cvNorm(&t, 0, CV_L2);
+    CV_Assert(fabs(nt) > 0);
+    nw = cvNorm(&ww, 0, CV_L2);
+    CV_Assert(fabs(nw) > 0);
+    cvConvertScale(&ww, &ww, 1 / nw);
+    cvCrossProduct(&t, &ww, &w3);
+    nw = cvNorm(&w3, 0, CV_L2);
+    CV_Assert(fabs(nw) > 0);
+    cvConvertScale(&w3, &w3, 1 / nw);
+    _uu[2] = 0;
+    for (i = 0; i < 3; ++i)
+    {
+        _wr[idx][i] = -_t[i] / nt;
+        _wr[idx ^ 1][i] = -_ww[i];
+        _wr[2][i] = _w3[i] * (1 - 2 * idx); // if idx == 1 -> opposite direction
+    }
+    // apply to both views
+    cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T);
+    cvConvert( &Ri, _R1 );
+    cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0);
+    cvConvert( &Ri, _R2 );
+    cvMatMul(&Ri, matT, &t);
+    // calculate projection/camera matrices
+    // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
+    double fc_new = DBL_MAX;
+    CvPoint2D64f cc_new[2] = {};
+
+    newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;
+    const double ratio_x = (double)newImgSize.width / imageSize.width / 2;
+    const double ratio_y = (double)newImgSize.height / imageSize.height / 2;
+    const double ratio = idx == 1 ? ratio_x : ratio_y;
+    fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio;
+    for( k = 0; k < 2; k++ )
+    {
+        const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
+        const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;
+        CvPoint2D32f _pts[4] = {};
+        CvPoint3D32f _pts_3[4] = {};
+        CvMat pts = cvMat(1, 4, CV_32FC2, _pts);
+        CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);
+
+        for( i = 0; i < 4; i++ )
+        {
+            int j = (i<2) ? 0 : 1;
+            _pts[i].x = (float)((i % 2)*(nx));
+            _pts[i].y = (float)(j*(ny));
+        }
+        cv::Mat ptsM(pts.rows, pts.cols, pts.type, pts.data.ptr);
+        cv::Mat A_m(A->rows, A->cols, A->type, A->data.ptr);
+        cv::Mat Dk_m(Dk->rows, Dk->cols, Dk->type, Dk->data.ptr);
+        cv::fisheye::undistortPoints( ptsM, ptsM, A_m, Dk_m, cv::Mat(), cv::Mat() );
+        cvConvertPointsHomogeneous( &pts, &pts_3 );
+
+        //Change camera matrix to have cc=[0,0] and fc = fc_new
+        double _a_tmp[3][3];
+        CvMat A_tmp  = cvMat(3, 3, CV_64F, _a_tmp);
+        _a_tmp[0][0]=fc_new;
+        _a_tmp[1][1]=fc_new;
+        _a_tmp[0][2]=0.0;
+        _a_tmp[1][2]=0.0;
+
+        cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts );
+        CvScalar avg = cvAvg(&pts);
+
+        cc_new[k].x = (nx)/2 - avg.val[0];
+        cc_new[k].y = (ny)/2 - avg.val[1];
+    }
+
+    // vertical focal length must be the same for both images to keep the epipolar constraint
+    // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines)
+    // use fy for fx also, for simplicity
+
+    // For simplicity, set the principal points for both cameras to be the average
+    // of the two principal points (either one of or both x- and y- coordinates)
+    if( flags & cv::CALIB_ZERO_DISPARITY )
+    {
+        cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
+        cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
+    }
+    else if( idx == 0 ) // horizontal stereo
+        cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
+    else // vertical stereo
+        cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
+
+    cvZero( &pp );
+    _pp[0][0] = _pp[1][1] = fc_new;
+    _pp[0][2] = cc_new[0].x;
+    _pp[1][2] = cc_new[0].y;
+    _pp[2][2] = 1;
+    cvConvert(&pp, _P1);
+
+    _pp[0][2] = cc_new[1].x;
+    _pp[1][2] = cc_new[1].y;
+    _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length
+    cvConvert(&pp, _P2);
+
+    alpha = MIN(alpha, 1.);
+
+    icvGetRectanglesFisheye( _cameraMatrix1, _distCoeffs1, _R1, _P1, imageSize, inner1, outer1 );
+    icvGetRectanglesFisheye( _cameraMatrix2, _distCoeffs2, _R2, _P2, imageSize, inner2, outer2 );
+
+    {
+    newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize;
+    double cx1_0 = cc_new[0].x;
+    double cy1_0 = cc_new[0].y;
+    double cx2_0 = cc_new[1].x;
+    double cy2_0 = cc_new[1].y;
+    double cx1 = newImgSize.width*cx1_0/imageSize.width;
+    double cy1 = newImgSize.height*cy1_0/imageSize.height;
+    double cx2 = newImgSize.width*cx2_0/imageSize.width;
+    double cy2 = newImgSize.height*cy2_0/imageSize.height;
+    double s = 1.;
+
+    if( alpha >= 0 )
+    {
+        double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)),
+                            (double)(newImgSize.width - cx1)/(inner1.x + inner1.width - cx1_0)),
+                        (double)(newImgSize.height - cy1)/(inner1.y + inner1.height - cy1_0));
+        s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)),
+                         (double)(newImgSize.width - cx2)/(inner2.x + inner2.width - cx2_0)),
+                     (double)(newImgSize.height - cy2)/(inner2.y + inner2.height - cy2_0)),
+                 s0);
+
+        double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)),
+                            (double)(newImgSize.width - cx1)/(outer1.x + outer1.width - cx1_0)),
+                        (double)(newImgSize.height - cy1)/(outer1.y + outer1.height - cy1_0));
+        s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)),
+                         (double)(newImgSize.width - cx2)/(outer2.x + outer2.width - cx2_0)),
+                     (double)(newImgSize.height - cy2)/(outer2.y + outer2.height - cy2_0)),
+                 s1);
+
+        s = s0*(1 - alpha) + s1*alpha;
+    }
+
+    fc_new *= s;
+    cc_new[0] = cvPoint2D64f(cx1, cy1);
+    cc_new[1] = cvPoint2D64f(cx2, cy2);
+
+    cvmSet(_P1, 0, 0, fc_new);
+    cvmSet(_P1, 1, 1, fc_new);
+    cvmSet(_P1, 0, 2, cx1);
+    cvmSet(_P1, 1, 2, cy1);
+
+    cvmSet(_P2, 0, 0, fc_new);
+    cvmSet(_P2, 1, 1, fc_new);
+    cvmSet(_P2, 0, 2, cx2);
+    cvmSet(_P2, 1, 2, cy2);
+    cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));
+
+    }
+
+    if( matQ )
+    {
+        double q[] =
+        {
+            1, 0, 0, -cc_new[0].x,
+            0, 1, 0, -cc_new[0].y,
+            0, 0, 0, fc_new,
+            0, 0, -1./_t[idx],
+            (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]
+        };
+        CvMat Q = cvMat(4, 4, CV_64F, q);
+        cvConvert( &Q, matQ );
+    }
+}
+
+void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCoeffs1,
+				cv::InputArray _cameraMatrix2, cv::InputArray _distCoeffs2,
+				cv::Size imageSize, cv::InputArray _Rmat, cv::InputArray _Tmat,
+				cv::OutputArray _Rmat1, cv::OutputArray _Rmat2,
+				cv::OutputArray _Pmat1, cv::OutputArray _Pmat2,
+				cv::OutputArray _Qmat, int flags,
+				double alpha, cv::Size newImageSize)
+{
+    cv::Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat();
+    cv::Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat();
+    cv::Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat();
+    CvMat c_cameraMatrix1 = CvMat(cameraMatrix1);
+    CvMat c_cameraMatrix2 = CvMat(cameraMatrix2);
+    CvMat c_distCoeffs1 = CvMat(distCoeffs1);
+    CvMat c_distCoeffs2 = CvMat(distCoeffs2);
+    CvMat c_R = CvMat(Rmat), c_T = CvMat(Tmat);
+
+    int rtype = CV_64F;
+    _Rmat1.create(3, 3, rtype);
+    _Rmat2.create(3, 3, rtype);
+    _Pmat1.create(3, 4, rtype);
+    _Pmat2.create(3, 4, rtype);
+    cv::Mat R1 = _Rmat1.getMat(), R2 = _Rmat2.getMat(), P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(), Q;
+    CvMat c_R1 = CvMat(R1), c_R2 = CvMat(R2), c_P1 = CvMat(P1), c_P2 = CvMat(P2);
+    CvMat c_Q, *p_Q = 0;
+
+    if( _Qmat.needed() )
+    {
+        _Qmat.create(4, 4, rtype);
+        p_Q = &(c_Q = CvMat(Q = _Qmat.getMat()));
+    }
+
+    CvMat *p_distCoeffs1 = distCoeffs1.empty() ? NULL : &c_distCoeffs1;
+    CvMat *p_distCoeffs2 = distCoeffs2.empty() ? NULL : &c_distCoeffs2;
+    cvStereoRectifyFisheye( &c_cameraMatrix1, &c_cameraMatrix2, p_distCoeffs1, p_distCoeffs2,
+        CvSize(imageSize), &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha,
+        CvSize(newImageSize));
+}
+
+}
+
+
+#endif /* GUILIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_ */

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>25</height>
+     <height>22</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -257,9 +257,20 @@
        </property>
        <addaction name="actionStereoTara"/>
       </widget>
+      <widget class="QMenu" name="menuRealSense_T265">
+       <property name="title">
+        <string>RealSense T265</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/t265.png</normaloff>:/images/t265.png</iconset>
+       </property>
+       <addaction name="actionRealSense2_T265"/>
+      </widget>
       <addaction name="menuBumblebee2_2"/>
       <addaction name="menuZed_camera"/>
       <addaction name="menuTara_Camera"/>
+      <addaction name="menuRealSense_T265"/>
      </widget>
      <addaction name="menuRGB_D_camera"/>
      <addaction name="menuStereo_camera"/>
@@ -1541,6 +1552,14 @@
     <string>RGBD-SLAM (stamp tx ty tz qx qy qz qw)</string>
    </property>
   </action>
+  <action name="actionRealSense2_T265">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>RealSense2</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -94,7 +94,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-359</y>
             <width>680</width>
             <height>3082</height>
            </rect>
@@ -126,7 +126,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>14</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -2993,7 +2993,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>3</number>
+                       <number>1</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -3122,7 +3122,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>7</number>
+                              <number>9</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -4235,6 +4235,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <string>Tara Camera</string>
                                 </property>
                                </item>
+                               <item>
+                                <property name="text">
+                                 <string>RealSense2</string>
+                                </property>
+                               </item>
                               </widget>
                              </item>
                              <item row="0" column="2">
@@ -4315,7 +4320,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_stereo">
                              <property name="currentIndex">
-                              <number>2</number>
+                              <number>7</number>
                              </property>
                              <widget class="QWidget" name="page_49">
                               <layout class="QVBoxLayout" name="verticalLayout_91">
@@ -4861,6 +4866,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                </item>
                               </layout>
                              </widget>
+                             <widget class="QWidget" name="page_80">
+                              <layout class="QVBoxLayout" name="verticalLayout_140">
+                               <item>
+                                <spacer name="verticalSpacer_78">
+                                 <property name="orientation">
+                                  <enum>Qt::Vertical</enum>
+                                 </property>
+                                 <property name="sizeHint" stdset="0">
+                                  <size>
+                                   <width>20</width>
+                                   <height>471</height>
+                                  </size>
+                                 </property>
+                                </spacer>
+                               </item>
+                              </layout>
+                             </widget>
                             </widget>
                            </item>
                           </layout>