From f180beb3f98c12a6d17862558875786e99b7247f Mon Sep 17 00:00:00 2001
From: lesterlo <lesterlo@link.cuhk.edu.hk>
Date: Fri, 26 Aug 2016 15:40:10 +0800
Subject: [PATCH] 1. Add freenect2 cuda pipeline 2. Fix TegraJPEG Color
 problem, convert from RGB to BGR

---
 corelib/src/CameraRGBD.cpp | 44 ++++++++++++++++++++++++++++++++++----
 1 file changed, 40 insertions(+), 4 deletions(-)

diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp
index 9133f58f..3372e751 100644
--- a/corelib/src/CameraRGBD.cpp
+++ b/corelib/src/CameraRGBD.cpp
@@ -1179,6 +1179,9 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
 	}
 
 	libfreenect2::PacketPipeline * pipeline;
+#ifdef LIBFREENECT2_WITH_CUDA_SUPPORT
+	pipeline = new libfreenect2::CudaPacketPipeline();
+#else
 #ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
 	pipeline = new libfreenect2::OpenGLPacketPipeline();
 #else
@@ -1187,6 +1190,7 @@ bool CameraFreenect2::init(const std::string & calibrationFolder, const std::str
 #else
 	pipeline = new libfreenect2::CpuPacketPipeline();
 #endif
+#endif
 #endif
 
 	if(deviceId_ <= 0)
@@ -1406,7 +1410,15 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 				{
 					cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
 					cv::Mat rgbMat; // rtabmap uses 3 channels RGB
-					cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
+					#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+					 cv::cvtColor(rgbMatC4, rgbMat, CV_RGBA2BGR); 
+
+					#else 
+
+					cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR); 
+
+					#endif 
 					cv::flip(rgbMat, rgb, 1);
 
 					//rectify color
@@ -1455,7 +1467,15 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 					{
 						cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
 						cv::Mat rgbMat; // rtabmap uses 3 channels RGB
-						cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR);
+						#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+						 cv::cvtColor(rgbMatC4, rgbMat, CV_RGB2BGR); 
+
+						#else 
+
+						cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR); 
+
+						#endif 
 						cv::flip(rgbMat, rgb, 1);
 
 						cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
@@ -1564,7 +1584,15 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 							}
 	
 							// rtabmap uses 3 channels RGB
-							cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
+							#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+							 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR); 
+
+							#else 
+
+							cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR); 
+
+							#endif 
 							cv::flip(rgb, rgb, 1);
 						}
 						else //register depth to color (OLD WAY)
@@ -1578,7 +1606,15 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
 								rgbMatBGRA = tmp;
 							}
 							// rtabmap uses 3 channels RGB
-							cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR);
+							#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+							 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR); 
+
+							#else 
+
+							cv::cvtColor(rgbMatC4, rgb, CV_BGRA2BGR); 
+
+							#endif 
 							cv::flip(rgb, rgb, 1);	
 
 							cv::Mat depthFrameMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);