Added CameraFreenect2 support for Kinect v2 (images are not yet registered)

CMakeLists.txt

@@ -134,6 +134,7 @@ IF("${VTK_MAJOR_VERSION}" EQUAL 5)
 ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
 FIND_PACKAGE(ZLIB REQUIRED)
 FIND_PACKAGE(Freenect)
+FIND_PACKAGE(freenect2)
 FIND_PACKAGE(OpenNI2)
 FIND_PACKAGE(G2O)
 
@@ -343,6 +344,12 @@ ELSE()
 MESSAGE(STATUS "  With OpenNI2  = NO (OpenNI2 not found)")
 ENDIF()
 
+IF(freenect2_FOUND)
+MESSAGE(STATUS "  With Freenect2 = YES")
+ELSE()
+MESSAGE(STATUS "  With Freenect2 = NO (libfreenect2 not found)")
+ENDIF()
+
 IF(G2O_FOUND)
 MESSAGE(STATUS "  With g2o  = YES")
 ELSE()

corelib/include/rtabmap/core/CameraRGBD.h

@@ -54,7 +54,14 @@ class VideoStream;
 
 namespace pcl
 {
-	class Grabber;
+class Grabber;
+}
+
+namespace libfreenect2
+{
+class Freenect2;
+class Freenect2Device;
+class SyncMultiFrameListener;
 }
 
 typedef struct _freenect_context freenect_context;
@@ -265,4 +272,37 @@ private:
 	FreenectDevice * freenectDevice_;
 };
 
+/////////////////////////
+// CameraFreenect2
+/////////////////////////
+
+class RTABMAP_EXP CameraFreenect2 :
+	public CameraRGBD
+{
+public:
+	static bool available();
+
+public:
+	// default local transform z in, x right, y down));
+	CameraFreenect2(int deviceId= 0,
+					float imageRate=0.0f,
+					const Transform & localTransform = Transform::getIdentity(),
+					float fx = 0.0f,
+					float fy = 0.0f,
+					float cx = 0.0f,
+					float cy = 0.0f);
+	virtual ~CameraFreenect2();
+
+	bool init();
+
+protected:
+	virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
+
+private:
+	int deviceId_;
+	libfreenect2::Freenect2 * freenect2_;
+	libfreenect2::Freenect2Device *dev_;
+	libfreenect2::SyncMultiFrameListener * listener_;
+};
+
 } // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -83,6 +83,18 @@ IF(OpenNI2_FOUND)
 	)
 ENDIF(OpenNI2_FOUND)
 
+IF(freenect2_FOUND)
+	ADD_DEFINITIONS("-DWITH_FREENECT2")
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${freenect2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${freenect2_LIBRARIES}
+	)
+ENDIF(freenect2_FOUND)
+
 IF(G2O_FOUND)
 	ADD_DEFINITIONS("-DWITH_G2O")
 	SET(INCLUDE_DIRS 

corelib/src/CameraRGBD.cpp

@@ -51,6 +51,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #endif
 
+#ifdef WITH_FREENECT2
+#include <libfreenect2/libfreenect2.hpp>
+#include <libfreenect2/frame_listener_impl.h>
+#endif
+
 #ifdef WITH_OPENNI2
 #include <OniVersion.h>
 #include <OpenNI.h>
@@ -940,20 +945,146 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl
 			delete freenectDevice_;
 			freenectDevice_ = 0;
 		}
-
+	}
-		if(depth.empty() || rgb.empty())
+	if(depth.empty() || rgb.empty())
-		{
+	{
-			rgb = cv::Mat();
+		rgb = cv::Mat();
-			depth = cv::Mat();
+		depth = cv::Mat();
-			fx = 0.0f;
+		fx = 0.0f;
-			fy = 0.0f;
+		fy = 0.0f;
-			cx = 0.0f;
+		cx = 0.0f;
-			cy = 0.0f;
+		cy = 0.0f;
-		}
 	}
 #else
 	UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
 #endif
 }
 
+//
+// CameraFreenect2
+//
+bool CameraFreenect2::available()
+{
+#ifdef WITH_FREENECT2
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraFreenect2::CameraFreenect2(int deviceId, float imageRate, const Transform & localTransform, float fx, float fy, float cx, float cy) :
+		CameraRGBD(imageRate, localTransform, fx, fy, cx, cy),
+		deviceId_(deviceId),
+		freenect2_(0),
+		dev_(0)
+{
+#ifdef WITH_FREENECT2
+	freenect2_ = new libfreenect2::Freenect2();
+	//listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
+	listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth);
+	UWARN("CameraFreenect2: Images are not yet registered!");
+#endif
+}
+
+CameraFreenect2::~CameraFreenect2()
+{
+#ifdef WITH_FREENECT2
+	if(dev_)
+	{
+		dev_->stop();
+		dev_->close();
+	}
+	if(listener_)
+	{
+		delete listener_;
+	}
+	if(freenect2_)
+	{
+		delete freenect2_;
+	}
+#endif
+}
+
+bool CameraFreenect2::init()
+{
+#ifdef WITH_FREENECT2
+	if(dev_)
+	{
+		dev_->stop();
+		dev_->close();
+		dev_ = 0;
+	}
+
+	if(deviceId_ <= 0)
+	{
+		dev_ = freenect2_->openDefaultDevice();
+	}
+	else
+	{
+		dev_ = freenect2_->openDevice(deviceId_);
+	}
+
+	if(dev_)
+	{
+		dev_->setColorFrameListener(listener_);
+		dev_->setIrAndDepthFrameListener(listener_);
+		dev_->start();
+		UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
+		UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
+		return true;
+	}
+	else
+	{
+		UERROR("CameraFreenect2: no device connected or failure opening the default one!");
+	}
+#else
+	UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
+#endif
+	return false;
+}
+
+void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
+{
+#ifdef WITH_FREENECT2
+	rgb = cv::Mat();
+	depth = cv::Mat();
+	fx = 0.0f;
+	fy = 0.0f;
+	cx = 0.0f;
+	cy = 0.0f;
+	if(dev_ && listener_)
+	{
+		libfreenect2::FrameMap frames;
+		if(listener_->waitForNewFrame(frames, 1000))
+		{
+			libfreenect2::Frame *rgbFrame = frames[libfreenect2::Frame::Color];
+			//libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
+			libfreenect2::Frame *depthFrame = frames[libfreenect2::Frame::Depth];
+
+			if(rgbFrame && depthFrame)
+			{
+				cv::flip(cv::Mat(rgbFrame->height, rgbFrame->width, CV_8UC3, rgbFrame->data), rgb, 1);
+				cv::Mat(depthFrame->height, depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+				cv::flip(depth, depth, 1);
+				libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
+				fx = params.fx;
+				fy = params.fy;
+				cx = params.cx;
+				cy = params.cy;
+			}
+
+			listener_->release(frames);
+		}
+		else
+		{
+			UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of "
+					"libfreenect2, this can be done by setting LD_LIBRARY_PATH to "
+					"\"libfreenect2/depends/libusb/lib\"");
+		}
+	}
+#else
+	UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
+#endif
+}
+
 } // namespace rtabmap

tools/CameraRGBD/main.cpp

@@ -36,7 +36,12 @@ void showUsage()
 {
 	printf("\nUsage:\n"
 			"rtabmap-rgbd_camera driver\n"
-			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
+			"  driver       Driver number to use: 0=OpenNI-PCL\n"
+			"                                     1=OpenNI2\n"
+			"                                     2=Freenect\n"
+			"                                     3=OpenNI-CV\n"
+			"                                     4=OpenNI-CV-ASUS\n"
+			"                                     5=Freenect2\n\n");
 	exit(1);
 }
 
@@ -53,9 +58,9 @@ int main(int argc, char * argv[])
 	else
 	{
 		driver = atoi(argv[argc-1]);
-		if(driver < 0 || driver > 4)
+		if(driver < 0 || driver > 5)
 		{
-			UERROR("driver should be between 0 and 4.");
+			UERROR("driver should be between 0 and 5.");
 			showUsage();
 		}
 	}
@@ -102,6 +107,15 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraOpenNICV(true, 0);
 	}
+	else if(driver == 5)
+	{
+		if(!rtabmap::CameraFreenect2::available())
+		{
+			UERROR("Not built with Freenect2 support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraFreenect2();
+	}
 	else
 	{
 		UFATAL("");
@@ -113,25 +127,36 @@ int main(int argc, char * argv[])
 		delete camera;
 		exit(1);
 	}
-
 	cv::Mat rgb, depth;
 	float fx, fy, cx, cy;
 	camera->takeImage(rgb, depth, fx, fy, cx, cy);
+	if(rgb.cols != depth.cols || rgb.rows != depth.rows)
+	{
+		UWARN("RGB (%d/%d) and depth (%d/%d) frames are not the same size! The registered cloud cannot be shown.",
+				rgb.cols, rgb.rows, depth.cols, depth.rows);
+	}
 	cv::namedWindow("Video", CV_WINDOW_AUTOSIZE); // create window
 	cv::namedWindow("Depth", CV_WINDOW_AUTOSIZE); // create window
 	pcl::visualization::CloudViewer viewer("cloud");
 	rtabmap::Transform opticalTransform(0,0,1,0, -1,0,0,0, 0,-1,0,0);
 	while(!rgb.empty() && !viewer.wasStopped())
 	{
+		if(depth.type() == CV_32FC1)
+		{
+			depth = rtabmap::util3d::cvtDepthFromFloat(depth);
+		}
 		cv::Mat tmp;
 		depth.convertTo(tmp, CV_8UC1, 255.0/2048.0);
 
 		cv::imshow("Video", rgb); // show frame
-		cv::imshow("Depth",tmp);
+		cv::imshow("Depth", tmp);
 
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
+		if(rgb.cols == depth.cols && rgb.rows == depth.rows)
-		cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, opticalTransform);
+		{
-		viewer.showCloud(cloud, "cloud");
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::cloudFromDepthRGB(rgb, depth, cx, cy, fx, fy);
+			cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, opticalTransform);
+			viewer.showCloud(cloud, "cloud");
+		}
 
 		int c = cv::waitKey(10); // wait 10 ms or for key stroke
 		if(c == 27)