RealSense: fixed color rectification. Fixed ZR300 odometry frame.

corelib/include/rtabmap/core/CameraRGBD.h

@@ -379,6 +379,7 @@ class RTABMAP_EXP CameraRealSense :
 {
 public:
 	static bool available();
+	enum RGBSource {kColor, kInfrared, kFishEye};
 
 public:
 	// default local transform z in, x right, y down));
@@ -391,11 +392,8 @@ public:
 		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense();
 
-	void setDepthScaledToRGBSize(bool enabled) {
+	void setDepthScaledToRGBSize(bool enabled);
-#ifdef RTABMAP_REALSENSE
+	void setRGBSource(RGBSource source);
-		depthScaledToRGBSize_ = enabled;
-#endif
-		}
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
@@ -413,6 +411,9 @@ private:
 	int presetDepth_;
 	bool computeOdometry_;
 	bool depthScaledToRGBSize_;
+	RGBSource rgbSource_;
+	CameraModel cameraModel_;
+	std::vector<int> rsRectificationTable_;
 
 	int motionSeq_[2];
 	rs::slam::slam * slam_;

corelib/src/CameraModel.cpp

@@ -278,26 +278,34 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 				n["data"] >> data;
 				UASSERT(rows*cols == (int)data.size());
 				UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
-				std::string distortionModel = (std::string)n["distortion_model"];
+				D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-				if(uStrContains(distortionModel, "fisheye") ||
-				   uStrContains(distortionModel, "equidistant"))
-				{
-					D_ = cv::Mat::zeros(1,6,CV_64FC1);
-					D_.at<double>(0,0) = data[0];
-					D_.at<double>(0,1) = data[1];
-					D_.at<double>(0,4) = data[2];
-					D_.at<double>(0,5) = data[3];
-				}
-				else
-				{
-					D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
-				}
 			}
 			else
 			{
 				UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
 			}
 
+			n = fs["distortion_model"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				std::string distortionModel = (std::string)n;
+				if(D_.cols>=4 &&
+				  (uStrContains(distortionModel, "fisheye") ||
+				   uStrContains(distortionModel, "equidistant")))
+				{
+					cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
+					D.at<double>(0,0) = D_.at<double>(0,0);
+					D.at<double>(0,1) = D_.at<double>(0,1);
+					D.at<double>(0,4) = D_.at<double>(0,2);
+					D.at<double>(0,5) = D_.at<double>(0,3);
+					D_ = D;
+				}
+			}
+			else
+			{
+				UWARN("Missing \"distortion_model\" field in \"%s\"", filePath.c_str());
+			}
+
 			n = fs["rectification_matrix"];
 			if(n.type() != cv::FileNode::NONE)
 			{
@@ -400,7 +408,7 @@ bool CameraModel::save(const std::string & directory) const
 			// compaibility with ROS
 			if(D_.cols == 6)
 			{
-				fs << "distortion_model" << "fisheye"; // equidistant
+				fs << "distortion_model" << "equidistant"; // equidistant, fisheye
 			}
 			else if(D.cols > 5)
 			{

corelib/src/CameraRGBD.cpp

@@ -2618,6 +2618,7 @@ CameraRealSense::CameraRealSense(
 	presetDepth_(presetDepth),
 	computeOdometry_(computeOdometry),
 	depthScaledToRGBSize_(false),
+	rgbSource_(kColor),
 	slam_(0)
 #endif
 {
@@ -2631,14 +2632,18 @@ CameraRealSense::~CameraRealSense()
 	UDEBUG("");
 	if(dev_)
 	{
-		if(slam_!=0)
+		try
 		{
-			dev_->stop(rs::source::all_sources);
+			if(slam_!=0)
-		}
+			{
-		else
+				dev_->stop(rs::source::all_sources);
-		{
+			}
-			dev_->stop();
+			else
+			{
+				dev_->stop();
+			}
 		}
+		catch(const rs::error & error){UWARN("%s", error.what());}
 		dev_ = 0;
 	}
 	UDEBUG("");
@@ -2685,6 +2690,118 @@ bool setStreamConfigIntrin(
 }
 #endif
 
+void CameraRealSense::setDepthScaledToRGBSize(bool enabled) {
+#ifdef RTABMAP_REALSENSE
+	depthScaledToRGBSize_ = enabled;
+#endif
+}
+void CameraRealSense::setRGBSource(RGBSource source)
+{
+#ifdef RTABMAP_REALSENSE
+	rgbSource_ = source;
+#endif
+}
+
+#ifdef RTABMAP_REALSENSE
+template<class GET_DEPTH, class TRANSFER_PIXEL> void align_images(const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
+{
+	// Iterate over the pixels of the depth image
+#pragma omp parallel for schedule(dynamic)
+	for(int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
+	{
+		int depth_pixel_index = depth_y * depth_intrin.width;
+		for(int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
+		{
+			// Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images
+			if(float depth = get_depth(depth_pixel_index))
+			{
+				// Map the top-left corner of the depth pixel onto the other image
+				float depth_pixel[2] = {depth_x-0.5f, depth_y-0.5f}, depth_point[3], other_point[3], other_pixel[2];
+				rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
+				rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
+				rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+				const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
+				const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
+
+				// Map the bottom-right corner of the depth pixel onto the other image
+				depth_pixel[0] = depth_x+0.5f; depth_pixel[1] = depth_y+0.5f;
+				rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
+				rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
+				rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+				const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
+				const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
+
+				if(other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height) continue;
+
+				// Transfer between the depth pixels and the pixels inside the rectangle on the other image
+				for(int y=other_y0; y<=other_y1; ++y) for(int x=other_x0; x<=other_x1; ++x) transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
+			}
+		}
+	}
+}
+typedef uint8_t byte;
+
+void align_z_to_other(byte * z_aligned_to_other, const uint16_t * z_pixels, float z_scale, const rs_intrinsics & z_intrin, const rs_extrinsics & z_to_other, const rs_intrinsics & other_intrin)
+{
+	auto out_z = (uint16_t *)(z_aligned_to_other);
+	align_images(z_intrin, z_to_other, other_intrin,
+		[z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
+		[out_z, z_pixels](int z_pixel_index, int other_pixel_index) { out_z[other_pixel_index] = out_z[other_pixel_index] ? std::min(out_z[other_pixel_index],z_pixels[z_pixel_index]) : z_pixels[z_pixel_index]; });
+}
+
+void align_disparity_to_other(byte * disparity_aligned_to_other, const uint16_t * disparity_pixels, float disparity_scale, const rs_intrinsics & disparity_intrin, const rs_extrinsics & disparity_to_other, const rs_intrinsics & other_intrin)
+{
+	auto out_disparity = (uint16_t *)(disparity_aligned_to_other);
+	align_images(disparity_intrin, disparity_to_other, other_intrin,
+		[disparity_pixels, disparity_scale](int disparity_pixel_index) { return disparity_scale / disparity_pixels[disparity_pixel_index]; },
+		[out_disparity, disparity_pixels](int disparity_pixel_index, int other_pixel_index) { out_disparity[other_pixel_index] = disparity_pixels[disparity_pixel_index]; });
+}
+
+template<int N> struct bytes { char b[N]; };
+template<int N, class GET_DEPTH> void align_other_to_depth_bytes(byte * other_aligned_to_depth, GET_DEPTH get_depth, const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, const byte * other_pixels)
+{
+	auto in_other = (const bytes<N> *)(other_pixels);
+	auto out_other = (bytes<N> *)(other_aligned_to_depth);
+	align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
+		[out_other, in_other](int depth_pixel_index, int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
+}
+
+/////////////////////////
+// Image rectification //
+/////////////////////////
+
+std::vector<int> compute_rectification_table(const rs_intrinsics & rect_intrin, const rs_extrinsics & rect_to_unrect, const rs_intrinsics & unrect_intrin)
+{
+	std::vector<int> rectification_table;
+	rectification_table.resize(rect_intrin.width * rect_intrin.height);
+	align_images(rect_intrin, rect_to_unrect, unrect_intrin, [](int) { return 1.0f; },
+		[&rectification_table](int rect_pixel_index, int unrect_pixel_index) { rectification_table[rect_pixel_index] = unrect_pixel_index; });
+	return rectification_table;
+}
+
+template<class T> void rectify_image_pixels(T * rect_pixels, const std::vector<int> & rectification_table, const T * unrect_pixels)
+{
+	for(auto entry : rectification_table) *rect_pixels++ = unrect_pixels[entry];
+}
+
+void rectify_image(uint8_t * rect_pixels, const std::vector<int> & rectification_table, const uint8_t * unrect_pixels, rs_format format)
+{
+	switch(format)
+	{
+	case RS_FORMAT_Y8:
+		return rectify_image_pixels((bytes<1> *)rect_pixels, rectification_table, (const bytes<1> *)unrect_pixels);
+	case RS_FORMAT_Y16: case RS_FORMAT_Z16:
+		return rectify_image_pixels((bytes<2> *)rect_pixels, rectification_table, (const bytes<2> *)unrect_pixels);
+	case RS_FORMAT_RGB8: case RS_FORMAT_BGR8:
+		return rectify_image_pixels((bytes<3> *)rect_pixels, rectification_table, (const bytes<3> *)unrect_pixels);
+	case RS_FORMAT_RGBA8: case RS_FORMAT_BGRA8:
+		return rectify_image_pixels((bytes<4> *)rect_pixels, rectification_table, (const bytes<4> *)unrect_pixels);
+	default:
+		assert(false); // NOTE: rectify_image_pixels(...) is not appropriate for RS_FORMAT_YUYV images, no logic prevents U/V channels from being written to one another
+	}
+}
+#endif
+
 bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -2692,10 +2809,22 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 
 	if(dev_)
 	{
-		dev_->stop(rs::source::all_sources);
+		try
+		{
+			if(slam_!=0)
+			{
+				dev_->stop(rs::source::all_sources);
+			}
+			else
+			{
+				dev_->stop();
+			}
+		}
+		catch(const rs::error & error){UWARN("%s", error.what());}
 		dev_ = 0;
 	}
 	bufferedFrames_.clear();
+	rsRectificationTable_.clear();
 
 #ifdef RTABMAP_REALSENSE_SLAM
 	motionSeq_[0] = motionSeq_[1] = 0;
@@ -2735,29 +2864,78 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 	UINFO("    Preset RGB: %d", presetRGB_);
 	UINFO("    Preset Depth: %d", presetDepth_);
 
-	bool computeOdometry = false;
+#ifndef RTABMAP_REALSENSE_SLAM
-#ifdef RTABMAP_REALSENSE_SLAM
+	computeOdometry_ = false;
-	if (name.find("ZR300") != std::string::npos && computeOdometry_)
-	{
-		// Only enable ZR300 functionality if fisheye stream is enabled.
-		// Accel/Gyro automatically enabled when fisheye requested
-		computeOdometry = true;
-	}
 #endif
 
+	if (name.find("ZR300") == std::string::npos)
+	{
+		// Only enable ZR300 functionality odometry if fisheye stream is enabled.
+		// Accel/Gyro automatically enabled when fisheye requested
+		computeOdometry_ = false;
+		// Only ZR300 has fisheye
+		if(rgbSource_ == kFishEye)
+		{
+			UWARN("Fisheye cannot be used with %s camera, using color instead...", name.c_str());
+			rgbSource_ = kColor;
+		}
+	}
+
+	rs::intrinsics depth_intrin;
+	rs::intrinsics fisheye_intrin;
+	rs::intrinsics color_intrin;
 	// Configure depth and color to run with the device's preferred settings
 	UINFO("Enabling streams...");
 	// R200: 
 	//    0=640x480   vs 480x360
 	//    1=1920x1080 vs 640x480
 	//    2=640x480   vs 320x240
-	dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
+	try {
-	dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
 
-	rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
+		// left/rgb stream
-	rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+		if(rgbSource_==kFishEye || computeOdometry_)
-	UINFO("    RGB:   %dx%d", color_intrin.width, color_intrin.height);
+		{
-	UINFO("    Depth: %dx%d", depth_intrin.width, depth_intrin.height);
+			dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
+			if(computeOdometry_)
+			{
+				// Needed to align image timestamps to common clock-domain with the motion events
+				dev_->set_option(rs::option::fisheye_strobe, 1);
+			}
+			// This option causes the fisheye image to be acquired in-sync with the depth image.
+			dev_->set_option(rs::option::fisheye_external_trigger, 1);
+			dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
+			fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
+			UINFO("    Fisheye:   %dx%d", fisheye_intrin.width, fisheye_intrin.height);
+			if(rgbSource_==kFishEye)
+			{
+				color_intrin = fisheye_intrin; // not rectified
+			}
+		}
+		if(rgbSource_!=kFishEye)
+		{
+			dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color); // rectified
+			UINFO("    RGB:   %dx%d", color_intrin.width, color_intrin.height);
+
+			if(rgbSource_==kInfrared)
+			{
+				dev_->enable_stream(rs::stream::infrared, (rs::preset)presetRGB_);
+				color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared); // rectified
+				UINFO("    IR left:   %dx%d", color_intrin.width, color_intrin.height);
+			}
+		}
+
+		dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
+		depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); // rectified
+		UINFO("    Depth: %dx%d", depth_intrin.width, depth_intrin.height);
+	}
+	catch(const rs::error & error)
+	{
+		UERROR("Failed starting the streams: %s", error.what());
+		return false;
+	}
+
+	Transform imu2Camera = Transform::getIdentity();
 
 #ifdef RTABMAP_REALSENSE_SLAM
 	UDEBUG("Setup frame callback");
@@ -2787,9 +2965,50 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		  frame.get_stride()
 		};
 		cv::Mat image;
-		if(frame.get_format() == rs::format::raw8)
+		if(frame.get_format() == rs::format::raw8 || frame.get_format() == rs::format::y8)
 		{
 			image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
+			if(frame.get_stream_type() == rs::stream::fisheye)
+			{
+				// fisheye always received just after the depth image (doesn't have exact timestamp with depth)
+				if(bufferedFrames_.size())
+				{
+					bufferedFrames_.rbegin()->second.first = image.clone();
+					UScopeMutex lock(dataMutex_);
+					bool notify = lastSyncFrames_.first.empty();
+					lastSyncFrames_ = bufferedFrames_.rbegin()->second;
+					if(notify)
+					{
+						dataReady_.release();
+					}
+					bufferedFrames_.clear();
+				}
+			}
+			else if(frame.get_stream_type() == rs::stream::infrared) // infrared (does have exact timestamp with depth)
+			{
+				if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
+				{
+					bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
+					UScopeMutex lock(dataMutex_);
+					bool notify = lastSyncFrames_.first.empty();
+					lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
+					if(notify)
+					{
+						dataReady_.release();
+					}
+					bufferedFrames_.erase(frame.get_timestamp());
+				}
+				else
+				{
+					bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
+				}
+				if(bufferedFrames_.size()>5)
+				{
+					UWARN("Frames cannot be synchronized!");
+					bufferedFrames_.clear();
+				}
+				return;
+			}
 		}
 		else if(frame.get_format() == rs::format::z16)
 		{
@@ -2818,7 +3037,15 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		}
 		else if(frame.get_format() == rs::format::rgb8)
 		{
-			image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
+			if(rsRectificationTable_.size())
+			{
+				image = cv::Mat(height, width, CV_8UC3);
+				rectify_image(image.data, rsRectificationTable_, (unsigned char*)frame.get_data(), (rs_format)frame.get_format());
+			}
+			else
+			{
+				image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
+			}
 			if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
 			{
 				bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
@@ -2868,26 +3095,27 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		}
 	};
 
+	UDEBUG("");
 	// Setup stream callback for stream
-	if(computeOdometry)
+	if(computeOdometry_ || rgbSource_ == kFishEye)
 	{
 		dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
 	}
-	dev_->set_frame_callback(rs::stream::depth, frameCallback);
+	if(rgbSource_ == kInfrared)
-	dev_->set_frame_callback(rs::stream::color, frameCallback);
-
-	if (computeOdometry)
 	{
-		dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
+		dev_->set_frame_callback(rs::stream::infrared, frameCallback);
-		rs::intrinsics fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
+	}
-		UINFO("    Fish: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
+	else if(rgbSource_ == kColor)
+	{
+		dev_->set_frame_callback(rs::stream::color, frameCallback);
+	}
 
-		// Needed to align image timestamps to common clock-domain with the motion events
+	dev_->set_frame_callback(rs::stream::depth, frameCallback);
-		dev_->set_option(rs::option::fisheye_strobe, 1);
-		// This option causes the fisheye image to be aquired in-sync with the depth image.
-		dev_->set_option(rs::option::fisheye_external_trigger, 1);
-		dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
 
+	UDEBUG("");
+
+	if (computeOdometry_)
+	{
 		UDEBUG("Setup motion callback");
 		//define callback to the motion events and set it.
 		std::function<void(rs::motion_data)> motion_callback;
@@ -2995,23 +3223,137 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 			return false;
 		}
 
-		dev_->start(rs::source::all_sources);
+		rs::extrinsics fisheye2imu = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
+		imu2Camera = Transform(
+				fisheye2imu.rotation[0], fisheye2imu.rotation[1], fisheye2imu.rotation[2], fisheye2imu.translation[0],
+				fisheye2imu.rotation[3], fisheye2imu.rotation[4], fisheye2imu.rotation[5], fisheye2imu.translation[1],
+				fisheye2imu.rotation[6], fisheye2imu.rotation[7], fisheye2imu.rotation[8], fisheye2imu.translation[2]).inverse();
+
+		if(rgbSource_ == kInfrared)
+		{
+			rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::infrared);
+			Transform fisheye2Color = Transform(
+					color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
+					color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
+					color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
+			imu2Camera *= fisheye2Color;
+		}
+		else if(rgbSource_ == kColor)
+		{
+			rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::rectified_color);
+			Transform fisheye2Color = Transform(
+					color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
+					color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
+					color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
+			imu2Camera *= fisheye2Color;
+		}
+
+		UDEBUG("start device!");
+		try
+		{
+			dev_->start(rs::source::all_sources);
+		}
+		catch(const rs::error & error)
+		{
+			UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
+			return false;
+		}
 	}
 	else
 	{
-		dev_->start();
+		UDEBUG("start device!");
+		try
+		{
+			dev_->start();
+		}
+		catch(const rs::error & error)
+		{
+			UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
+			return false;
+		}
 	}
 #else
-	dev_->start();
 	try {
+		dev_->start();
 		dev_->wait_for_frames();
 	}
 	catch (const rs::error & e)
 	{
-		UERROR("Exception: %s", e.what());
+		UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
 	}
 #endif
 
+	cv::Mat D;
+	if(rgbSource_ == kFishEye)
+	{
+		// ftheta/equidistant model
+		D = cv::Mat::zeros(1,6,CV_64FC1);
+		D.at<double>(0,0) = color_intrin.coeffs[0];
+		D.at<double>(0,1) = color_intrin.coeffs[1];
+		D.at<double>(0,4) = color_intrin.coeffs[2];
+		D.at<double>(0,5) = color_intrin.coeffs[3];
+	}
+	else
+	{
+		// Brown-Conrady / radtan
+		D = cv::Mat::zeros(1,5,CV_64FC1);
+		D.at<double>(0,0) = color_intrin.coeffs[0];
+		D.at<double>(0,1) = color_intrin.coeffs[1];
+		D.at<double>(0,2) = color_intrin.coeffs[2];
+		D.at<double>(0,3) = color_intrin.coeffs[3];
+		D.at<double>(0,4) = color_intrin.coeffs[4];
+	}
+	cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+	K.at<double>(0,0) = color_intrin.fx;
+	K.at<double>(1,1) = color_intrin.fy;
+	K.at<double>(0,2) = color_intrin.ppx;
+	K.at<double>(1,2) = color_intrin.ppy;
+	cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+	cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+	K(cv::Range(0,2), cv::Range(0,3)).copyTo(P(cv::Range(0,2), cv::Range(0,3)));
+	cameraModel_ = CameraModel(
+			dev_->get_name(),
+			cv::Size(color_intrin.width, color_intrin.height),
+			K,
+			D,
+			R,
+			P,
+			this->getLocalTransform()*imu2Camera);
+
+	UDEBUG("");
+
+	if(rgbSource_ == kColor)
+	{
+		rs::extrinsics rect_to_unrect = dev_->get_extrinsics(rs::stream::rectified_color, rs::stream::color);
+		rs::intrinsics unrect_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+		rsRectificationTable_ = compute_rectification_table(color_intrin, rect_to_unrect, unrect_intrin);
+	}
+	else if(rgbSource_ == kFishEye)
+	{
+		UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
+		if(!calibrationFolder.empty() && !cameraName.empty())
+		{
+			CameraModel model;
+			if(!model.load(calibrationFolder, cameraName))
+			{
+				UWARN("Failed to load calibration \"%s\" in \"%s\" folder, you should calibrate the camera!",
+						cameraName.c_str(), calibrationFolder.c_str());
+			}
+			else
+			{
+				UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
+						model.fx(),
+						model.fy(),
+						model.cx(),
+						model.cy());
+				cameraModel_ = model;
+				cameraModel_.setName(cameraName);
+				cameraModel_.initRectificationMap();
+				cameraModel_.setLocalTransform(this->getLocalTransform()*imu2Camera);
+			}
+		}
+	}
+
 	uSleep(1000); // ignore the first frames
 	UINFO("Enabling streams...done!");
 
@@ -3073,8 +3415,23 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 
 		// Retrieve camera parameters for mapping between depth and color
 		rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
-		rs::extrinsics depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::color);
+		rs::extrinsics depth_to_color;
-		rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+		rs::intrinsics color_intrin;
+		if(rgbSource_ == kFishEye)
+		{
+			depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
+		}
+		else if(rgbSource_ == kInfrared)
+		{
+			depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::infrared);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared);
+		}
+		else // color
+		{
+			depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::rectified_color);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color);
+		}
 
 #ifdef RTABMAP_REALSENSE_SLAM
 		if(!dataReady_.acquire(1, 5000))
@@ -3082,6 +3439,7 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 			UWARN("Not received new frames since 5 seconds, end of stream reached!");
 			return data;
 		}
+
 		{
 			UScopeMutex lock(dataMutex_);
 			rgb = lastSyncFrames_.first;
@@ -3106,84 +3464,124 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 
 		// Retrieve our images
 		depthIn = cv::Mat(depth_intrin.height, depth_intrin.width, CV_16UC1, (unsigned char*)dev_->get_frame_data(rs::stream::depth));
-		rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
+		if(rgbSource_ == kFishEye)
-#endif
-
-		float scale = dev_->get_depth_scale();
-
-		// factory registration...
-		cv::Mat bgr;
-		cv::cvtColor(rgb, bgr, CV_RGB2BGR);
-
-		CameraModel model(
-			color_intrin.fx, //fx
-			color_intrin.fy, //fy
-			color_intrin.ppx,  //cx
-			color_intrin.ppy,  //cy
-			this->getLocalTransform(),
-			0,
-			bgr.size());
-
-		cv::Mat depth;
-		if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
-			depth_intrin.width < color_intrin.width &&
-			depth_intrin.height < color_intrin.height &&
-			!depthScaledToRGBSize_)
 		{
-			//we can keep the depth image size as is
+			rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::fisheye));
-			depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
+		}
-			float scaleX = float(depth_intrin.width) / float(color_intrin.width);
+		else if(rgbSource_ == kInfrared)
-			float scaleY = float(depth_intrin.height) / float(color_intrin.height);
+		{
-			color_intrin.fx *= scaleX;
+			rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::infrared));
-			color_intrin.fy *= scaleY;
-			color_intrin.ppx *= scaleX;
-			color_intrin.ppy *= scaleY;
-			color_intrin.height = depth_intrin.height;
-			color_intrin.width = depth_intrin.width;
 		}
 		else
 		{
-			//depth to color
+			rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
-			depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
 		}
-		for (int dy = 0; dy < depth_intrin.height; ++dy)
+#endif
+
+		// factory registration...
+		cv::Mat bgr;
+		if(rgbSource_ != kColor)
 		{
-			for (int dx = 0; dx < depth_intrin.width; ++dx)
+			bgr = rgb;
+		}
+		else
+		{
+			cv::cvtColor(rgb, bgr, CV_RGB2BGR);
+		}
+
+		bool rectified = false;
+		if(rgbSource_ == kFishEye && cameraModel_.isRectificationMapInitialized())
+		{
+			bgr = cameraModel_.rectifyImage(bgr);
+			rectified = true;
+			color_intrin.fx = cameraModel_.fx();
+			color_intrin.fy = cameraModel_.fy();
+			color_intrin.ppx = cameraModel_.cx();
+			color_intrin.ppy = cameraModel_.cy();
+			UASSERT_MSG(color_intrin.width == cameraModel_.imageWidth() && color_intrin.height == cameraModel_.imageHeight(),
+					uFormat("color_intrin=%dx%d cameraModel_=%dx%d",
+							color_intrin.width, color_intrin.height, cameraModel_.imageWidth(), cameraModel_.imageHeight()).c_str());
+			((rs_intrinsics*)&color_intrin)->model = RS_DISTORTION_NONE;
+		}
+#ifndef RTABMAP_REALSENSE_SLAM
+		else if(rgbSource_ != kColor)
+		{
+			bgr = bgr.clone();
+		}
+#endif
+
+		cv::Mat depth;
+		if(rgbSource_ != kFishEye || rectified)
+		{
+			if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
+				depth_intrin.width < color_intrin.width &&
+				depth_intrin.height < color_intrin.height &&
+				!depthScaledToRGBSize_)
 			{
-				// Retrieve the 16-bit depth value and map it into a depth in meters
+				//we can keep the depth image size as is
-				uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
+				depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
-				float depth_in_meters = depth_value * scale;
+				float scaleX = float(depth_intrin.width) / float(color_intrin.width);
+				float scaleY = float(depth_intrin.height) / float(color_intrin.height);
+				color_intrin.fx *= scaleX;
+				color_intrin.fy *= scaleY;
+				color_intrin.ppx *= scaleX;
+				color_intrin.ppy *= scaleY;
+				color_intrin.height = depth_intrin.height;
+				color_intrin.width = depth_intrin.width;
+			}
+			else
+			{
+				//depth to color
+				depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
+			}
 
-				// Skip over pixels with a depth value of zero, which is used to indicate no data
+			float scale = dev_->get_depth_scale();
-				if (depth_value == 0 || depth_in_meters>10.0f) continue;
+			for (int dy = 0; dy < depth_intrin.height; ++dy)
-
+			{
-				// Map from pixel coordinates in the depth image to pixel coordinates in the color image
+				for (int dx = 0; dx < depth_intrin.width; ++dx)
-				rs::float2 depth_pixel = { (float)dx, (float)dy };
-				rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
-				rs::float3 color_point = depth_to_color.transform(depth_point);
-				rs::float2 color_pixel = color_intrin.project(color_point);
-
-				int pdx = color_pixel.x;
-				int pdy = color_pixel.y;
-				if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
 				{
-					depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
+					// Retrieve the 16-bit depth value and map it into a depth in meters
+					uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
+					float depth_in_meters = depth_value * scale;
+
+					// Skip over pixels with a depth value of zero, which is used to indicate no data
+					if (depth_value == 0 || depth_in_meters>10.0f) continue;
+
+					// Map from pixel coordinates in the depth image to pixel coordinates in the color image
+					int pdx = dx;
+					int pdy = dy;
+					if(rgbSource_ == kColor || rgbSource_ == kFishEye)
+					{
+						rs::float2 depth_pixel = { (float)dx, (float)dy };
+						rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
+						rs::float3 color_point = depth_to_color.transform(depth_point);
+						rs::float2 color_pixel = color_intrin.project(color_point);
+
+						pdx = color_pixel.x;
+						pdy = color_pixel.y;
+					}
+					//else infrared is already registered
+
+					if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
+					{
+						depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
+					}
 				}
 			}
+
+			if (color_intrin.width > depth_intrin.width)
+			{
+				// Fill holes
+				UTimer time;
+				util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
+				util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
+				UDEBUG("Filling depth holes: %fs", time.ticks());
+			}
 		}
 
-		if (color_intrin.width > depth_intrin.width)
+		if (!bgr.empty() && ((rgbSource_==kFishEye && !rectified) || !depth.empty()))
 		{
-			// Fill holes
+			data = SensorData(bgr, depth, cameraModel_, this->getNextSeqID(), UTimer::now());
-			UTimer time;
-			util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
-			util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
-			UDEBUG("Filling depth holes: %fs", time.ticks());
-		}
-		
-		if (!bgr.empty() && !depth.empty())
-		{
-			data = SensorData(bgr, depth, model, this->getNextSeqID(), UTimer::now());
 #ifdef RTABMAP_REALSENSE_SLAM
 			if(info && slam_)
 			{
@@ -3191,8 +3589,20 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 				rs::slam::PoseMatrix4f pose;
 				if(slam_->get_camera_pose(pose) == rs::core::status_no_error)
 				{
-					Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+					/*rs::slam::tracking_accuracy accuracy = slam_->get_tracking_accuracy();
-					info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
+					if( accuracy == rs::slam::tracking_accuracy::low ||
+						accuracy == rs::slam::tracking_accuracy::medium ||
+						accuracy == rs::slam::tracking_accuracy::high)*/
+					{
+						// the pose is in camera link or IMU frame, get pose of the color camera
+						Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+						info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
+					}
+					/*else
+					{
+						UERROR("Odometry failed: accuracy=%d", accuracy);
+					}*/
 				}
 				else
 				{

corelib/src/Memory.cpp

@@ -3822,7 +3822,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 
 			cv::Mat depthMask;
-			if(!decimatedData.depthRaw().empty() && _depthAsMask)
+			if(imagesRectified && !decimatedData.depthRaw().empty() && _depthAsMask)
 			{
 				if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
 					imageMono.cols % decimatedData.depthRaw().cols == 0 &&

guilib/src/CalibrationDialog.cpp

@@ -630,6 +630,7 @@ void CalibrationDialog::restart()
 	maxIrs_[1] = 0x7fff;
 
 	ui_->pushButton_calibrate->setEnabled(ui_->checkBox_unlock->isChecked());
+	ui_->checkBox_fisheye->setEnabled(ui_->checkBox_unlock->isChecked());
 	ui_->pushButton_save->setEnabled(false);
 	ui_->radioButton_raw->setChecked(true);
 	ui_->radioButton_rectified->setEnabled(false);
@@ -673,6 +674,7 @@ void CalibrationDialog::restart()
 void CalibrationDialog::unlock()
 {
 	ui_->pushButton_calibrate->setEnabled(true);
+	ui_->checkBox_fisheye->setEnabled(true);
 }
 
 void CalibrationDialog::calibrate()
@@ -715,13 +717,26 @@ void CalibrationDialog::calibrate()
 
 		if(fishEye)
 		{
-			rms = cv::fisheye::calibrate(objectPoints,
+			try
-				imagePoints_[id],
+			{
-				imageSize_[id],
+				rms = cv::fisheye::calibrate(objectPoints,
-				K,
+					imagePoints_[id],
-				D,
+					imageSize_[id],
-				rvecs,
+					K,
-				tvecs);
+					D,
+					rvecs,
+					tvecs,
+					cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
+					cv::fisheye::CALIB_CHECK_COND |
+					cv::fisheye::CALIB_FIX_SKEW);
+			}
+			catch(const cv::Exception & e)
+			{
+				UERROR("Error: %s (try restarting the calibration)", e.what());
+				QMessageBox::warning(this, tr("Calibration failed!"), tr("Error: %1 (try restarting the calibration)").arg(e.what()));
+				processingData_ = false;
+				return;
+			}
 		}
 		else
 #endif
@@ -748,7 +763,7 @@ void CalibrationDialog::calibrate()
 #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)))
 			if(fishEye)
 			{
-				cv::fisheye::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
+				cv::fisheye::projectPoints( cv::Mat(objectPoints[i]), imagePoints2, rvecs[i], tvecs[i], K, D);
 			}
 			else
 #endif
@@ -1066,8 +1081,7 @@ bool CalibrationDialog::save()
 	else
 	{
 		UASSERT(stereoModel_.left().isValidForRectification() &&
-				stereoModel_.right().isValidForRectification() &&
+				stereoModel_.right().isValidForRectification());
-				(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0));
 		QString cameraName = stereoModel_.name().c_str();
 		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
 		QString name = QFileInfo(filePath).baseName();

guilib/src/PreferencesDialog.cpp

@@ -574,6 +574,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_realsensePresetDepth, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_realsenseOdom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_realsenseDepthScaledToRGBSize, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_realsenseRGBSource, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_emitter, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
@@ -1662,6 +1663,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->comboBox_realsensePresetDepth->setCurrentIndex(2);
 		_ui->checkbox_realsenseOdom->setChecked(false);
 		_ui->checkbox_realsenseDepthScaledToRGBSize->setChecked(false);
+		_ui->comboBox_realsenseRGBSource->setCurrentIndex(0);
 		_ui->checkbox_rs2_emitter->setChecked(true);
 		_ui->checkbox_rs2_irDepth->setChecked(false);
 		_ui->lineEdit_openniOniPath->clear();
@@ -2061,6 +2063,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_realsensePresetDepth->setCurrentIndex(settings.value("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()).toInt());
 	_ui->checkbox_realsenseOdom->setChecked(settings.value("odom", _ui->checkbox_realsenseOdom->isChecked()).toBool());
 	_ui->checkbox_realsenseDepthScaledToRGBSize->setChecked(settings.value("depthScaled", _ui->checkbox_realsenseDepthScaledToRGBSize->isChecked()).toBool());
+	_ui->comboBox_realsenseRGBSource->setCurrentIndex(settings.value("rgbSource", _ui->comboBox_realsenseRGBSource->currentIndex()).toInt());
 	settings.endGroup(); // RealSense
 
 	settings.beginGroup("RealSense2");
@@ -2484,6 +2487,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("presetDepth",         _ui->comboBox_realsensePresetDepth->currentIndex());
 	settings.setValue("odom",                _ui->checkbox_realsenseOdom->isChecked());
 	settings.setValue("depthScaled",         _ui->checkbox_realsenseDepthScaledToRGBSize->isChecked());
+	settings.setValue("rgbSource",           _ui->comboBox_realsenseRGBSource->currentIndex());
 	settings.endGroup(); // RealSense
 
 	settings.beginGroup("RealSense2");
@@ -5021,10 +5025,10 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 	}
 	else if (driver == kSrcRealSense)
 	{
-		if(useRawImages)
+		if(useRawImages && _ui->comboBox_realsenseRGBSource->currentIndex()!=2)
 		{
 			QMessageBox::warning(this, tr("Calibration"),
-					tr("Using raw images for \"RealSense\" driver is not yet supported. "
+					tr("Using raw images for \"RealSense\" driver is not yet supported for color and infrared streams. "
 						"Factory calibration loaded from RealSense is used."), QMessageBox::Ok);
 			return 0;
 		}
@@ -5038,6 +5042,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 			((CameraRealSense*)camera)->setDepthScaledToRGBSize(_ui->checkbox_realsenseDepthScaledToRGBSize->isChecked());
+			((CameraRealSense*)camera)->setRGBSource((CameraRealSense::RGBSource)_ui->comboBox_realsenseRGBSource->currentIndex());
 		}
 	}
 	else if (driver == kSrcRealSense2)
@@ -5824,7 +5829,7 @@ void PreferencesDialog::calibrate()
 		}
 
 		bool freenect2 = driver == kSrcFreenect2;
-		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
+		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB && driver != kSrcRealSense, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
 		_calibrationDialog->setSwitchedImages(freenect2);
 		_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 		_calibrationDialog->registerToEventsManager();

guilib/src/ui/preferencesDialog.ui

@@ -94,8 +94,8 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-287</y>
-            <width>678</width>
+            <width>681</width>
             <height>2943</height>
            </rect>
           </property>
@@ -117,7 +117,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>7</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3600,7 +3600,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <string>RealSense</string>
                                  </property>
                                  <layout class="QGridLayout" name="gridLayout_73" columnstretch="0,1">
-                                  <item row="4" column="0">
+                                  <item row="5" column="0">
                                    <spacer name="verticalSpacer_51">
                                     <property name="orientation">
                                      <enum>Qt::Vertical</enum>
@@ -3706,6 +3706,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="3" column="0">
+                                   <widget class="QCheckBox" name="checkbox_realsenseDepthScaledToRGBSize">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>false</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
                                   <item row="3" column="1">
                                    <widget class="QLabel" name="label_realsenseOdom_2">
                                     <property name="text">
@@ -3719,13 +3729,38 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="3" column="0">
+                                  <item row="4" column="0">
-                                   <widget class="QCheckBox" name="checkbox_realsenseDepthScaledToRGBSize">
+                                   <widget class="QComboBox" name="comboBox_realsenseRGBSource">
-                                    <property name="text">
+                                    <property name="sizeAdjustPolicy">
-                                     <string/>
+                                     <enum>QComboBox::AdjustToContents</enum>
                                     </property>
-                                    <property name="checked">
+                                    <item>
-                                     <bool>false</bool>
+                                     <property name="text">
+                                      <string>Color</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>Infrared</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>Fisheye</string>
+                                     </property>
+                                    </item>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="1">
+                                   <widget class="QLabel" name="label_realsenseOdom_3">
+                                    <property name="text">
+                                     <string>RGB camera source. For fisheye has depth available only if it is calibrated.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                                     </property>
                                    </widget>
                                   </item>