CameraK4W2: added depth to color registration. For CameraFreenect2, kTypeColor2DepthSD is now used by default.

corelib/include/rtabmap/core/CameraRGBD.h

@@ -80,6 +80,7 @@ typedef struct _freenect_device freenect_device;
 typedef struct IKinectSensor IKinectSensor;
 typedef struct ICoordinateMapper ICoordinateMapper;
 typedef struct _DepthSpacePoint DepthSpacePoint;
+typedef struct _ColorSpacePoint ColorSpacePoint;
 typedef struct tagRGBQUAD RGBQUAD;
 typedef struct IMultiSourceFrameReader IMultiSourceFrameReader;
 
@@ -265,7 +266,7 @@ public:
 public:
 	// default local transform z in, x right, y down));
 	CameraFreenect2(int deviceId= 0,
-					Type type = kTypeColor2DepthSD,
+					Type type = kTypeDepth2ColorSD,
 					float imageRate=0.0f,
 					const Transform & localTransform = Transform::getIdentity(),
 					float minDepth = 0.3f,
@@ -311,6 +312,12 @@ class RTABMAP_EXP CameraK4W2 :
 public:
 	static bool available();
 
+	enum Type {
+		kTypeColor2DepthSD,
+		kTypeDepth2ColorSD,
+		kTypeDepth2ColorHD
+	};
+
 public:
 	static const int        cDepthWidth = 512;
 	static const int        cDepthHeight = 424;
@@ -319,7 +326,8 @@ public:
 
 public:
 	// default local transform z in, x right, y down));
-	CameraK4W2(int deviceId = 0,
+	CameraK4W2(int deviceId = 0, // not used
+		Type type = kTypeDepth2ColorSD,
 		float imageRate = 0.0f,
 		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraK4W2();
@@ -331,14 +339,20 @@ public:
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 
+private:
+	void close();
+
 private:
 #ifdef RTABMAP_K4W2
+	Type type_;
 	IKinectSensor*          pKinectSensor_;
 	ICoordinateMapper*      pCoordinateMapper_;
 	DepthSpacePoint*        pDepthCoordinates_;
+	ColorSpacePoint*        pColorCoordinates_;
 	IMultiSourceFrameReader* pMultiSourceFrameReader_;
 	RGBQUAD * pColorRGBX_;
 	INT_PTR hMSEvent;
+	CameraModel colorCameraModel_;
 #endif
 };
 

corelib/src/CameraRGBD.cpp

@@ -2073,14 +2073,17 @@ bool CameraK4W2::available()
 
 CameraK4W2::CameraK4W2(
 	int deviceId,
+	Type type,
 	float imageRate,
 	const Transform & localTransform) :
 	Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4W2
 	,
+	type_(type),
 	pKinectSensor_(NULL),
 	pCoordinateMapper_(NULL),
 	pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]),
+	pColorCoordinates_(new ColorSpacePoint[cDepthWidth * cDepthHeight]),
 	pMultiSourceFrameReader_(NULL),
 	pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]),
 	hMSEvent(NULL)
@@ -2097,12 +2100,25 @@ CameraK4W2::~CameraK4W2()
 		pDepthCoordinates_ = NULL;
 	}
 
+	if (pColorCoordinates_)
+	{
+		delete[] pColorCoordinates_;
+		pColorCoordinates_ = NULL;
+	}
+
 	if (pColorRGBX_)
 	{
 		delete[] pColorRGBX_;
 		pColorRGBX_ = NULL;
 	}
 
+	close();
+#endif
+}
+
+void CameraK4W2::close()
+{
+#ifdef RTABMAP_K4W2
 	if (pMultiSourceFrameReader_)
 	{
 		pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent);
@@ -2123,6 +2139,8 @@ CameraK4W2::~CameraK4W2()
 	}
 
 	SafeRelease(pKinectSensor_);
+
+	colorCameraModel_ = CameraModel();
 #endif
 }
 
@@ -2131,6 +2149,8 @@ bool CameraK4W2::init(const std::string & calibrationFolder, const std::string &
 #ifdef RTABMAP_K4W2
 	HRESULT hr;
 
+	close();
+
 	hr = GetDefaultKinectSensor(&pKinectSensor_);
 	if (FAILED(hr))
 	{
@@ -2141,22 +2161,22 @@ bool CameraK4W2::init(const std::string & calibrationFolder, const std::string &
 	{
 		// Initialize the Kinect and get coordinate mapper and the frame reader
 
-		if (SUCCEEDED(hr))
-		{
-			hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_);
-		}
-
 		hr = pKinectSensor_->Open();
 
 		if (SUCCEEDED(hr))
 		{
-			hr = pKinectSensor_->OpenMultiSourceFrameReader(
+			hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_);
-				FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
-				&pMultiSourceFrameReader_);
 
 			if (SUCCEEDED(hr))
 			{
-				hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
+				hr = pKinectSensor_->OpenMultiSourceFrameReader(
+					FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
+					&pMultiSourceFrameReader_);
+
+				if (SUCCEEDED(hr))
+				{
+					hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
+				}
 			}
 		}
 	}
@@ -2164,9 +2184,82 @@ bool CameraK4W2::init(const std::string & calibrationFolder, const std::string &
 	if (!pKinectSensor_ || FAILED(hr))
 	{
 		UERROR("No ready Kinect found!");
+		close();
 		return false;
 	}
 
+	// to query camera parameters, we should wait a little
+	uSleep(3000);
+
+	// initialize color calibration if not set yet
+	CameraIntrinsics intrinsics;
+	hr = pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
+	if (SUCCEEDED(hr) && intrinsics.FocalLengthX > 0.0f)
+	{
+		// guess color intrinsics by comparing two reprojections
+		CameraModel depthModel(
+			intrinsics.FocalLengthX,
+			intrinsics.FocalLengthY,
+			intrinsics.PrincipalPointX,
+			intrinsics.PrincipalPointY);
+
+		cv::Mat fakeDepth = cv::Mat::ones(cDepthHeight, cDepthWidth, CV_16UC1) * 1000;
+		hr = pCoordinateMapper_->MapDepthFrameToColorSpace(cDepthWidth * cDepthHeight, (UINT16*)fakeDepth.data, cDepthWidth * cDepthHeight, pColorCoordinates_);
+		if (SUCCEEDED(hr))
+		{
+			int firstIndex = -1;
+			int lastIndex = -1;
+			for (int depthIndex = 0; depthIndex < (cDepthWidth*cDepthHeight); ++depthIndex)
+			{
+				ColorSpacePoint p = pColorCoordinates_[depthIndex];
+				// Values that are negative infinity means it is an invalid color to depth mapping so we
+				// skip processing for this pixel
+				if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
+				{
+					if (firstIndex == -1)
+					{
+						firstIndex = depthIndex;
+					}
+					lastIndex = depthIndex;
+				}
+			}
+
+			UASSERT(firstIndex >= 0 && lastIndex >= 0);
+			float fx, fy, cx, cy;
+			float x1, y1, z1, x2, y2, z2;
+			depthModel.project(firstIndex - (firstIndex / cDepthWidth)*cDepthWidth, firstIndex / cDepthWidth, 1.0f, x1, y1, z1);
+			depthModel.project(lastIndex - (lastIndex / cDepthWidth)*cDepthWidth, lastIndex / cDepthWidth, 1.0f, x2, y2, z2);
+			ColorSpacePoint uv1 = pColorCoordinates_[firstIndex];
+			ColorSpacePoint uv2 = pColorCoordinates_[lastIndex];
+			fx = ((uv1.X - uv2.X)*z1*z2) / (x1*z2 - x2*z1);
+			cx = uv1.X - (x1 / z1) * fx;
+			fy = ((uv1.Y - uv2.Y)*z1*z2) / (y1*z2 - y2*z1);
+			cy = uv1.Y - (y1 / z1) * fy;
+
+			colorCameraModel_ = CameraModel(
+				fx,
+				fy,
+				float(cColorWidth) - cx,
+				cy,
+				this->getLocalTransform(),
+				0,
+				cv::Size(cColorWidth, cColorHeight));
+		}
+	}
+
+	if (!colorCameraModel_.isValidForProjection())
+	{
+		UERROR("Failed to get camera parameters! Is the camera connected? Try restarting the camera again or use kTypeColor2DepthSD.");
+		close();
+		return false;
+	}
+
+	std::string serial = getSerial();
+	if (!serial.empty())
+	{
+		UINFO("Running kinect device \"%s\"", serial.c_str());
+	}
+
 	return true;
 #else
 	UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
@@ -2181,6 +2274,19 @@ bool CameraK4W2::isCalibrated() const
 
 std::string CameraK4W2::getSerial() const
 {
+#ifdef RTABMAP_K4W2
+	if (pKinectSensor_)
+	{
+		wchar_t uid[255] = { 0 };
+		// It seems to fail every time!?
+		HRESULT hr = pKinectSensor_->get_UniqueKinectId(255, uid);
+		if (SUCCEEDED(hr))
+		{
+			std::wstring ws(uid);
+			return std::string(ws.begin(), ws.end());
+		}
+	}
+#endif
 	return "";
 }
 
@@ -2240,7 +2346,6 @@ SensorData CameraK4W2::captureImage(CameraInfo * info)
 
 				if (pDepthFrame && pColorFrame)
 				{
-					INT64 nDepthTime = 0;
 					IFrameDescription* pDepthFrameDescription = NULL;
 					int nDepthWidth = 0;
 					int nDepthHeight = 0;
@@ -2255,49 +2360,24 @@ SensorData CameraK4W2::captureImage(CameraInfo * info)
 					RGBQUAD *pColorBuffer = NULL;
 
 					// get depth frame data
-					hr = pDepthFrame->get_RelativeTime(&nDepthTime);
-
 					if (SUCCEEDED(hr))
-					{
 						hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
-					}
-
 					if (SUCCEEDED(hr))
-					{
 						hr = pDepthFrameDescription->get_Width(&nDepthWidth);
-					}
-
 					if (SUCCEEDED(hr))
-					{
 						hr = pDepthFrameDescription->get_Height(&nDepthHeight);
-					}
-
 					if (SUCCEEDED(hr))
-					{
 						hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
-					}
+
 					// get color frame data
-
 					if (SUCCEEDED(hr))
-					{
 						hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
-					}
-
 					if (SUCCEEDED(hr))
-					{
 						hr = pColorFrameDescription->get_Width(&nColorWidth);
-					}
-
 					if (SUCCEEDED(hr))
-					{
 						hr = pColorFrameDescription->get_Height(&nColorHeight);
-					}
-
 					if (SUCCEEDED(hr))
-					{
 						hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
-					}
-
 					if (SUCCEEDED(hr))
 					{
 						if (imageFormat == ColorImageFormat_Bgra)
@@ -2315,7 +2395,8 @@ SensorData CameraK4W2::captureImage(CameraInfo * info)
 							hr = E_FAIL;
 						}
 					}
-					if (SUCCEEDED(hr))
+
+					if(SUCCEEDED(hr))
 					{
 						//ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight,
 						//	pColorBuffer, nColorWidth, nColorHeight,
@@ -2326,74 +2407,161 @@ SensorData CameraK4W2::captureImage(CameraInfo * info)
 							pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) &&
 							pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight))
 						{
-							HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
+							if (type_ == kTypeColor2DepthSD)
-							if (SUCCEEDED(hr))
 							{
-								cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1);
+								HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
-								cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3);
+								if (SUCCEEDED(hr))
-								// loop over output pixels
-								for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex)
 								{
-									DepthSpacePoint p = pDepthCoordinates_[colorIndex];
+									cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1);
-									// Values that are negative infinity means it is an invalid color to depth mapping so we
+									cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3);
-									// skip processing for this pixel
+									// loop over output pixels
-									if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
+									for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex)
 									{
-										// To avoid black lines caused by rounding pixel values, we should set 4 pixels
+										DepthSpacePoint p = pDepthCoordinates_[colorIndex];
-										// At the same do mirror
+										// Values that are negative infinity means it is an invalid color to depth mapping so we
-										int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+										// skip processing for this pixel
-										pixel_x_l = nDepthWidth-static_cast<int>(p.X);
+										if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
-										pixel_y_l = static_cast<int>(p.Y);
+										{
-										pixel_x_h = pixel_x_l-1;
+											// To avoid black lines caused by rounding pixel values, we should set 4 pixels
-										pixel_y_h = pixel_y_l+1;
+											// At the same do mirror
+											int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+											pixel_x_l = nDepthWidth - static_cast<int>(p.X);
+											pixel_y_l = static_cast<int>(p.Y);
+											pixel_x_h = pixel_x_l - 1;
+											pixel_y_h = pixel_y_l + 1;
 
-										const RGBQUAD* pSrc = pColorBuffer + colorIndex;
+											const RGBQUAD* pSrc = pColorBuffer + colorIndex;
-										if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
+											if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
-										{
+											{
-											unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_l);
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_l);
-											 ptr[0] = pSrc->rgbBlue;
+												ptr[0] = pSrc->rgbBlue;
-											 ptr[1] = pSrc->rgbGreen;
+												ptr[1] = pSrc->rgbGreen;
-											 ptr[2] = pSrc->rgbRed;
+												ptr[2] = pSrc->rgbRed;
-											 depth.at<unsigned short>(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth);
+												depth.at<unsigned short>(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth);
-										}
+											}
-										if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
+											if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
-										{
+											{
-											unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_l);
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_l);
-											ptr[0] = pSrc->rgbBlue;
+												ptr[0] = pSrc->rgbBlue;
-											ptr[1] = pSrc->rgbGreen;
+												ptr[1] = pSrc->rgbGreen;
-											ptr[2] = pSrc->rgbRed;
+												ptr[2] = pSrc->rgbRed;
-											depth.at<unsigned short>(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth);
+												depth.at<unsigned short>(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth);
-										}
+											}
-										if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
+											if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
-										{
+											{
-											unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_h);
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_h);
-											ptr[0] = pSrc->rgbBlue;
+												ptr[0] = pSrc->rgbBlue;
-											ptr[1] = pSrc->rgbGreen;
+												ptr[1] = pSrc->rgbGreen;
-											ptr[2] = pSrc->rgbRed;
+												ptr[2] = pSrc->rgbRed;
-											depth.at<unsigned short>(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth);
+												depth.at<unsigned short>(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth);
-										}
+											}
-										if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
+											if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
-										{
+											{
-											unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_h);
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_h);
-											ptr[0] = pSrc->rgbBlue;
+												ptr[0] = pSrc->rgbBlue;
-											ptr[1] = pSrc->rgbGreen;
+												ptr[1] = pSrc->rgbGreen;
-											ptr[2] = pSrc->rgbRed;
+												ptr[2] = pSrc->rgbRed;
-											depth.at<unsigned short>(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth);
+												depth.at<unsigned short>(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth);
+											}
 										}
 									}
-								}
 
-								CameraIntrinsics intrinsics;
+									CameraIntrinsics intrinsics;
-								pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
+									pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
-								CameraModel model(
+									CameraModel model(
-									intrinsics.FocalLengthX,
+										intrinsics.FocalLengthX,
-									intrinsics.FocalLengthY,
+										intrinsics.FocalLengthY,
-									intrinsics.PrincipalPointX,
+										intrinsics.PrincipalPointX,
-									intrinsics.PrincipalPointY,
+										intrinsics.PrincipalPointY,
-									this->getLocalTransform(),
+										this->getLocalTransform(),
-									0,
+										0,
-									depth.size());
+										depth.size());
-								data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now());
+									data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now());
+								}
+								else
+								{
+									UERROR("Failed color to depth registration!");
+								}
+							}
+							else //depthToColor
+							{
+								HRESULT hr = pCoordinateMapper_->MapDepthFrameToColorSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nDepthWidth * nDepthHeight, pColorCoordinates_);
+								if (SUCCEEDED(hr))
+								{
+									cv::Mat depthSource(nDepthHeight, nDepthWidth, CV_16UC1, pDepthBuffer);
+									cv::Mat depthRegistered = cv::Mat::zeros(
+										type_ == kTypeDepth2ColorSD ? nColorHeight/2 : nColorHeight, 
+										type_ == kTypeDepth2ColorSD ? nColorWidth/2 : nColorWidth,
+										CV_16UC1);
+									cv::Mat imageColor;
+									if(type_ == kTypeDepth2ColorSD)
+									{
+										cv::Mat tmp;
+										cv::resize(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
+										cv::cvtColor(tmp, imageColor, CV_BGRA2BGR);
+									}
+									else
+									{
+										cv::cvtColor(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), imageColor, CV_BGRA2BGR);
+									}
+									// loop over output pixels
+									for (int depthIndex = 0; depthIndex < (nDepthWidth*nDepthHeight); ++depthIndex)
+									{
+										ColorSpacePoint p = pColorCoordinates_[depthIndex];
+										// Values that are negative infinity means it is an invalid color to depth mapping so we
+										// skip processing for this pixel
+										if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
+										{
+											if (type_ == kTypeDepth2ColorSD)
+											{
+												p.X /= 2.0f;
+												p.Y /= 2.0f;
+											}
+											const unsigned short & depth_value = depthSource.at<unsigned short>(0, depthIndex);
+											int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+											// get the coordinate on image plane.
+											pixel_x_l = depthRegistered.cols - p.X; // flip depth
+											pixel_y_l = p.Y;
+											pixel_x_h = pixel_x_l - 1;
+											pixel_y_h = pixel_y_l + 1;
+
+											if (pixel_x_l >= 0 && pixel_x_l < depthRegistered.cols &&
+												pixel_y_l>0 && pixel_y_l < depthRegistered.rows && // ignore first line
+												depth_value)
+											{
+												unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_l, pixel_x_l);
+												if (depthPixel == 0 || depthPixel > depth_value)
+												{
+													depthPixel = depth_value;
+												}
+											}
+											if (pixel_x_h >= 0 && pixel_x_h < depthRegistered.cols &&
+												pixel_y_h>0 && pixel_y_h < depthRegistered.rows && // ignore first line
+												depth_value)
+											{
+												unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_h, pixel_x_h);
+												if (depthPixel == 0 || depthPixel > depth_value)
+												{
+													depthPixel = depth_value;
+												}
+											}
+										}
+									}
+
+									CameraModel model = colorCameraModel_;
+									if (type_ == kTypeDepth2ColorSD)
+									{
+										model = model.scaled(0.5);
+									}
+									util2d::fillRegisteredDepthHoles(depthRegistered, true, true, type_ == kTypeDepth2ColorHD);
+									depthRegistered = rtabmap::util2d::fillDepthHoles(depthRegistered, 1);
+									cv::flip(imageColor, imageColor, 1);
+									data = SensorData(imageColor, depthRegistered, model, this->getNextSeqID(), UTimer::now());
+								}
+								else
+								{
+									UERROR("Failed depth to color registration!");
+								}
 							}
 						}
 					}

guilib/src/CameraViewer.cpp

@@ -52,7 +52,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	imageView_(new ImageView(this)),
 	cloudView_(new CloudViewer(this)),
 	processingImages_(false),
-	validDecimationValue_(1),
+	validDecimationValue_(2),
 	parameters_(parameters)
 {
 	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
@@ -68,7 +68,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
 	decimationSpin_ = new QSpinBox(this);
 	decimationSpin_->setMinimum(1);
 	decimationSpin_->setMaximum(16);
-	decimationSpin_->setValue(1);
+	decimationSpin_->setValue(2);
 
 	pause_ = new QPushButton("Pause", this);
 	pause_->setCheckable(true);

guilib/src/PreferencesDialog.cpp

@@ -527,6 +527,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_freenect2EdgeAwareFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_freenect2NoiseFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_freenect2Pipeline, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_k4w2Format, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_realsensePresetRGB, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_realsensePresetDepth, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_realsenseOdom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1519,13 +1520,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->openni2_stampsIdsUsed->setChecked(false);
 		_ui->openni2_hshift->setValue(0);
 		_ui->openni2_vshift->setValue(0);
-		_ui->comboBox_freenect2Format->setCurrentIndex(0);
+		_ui->comboBox_freenect2Format->setCurrentIndex(1);
 		_ui->doubleSpinBox_freenect2MinDepth->setValue(0.3);
 		_ui->doubleSpinBox_freenect2MaxDepth->setValue(12.0);
 		_ui->checkBox_freenect2BilateralFiltering->setChecked(true);
 		_ui->checkBox_freenect2EdgeAwareFiltering->setChecked(true);
 		_ui->checkBox_freenect2NoiseFiltering->setChecked(true);
 		_ui->lineEdit_freenect2Pipeline->setText("");
+		_ui->comboBox_k4w2Format->setCurrentIndex(1);
 		_ui->comboBox_realsensePresetRGB->setCurrentIndex(0);
 		_ui->comboBox_realsensePresetDepth->setCurrentIndex(2);
 		_ui->checkbox_realsenseOdom->setChecked(false);
@@ -1903,6 +1905,10 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->lineEdit_freenect2Pipeline->setText(settings.value("pipeline", _ui->lineEdit_freenect2Pipeline->text()).toString());
 	settings.endGroup(); // Freenect2
 
+	settings.beginGroup("K4W2");
+	_ui->comboBox_k4w2Format->setCurrentIndex(settings.value("format", _ui->comboBox_k4w2Format->currentIndex()).toInt());
+	settings.endGroup(); // K4W2
+
 	settings.beginGroup("RealSense");
 	_ui->comboBox_realsensePresetRGB->setCurrentIndex(settings.value("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()).toInt());
 	_ui->comboBox_realsensePresetDepth->setCurrentIndex(settings.value("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()).toInt());
@@ -2301,6 +2307,10 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("pipeline",           _ui->lineEdit_freenect2Pipeline->text());
 	settings.endGroup(); // Freenect2
 
+	settings.beginGroup("K4W2");
+	settings.setValue("format",				_ui->comboBox_k4w2Format->currentIndex());
+	settings.endGroup(); // K4W2
+
 	settings.beginGroup("RealSense");
 	settings.setValue("presetRGB",           _ui->comboBox_realsensePresetRGB->currentIndex());
 	settings.setValue("presetDepth",         _ui->comboBox_realsensePresetDepth->currentIndex());
@@ -4138,6 +4148,7 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			 _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD));
 	_ui->groupBox_openni2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD);
 	_ui->groupBox_freenect2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD);
+	_ui->groupBox_k4w2->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4W2 - kSrcRGBD);
 	_ui->groupBox_realsense->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense - kSrcRGBD);
 	_ui->groupBox_cameraRGBDImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD);
 	_ui->groupBox_openni->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD);
@@ -4768,6 +4779,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 	{
 		camera = new CameraK4W2(
 			this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()),
+			(CameraK4W2::Type)_ui->comboBox_k4w2Format->currentIndex(),
 			this->getGeneralInputRate(),
 			this->getSourceLocalTransform());
 	}

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-174</y>
+            <y>-292</y>
             <width>677</width>
             <height>2682</height>
            </rect>
@@ -2865,7 +2865,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>7</number>
+                              <number>8</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -3640,7 +3640,70 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </layout>
                              </widget>
                              <widget class="QWidget" name="page_72">
-                              <layout class="QVBoxLayout" name="verticalLayout_127">
+                              <layout class="QVBoxLayout" name="verticalLayout_127" stretch="1,0">
+                               <item>
+                                <widget class="QGroupBox" name="groupBox_k4w2">
+                                 <property name="sizePolicy">
+                                  <sizepolicy hsizetype="Ignored" vsizetype="Ignored">
+                                   <horstretch>0</horstretch>
+                                   <verstretch>0</verstretch>
+                                  </sizepolicy>
+                                 </property>
+                                 <property name="title">
+                                  <string>Kinect for Windows SDK v2</string>
+                                 </property>
+                                 <layout class="QGridLayout" name="gridLayout_96" columnstretch="0,1">
+                                  <item row="0" column="0">
+                                   <widget class="QComboBox" name="comboBox_k4w2Format">
+                                    <property name="sizeAdjustPolicy">
+                                     <enum>QComboBox::AdjustToContents</enum>
+                                    </property>
+                                    <item>
+                                     <property name="text">
+                                      <string>Registered Color to Depth SD</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>Registered Depth to Color SD</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>Registered Depth to Color HD</string>
+                                     </property>
+                                    </item>
+                                   </widget>
+                                  </item>
+                                  <item row="0" column="1">
+                                   <widget class="QLabel" name="label_440">
+                                    <property name="text">
+                                     <string>Format.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="0">
+                                   <spacer name="verticalSpacer_68">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
+                                 </layout>
+                                </widget>
+                               </item>
                                <item>
                                 <spacer name="verticalSpacer_67">
                                  <property name="orientation">