Freenect Driver

Makefile

@@ -11,6 +11,8 @@
 # To force CLR projects use:
 #   make FORCE_BUILD_CLR=1
 #
+# To enable the beta FreenectDriver that uses libfreenect for Kinect support:
+# 	make USE_FREENECT=1
 #############################################################################
 
 include ThirdParty/PSCommon/BuildSystem/CommonDefs.mak
@@ -20,9 +22,14 @@ XNLIB  = ThirdParty/PSCommon/XnLib/Source
 
 # list all drivers
 ALL_DRIVERS = \
-	Source/Drivers/DummyDevice   \
+	Source/Drivers/DummyDevice \
 	Source/Drivers/PS1080 \
 	Source/Drivers/OniFile
+
+ifeq "$(USE_FREENECT)" "1"
+	ALL_DRIVERS += \
+		Source/Drivers/Freenect
+endif
 
 # list all tools
 ALL_TOOLS = \
@@ -41,7 +48,7 @@ CORE_SAMPLES = \
 	Samples/EventBasedRead \
 	Samples/MultipleStreamRead \
 	Samples/MWClosestPoint \
-	Samples/MWClosestPointApp 
+	Samples/MWClosestPointApp
 
 ifeq "$(GLUT_SUPPORTED)" "1"
 	CORE_SAMPLES += \
@@ -96,6 +103,7 @@ Source/Drivers/DummyDevice:	$(OPENNI) $(XNLIB)
 Source/Drivers/RawDevice:	$(OPENNI) $(XNLIB)
 Source/Drivers/PS1080:		$(OPENNI) $(XNLIB)
 Source/Drivers/OniFile:		$(OPENNI) $(XNLIB)
+Source/Drivers/Freenect:	$(OPENNI) $(XNLIB)
 
 Source/Tools/NiViewer:		$(OPENNI) $(XNLIB)
 

Samples/MultipleStreamRead/main.cpp

@@ -80,7 +80,7 @@ int main()
 			rc = depth.start();
 			if (rc != STATUS_OK)
 			{
-				printf("Couldn't start the color stream\n%s\n", OpenNI::getExtendedError());
+				printf("Couldn't start depth stream\n%s\n", OpenNI::getExtendedError());
 			}
 		}
 		else
@@ -97,7 +97,7 @@ int main()
 			rc = color.start();
 			if (rc != STATUS_OK)
 			{
-				printf("Couldn't start the color stream\n%s\n", OpenNI::getExtendedError());
+				printf("Couldn't start color stream\n%s\n", OpenNI::getExtendedError());
 			}
 		}
 		else

Source/Core/OniContext.cpp

@@ -112,7 +112,7 @@ OniStatus Context::initialize()
 	// Use path specified in ini file
 	if (repositoryOverridden)
 	{
-		xnLogVerbose(XN_LOG_MASK_ALL, "Using '%s' as driver path, as configured in file '%s'", ONI_CONFIGURATION_FILE);
+		xnLogVerbose(XN_LOG_MASK_ALL, "Using '%s' as driver path, as configured in file '%s'", repositoryFromINI, ONI_CONFIGURATION_FILE);
 		rc = loadLibraries(repositoryFromINI);
 		return OniStatusFromXnStatus(rc);
 	}
@@ -144,19 +144,20 @@ OniStatus Context::initialize()
 XnStatus Context::loadLibraries(const char* directoryName)
 {
 	XnStatus nRetVal;
+	XnChar cpSearchPath[XN_FILE_MAX_PATH] = "";
+	XnChar cpSearchPattern[XN_FILE_MAX_PATH] = "";
 	XnChar cpSearchString[XN_FILE_MAX_PATH] = "";
 
-	xnLogVerbose(XN_LOG_MASK_ALL, "Looking for drivers in drivers repository '%s'", directoryName);
-
-	// Build the search pattern string
-	XN_VALIDATE_STR_APPEND(cpSearchString, directoryName, XN_FILE_MAX_PATH, nRetVal);
-	XN_VALIDATE_STR_APPEND(cpSearchString, XN_FILE_DIR_SEP, XN_FILE_MAX_PATH, nRetVal);
-	XN_VALIDATE_STR_APPEND(cpSearchString, XN_SHARED_LIBRARY_PREFIX, XN_FILE_MAX_PATH, nRetVal);
-	XN_VALIDATE_STR_APPEND(cpSearchString, XN_FILE_ALL_WILDCARD, XN_FILE_MAX_PATH, nRetVal);
-	XN_VALIDATE_STR_APPEND(cpSearchString, XN_SHARED_LIBRARY_POSTFIX, XN_FILE_MAX_PATH, nRetVal);
+	// Build the search pattern strings
+	XN_VALIDATE_STR_APPEND(cpSearchPath, directoryName, XN_FILE_MAX_PATH, nRetVal);
+	XN_VALIDATE_STR_APPEND(cpSearchPath, XN_FILE_DIR_SEP, XN_FILE_MAX_PATH, nRetVal);
+	XN_VALIDATE_STR_APPEND(cpSearchPattern, XN_SHARED_LIBRARY_PREFIX, XN_FILE_MAX_PATH, nRetVal);
+	XN_VALIDATE_STR_APPEND(cpSearchPattern, XN_FILE_ALL_WILDCARD, XN_FILE_MAX_PATH, nRetVal);
+	XN_VALIDATE_STR_APPEND(cpSearchPattern, XN_SHARED_LIBRARY_POSTFIX, XN_FILE_MAX_PATH, nRetVal);
+	XN_VALIDATE_STR_APPEND(cpSearchString, cpSearchPath, XN_FILE_MAX_PATH, nRetVal);
+	XN_VALIDATE_STR_APPEND(cpSearchString, cpSearchPattern, XN_FILE_MAX_PATH, nRetVal);	
 
 	// Get a file list of Xiron devices
-
 	XnInt32 nFileCount = 0;
 	nRetVal = xnOSCountFiles(cpSearchString, &nFileCount);
 	if (nRetVal != XN_STATUS_OK || nFileCount == 0)
@@ -165,16 +166,16 @@ XnStatus Context::loadLibraries(const char* directoryName)
 		m_errorLogger.Append("Found no files matching '%s'", cpSearchString);
 		return XN_STATUS_NO_MODULES_FOUND;
 	}
-
-	typedef XnChar FileName[XN_FILE_MAX_PATH];
-	FileName* acsFileList = XN_NEW_ARR(FileName, nFileCount);
-	nRetVal = xnOSGetFileList(cpSearchString, NULL, acsFileList, nFileCount, &nFileCount);
-
+
 	// Save directory
 	XnChar workingDir[XN_FILE_MAX_PATH];
 	xnOSGetCurrentDir(workingDir, XN_FILE_MAX_PATH);
 	// Change directory
-	xnOSSetCurrentDir(directoryName);
+	xnOSSetCurrentDir(cpSearchPath);
+
+	typedef XnChar FileName[XN_FILE_MAX_PATH];
+	FileName* acsFileList = XN_NEW_ARR(FileName, nFileCount);
+	nRetVal = xnOSGetFileList(cpSearchPattern, cpSearchPath, acsFileList, nFileCount, &nFileCount);
 
 	for (int i = 0; i < nFileCount; ++i)
 	{

Source/Drivers/Freenect/FreenectColorStream.cpp

@@ -0,0 +1,94 @@
+#include "FreenectColorStream.h"
+
+
+const OniVideoMode FreenectColorStream::default_video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30);
+
+// Add video modes here as you implement them
+FreenectVideoStream::FreenectVideoModeMap FreenectColorStream::getSupportedVideoModes()
+{
+	FreenectVideoModeMap modes;
+	//										pixelFormat, resolutionX, resolutionY, fps		freenect_video_format, freenect_resolution											
+	modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30)] = { FREENECT_VIDEO_RGB, FREENECT_RESOLUTION_MEDIUM };
+
+
+	return modes;	
+
+	/* working format possiblities
+	FREENECT_VIDEO_RGB
+	FREENECT_VIDEO_YUV_RGB
+	FREENECT_VIDEO_YUV_RAW
+	*/
+}
+void FreenectColorStream::populateFrame(void* data, OniDriverFrame* pFrame) const
+{
+	pFrame->frame.sensorType = sensor_type;
+	pFrame->frame.stride = video_mode.resolutionX*3;
+	pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0;
+	pFrame->frame.croppingEnabled = FALSE;
+	pFrame->frame.dataSize = device->getVideoBufferSize();
+	pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN);
+	if (pFrame->frame.data == NULL)
+	{
+		XN_ASSERT(FALSE);
+		return;
+	}
+	// copy stream buffer from freenect
+	switch (video_mode.pixelFormat)
+	{
+		default:
+			printf("pixelFormat %s not supported by populateFrame\n", video_mode.pixelFormat);
+			return;  
+		case ONI_PIXEL_FORMAT_RGB888:
+			unsigned char* _data = static_cast<unsigned char*>(data);
+			unsigned char* frame_data = static_cast<unsigned char*>(pFrame->frame.data);
+			if (mirroring)
+			{
+				for (unsigned int i = 0; i < pFrame->frame.dataSize; i += 3)
+				{
+					// find corresponding mirrored pixel
+					unsigned int pixel = i / 3;
+					unsigned int row = pixel / video_mode.resolutionX;
+					unsigned int col = video_mode.resolutionX - (pixel % video_mode.resolutionX);
+					unsigned int target = 3 * (row * video_mode.resolutionX + col);
+					// copy it to this pixel
+					frame_data[i] = _data[target];
+					frame_data[i+1] = _data[target+1];
+					frame_data[i+2] = _data[target+2];
+				}
+			}
+			else
+				std::copy(_data, _data+pFrame->frame.dataSize, frame_data);
+			return;
+	}
+}
+
+// for StreamBase
+OniStatus FreenectColorStream::setProperty(int propertyId, const void* data, int dataSize)
+{
+	switch (propertyId)
+	{
+		default:
+			return FreenectVideoStream::setProperty(propertyId, data, dataSize);
+		case ONI_STREAM_PROPERTY_MIRRORING:		// OniBool
+			if (dataSize != sizeof(OniBool))
+			{
+				printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniBool));
+				return ONI_STATUS_ERROR;
+			}
+			mirroring = *(static_cast<const OniBool*>(data));
+			return ONI_STATUS_OK;
+	}
+}
+
+
+/* color video modes reference
+
+FREENECT_VIDEO_RGB             = 0, //< Decompressed RGB mode (demosaicing done by libfreenect)
+FREENECT_VIDEO_BAYER           = 1, //< Bayer compressed mode (raw information from camera)
+FREENECT_VIDEO_YUV_RGB         = 5, //< YUV RGB mode
+FREENECT_VIDEO_YUV_RAW         = 6, //< YUV Raw mode
+
+ONI_PIXEL_FORMAT_RGB888 = 200,
+ONI_PIXEL_FORMAT_YUV422 = 201,
+ONI_PIXEL_FORMAT_JPEG = 204,
+*/

Source/Drivers/Freenect/FreenectColorStream.h

@@ -0,0 +1,53 @@
+#ifndef _FREENECT_COLOR_STREAM_H_
+#define _FREENECT_COLOR_STREAM_H_
+
+#include "FreenectVideoStream.h"
+#include "Driver/OniDriverAPI.h"
+#include "libfreenect.hpp"
+
+
+class FreenectColorStream : public FreenectVideoStream
+{
+private:
+	static const OniSensorType sensor_type = ONI_SENSOR_COLOR;
+	static const OniVideoMode default_video_mode;
+	static FreenectVideoModeMap getSupportedVideoModes();
+	virtual void populateFrame(void* data, OniDriverFrame* pFrame) const;
+	OniStatus setVideoMode(OniVideoMode requested_mode)
+	{
+		FreenectVideoModeMap supported_video_modes = getSupportedVideoModes();
+		FreenectVideoModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode);
+		if (matched_mode_iter == supported_video_modes.end())
+			return ONI_STATUS_NOT_SUPPORTED;			
+
+		freenect_video_format format = matched_mode_iter->second.first;
+		freenect_resolution resolution = matched_mode_iter->second.second;
+
+		try { device->setVideoFormat(format, resolution); }
+		catch (std::runtime_error e)
+		{
+			printf("format-resolution combination not supported by libfreenect: %d-%d\n", format, resolution);
+			return ONI_STATUS_NOT_SUPPORTED;
+		}
+		video_mode = requested_mode;
+		return ONI_STATUS_OK;
+	}
+
+public:
+	FreenectColorStream(Freenect::FreenectDevice* pDevice) : FreenectVideoStream(pDevice) { mirroring = false; setVideoMode(default_video_mode); }
+	~FreenectColorStream() { }
+
+	static OniSensorInfo getSensorInfo()
+	{
+		FreenectVideoModeMap supported_modes = getSupportedVideoModes();
+		OniVideoMode* modes = new OniVideoMode[supported_modes.size()];
+		std::transform(supported_modes.begin(), supported_modes.end(), modes, RetrieveKey());
+		return { sensor_type, SIZE(modes), modes }; // sensorType, numSupportedVideoModes, pSupportedVideoModes
+	}
+
+	// from StreamBase
+	virtual OniStatus setProperty(int propertyId, const void* data, int dataSize);
+};
+
+
+#endif // _FREENECT_COLOR_STREAM_H_

Source/Drivers/Freenect/FreenectDepthStream.cpp

@@ -0,0 +1,82 @@
+#include "FreenectDepthStream.h"
+
+
+const OniVideoMode FreenectDepthStream::default_video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30);
+
+// Add video modes here as you implement them
+FreenectDepthStream::FreenectDepthModeMap FreenectDepthStream::getSupportedVideoModes()
+{
+	FreenectDepthModeMap modes;
+	//										pixelFormat, resolutionX, resolutionY, fps		freenect_video_format, freenect_resolution											
+	modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30)] = { FREENECT_DEPTH_REGISTERED, FREENECT_RESOLUTION_MEDIUM };
+
+
+	return modes;
+}
+void FreenectDepthStream::populateFrame(void* data, OniDriverFrame* pFrame) const
+{	
+	pFrame->frame.sensorType = sensor_type;
+	pFrame->frame.stride = video_mode.resolutionX*sizeof(uint16_t);
+	pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0;
+	pFrame->frame.croppingEnabled = FALSE;	
+	pFrame->frame.dataSize = device->getDepthBufferSize();
+	pFrame->frame.data = xnOSMallocAligned(sizeof(uint16_t)*pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN);
+	if (pFrame->frame.data == NULL)
+	{
+		XN_ASSERT(FALSE);
+		return;
+	}
+
+	// copy stream buffer from freenect
+	uint16_t* _data = static_cast<uint16_t*>(data);
+	uint16_t* frame_data = static_cast<uint16_t*>(pFrame->frame.data);
+	if (mirroring)
+	{
+		for (unsigned int i = 0; i < pFrame->frame.dataSize; i++)
+		{
+			// find corresponding mirrored pixel
+			unsigned int row = i / video_mode.resolutionX;
+			unsigned int col = video_mode.resolutionX - (i % video_mode.resolutionX);
+			unsigned int target = (row * video_mode.resolutionX + col);
+			// copy it to this pixel
+			frame_data[i] = _data[target];
+		}
+	}
+	else
+		std::copy(_data, _data+pFrame->frame.dataSize, frame_data);
+}
+
+// for StreamBase
+OniStatus FreenectDepthStream::setProperty(int propertyId, const void* data, int dataSize)
+{
+	switch (propertyId)
+	{
+		default:
+			return FreenectVideoStream::setProperty(propertyId, data, dataSize);
+		case ONI_STREAM_PROPERTY_MIRRORING:		// OniBool
+			if (dataSize != sizeof(OniBool))
+			{
+				printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniBool));
+				return ONI_STATUS_ERROR;
+			}
+			mirroring = *(static_cast<const OniBool*>(data));
+			return ONI_STATUS_OK;
+	}
+}
+
+
+/* depth video modes reference
+
+FREENECT_DEPTH_11BIT        = 0, //< 11 bit depth information in one uint16_t/pixel
+FREENECT_DEPTH_10BIT        = 1, //< 10 bit depth information in one uint16_t/pixel
+FREENECT_DEPTH_11BIT_PACKED = 2, //< 11 bit packed depth information
+FREENECT_DEPTH_10BIT_PACKED = 3, //< 10 bit packed depth information
+FREENECT_DEPTH_REGISTERED   = 4, //< processed depth data in mm, aligned to 640x480 RGB
+FREENECT_DEPTH_MM           = 5, //< depth to each pixel in mm, but left unaligned to RGB image
+FREENECT_DEPTH_DUMMY        = 2147483647, //< Dummy value to force enum to be 32 bits wide
+
+ONI_PIXEL_FORMAT_DEPTH_1_MM = 100,
+ONI_PIXEL_FORMAT_DEPTH_100_UM = 101,
+ONI_PIXEL_FORMAT_SHIFT_9_2 = 102,
+ONI_PIXEL_FORMAT_SHIFT_9_3 = 103,
+*/