Added OpenNI2 and Freenect calibration #115

corelib/include/rtabmap/core/CameraRGBD.h

@@ -155,9 +155,11 @@ class RTABMAP_EXP CameraOpenNI2 :
 public:
 	static bool available();
 	static bool exposureGainAvailable();
+	enum Type {kTypeColorDepth, kTypeIRDepth, kTypeIR};
 
 public:
 	CameraOpenNI2(const std::string & deviceId = "",
+					Type type = kTypeColorDepth,
 					float imageRate = 0,
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraOpenNI2();
@@ -177,6 +179,7 @@ protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
 private:
+	Type _type;
 	openni::Device * _device;
 	openni::VideoStream * _color;
 	openni::VideoStream * _depth;
@@ -184,6 +187,7 @@ private:
 	float _depthFy;
 	std::string _deviceId;
 	bool _openNI2StampsAndIDsUsed;
+	StereoCameraModel _stereoModel;
 };
 
 
@@ -197,10 +201,12 @@ class RTABMAP_EXP CameraFreenect :
 {
 public:
 	static bool available();
+	enum Type {kTypeColorDepth, kTypeIRDepth};
 
 public:
 	// default local transform z in, x right, y down));
 	CameraFreenect(int deviceId= 0,
+					Type type = kTypeColorDepth,
 					float imageRate=0.0f,
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect();
@@ -214,8 +220,10 @@ protected:
 
 private:
 	int deviceId_;
+	Type type_;
 	freenect_context * ctx_;
 	FreenectDevice * freenectDevice_;
+	StereoCameraModel stereoModel_;
 };
 
 /////////////////////////

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -87,7 +87,7 @@ public:
 
 	void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
 
-	void setName(const std::string & name);
+	void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
 	const std::string & name() const {return name_;}
 
 	// backward compatibility
@@ -95,6 +95,7 @@ public:
 
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
 	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
+	bool saveStereoTransform(const std::string & directory) const;
 
 	double baseline() const {return right_.fx()!=0.0?-right_.Tx()/right_.fx():0.0;}
 
@@ -117,7 +118,12 @@ public:
 	const CameraModel & left() const {return left_;}
 	const CameraModel & right() const {return right_;}
 
+	const std::string & getLeftSuffix() const {return leftSuffix_;}
+	const std::string & getRightSuffix() const {return rightSuffix_;}
+
 private:
+	std::string leftSuffix_;
+	std::string rightSuffix_;
 	CameraModel left_;
 	CameraModel right_;
 	std::string name_;

corelib/include/rtabmap/core/util2d.h

@@ -121,6 +121,7 @@ cv::Mat RTABMAP_EXP interpolate(const cv::Mat & image, int factor, float depthEr
 cv::Mat RTABMAP_EXP registerDepth(
 		const cv::Mat & depth,
 		const cv::Mat & depthK,
+		const cv::Size & colorSize,
 		const cv::Mat & colorK,
 		const rtabmap::Transform & transform);
 

corelib/src/CameraRGBD.cpp

@@ -213,9 +213,9 @@ SensorData CameraOpenni::captureImage(CameraInfo * info)
 #ifdef HAVE_OPENNI
 	if(interface_ && interface_->isRunning())
 	{
-		if(!dataReady_.acquire(1, 2000))
+		if(!dataReady_.acquire(1, 5000))
 		{
-			UWARN("Not received new frames since 2 seconds, end of stream reached!");
+			UWARN("Not received new frames since 5 seconds, end of stream reached!");
 		}
 		else
 		{
@@ -379,9 +379,11 @@ bool CameraOpenNI2::exposureGainAvailable()
 
 CameraOpenNI2::CameraOpenNI2(
 		const std::string & deviceId,
+		Type type,
 		float imageRate,
 		const rtabmap::Transform & localTransform) :
 	Camera(imageRate, localTransform),
+	_type(type),
 #ifdef RTABMAP_OPENNI2
 	_device(new openni::Device()),
 	_color(new openni::VideoStream()),
@@ -491,21 +493,81 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 #ifdef RTABMAP_OPENNI2
 	openni::OpenNI::initialize();
 
-	if(_device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str()) != openni::STATUS_OK)
+	openni::Array<openni::DeviceInfo> devices;
+	openni::OpenNI::enumerateDevices(&devices);
+	for(int i=0; i<devices.getSize(); ++i)
+	{
+		UINFO("Device %d: Name=%s URI=%s Vendor=%s",
+				i,
+				devices[i].getName(),
+				devices[i].getUri(),
+				devices[i].getVendor());
+	}
+	if(_deviceId.empty() && devices.getSize() == 0)
+	{
+		UERROR("CameraOpenNI2: No device detected!");
+		return false;
+	}
+
+	openni::Status error = _device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str());
+	if(error != openni::STATUS_OK)
 	{
 		if(!_deviceId.empty())
 		{
-			UERROR("CameraOpenNI2: Cannot open device \"%s\".", _deviceId.c_str());
+			UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", _deviceId.c_str(), error);
 		}
 		else
 		{
-			UERROR("CameraOpenNI2: Cannot open device.");
+#ifdef _WIN32
+			UERROR("CameraOpenNI2: Cannot open device \"%s\" with uri=\"%s\" (error=%d).", devices[0].getName(), devices[0].uri, error);
+#else
+			UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d). Verify if \"%s\" is in udev rules: \"/lib/udev/rules.d/40-libopenni2-0.rules\". If not, add it and reboot.", devices[0].getName(), error, devices[0].getUri());
+#endif
 		}
+
 		_device->close();
 		openni::OpenNI::shutdown();
 		return false;
 	}
 
+	// look for calibration files
+	_stereoModel = StereoCameraModel();
+	bool hardwareRegistration = true;
+	if(!calibrationFolder.empty())
+	{
+		// we need the serial
+		std::string calibrationName = _device->getDeviceInfo().getName();
+		if(!cameraName.empty())
+		{
+			calibrationName = cameraName;
+		}
+		_stereoModel.setName(calibrationName, "depth", "rgb");
+		hardwareRegistration = !_stereoModel.load(calibrationFolder, calibrationName, false);
+
+		if(_type != kTypeColorDepth)
+		{
+			hardwareRegistration = false;
+		}
+
+
+		if((_type != kTypeColorDepth && !_stereoModel.left().isValidForRectification()) ||
+		   (_type == kTypeColorDepth && !_stereoModel.right().isValidForRectification()))
+		{
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
+					calibrationName.c_str(), calibrationFolder.c_str());
+		}
+		else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && hardwareRegistration)
+		{
+			UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
+					calibrationName.c_str(), calibrationFolder.c_str());
+		}
+		else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && !hardwareRegistration)
+		{
+			UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
+				  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
+		}
+	}
+
 	if(UFile::getExtension(_deviceId).compare("oni")==0)
 	{
 		if(_device->getPlaybackControl() &&
@@ -517,7 +579,8 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 			return false;
 		}
 	}
-	else if(!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
+	else if(_type==kTypeColorDepth && hardwareRegistration &&
+			!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
 	{
 		UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
 		_device->close();
@@ -526,9 +589,9 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 	}
 
 	if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
-	  _device->getSensorInfo(openni::SENSOR_COLOR) == NULL)
+	  _device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) == NULL)
 	{
-		UERROR("CameraOpenNI2: Cannot get sensor info for depth and color.");
+		UERROR("CameraOpenNI2: Cannot get sensor info for depth and %s.", _type==kTypeColorDepth?"color":"ir");
 		_device->close();
 		openni::OpenNI::shutdown();
 		return false;
@@ -542,16 +605,17 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 		return false;
 	}
 
-	if(_color->create(*_device, openni::SENSOR_COLOR) != openni::STATUS_OK)
+	if(_color->create(*_device, _type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != openni::STATUS_OK)
 	{
-		UERROR("CameraOpenNI2: Cannot create color stream.");
+		UERROR("CameraOpenNI2: Cannot create %s stream.", _type==kTypeColorDepth?"color":"ir");
 		_depth->destroy();
 		_device->close();
 		openni::OpenNI::shutdown();
 		return false;
 	}
 
-	if(_device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
+	if(_type==kTypeColorDepth && hardwareRegistration &&
+	   _device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
 	{
 		UERROR("CameraOpenNI2: Failed to set depth/color registration.");
 	}
@@ -578,7 +642,8 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 	const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
 	for(int i=0; i<colorVideoModes.getSize(); ++i)
 	{
-		UINFO("CameraOpenNI2: Color video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
+		UINFO("CameraOpenNI2: %s video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
+				_type==kTypeColorDepth?"color":"ir",
 				i,
 				colorVideoModes[i].getFps(),
 				colorVideoModes[i].getPixelFormat(),
@@ -605,6 +670,40 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 			_depth->getVideoMode().getResolutionY(),
 			_depth->getHorizontalFieldOfView(),
 			_depth->getVerticalFieldOfView());
+	UINFO("CameraOpenNI2: Using %s video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
+			_type==kTypeColorDepth?"color":"ir",
+			_color->getVideoMode().getFps(),
+			_color->getVideoMode().getPixelFormat(),
+			_color->getVideoMode().getResolutionX(),
+			_color->getVideoMode().getResolutionY(),
+			_color->getHorizontalFieldOfView(),
+			_color->getVerticalFieldOfView());
+
+	if(_depth->getVideoMode().getResolutionX() != 640 ||
+		_depth->getVideoMode().getResolutionY() != 480 ||
+		_depth->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_DEPTH_1_MM)
+	{
+		UERROR("Could not set depth format to 640x480 pixel=%d(mm)!",
+				openni::PIXEL_FORMAT_DEPTH_1_MM);
+		_depth->destroy();
+		_color->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+	if(_color->getVideoMode().getResolutionX() != 640 ||
+		_color->getVideoMode().getResolutionY() != 480 ||
+		_color->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_RGB888)
+	{
+		UERROR("Could not set %s format to 640x480 pixel=%d!",
+				_type==kTypeColorDepth?"color":"ir",
+				openni::PIXEL_FORMAT_RGB888);
+		_depth->destroy();
+		_color->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
 
 	if(_color->getCameraSettings())
 	{
@@ -616,8 +715,7 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 #endif
 	}
 
-	bool registered = true;
+	if(_type==kTypeColorDepth && hardwareRegistration)
-	if(registered)
 	{
 		_depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
 		_depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
@@ -629,15 +727,12 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 	}
 	UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);
 
-	UINFO("CameraOpenNI2: Using color video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
+	if(_type == kTypeIR)
-			_color->getVideoMode().getFps(),
+	{
-			_color->getVideoMode().getPixelFormat(),
+		UWARN("With type IR-only, depth stream will not be started");
-			_color->getVideoMode().getResolutionX(),
+	}
-			_color->getVideoMode().getResolutionY(),
-			_color->getHorizontalFieldOfView(),
-			_color->getVerticalFieldOfView());
 
-	if(_depth->start() != openni::STATUS_OK ||
+	if((_type != kTypeIR && _depth->start() != openni::STATUS_OK) ||
 		_color->start() != openni::STATUS_OK)
 	{
 		UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
@@ -650,7 +745,7 @@ bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::strin
 		return false;
 	}
 
-	uSleep(1000); // just to make sure the sensor is correctly initialized
+	uSleep(3000); // just to make sure the sensor is correctly initialized and exposure is set
 
 	return true;
 #else
@@ -684,35 +779,49 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 		_depth->isValid() &&
 		_color->isValid() &&
 		_device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
-		_device->getSensorInfo(openni::SENSOR_COLOR) != NULL)
+		_device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != NULL)
 	{
 		openni::VideoStream* depthStream[] = {_depth};
 		openni::VideoStream* colorStream[] = {_color};
-		if(openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 2000) != openni::STATUS_OK ||
+		if((_type != kTypeIR && openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 5000) != openni::STATUS_OK) ||
-		   openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 2000) != openni::STATUS_OK)
+		   openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 5000) != openni::STATUS_OK)
 		{
-			UWARN("No frames received since the last 2 seconds, end of stream is reached!");
+			UWARN("No frames received since the last 5 seconds, end of stream is reached!");
 		}
 		else
 		{
 			openni::VideoFrameRef depthFrame, colorFrame;
+			if(_type != kTypeIR)
+			{
 				_depth->readFrame(&depthFrame);
+			}
 			_color->readFrame(&colorFrame);
 			cv::Mat depth, rgb;
-			if(depthFrame.isValid() && colorFrame.isValid())
+			if((_type == kTypeIR || depthFrame.isValid()) && colorFrame.isValid())
 			{
-				int h=depthFrame.getHeight();
+				int h,w;
-				int w=depthFrame.getWidth();
+				if(_type != kTypeIR)
+				{
+					h=depthFrame.getHeight();
+					w=depthFrame.getWidth();
 					depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
-
+				}
 				h=colorFrame.getHeight();
 				w=colorFrame.getWidth();
 				cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
+				if(_type==kTypeColorDepth)
+				{
 					cv::cvtColor(tmp, rgb, CV_RGB2BGR);
 				}
-			UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
+				else // IR
-			if(!rgb.empty() && !depth.empty())
 				{
+					rgb = tmp.clone();
+				}
+			}
+			UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
+			if(!rgb.empty() && (_type == kTypeIR || !depth.empty()))
+			{
+				// default calibration
 				CameraModel model(
 						_depthFx, //fx
 						_depthFy, //fy
@@ -721,6 +830,35 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 						this->getLocalTransform(),
 						0,
 						rgb.size());
+
+				if(_type==kTypeColorDepth)
+				{
+					if(_stereoModel.right().isValidForRectification())
+					{
+						rgb = _stereoModel.right().rectifyImage(rgb);
+						model = _stereoModel.right();
+
+						if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
+						{
+							depth = _stereoModel.left().rectifyImage(depth, 0);
+							depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
+						}
+					}
+				}
+				else // IR
+				{
+					if(_stereoModel.left().isValidForRectification())
+					{
+						rgb = _stereoModel.left().rectifyImage(rgb);
+						if(_type!=kTypeIR)
+						{
+							depth = _stereoModel.left().rectifyImage(depth, 0);
+						}
+						model = _stereoModel.left();
+					}
+				}
+				model.setLocalTransform(this->getLocalTransform());
+
 				if(_openNI2StampsAndIDsUsed)
 				{
 					data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
@@ -748,8 +886,10 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
 //
 class FreenectDevice : public UThread {
   public:
-	FreenectDevice(freenect_context * ctx, int index) :
+	FreenectDevice(freenect_context * ctx, int index, bool color = true, bool registered = true) :
 		index_(index),
+		color_(color),
+		registered_(registered),
 		ctx_(ctx),
 		device_(0),
 		depthFocal_(0.0f)
@@ -798,22 +938,37 @@ class FreenectDevice : public UThread {
 			UERROR("Could not get serial for index %d", index_);
 		}
 
+		UINFO("color=%d registered=%d", color_?1:0, registered_?1:0);
+
 		freenect_set_user(device_, this);
-		freenect_set_video_mode(device_, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
+		freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, color_?FREENECT_VIDEO_RGB:FREENECT_VIDEO_IR_8BIT);
-		freenect_set_depth_mode(device_, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
+		freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, color_ && registered_?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
-		depthBuffer_ = cv::Mat(cv::Size(640,480),CV_16UC1);
+		if(!videoMode.is_valid)
-		rgbBuffer_ = cv::Mat(cv::Size(640,480), CV_8UC3);
+		{
+			UERROR("Freenect: video mode selected not valid!");
+			return false;
+		}
+		if(!depthMode.is_valid)
+		{
+			UERROR("Freenect: depth mode selected not valid!");
+			return false;
+		}
+		UASSERT(videoMode.data_bits_per_pixel == 8 || videoMode.data_bits_per_pixel == 24);
+		UASSERT(depthMode.data_bits_per_pixel == 16);
+		freenect_set_video_mode(device_, videoMode);
+		freenect_set_depth_mode(device_, depthMode);
+		rgbIrBuffer_ = cv::Mat(cv::Size(videoMode.width,videoMode.height), color_?CV_8UC3:CV_8UC1);
+		depthBuffer_ = cv::Mat(cv::Size(depthMode.width,depthMode.height), CV_16UC1);
 		freenect_set_depth_buffer(device_, depthBuffer_.data);
-		freenect_set_video_buffer(device_, rgbBuffer_.data);
+		freenect_set_video_buffer(device_, rgbIrBuffer_.data);
 		freenect_set_depth_callback(device_, freenect_depth_callback);
 		freenect_set_video_callback(device_, freenect_video_callback);
 
-		bool registered = true;
 		float rgb_focal_length_sxga = 1050.0f;
 		float width_sxga = 1280.0f;
 		float width = freenect_get_current_depth_mode(device_).width;
 		float scale = width / width_sxga;
-		if(registered)
+		if(color_ && registered_)
 		{
 			depthFocal_ =  rgb_focal_length_sxga * scale;
 		}
@@ -836,16 +991,16 @@ class FreenectDevice : public UThread {
 	{
 		if(this->isRunning())
 		{
-			if(!dataReady_.acquire(1, 2000))
+			if(!dataReady_.acquire(1, 5000))
 			{
-				UERROR("Not received any frames since 2 seconds, try to restart the camera again.");
+				UERROR("Not received any frames since 5 seconds, try to restart the camera again.");
 			}
 			else
 			{
 				UScopeMutex s(dataMutex_);
-				rgb = rgbLastFrame_;
+				rgb = rgbIrLastFrame_;
 				depth = depthLastFrame_;
-				rgbLastFrame_ = cv::Mat();
+				rgbIrLastFrame_ = cv::Mat();
 				depthLastFrame_= cv::Mat();
 			}
 		}
@@ -855,10 +1010,18 @@ private:
 	// Do not call directly even in child
 	void VideoCallback(void* rgb)
 	{
-		UASSERT(rgbBuffer_.data == rgb);
+		UASSERT(rgbIrBuffer_.data == rgb);
 		UScopeMutex s(dataMutex_);
-		bool notify = rgbLastFrame_.empty();
+		bool notify = rgbIrLastFrame_.empty();
-		cv::cvtColor(rgbBuffer_, rgbLastFrame_, CV_RGB2BGR);
+
+		if(color_)
+		{
+			cv::cvtColor(rgbIrBuffer_, rgbIrLastFrame_, CV_RGB2BGR);
+		}
+		else // IrDepth
+		{
+			rgbIrLastFrame_ = rgbIrBuffer_.clone();
+		}
 		if(!depthLastFrame_.empty() && notify)
 		{
 			dataReady_.release();
@@ -872,7 +1035,7 @@ private:
 		UScopeMutex s(dataMutex_);
 		bool notify = depthLastFrame_.empty();
 		depthLastFrame_ = depthBuffer_.clone();
-		if(!rgbLastFrame_.empty() && notify)
+		if(!rgbIrLastFrame_.empty() && notify)
 		{
 			dataReady_.release();
 		}
@@ -931,14 +1094,16 @@ private:
 
   private:
 	int index_;
+	bool color_;
+	bool registered_;
 	std::string serial_;
 	freenect_context * ctx_;
 	freenect_device * device_;
 	cv::Mat depthBuffer_;
-	cv::Mat rgbBuffer_;
+	cv::Mat rgbIrBuffer_;
 	UMutex dataMutex_;
 	cv::Mat depthLastFrame_;
-	cv::Mat rgbLastFrame_;
+	cv::Mat rgbIrLastFrame_;
 	float depthFocal_;
 	USemaphore dataReady_;
 };
@@ -956,9 +1121,10 @@ bool CameraFreenect::available()
 #endif
 }
 
-CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform) :
+CameraFreenect::CameraFreenect(int deviceId, Type type, float imageRate, const Transform & localTransform) :
 		Camera(imageRate, localTransform),
 		deviceId_(deviceId),
+		type_(type),
 		ctx_(0),
 		freenectDevice_(0)
 {
@@ -997,7 +1163,50 @@ bool CameraFreenect::init(const std::string & calibrationFolder, const std::stri
 
 	if(ctx_ && freenect_num_devices(ctx_) > 0)
 	{
-		freenectDevice_ = new FreenectDevice(ctx_, deviceId_);
+		// look for calibration files
+		bool hardwareRegistration = true;
+		stereoModel_ = StereoCameraModel();
+		if(!calibrationFolder.empty())
+		{
+			// we need the serial, HACK: init a temp device to get it
+			FreenectDevice dev(ctx_, deviceId_);
+			if(!dev.init())
+			{
+				UERROR("CameraFreenect: Init failed!");
+			}
+			std::string calibrationName = dev.getSerial();
+			if(!cameraName.empty())
+			{
+				calibrationName = cameraName;
+			}
+			stereoModel_.setName(calibrationName, "depth", "rgb");
+			hardwareRegistration = !stereoModel_.load(calibrationFolder, calibrationName, false);
+
+			if(type_ == kTypeIRDepth)
+			{
+				hardwareRegistration = false;
+			}
+
+
+			if((type_ == kTypeIRDepth && !stereoModel_.left().isValidForRectification()) ||
+			   (type_ == kTypeColorDepth && !stereoModel_.right().isValidForRectification()))
+			{
+				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
+						calibrationName.c_str(), calibrationFolder.c_str());
+			}
+			else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && hardwareRegistration)
+			{
+				UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
+						calibrationName.c_str(), calibrationFolder.c_str());
+			}
+			else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && !hardwareRegistration)
+			{
+				UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
+					  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
+			}
+		}
+
+		freenectDevice_ = new FreenectDevice(ctx_, deviceId_, type_==kTypeColorDepth, hardwareRegistration);
 		if(freenectDevice_->init())
 		{
 			freenectDevice_->start();
@@ -1050,8 +1259,8 @@ SensorData CameraFreenect::captureImage(CameraInfo * info)
 			if(!rgb.empty() && !depth.empty())
 			{
 				UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
-				if(!rgb.empty() && !depth.empty())
+
-				{
+				// default calibration
 				CameraModel model(
 						freenectDevice_->getDepthFocal(), //fx
 						freenectDevice_->getDepthFocal(), //fy
@@ -1060,9 +1269,34 @@ SensorData CameraFreenect::captureImage(CameraInfo * info)
 						this->getLocalTransform(),
 						0,
 						rgb.size());
-					data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+
+				if(type_==kTypeIRDepth)
+				{
+					if(stereoModel_.left().isValidForRectification())
+					{
+						rgb = stereoModel_.left().rectifyImage(rgb);
+						depth = stereoModel_.left().rectifyImage(depth, 0);
+						model = stereoModel_.left();
 					}
 				}
+				else
+				{
+					if(stereoModel_.right().isValidForRectification())
+					{
+						rgb = stereoModel_.right().rectifyImage(rgb);
+						model = stereoModel_.right();
+
+						if(stereoModel_.left().isValidForRectification() && !stereoModel_.stereoTransform().isNull())
+						{
+							depth = stereoModel_.left().rectifyImage(depth, 0);
+							depth = util2d::registerDepth(depth, stereoModel_.left().K(), rgb.size(), stereoModel_.right().K(), stereoModel_.stereoTransform());
+						}
+					}
+				}
+				model.setLocalTransform(this->getLocalTransform());
+
+				data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+			}
 		}
 		else
 		{
@@ -1248,6 +1482,9 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
 			}
 			else
 			{
+				UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
+					  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
+
 				if(type_==kTypeColor2DepthSD)
 				{
 					UWARN("Freenect2: When using custom calibration file, type "
@@ -1445,6 +1682,7 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 							depth = util2d::registerDepth(
 									depth,
 									stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
+									depth.size(),
 									stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
 									stereoModel_.stereoTransform());
 							util2d::fillRegisteredDepthHoles(depth, true, false);

corelib/src/StereoCameraModel.cpp

@@ -42,8 +42,10 @@ StereoCameraModel::StereoCameraModel(
 		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,
 		const Transform & localTransform) :
-	left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
+				leftSuffix_("left"),
-	right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
+				rightSuffix_("right"),
+				left_(name+"_"+leftSuffix_, imageSize1, K1, D1, R1, P1, localTransform),
+				right_(name+"_"+rightSuffix_, imageSize2, K2, D2, R2, P2, localTransform),
 				name_(name),
 				R_(R),
 				T_(T),
@@ -64,6 +66,8 @@ StereoCameraModel::StereoCameraModel(
 		const cv::Mat & T,
 		const cv::Mat & E,
 		const cv::Mat & F) :
+				leftSuffix_("left"),
+				rightSuffix_("right"),
 				left_(leftCameraModel),
 				right_(rightCameraModel),
 				name_(name),
@@ -72,8 +76,8 @@ StereoCameraModel::StereoCameraModel(
 				E_(E),
 				F_(F)
 {
-	left_.setName(name+"_left");
+	left_.setName(name+"_"+getLeftSuffix());
-	right_.setName(name+"_right");
+	right_.setName(name+"_"+getRightSuffix());
 	UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
 	UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
 	UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
@@ -99,12 +103,14 @@ StereoCameraModel::StereoCameraModel(
 		const CameraModel & leftCameraModel,
 		const CameraModel & rightCameraModel,
 		const Transform & extrinsics) :
+				leftSuffix_("left"),
+				rightSuffix_("right"),
 				left_(leftCameraModel),
 				right_(rightCameraModel),
 				name_(name)
 {
-	left_.setName(name+"_left");
+	left_.setName(name+"_"+getLeftSuffix());
-	right_.setName(name+"_right");
+	right_.setName(name+"_"+getRightSuffix());
 
 	if(!extrinsics.isNull())
 	{
@@ -132,6 +138,8 @@ StereoCameraModel::StereoCameraModel(
 		double baseline,
 		const Transform & localTransform,
 		const cv::Size & imageSize) :
+				leftSuffix_("left"),
+				rightSuffix_("right"),
 				left_(fx, fy, cx, cy, localTransform, 0, imageSize),
 				right_(fx, fy, cx, cy, localTransform, baseline*-fx, imageSize)
 {
@@ -147,23 +155,29 @@ StereoCameraModel::StereoCameraModel(
 		double baseline,
 		const Transform & localTransform,
 		const cv::Size & imageSize) :
-	left_(name+"_left", fx, fy, cx, cy, localTransform, 0, imageSize),
+				leftSuffix_("left"),
-	right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx, imageSize),
+				rightSuffix_("right"),
+				left_(name+"_"+getLeftSuffix(), fx, fy, cx, cy, localTransform, 0, imageSize),
+				right_(name+"_"+getRightSuffix(), fx, fy, cx, cy, localTransform, baseline*-fx, imageSize),
 				name_(name)
 {
 }
 
-void StereoCameraModel::setName(const std::string & name)
+void StereoCameraModel::setName(const std::string & name, const std::string & leftSuffix, const std::string & rightSuffix)
 {
 	name_=name;
-	left_.setName(name_+"_left");
+	leftSuffix_ = leftSuffix;
-	right_.setName(name_+"_right");
+	rightSuffix_ = rightSuffix;
+	left_.setName(name_+"_"+getLeftSuffix());
+	right_.setName(name_+"_"+getRightSuffix());
 }
 
 bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
 {
 	name_ = cameraName;
-	if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
+	bool leftLoaded = left_.load(directory, cameraName+"_"+getLeftSuffix());
+	bool rightLoaded = right_.load(directory, cameraName+"_"+getRightSuffix());
+	if(leftLoaded && rightLoaded)
 	{
 		if(ignoreStereoTransform)
 		{
@@ -277,6 +291,13 @@ bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTra
 		{
 			return true;
 		}
+		return saveStereoTransform(directory);
+	}
+	return false;
+}
+
+bool StereoCameraModel::saveStereoTransform(const std::string & directory) const
+{
 	std::string filePath = directory+"/"+name_+"_pose.yaml";
 	if(!filePath.empty() && (!R_.empty() && !T_.empty()))
 	{
@@ -334,7 +355,6 @@ bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTra
 	{
 		UERROR("Failed saving stereo extrinsics (they are null).");
 	}
-	}
 	return false;
 }
 

corelib/src/util2d.cpp

@@ -1337,6 +1337,7 @@ cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
 cv::Mat registerDepth(
 		const cv::Mat & depth,
 		const cv::Mat & depthK,
+		const cv::Size & colorSize,
 		const cv::Mat & colorK,
 		const rtabmap::Transform & transform)
 {
@@ -1356,10 +1357,13 @@ cv::Mat registerDepth(
 	float rcx = colorK.at<double>(0,2);
 	float rcy = colorK.at<double>(1,2);
 
+	//UDEBUG("depth(%dx%d) fx=%f fy=%f cx=%f cy=%f", depth.cols, depth.rows, fx, fy, cx, cy);
+	//UDEBUG("color(%dx%d) fx=%f fy=%f cx=%f cy=%f", colorSize.width, colorSize.height, rfx, rfy, rcx, rcy);
+
 	Eigen::Affine3f proj = transform.toEigen3f();
 	Eigen::Vector4f P4,P3;
 	P4[3] = 1;
-	cv::Mat registered = cv::Mat::zeros(depth.rows, depth.cols, depth.type());
+	cv::Mat registered = cv::Mat::zeros(colorSize, depth.type());
 
 	bool depthInMM = depth.type() == CV_16UC1;
 	for(int y=0; y<depth.rows; ++y)

corelib/src/util3d.cpp

@@ -778,8 +778,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 						sensorData.cameraModels().size()==1?validIndices:0);
 
 				if(tmp->size())
+				{
+					if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
 					{
 						tmp = util3d::transformPointCloud(tmp, model.localTransform());
+					}
 
 					if(sensorData.cameraModels().size() > 1)
 					{
@@ -833,7 +836,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 
 		if(cloud->size())
 		{
-			if(cloud->size())
+			if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity())
 			{
 				cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
 			}
@@ -862,8 +865,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		UDEBUG("");
 		UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
 		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
-		UASSERT(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0);
+		UASSERT_MSG(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().cols, sensorData.depthRaw().cols).c_str());
-		UASSERT(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0);
+		UASSERT_MSG(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().rows, sensorData.depthRaw().rows).c_str());
 		int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
 		int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
 
@@ -928,8 +931,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 						sensorData.cameraModels().size() == 1?validIndices:0);
 
 				if(tmp->size())
+				{
+					if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
 					{
 						tmp = util3d::transformPointCloud(tmp, model.localTransform());
+					}
 
 					if(sensorData.cameraModels().size() > 1)
 					{
@@ -972,7 +978,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 				validIndices,
 				stereoParameters);
 
-		if(cloud->size())
+		if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity())
 		{
 			cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
 		}

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -56,26 +56,32 @@ public:
 	const rtabmap::CameraModel & getRightCameraModel() const {return models_[1];}
 	const rtabmap::StereoCameraModel & getStereoCameraModel() const {return stereoModel_;}
 	bool isProcessing() const {return processingData_;}
+	int getStereoPairs() const {return (int)stereoImagePoints_[0].size();}
 
 	void saveSettings(QSettings & settings, const QString & group = "") const;
 	void loadSettings(QSettings & settings, const QString & group = "");
 	void resetSettings();
 
+	void setCameraName(const QString & name);
+	void setProgressVisibility(bool visible);
 	void setSwitchedImages(bool switched);
-	void setStereoMode(bool stereo);
+	void setStereoMode(bool stereo, const QString & leftSuffix = "left", const QString & rightSuffix = "right");
 	void setSavingDirectory(const QString & savingDirectory) {savingDirectory_ = savingDirectory;}
 
+	StereoCameraModel stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification) const;
+
 public slots:
 	void setBoardWidth(int width);
 	void setBoardHeight(int height);
 	void setSquareSize(double size);
 	void setMaxScale(int scale);
 
-private slots:
 	void processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName);
-	void restart();
 	void calibrate();
+	void restart();
 	bool save();
+
+private slots:
 	void unlock();
 
 protected:
@@ -96,6 +102,8 @@ private:
 private:
 	// parameters
 	bool stereo_;
+	QString leftSuffix_;
+	QString rightSuffix_;
 	QString savingDirectory_;
 
 	QString cameraName_;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -226,7 +226,7 @@ public:
 	int getSourceScanNormalsK() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Transform getLaserLocalTransform() const; // directory images
-	Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
+	Camera * createCamera(bool useRawImages = false, bool useColor = true); // return camera should be deleted if not null
 
 	int getIgnoredDCComponents() const;
 

guilib/src/CalibrationDialog.cpp

@@ -49,6 +49,8 @@ namespace rtabmap {
 CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, bool switchImages, QWidget * parent) :
 	QDialog(parent),
 	stereo_(stereo),
+	leftSuffix_("left"),
+	rightSuffix_("right"),
 	savingDirectory_(savingDirectory),
 	processingData_(false),
 	savedCalibration_(false)
@@ -149,15 +151,28 @@ void CalibrationDialog::resetSettings()
 	this->setSquareSize(0.033);
 }
 
+void CalibrationDialog::setCameraName(const QString & name)
+{
+	cameraName_ = name;
+}
+
+void CalibrationDialog::setProgressVisibility(bool visible)
+{
+	ui_->groupBox_progress->setVisible(visible);
+}
+
 void CalibrationDialog::setSwitchedImages(bool switched)
 {
 	ui_->checkBox_switchImages->setChecked(switched);
 }
 
-void CalibrationDialog::setStereoMode(bool stereo)
+void CalibrationDialog::setStereoMode(bool stereo, const QString & leftSuffix, const QString & rightSuffix)
 {
+	leftSuffix_ = leftSuffix;
+	rightSuffix_ = rightSuffix;
 	this->restart();
 
+	ui_->groupBox_progress->setVisible(true);
 	stereo_ = stereo;
 	ui_->progressBar_x_2->setVisible(stereo_);
 	ui_->progressBar_y_2->setVisible(stereo_);
@@ -601,7 +616,6 @@ void CalibrationDialog::restart()
 	models_[0] = CameraModel();
 	models_[1] = CameraModel();
 	stereoModel_ = StereoCameraModel();
-	cameraName_.clear();
 	minIrs_[0] = 0x0000;
 	maxIrs_[0] = 0x7fff;
 	minIrs_[1] = 0x0000;
@@ -769,101 +783,7 @@ void CalibrationDialog::calibrate()
 
 	if(stereo_ && models_[0].isValidForRectification() && models_[1].isValidForRectification())
 	{
-		UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
+		stereoModel_ = stereoCalibration(models_[0], models_[1], false);
-		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]);
-
-		// calibrate extrinsic
-#if CV_MAJOR_VERSION < 3
-		double rms = cv::stereoCalibrate(
-				objectPoints,
-				stereoImagePoints_[0],
-				stereoImagePoints_[1],
-				models_[0].K_raw(), models_[0].D_raw(),
-				models_[1].K_raw(), models_[1].D_raw(),
-				imageSize, R, T, E, F,
-				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
-				cv::CALIB_FIX_INTRINSIC);
-#else
-		double rms = cv::stereoCalibrate(
-				objectPoints,
-				stereoImagePoints_[0],
-				stereoImagePoints_[1],
-				models_[0].K_raw(), models_[0].D_raw(),
-				models_[1].K_raw(), models_[1].D_raw(),
-				imageSize, R, T, E, F,
-				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;
-		std::cout << "T = " << T << std::endl;
-		std::cout << "E = " << E << std::endl;
-		std::cout << "F = " << F << std::endl;
-
-		if(imageSize_[0] == imageSize_[1])
-		{
-			//Stereo, compute stereo rectification
-
-			cv::Mat R1, R2, P1, P2, Q;
-			cv::stereoRectify(models_[0].K_raw(), models_[0].D_raw(),
-							models_[1].K_raw(), models_[1].D_raw(),
-							imageSize, R, T, R1, R2, P1, P2, Q,
-							cv::CALIB_ZERO_DISPARITY, 0, imageSize);
-
-			double err = 0;
-			int npoints = 0;
-			std::vector<cv::Vec3f> lines[2];
-			UINFO("Computing avg re-projection error...");
-			for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
-			{
-				int npt = (int)stereoImagePoints_[0][i].size();
-
-					cv::Mat imgpt0 = cv::Mat(stereoImagePoints_[0][i]);
-					cv::Mat imgpt1 = cv::Mat(stereoImagePoints_[1][i]);
-					cv::undistortPoints(imgpt0, imgpt0, models_[0].K_raw(), models_[0].D_raw(), R1, P1);
-					cv::undistortPoints(imgpt1, imgpt1, models_[1].K_raw(), models_[1].D_raw(), R2, P2);
-					computeCorrespondEpilines(imgpt0, 1, F, lines[0]);
-					computeCorrespondEpilines(imgpt1, 2, F, lines[1]);
-
-				for(int j = 0; j < npt; j++ )
-				{
-					double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
-										stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
-								   fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
-										stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
-					err += errij;
-				}
-				npoints += npt;
-			}
-			double totalAvgErr = err/(double)npoints;
-			UINFO("stereo avg re projection error = %f", totalAvgErr);
-
-			stereoModel_ = StereoCameraModel(
-							cameraName_.toStdString(),
-							imageSize_[0], models_[0].K_raw(), models_[0].D_raw(), R1, P1,
-							imageSize_[1], models_[1].K_raw(), models_[1].D_raw(), R2, P2,
-							R, T, E, F);
-		}
-		else
-		{
-			//Kinect, ignore the stereo rectification
-			stereoModel_ = StereoCameraModel(
-							cameraName_.toStdString(),
-							imageSize_[0], models_[0].K_raw(), models_[0].D_raw(), models_[0].R(), models_[0].P(),
-							imageSize_[1], models_[1].K_raw(), models_[1].D_raw(), models_[1].R(), models_[1].P(),
-							R, T, E, F);
-		}
 
 		std::stringstream strR1, strP1, strR2, strP2;
 		strR1 << stereoModel_.left().R();
@@ -902,6 +822,127 @@ void CalibrationDialog::calibrate()
 	processingData_ = false;
 }
 
+StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification) const
+{
+	StereoCameraModel output;
+	if(stereoImagePoints_[0].empty())
+	{
+		UERROR("No stereo correspondences!");
+		return output;
+	}
+	UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
+
+	if(left.imageSize()!=imageSize_[0])
+	{
+		UERROR("left model (%dx%d) has not the same size as the processed images (%dx%d)",
+				left.imageSize().width, left.imageSize().height,
+				imageSize_[0].width, imageSize_[0].height);
+	}
+	if(right.imageSize()!=imageSize_[1])
+	{
+		UERROR("right model (%dx%d) has not the same size as the processed images (%dx%d)",
+				right.imageSize().width, right.imageSize().height,
+				imageSize_[1].width, imageSize_[1].height);
+	}
+
+	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]);
+
+	// calibrate extrinsic
+#if CV_MAJOR_VERSION < 3
+	double rms = cv::stereoCalibrate(
+			objectPoints,
+			stereoImagePoints_[0],
+			stereoImagePoints_[1],
+			left.K_raw(), left.D_raw(),
+			right.K_raw(), right.D_raw(),
+			imageSize, R, T, E, F,
+			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
+			cv::CALIB_FIX_INTRINSIC);
+#else
+	double rms = cv::stereoCalibrate(
+			objectPoints,
+			stereoImagePoints_[0],
+			stereoImagePoints_[1],
+			left.K_raw(), left.D_raw(),
+			right.K_raw(), right.D_raw(),
+			imageSize, R, T, E, F,
+			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;
+	std::cout << "T = " << T << std::endl;
+	std::cout << "E = " << E << std::endl;
+	std::cout << "F = " << F << std::endl;
+
+	if(imageSize_[0] == imageSize_[1] && !ignoreStereoRectification)
+	{
+		UINFO("Compute stereo rectification");
+
+		cv::Mat R1, R2, P1, P2, Q;
+		cv::stereoRectify(left.K_raw(), left.D_raw(),
+						right.K_raw(), right.D_raw(),
+						imageSize, R, T, R1, R2, P1, P2, Q,
+						cv::CALIB_ZERO_DISPARITY, 0, imageSize);
+
+		double err = 0;
+		int npoints = 0;
+		std::vector<cv::Vec3f> lines[2];
+		UINFO("Computing avg re-projection error...");
+		for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
+		{
+			int npt = (int)stereoImagePoints_[0][i].size();
+
+				cv::Mat imgpt0 = cv::Mat(stereoImagePoints_[0][i]);
+				cv::Mat imgpt1 = cv::Mat(stereoImagePoints_[1][i]);
+				cv::undistortPoints(imgpt0, imgpt0, left.K_raw(), left.D_raw(), R1, P1);
+				cv::undistortPoints(imgpt1, imgpt1, right.K_raw(), right.D_raw(), R2, P2);
+				computeCorrespondEpilines(imgpt0, 1, F, lines[0]);
+				computeCorrespondEpilines(imgpt1, 2, F, lines[1]);
+
+			for(int j = 0; j < npt; j++ )
+			{
+				double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
+									stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
+							   fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
+									stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
+				err += errij;
+			}
+			npoints += npt;
+		}
+		double totalAvgErr = err/(double)npoints;
+		UINFO("stereo avg re projection error = %f", totalAvgErr);
+
+		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);
+	}
+	return output;
+}
+
 bool CalibrationDialog::save()
 {
 	bool saved = false;
@@ -941,10 +982,11 @@ bool CalibrationDialog::save()
 		QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
 		if(!name.isEmpty())
 		{
-			stereoModel_.setName(name.toStdString());
+			bool switched = ui_->checkBox_switchImages->isChecked();
+			stereoModel_.setName(name.toStdString(), switched?rightSuffix_.toStdString():leftSuffix_.toStdString(), switched?leftSuffix_.toStdString():rightSuffix_.toStdString());
 			std::string base = (dir+QDir::separator()+name).toStdString();
-			std::string leftPath = base+"_left.yaml";
+			std::string leftPath = base+"_"+stereoModel_.getLeftSuffix()+".yaml";
-			std::string rightPath = base+"_right.yaml";
+			std::string rightPath = base+"_"+stereoModel_.getRightSuffix()+".yaml";
 			std::string posePath = base+"_pose.yaml";
 			if(stereoModel_.save(dir.toStdString(), false))
 			{

guilib/src/PreferencesDialog.cpp

@@ -4091,7 +4091,7 @@ int PreferencesDialog::getSourceScanNormalsK() const
 	return _ui->spinBox_cameraImages_scanNormalsK->value();
 }
 
-Camera * PreferencesDialog::createCamera(bool useRawImages)
+Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 {
 	Src driver = this->getSourceDriver();
 	Camera * camera = 0;
@@ -4113,39 +4113,21 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 		}
 	}
 	else if(driver == PreferencesDialog::kSrcOpenNI2)
-	{
-		if(useRawImages)
-		{
-			QMessageBox::warning(this, tr("Calibration"),
-					tr("Using raw images for \"OpenNI2\" driver is not yet supported. "
-						"Factory calibration loaded from OpenNI2 is used."), QMessageBox::Ok);
-			return 0;
-		}
-		else
 	{
 		camera = new CameraOpenNI2(
 				_ui->lineEdit_openni2OniPath->text().isEmpty()?this->getSourceDevice().toStdString():_ui->lineEdit_openni2OniPath->text().toStdString(),
+				useColor?CameraOpenNI2::kTypeColorDepth:CameraOpenNI2::kTypeIRDepth,
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 	}
-	}
 	else if(driver == PreferencesDialog::kSrcFreenect)
-	{
-		if(useRawImages)
-		{
-			QMessageBox::warning(this, tr("Calibration"),
-					tr("Using raw images for \"Freenect\" driver is not yet supported. "
-						"Factory calibration loaded from Freenect is used."), QMessageBox::Ok);
-			return 0;
-		}
-		else
 	{
 		camera = new CameraFreenect(
 				this->getSourceDevice().isEmpty()?0:atoi(this->getSourceDevice().toStdString().c_str()),
+				useColor?CameraFreenect::kTypeColorDepth:CameraFreenect::kTypeIRDepth,
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 	}
-	}
 	else if(driver == PreferencesDialog::kSrcOpenNI_CV ||
 			driver == PreferencesDialog::kSrcOpenNI_CV_ASUS)
 	{
@@ -4178,6 +4160,15 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			_ui->checkBox_freenect2NoiseFiltering->isChecked());
 	}
 	else if (driver == kSrcRealSense)
+	{
+		if(useRawImages)
+		{
+			QMessageBox::warning(this, tr("Calibration"),
+					tr("Using raw images for \"RealSense\" driver is not yet supported. "
+						"Factory calibration loaded from RealSense is used."), QMessageBox::Ok);
+			return 0;
+		}
+		else
 		{
 			camera = new CameraRealSense(
 				this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
@@ -4186,6 +4177,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 		}
+	}
 	else if(driver == kSrcRGBDImages)
 	{
 		camera = new CameraRGBDImages(
@@ -4372,12 +4364,11 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 
 	if(camera)
 	{
-		// don't set calibration folder if we want raw images
 		QString dir = this->getCameraInfoDir();
 		QString calibrationFile = _ui->lineEdit_calibrationFile->text();
 		if(!(driver >= kSrcRGB && driver <= kSrcVideo))
 		{
-			calibrationFile.remove("_left.yaml").remove("_right.yaml").remove("_pose.yaml");
+			calibrationFile.remove("_left.yaml").remove("_right.yaml").remove("_pose.yaml").remove("_rgb.yaml").remove("_depth.yaml");
 		}
 		QString name = QFileInfo(calibrationFile.remove(".yaml")).baseName();
 		if(!_ui->lineEdit_calibrationFile->text().isEmpty())
@@ -4389,6 +4380,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 			}
 		}
 
+		// don't set calibration folder if we want raw images
 		if(!camera->init(useRawImages?"":dir.toStdString(), name.toStdString()))
 		{
 			UWARN("init camera failed... ");
@@ -4642,11 +4634,6 @@ void PreferencesDialog::calibrate()
 				   tr("Cannot calibrate database source!"));
 		return;
 	}
-	Camera * camera = this->createCamera(true);
-	if(!camera)
-	{
-		return;
-	}
 
 	if(!this->getCameraInfoDir().isEmpty())
 	{
@@ -4660,8 +4647,182 @@ void PreferencesDialog::calibrate()
 			}
 		}
 	}
-	_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB); // RGB+Depth or left+right
+
-	_calibrationDialog->setSwitchedImages(dynamic_cast<CameraFreenect2*>(camera) != 0);
+	Src driver = this->getSourceDriver();
+	if(driver == PreferencesDialog::kSrcFreenect || driver == PreferencesDialog::kSrcOpenNI2)
+	{
+		// 3 steps calibration: RGB -> IR -> Extrinsic
+		QMessageBox::StandardButton button = QMessageBox::question(this, tr("Calibration"),
+				tr("With \"%1\" driver, Color and IR cameras cannot be streamed at the "
+					"same time. A 3-steps calibration is required (Color -> IR -> extrinsics). We will "
+					"start with the Color camera calibration. Do you want to continue?").arg(this->getSourceDriverStr()),
+					QMessageBox::Yes | QMessageBox::No | QMessageBox::Ignore, QMessageBox::Yes);
+
+		if(button == QMessageBox::Yes || button == QMessageBox::Ignore)
+		{
+			_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
+
+			Camera * camera = 0;
+
+			// Step 1: RGB
+			if(button != QMessageBox::Ignore)
+			{
+				camera = this->createCamera(true, true); // RAW color
+				if(!camera)
+				{
+					return;
+				}
+
+				_calibrationDialog->setStereoMode(false); // this forces restart
+				_calibrationDialog->setCameraName(QString(camera->getSerial().c_str())+"_rgb");
+				_calibrationDialog->registerToEventsManager();
+				CameraThread cameraThread(camera, this->getAllParameters());
+				UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent");
+				cameraThread.start();
+				_calibrationDialog->exec();
+				_calibrationDialog->unregisterFromEventsManager();
+				cameraThread.join(true);
+				camera = 0;
+			}
+
+			button = QMessageBox::question(this, tr("Calibration"),
+						tr("We will now calibrate the IR camera. Hide the IR projector with a Post-It and "
+							"make sure you have enough ambient IR light (e.g., external IR source or sunlight!) to see the "
+							"checkboard with the IR camera. Do you want to continue?"),
+							QMessageBox::Yes | QMessageBox::No | QMessageBox::Ignore, QMessageBox::Yes);
+			if(button == QMessageBox::Yes || button == QMessageBox::Ignore)
+			{
+				// Step 2: IR
+				if(button != QMessageBox::Ignore)
+				{
+					camera = this->createCamera(true, false); // RAW ir
+					if(!camera)
+					{
+						return;
+					}
+
+					_calibrationDialog->setStereoMode(false); // this forces restart
+					_calibrationDialog->setCameraName(QString(camera->getSerial().c_str())+"_depth");
+					_calibrationDialog->registerToEventsManager();
+					CameraThread cameraThread(camera, this->getAllParameters());
+					UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent");
+					cameraThread.start();
+					_calibrationDialog->exec();
+					_calibrationDialog->unregisterFromEventsManager();
+					cameraThread.join(true);
+					camera = 0;
+				}
+
+				button = QMessageBox::question(this, tr("Calibration"),
+							tr("We will now calibrate the extrinsics. Important: Make sure "
+								"the cameras and the checkboard don't move and that both "
+								"cameras can see the checkboard. We will repeat this "
+								"multiple times. Each time, you will have to move the camera (or "
+								"checkboard) for a different point of view. Do you want to "
+								"continue?"),
+								QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
+
+				bool ok = false;
+				int totalSamples = 0;
+				if(button == QMessageBox::Yes)
+				{
+					totalSamples = QInputDialog::getInt(this, tr("Calibration"), tr("Samples: "), 3, 1, 99, 1, &ok);
+				}
+
+				if(ok)
+				{
+					int count = 0;
+					_calibrationDialog->setStereoMode(true, "depth", "rgb"); // this forces restart
+					_calibrationDialog->setCameraName("");
+					_calibrationDialog->setModal(true);
+					_calibrationDialog->setProgressVisibility(false);
+					_calibrationDialog->show();
+					CameraModel irModel;
+					CameraModel rgbModel;
+					for(;count < totalSamples && button == QMessageBox::Yes; )
+					{
+						// Step 3: Extrinsics
+						camera = this->createCamera(false, true); // Rectified color
+						if(!camera)
+						{
+							return;
+						}
+						SensorData rgbData = camera->takeImage();
+						UASSERT(rgbData.cameraModels().size() == 1);
+						rgbModel = rgbData.cameraModels()[0];
+						delete camera;
+						camera = this->createCamera(false, false); // Rectified ir
+						if(!camera)
+						{
+							return;
+						}
+						SensorData irData = camera->takeImage();
+						std::string serial = camera->getSerial();
+						UASSERT(irData.cameraModels().size() == 1);
+						irModel = irData.cameraModels()[0];
+						delete camera;
+
+						if(!rgbData.imageRaw().empty() && !irData.imageRaw().empty())
+						{
+							// assume rgb sensor is on right (e.g., Kinect, Xtion Live Pro)
+							int pair = _calibrationDialog->getStereoPairs();
+							_calibrationDialog->processImages(irData.imageRaw(), rgbData.imageRaw(), serial.c_str());
+							if(_calibrationDialog->getStereoPairs() - pair > 0)
+							{
+								++count;
+								if(count < totalSamples)
+								{
+									button = QMessageBox::question(this, tr("Calibration"),
+										tr("A stereo pair has been taken (total=%1/%2). Move the checkboard or "
+											"camera to another position. Press \"Yes\" when you are ready "
+											"for the next capture.").arg(count).arg(totalSamples),
+											QMessageBox::Yes | QMessageBox::Abort, QMessageBox::Yes);
+								}
+							}
+							else
+							{
+								button = QMessageBox::question(this, tr("Calibration"),
+										tr("Could not detect the checkboard on both images or "
+										   "the point of view didn't change enough. Try again?"),
+											QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
+							}
+						}
+						else
+						{
+							button = QMessageBox::question(this, tr("Calibration"),
+										tr("Failed to start the camera. Try again?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
+						}
+					}
+					if(count == totalSamples && button == QMessageBox::Yes)
+					{
+						StereoCameraModel stereoModel = _calibrationDialog->stereoCalibration(irModel, rgbModel, true);
+						stereoModel.setName(stereoModel.name(), "depth", "rgb");
+						if(stereoModel.stereoTransform().isNull())
+						{
+							QMessageBox::warning(this, tr("Calibration"),
+									tr("Extrinsic calibration has failed!"), QMessageBox::Ok);
+						}
+						else if(stereoModel.saveStereoTransform(this->getCameraInfoDir().toStdString()))
+						{
+							QMessageBox::information(this, tr("Calibration"),
+									tr("Calibration is completed! Extrinsics have been saved to \"%1/%2_pose.yaml\"").arg(this->getCameraInfoDir()).arg(stereoModel.name().c_str()), QMessageBox::Ok);
+						}
+					}
+				}
+			}
+		}
+	}
+	else // standard calibration
+	{
+		Camera * camera = this->createCamera(true);
+		if(!camera)
+		{
+			return;
+		}
+
+		bool freenect2 = this->getSourceType() == kSrcFreenect2;
+		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
+		_calibrationDialog->setSwitchedImages(freenect2);
 		_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 		_calibrationDialog->registerToEventsManager();
 
@@ -4674,6 +4835,7 @@ void PreferencesDialog::calibrate()
 		_calibrationDialog->unregisterFromEventsManager();
 
 		cameraThread.join(true);
+	}
 }
 
 void PreferencesDialog::calibrateSimple()

guilib/src/ui/calibrationDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1314</width>
-    <height>853</height>
+    <height>847</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -145,8 +145,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>371</width>
+          <width>353</width>
-          <height>822</height>
+          <height>816</height>
          </rect>
         </property>
         <layout class="QVBoxLayout" name="verticalLayout">
@@ -275,43 +275,41 @@
           </widget>
          </item>
          <item>
-          <widget class="QGroupBox" name="groupBox_2">
+          <widget class="QGroupBox" name="groupBox_progress">
            <property name="title">
             <string>Progress</string>
            </property>
-           <layout class="QGridLayout" name="gridLayout">
+           <layout class="QVBoxLayout" name="verticalLayout_6">
-            <item row="2" column="0">
+            <item>
-             <widget class="QLabel" name="label">
+             <layout class="QGridLayout" name="gridLayout" columnstretch="0,1,1">
+              <item row="0" column="0">
+               <widget class="QLabel" name="label_19">
                 <property name="text">
-               <string>X</string>
+                 <string>Serial</string>
                 </property>
                </widget>
               </item>
-            <item row="5" column="0">
+              <item row="0" column="1">
-             <widget class="QLabel" name="label_4">
+               <widget class="QLabel" name="label_serial">
                 <property name="text">
-               <string>Skew</string>
+                 <string>0</string>
+                </property>
+                <property name="textInteractionFlags">
+                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                 </property>
                </widget>
               </item>
-            <item row="5" column="1">
+              <item row="0" column="2">
-             <widget class="QProgressBar" name="progressBar_skew">
+               <widget class="QCheckBox" name="checkBox_switchImages">
-              <property name="value">
-               <number>24</number>
-              </property>
-             </widget>
-            </item>
-            <item row="4" column="1">
-             <widget class="QProgressBar" name="progressBar_size">
-              <property name="value">
-               <number>24</number>
-              </property>
-             </widget>
-            </item>
-            <item row="3" column="0">
-             <widget class="QLabel" name="label_2">
                 <property name="text">
-               <string>Y</string>
+                 <string>Switch images</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QLabel" name="label_5">
+                <property name="text">
+                 <string>Count</string>
                 </property>
                </widget>
               </item>
@@ -325,6 +323,26 @@
                 </property>
                </widget>
               </item>
+              <item row="1" column="2">
+               <widget class="QProgressBar" name="progressBar_count_2">
+                <property name="value">
+                 <number>24</number>
+                </property>
+                <property name="invertedAppearance">
+                 <bool>true</bool>
+                </property>
+                <property name="format">
+                 <string>%p</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QLabel" name="label">
+                <property name="text">
+                 <string>X</string>
+                </property>
+               </widget>
+              </item>
               <item row="2" column="1">
                <widget class="QProgressBar" name="progressBar_x">
                 <property name="value">
@@ -335,27 +353,6 @@
                 </property>
                </widget>
               </item>
-            <item row="3" column="1">
-             <widget class="QProgressBar" name="progressBar_y">
-              <property name="value">
-               <number>24</number>
-              </property>
-             </widget>
-            </item>
-            <item row="4" column="0">
-             <widget class="QLabel" name="label_3">
-              <property name="text">
-               <string>Size</string>
-              </property>
-             </widget>
-            </item>
-            <item row="1" column="0">
-             <widget class="QLabel" name="label_5">
-              <property name="text">
-               <string>Count</string>
-              </property>
-             </widget>
-            </item>
               <item row="2" column="2">
                <widget class="QProgressBar" name="progressBar_x_2">
                 <property name="value">
@@ -372,6 +369,20 @@
                 </property>
                </widget>
               </item>
+              <item row="3" column="0">
+               <widget class="QLabel" name="label_2">
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </widget>
+              </item>
+              <item row="3" column="1">
+               <widget class="QProgressBar" name="progressBar_y">
+                <property name="value">
+                 <number>24</number>
+                </property>
+               </widget>
+              </item>
               <item row="3" column="2">
                <widget class="QProgressBar" name="progressBar_y_2">
                 <property name="value">
@@ -382,6 +393,20 @@
                 </property>
                </widget>
               </item>
+              <item row="4" column="0">
+               <widget class="QLabel" name="label_3">
+                <property name="text">
+                 <string>Size</string>
+                </property>
+               </widget>
+              </item>
+              <item row="4" column="1">
+               <widget class="QProgressBar" name="progressBar_size">
+                <property name="value">
+                 <number>24</number>
+                </property>
+               </widget>
+              </item>
               <item row="4" column="2">
                <widget class="QProgressBar" name="progressBar_size_2">
                 <property name="value">
@@ -392,6 +417,20 @@
                 </property>
                </widget>
               </item>
+              <item row="5" column="0">
+               <widget class="QLabel" name="label_4">
+                <property name="text">
+                 <string>Skew</string>
+                </property>
+               </widget>
+              </item>
+              <item row="5" column="1">
+               <widget class="QProgressBar" name="progressBar_skew">
+                <property name="value">
+                 <number>24</number>
+                </property>
+               </widget>
+              </item>
               <item row="5" column="2">
                <widget class="QProgressBar" name="progressBar_skew_2">
                 <property name="value">
@@ -402,45 +441,7 @@
                 </property>
                </widget>
               </item>
-            <item row="0" column="0">
-             <widget class="QLabel" name="label_19">
-              <property name="text">
-               <string>Serial</string>
-              </property>
-             </widget>
-            </item>
-            <item row="0" column="1">
-             <widget class="QLabel" name="label_serial">
-              <property name="text">
-               <string>0</string>
-              </property>
-              <property name="textInteractionFlags">
-               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-              </property>
-             </widget>
-            </item>
-            <item row="1" column="2">
-             <widget class="QProgressBar" name="progressBar_count_2">
-              <property name="value">
-               <number>24</number>
-              </property>
-              <property name="invertedAppearance">
-               <bool>true</bool>
-              </property>
-              <property name="format">
-               <string>%p</string>
-              </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>
             <item>
              <layout class="QHBoxLayout" name="horizontalLayout_5" stretch="1,0">
@@ -721,8 +722,6 @@
               </property>
              </widget>
             </item>
-         <item>
-          <layout class="QHBoxLayout" name="horizontalLayout">
             <item>
              <widget class="QPushButton" name="pushButton_restart">
               <property name="text">
@@ -731,6 +730,7 @@
              </widget>
             </item>
            </layout>
+          </widget>
          </item>
          <item>
           <spacer name="verticalSpacer">

tools/DataRecorder/main.cpp

@@ -188,7 +188,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with OpenNI2 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraOpenNI2(deviceId, rate, t);
+		camera = new rtabmap::CameraOpenNI2(deviceId, CameraOpenNI2::kTypeColorDepth, rate, t);
 	}
 	else if(driver == 2)
 	{
@@ -197,7 +197,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with Freenect support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraFreenect(deviceId.size()?atoi(deviceId.c_str()):0, rate, t);
+		camera = new rtabmap::CameraFreenect(deviceId.size()?atoi(deviceId.c_str()):0, CameraFreenect::kTypeColorDepth, rate, t);
 	}
 	else if(driver == 3)
 	{

tools/OdometryViewer/main.cpp

@@ -241,7 +241,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with OpenNI2 support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraOpenNI2("", rate, t);
+		camera = new rtabmap::CameraOpenNI2("", rtabmap::CameraOpenNI2::kTypeColorDepth, rate, t);
 	}
 	else if(driver == 2)
 	{
@@ -250,7 +250,7 @@ int main (int argc, char * argv[])
 			UERROR("Not built with Freenect support...");
 			exit(-1);
 		}
-		camera = new rtabmap::CameraFreenect(0, rate, t);
+		camera = new rtabmap::CameraFreenect(0, rtabmap::CameraFreenect::kTypeColorDepth, rate, t);
 	}
 	else if(driver == 3)
 	{