diff --git a/Makefile b/Makefile index d04a48b1..c82b4b5f 100644 --- a/Makefile +++ b/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) diff --git a/Samples/MultipleStreamRead/main.cpp b/Samples/MultipleStreamRead/main.cpp index c8b736f0..32958477 100644 --- a/Samples/MultipleStreamRead/main.cpp +++ b/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 diff --git a/Source/Core/OniContext.cpp b/Source/Core/OniContext.cpp index 8871ec04..c6d6a764 100644 --- a/Source/Core/OniContext.cpp +++ b/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) { diff --git a/Source/Drivers/Freenect/FreenectColorStream.cpp b/Source/Drivers/Freenect/FreenectColorStream.cpp new file mode 100644 index 00000000..c14412a9 --- /dev/null +++ b/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(data); + unsigned char* frame_data = static_cast(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(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, +*/ diff --git a/Source/Drivers/Freenect/FreenectColorStream.h b/Source/Drivers/Freenect/FreenectColorStream.h new file mode 100644 index 00000000..5911dbf1 --- /dev/null +++ b/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_ diff --git a/Source/Drivers/Freenect/FreenectDepthStream.cpp b/Source/Drivers/Freenect/FreenectDepthStream.cpp new file mode 100644 index 00000000..4e463b4e --- /dev/null +++ b/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(data); + uint16_t* frame_data = static_cast(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(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, +*/ diff --git a/Source/Drivers/Freenect/FreenectDepthStream.h b/Source/Drivers/Freenect/FreenectDepthStream.h new file mode 100644 index 00000000..8a4ad503 --- /dev/null +++ b/Source/Drivers/Freenect/FreenectDepthStream.h @@ -0,0 +1,56 @@ +#ifndef _FREENECT_DEPTH_STREAM_H_ +#define _FREENECT_DEPTH_STREAM_H_ + +#include "FreenectVideoStream.h" +#include "Driver/OniDriverAPI.h" +#include "libfreenect.hpp" + + +class FreenectDepthStream : public FreenectVideoStream +{ +protected: + typedef std::map< OniVideoMode, std::pair > FreenectDepthModeMap; + +private: + static const OniSensorType sensor_type = ONI_SENSOR_DEPTH; + static const OniVideoMode default_video_mode; + static FreenectDepthModeMap getSupportedVideoModes(); + virtual void populateFrame(void* data, OniDriverFrame* pFrame) const; + OniStatus setVideoMode(OniVideoMode requested_mode) + { + FreenectDepthModeMap supported_video_modes = getSupportedVideoModes(); + FreenectDepthModeMap::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_depth_format format = matched_mode_iter->second.first; + freenect_resolution resolution = matched_mode_iter->second.second; + + try { device->setDepthFormat(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: + FreenectDepthStream(Freenect::FreenectDevice* pDevice) : FreenectVideoStream(pDevice) { mirroring = false; setVideoMode(default_video_mode); } + ~FreenectDepthStream() { } + + static OniSensorInfo getSensorInfo() + { + FreenectDepthModeMap 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_DEPTH_STREAM_H_ diff --git a/Source/Drivers/Freenect/FreenectDeviceNI.cpp b/Source/Drivers/Freenect/FreenectDeviceNI.cpp new file mode 100644 index 00000000..73ee975d --- /dev/null +++ b/Source/Drivers/Freenect/FreenectDeviceNI.cpp @@ -0,0 +1,60 @@ +#include "FreenectDeviceNI.h" +#include "XnLib.h" + + +FreenectDeviceNI::FreenectDeviceNI(freenect_context *_ctx, int _index) : Freenect::FreenectDevice(_ctx, _index) +{ + depth_stream = NULL; + color_stream = NULL; +} +FreenectDeviceNI::~FreenectDeviceNI() +{ + destroyStream(depth_stream); + destroyStream(color_stream); +} + +// for DeviceBase +OniStatus FreenectDeviceNI::getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) +{ + *numSensors = 2; + OniSensorInfo * sensors = new OniSensorInfo[*numSensors]; + sensors[0] = depth_stream->getSensorInfo(); + sensors[1] = color_stream->getSensorInfo(); + *pSensors = sensors; + return ONI_STATUS_OK; +} +StreamBase* FreenectDeviceNI::createStream(OniSensorType sensorType) +{ + switch(sensorType) + { + case ONI_SENSOR_DEPTH: + Freenect::FreenectDevice::startDepth(); + if (depth_stream == NULL) + depth_stream = XN_NEW(FreenectDepthStream, this); + return depth_stream; + case ONI_SENSOR_COLOR: + Freenect::FreenectDevice::startVideo(); + if (color_stream == NULL) + color_stream = XN_NEW(FreenectColorStream, this); + return color_stream; + // todo: IR + default: + //m_driverServices.errorLoggerAppend("FreenectDeviceNI: Can't create a stream of type %d", sensorType); + return NULL; + } +} +void FreenectDeviceNI::destroyStream(StreamBase* pStream) +{ + if (pStream == depth_stream) + { + Freenect::FreenectDevice::stopDepth(); + depth_stream = NULL; + } + if (pStream == color_stream) + { + Freenect::FreenectDevice::stopVideo(); + color_stream = NULL; + } + + XN_DELETE(pStream); +} diff --git a/Source/Drivers/Freenect/FreenectDeviceNI.h b/Source/Drivers/Freenect/FreenectDeviceNI.h new file mode 100644 index 00000000..ce380dbd --- /dev/null +++ b/Source/Drivers/Freenect/FreenectDeviceNI.h @@ -0,0 +1,94 @@ +#ifndef _FREENECT_DEVICE_NI_H_ +#define _FREENECT_DEVICE_NI_H_ + +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +#include "FreenectDepthStream.h" +#include "FreenectColorStream.h" + + +using namespace oni::driver; + +class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice +{ +protected: + FreenectDepthStream* depth_stream; + FreenectColorStream* color_stream; + +public: + FreenectDeviceNI(freenect_context *_ctx, int _index); + ~FreenectDeviceNI(); + + // from DeviceBase + OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors); + virtual OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); } + StreamBase* createStream(OniSensorType sensorType); + void destroyStream(StreamBase* pStream); + // property and command handlers are empty skeletons by default + // only add here if the property is generic to all children + // otherwise, implement in child and call these in default case + OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + case ONI_DEVICE_PROPERTY_FIRMWARE_VERSION: // string + case ONI_DEVICE_PROPERTY_DRIVER_VERSION: // OniVersion + case ONI_DEVICE_PROPERTY_HARDWARE_VERSION: // int + case ONI_DEVICE_PROPERTY_SERIAL_NUMBER: // string + case ONI_DEVICE_PROPERTY_ERROR_STATE: // ? + case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode + // files + case ONI_DEVICE_PROPERTY_PLAYBACK_SPEED: // float + case ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED: // OniBool + return ONI_STATUS_NOT_SUPPORTED; + } + } + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + switch (propertyId) + { + default: + case ONI_DEVICE_PROPERTY_FIRMWARE_VERSION: // By implementation + case ONI_DEVICE_PROPERTY_DRIVER_VERSION: // OniVersion + case ONI_DEVICE_PROPERTY_HARDWARE_VERSION: // int + case ONI_DEVICE_PROPERTY_SERIAL_NUMBER: // string + case ONI_DEVICE_PROPERTY_ERROR_STATE: // ? + case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode + // files + case ONI_DEVICE_PROPERTY_PLAYBACK_SPEED: // float + case ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED: // OniBool + return ONI_STATUS_NOT_SUPPORTED; + } + } + OniBool isCommandSupported(int propertyId) { return (invoke(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } + virtual OniStatus invoke(int commandId, const void* data, int dataSize) + { + switch (commandId) + { + default: + case ONI_DEVICE_COMMAND_SEEK: // OniSeek + return ONI_STATUS_NOT_SUPPORTED; + } + } + + // from Freenect::FreenectDevice + // Do not call these directly, even in child + void DepthCallback(void *depth, uint32_t timestamp) + { + depth_stream->acquireFrame(depth, timestamp); + } + void VideoCallback(void *image, uint32_t timestamp) + { + color_stream->acquireFrame(image, timestamp); + } + + + /* todo : from DeviceBase + virtual OniStatus tryManualTrigger() {return ONI_STATUS_OK;} + */ +}; + + +#endif //_FREENECT_DEVICE_NI_H_ diff --git a/Source/Drivers/Freenect/FreenectDriver.cpp b/Source/Drivers/Freenect/FreenectDriver.cpp new file mode 100644 index 00000000..a9dfd60c --- /dev/null +++ b/Source/Drivers/Freenect/FreenectDriver.cpp @@ -0,0 +1,81 @@ +#include "FreenectDriver.h" +#include +#include "XnLib.h" + + +static const char VENDOR_VAL[] = "Microsoft"; +static const char NAME_VAL[] = "Kinect"; + +FreenectDriver::FreenectDriver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) +{ + freenect_set_log_level(m_ctx, FREENECT_LOG_NOTICE); + // MOTOR doesn't work with k4w branch; todo: fix it + //freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); + freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_CAMERA)); +} + +// for DriverBase +OniStatus FreenectDriver::initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) +{ + DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); + for (unsigned int i = 1; i <= Freenect::deviceCount(); i++) + { + OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); + std::ostringstream uri; + uri << "freenect://" << i; + xnOSStrCopy(pInfo->uri, uri.str().c_str(), ONI_MAX_STR); + xnOSStrCopy(pInfo->vendor, VENDOR_VAL, ONI_MAX_STR); + xnOSStrCopy(pInfo->name, NAME_VAL, ONI_MAX_STR); + devices[pInfo] = NULL; + deviceConnected(pInfo); + deviceStateChanged(pInfo, 0); + } + return ONI_STATUS_OK; +} +DeviceBase* FreenectDriver::deviceOpen(const char* uri) +{ + for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); iter++) + { + if (xnOSStrCmp(iter->Key()->uri, uri) == 0) + { + // found + if (iter->Value() != NULL) + { + // already using + return iter->Value(); + } + else + { + int id; + std::istringstream(iter->Key()->uri) >> id; + FreenectDeviceNI * device = &createDevice(id); + iter->Value() = device; + return device; + } + } + } + getServices().errorLoggerAppend("Looking for '%s'", uri); + return NULL; +} +void FreenectDriver::deviceClose(DeviceBase* pDevice) +{ + for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); iter++) + { + if (iter->Value() == pDevice) + { + iter->Value() = NULL; + XN_DELETE(pDevice); + return; + } + } + // not our device?! + XN_ASSERT(FALSE); +} +OniStatus FreenectDriver::tryDevice(const char* uri) +{ + DeviceBase* device = deviceOpen(uri); + if (device == NULL) + return ONI_STATUS_ERROR; + deviceClose(device); + return ONI_STATUS_OK; +} diff --git a/Source/Drivers/Freenect/FreenectDriver.h b/Source/Drivers/Freenect/FreenectDriver.h new file mode 100644 index 00000000..5ff0a893 --- /dev/null +++ b/Source/Drivers/Freenect/FreenectDriver.h @@ -0,0 +1,67 @@ +/** +* FreenectDriver +* Copyright 2012 Benn Snyder +* +* OpenNI 2.x Alpha +* Copyright (C) 2012 PrimeSense Ltd. +* +* This file is part of OpenNI. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#ifndef _FREENECT_DRIVER_H_ +#define _FREENECT_DRIVER_H_ + +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +#include "XnHash.h" +#include "FreenectDeviceNI.h" + + +using namespace oni::driver; + +class FreenectDriver : public DriverBase, private Freenect::Freenect +{ +private: + xnl::Hash devices; + +public: + FreenectDriver(OniDriverServices* pDriverServices); + ~FreenectDriver() { shutdown(); } + + // from DriverBase + OniStatus initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie); + DeviceBase* deviceOpen(const char* uri); + void deviceClose(DeviceBase* pDevice); + OniStatus tryDevice(const char* uri); + void shutdown() + { + for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); ++iter) + { + deviceClose(iter->Value()); + } + } + + + /* todo : from DriverBase + virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); + virtual void disableFrameSync(void* frameSyncGroup); + */ +}; + + +ONI_EXPORT_DRIVER(FreenectDriver); + + +#endif //_FREENECT_DRIVER_H_ diff --git a/Source/Drivers/Freenect/FreenectStream.h b/Source/Drivers/Freenect/FreenectStream.h new file mode 100644 index 00000000..01f64cf0 --- /dev/null +++ b/Source/Drivers/Freenect/FreenectStream.h @@ -0,0 +1,132 @@ +#ifndef _FREENECT_STREAM_H_ +#define _FREENECT_STREAM_H_ + +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +#include "XnLib.h" + + +#define SIZE(array) sizeof array / sizeof 0[array] + +struct RetrieveKey +{ + template + typename T::first_type operator()(T pair) const + { + return pair.first; + } +}; + +typedef struct +{ + int refCount; +} FreenectStreamFrameCookie; + + +using namespace oni::driver; + +class FreenectStream : public StreamBase +{ +private: + int frame_id; // number each frame + virtual void buildFrame(void* data, OniDriverFrame* pFrame) const = 0; + +protected: + static const OniSensorType sensor_type; + Freenect::FreenectDevice* device; + bool running; // acquireFrame() does something iff true + +public: + FreenectStream(Freenect::FreenectDevice* pDevice) + { + device = pDevice; + frame_id = 1; + } + ~FreenectStream() { stop(); } + + virtual void acquireFrame(void* data, uint32_t timestamp) + { + if (!running) + return; + + OniDriverFrame* pFrame = (OniDriverFrame*)xnOSCalloc(1, sizeof(OniDriverFrame)); + if (pFrame == NULL) + { + XN_ASSERT(FALSE); + return; + } + pFrame->pDriverCookie = xnOSMalloc(sizeof(FreenectStreamFrameCookie)); + ((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount = 1; + pFrame->frame.frameIndex = frame_id++; + pFrame->frame.timestamp = timestamp; + + buildFrame(data, pFrame); + raiseNewFrame(pFrame); + } + + // from StreamBase + OniStatus start() { running = true; return ONI_STATUS_OK; } + void stop() { running = false; } + virtual void addRefToFrame(OniDriverFrame* pFrame) { ++((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount; } + virtual void releaseFrame(OniDriverFrame* pFrame) + { + if (0 == --((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount) + { + xnOSFree(pFrame->pDriverCookie); + xnOSFreeAligned(pFrame->frame.data); + xnOSFree(pFrame); + } + } + // property handlers are empty skeletons by default + // only add here if the property is generic to all children + // otherwise, implement in child and call these in default case (see FreenectVideoStream.h) + OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + case ONI_STREAM_PROPERTY_MIN_VALUE: // int + case ONI_STREAM_PROPERTY_STRIDE: // int + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + return ONI_STATUS_NOT_SUPPORTED; + } + } + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + switch (propertyId) + { + default: + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + case ONI_STREAM_PROPERTY_MIN_VALUE: // int + case ONI_STREAM_PROPERTY_STRIDE: // int + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + return ONI_STATUS_NOT_SUPPORTED; + } + } + + + /* todo : from StreamBase + virtual OniStatus convertDepthToColorCoordinates(StreamBase* colorStream, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY) { return ONI_STATUS_NOT_SUPPORTED; } + */ +}; + + +#endif // _FREENECT_STREAM_H_ diff --git a/Source/Drivers/Freenect/FreenectVideoStream.h b/Source/Drivers/Freenect/FreenectVideoStream.h new file mode 100644 index 00000000..b040b394 --- /dev/null +++ b/Source/Drivers/Freenect/FreenectVideoStream.h @@ -0,0 +1,119 @@ +#ifndef _FREENECT_VIDEO_STREAM_H_ +#define _FREENECT_VIDEO_STREAM_H_ + +#include "FreenectStream.h" +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +#include + + +static OniVideoMode makeOniVideoMode(OniPixelFormat pixel_format, int resolution_x, int resolution_y, int frames_per_second) +{ + OniVideoMode mode; + mode.pixelFormat = pixel_format; + mode.resolutionX = resolution_x; + mode.resolutionY = resolution_y; + mode.fps = frames_per_second; + return mode; +} +static bool operator==(const OniVideoMode& left, const OniVideoMode& right) +{ + return (left.pixelFormat == right.pixelFormat && left.resolutionX == right.resolutionX + && left.resolutionY == right.resolutionY && left.fps == right.fps); +} +static bool operator<(const OniVideoMode& left, const OniVideoMode& right) +{ + return (left.resolutionX*left.resolutionY < right.resolutionX*right.resolutionY); +} + + +class FreenectVideoStream : public FreenectStream +{ +private: + virtual OniStatus setVideoMode(OniVideoMode requested_mode) = 0; + virtual void populateFrame(void* data, OniDriverFrame* pFrame) const = 0; + virtual void buildFrame(void* data, OniDriverFrame* pFrame) const + { + pFrame->frame.videoMode = video_mode; + pFrame->frame.width = video_mode.resolutionX; + pFrame->frame.height = video_mode.resolutionY; + populateFrame(data, pFrame); + } + +protected: + typedef std::map< OniVideoMode, std::pair > FreenectVideoModeMap; + OniVideoMode video_mode; + bool mirroring; + +public: + FreenectVideoStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) { } + ~FreenectVideoStream() { } + + // from StreamBase + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + return FreenectStream::getProperty(propertyId, data, pDataSize); + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (*pDataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = video_mode; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (*pDataSize != sizeof(OniBool)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniBool)); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = mirroring; + return ONI_STATUS_OK; + } + } + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + switch (propertyId) + { + default: + return FreenectStream::setProperty(propertyId, data, dataSize); + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (dataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + if (ONI_STATUS_OK != setVideoMode(*(static_cast(data)))) + { + return ONI_STATUS_NOT_SUPPORTED; + } + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + } + } +}; + + +/* image 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_IR_8BIT = 2, //< 8-bit IR mode +FREENECT_VIDEO_IR_10BIT = 3, //< 10-bit IR mode +FREENECT_VIDEO_IR_10BIT_PACKED = 4, //< 10-bit packed IR mode +FREENECT_VIDEO_YUV_RGB = 5, //< YUV RGB mode +FREENECT_VIDEO_YUV_RAW = 6, //< YUV Raw mode +FREENECT_VIDEO_DUMMY = 2147483647, //< Dummy value to force enum to be 32 bits wide + +ONI_PIXEL_FORMAT_RGB888 = 200, +ONI_PIXEL_FORMAT_YUV422 = 201, +ONI_PIXEL_FORMAT_GRAY8 = 202, +ONI_PIXEL_FORMAT_GRAY16 = 203, +ONI_PIXEL_FORMAT_JPEG = 204, +*/ + + +#endif // _FREENECT_VIDEO_STREAM_H_ diff --git a/Source/Drivers/Freenect/Makefile b/Source/Drivers/Freenect/Makefile new file mode 100644 index 00000000..689a39c3 --- /dev/null +++ b/Source/Drivers/Freenect/Makefile @@ -0,0 +1,31 @@ +include ../../../ThirdParty/PSCommon/BuildSystem/CommonDefs.mak + +BIN_DIR = ../../../Bin + +INC_DIRS = \ + /usr/include/libfreenect \ + ../../../Include \ + ../../../ThirdParty/PSCommon/XnLib/Include + +SRC_FILES = \ + *.cpp + +ifeq ("$(OSTYPE)","Darwin") + INC_DIRS += /opt/local/include + LIB_DIRS += /opt/local/lib + LDFLAGS += -framework CoreFoundation -framework IOKit +endif + +LIB_NAME = FreenectDriver + +LIB_DIRS = ../../../ThirdParty/PSCommon/XnLib/Bin/$(PLATFORM)-$(CFG) +USED_LIBS = XnLib dl pthread freenect +ifneq ("$(OSTYPE)","Darwin") + USED_LIBS += rt +endif + +CFLAGS += -Wall -w + +OUT_DIR := $(OUT_DIR)/OpenNI2/Drivers + +include ../../../ThirdParty/PSCommon/BuildSystem/CommonCppMakefile diff --git a/Source/Drivers/Freenect/README b/Source/Drivers/Freenect/README new file mode 100644 index 00000000..d9dca720 --- /dev/null +++ b/Source/Drivers/Freenect/README @@ -0,0 +1,53 @@ +FreenectDriver is a translation layer to libfreenect implemented as an OpenNI2 driver. +It allows use of Kinect hardware as supported by libfreenect. +It is currently capable of providing depth and video streams at 640x480 @ 30 fps. +Copyright information appears in FreenectDriver.h +The Apache 2.0 license is available in the root of this project. + + + dependencies: +libfreenect - https://github.com/OpenKinect/libfreenect +libfreenect with K4W support - https://github.com/zarvox/libfreenect/tree/k4w-wip + + build: +make USE_FREENECT=1 + + + structure: +This module is modeled heavily on TestDevice.cpp and Drivers/Kinect/ +It ties together the C++ interfaces of OpenNI2 and libfreenect using multiple inheritance. + +FreenectDriver inherits publically from DriverBase and privately from Freenect::Freenect. +A custom libfreenect.hpp allows protected access to the Freenect context, so that FreenectDriver can call the Freenect's C API. +As a DriverBase, FreenectDriver manages devices and sets up device state callbacks. + +FreenectDeviceNI inherits publically from DeviceBase and Freenect::FreenectDevice. +Because of this, it can be built by Freenect::Freenect::createDevice() and it can define FreenectDevice's depth and video callbacks. +Those callbacks trigger acquireFrame() in FreenectStream. + +FreenectStream is a virtual base class inheriting from StreamBase. +It does generic frame setup in acquireFrame() and then calls pure virtual buildFrame() to let derived classes finish the frame. +It also provides the base skeleton for setting and getting properties, which cascades down the inheritance tree. + +FreenectVideoStream is another virtual base that extends FreenectStream to be specific to visual data. + +FreenectDepthStream and FreenectColorStream are nearly identical in definition and implementation, both inheriting from FreenectVideoStream. +They differ mostly in the formats they use to process data and the video modes they support. +These two implementations offer a system to store and report supported video modes. +To implement a new mode, simply add it to getSupportedVideoModes() and modify populateFrame() if necessary. + + + + TODO: +FreenectIRStream +polish and publish Gentoo ebuilds +acquire and test with original Kinect (within 2 weeks) - please report issues! +support more FREENECT_RESOLUTION and FREENECT_VIDEO +allow toggling image registration +PROPER LOGGING! +provide more OniVideoMode and OniStreamProperty +improve K4W support in libfreenect +implement interesting derived functions +tilt motor support +OSX support - please hack! +audio stream diff --git a/Source/Drivers/Freenect/libfreenect.hpp b/Source/Drivers/Freenect/libfreenect.hpp new file mode 100644 index 00000000..15123c42 --- /dev/null +++ b/Source/Drivers/Freenect/libfreenect.hpp @@ -0,0 +1,243 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file + * for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace Freenect { + class Noncopyable { + public: + Noncopyable() {} + ~Noncopyable() {} + private: + Noncopyable( const Noncopyable& ); + const Noncopyable& operator=( const Noncopyable& ); + }; + + class FreenectTiltState { + friend class FreenectDevice; + FreenectTiltState(freenect_raw_tilt_state *_state): + m_code(_state->tilt_status), m_state(_state) + {} + public: + void getAccelerometers(double* x, double* y, double* z) { + freenect_get_mks_accel(m_state, x, y, z); + } + double getTiltDegs() { + return freenect_get_tilt_degs(m_state); + } + public: + freenect_tilt_status_code m_code; + private: + freenect_raw_tilt_state *m_state; + }; + + class FreenectDevice : Noncopyable { + public: + FreenectDevice(freenect_context *_ctx, int _index) + : m_video_resolution(FREENECT_RESOLUTION_MEDIUM), m_depth_resolution(FREENECT_RESOLUTION_MEDIUM) + { + if(freenect_open_device(_ctx, &m_dev, _index) < 0) throw std::runtime_error("Cannot open Kinect"); + freenect_set_user(m_dev, this); + freenect_set_video_mode(m_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB)); + freenect_set_depth_mode(m_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT)); + freenect_set_depth_callback(m_dev, freenect_depth_callback); + freenect_set_video_callback(m_dev, freenect_video_callback); + } + virtual ~FreenectDevice() { + if(freenect_close_device(m_dev) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion"); + } + void startVideo() { + if(freenect_start_video(m_dev) < 0) throw std::runtime_error("Cannot start RGB callback"); + } + void stopVideo() { + if(freenect_stop_video(m_dev) < 0) throw std::runtime_error("Cannot stop RGB callback"); + } + void startDepth() { + if(freenect_start_depth(m_dev) < 0) throw std::runtime_error("Cannot start depth callback"); + } + void stopDepth() { + if(freenect_stop_depth(m_dev) < 0) throw std::runtime_error("Cannot stop depth callback"); + } + void setTiltDegrees(double _angle) { + if(freenect_set_tilt_degs(m_dev, _angle) < 0) throw std::runtime_error("Cannot set angle in degrees"); + } + void setLed(freenect_led_options _option) { + if(freenect_set_led(m_dev, _option) < 0) throw std::runtime_error("Cannot set led"); + } + void updateState() { + if (freenect_update_tilt_state(m_dev) < 0) throw std::runtime_error("Cannot update device state"); + } + FreenectTiltState getState() const { + return FreenectTiltState(freenect_get_tilt_state(m_dev)); + } + void setVideoFormat(freenect_video_format requested_format, freenect_resolution requested_resolution = FREENECT_RESOLUTION_MEDIUM) { + if (requested_format != m_video_format || requested_resolution != m_video_resolution) { + freenect_stop_video(m_dev); + freenect_frame_mode mode = freenect_find_video_mode(requested_resolution, requested_format); + if (!mode.is_valid) throw std::runtime_error("Cannot set video format: invalid mode"); + if (freenect_set_video_mode(m_dev, mode) < 0) throw std::runtime_error("Cannot set video format"); + freenect_start_video(m_dev); + m_video_format = requested_format; + m_video_resolution = requested_resolution; + } + } + freenect_video_format getVideoFormat() { + return m_video_format; + } + freenect_resolution getVideoResolution() { + return m_video_resolution; + } + int getVideoBufferSize(){ + switch(m_video_format) { + case FREENECT_VIDEO_RGB: + case FREENECT_VIDEO_BAYER: + case FREENECT_VIDEO_IR_8BIT: + case FREENECT_VIDEO_IR_10BIT: + case FREENECT_VIDEO_IR_10BIT_PACKED: + case FREENECT_VIDEO_YUV_RGB: + case FREENECT_VIDEO_YUV_RAW: + return freenect_find_video_mode(m_video_resolution, m_video_format).bytes; + default: + return 0; + } + } + void setDepthFormat(freenect_depth_format requested_format, freenect_resolution requested_resolution = FREENECT_RESOLUTION_MEDIUM) { + if (requested_format != m_depth_format || requested_resolution != m_depth_resolution) { + freenect_stop_depth(m_dev); + freenect_frame_mode mode = freenect_find_depth_mode(requested_resolution, requested_format); + if (!mode.is_valid) throw std::runtime_error("Cannot set depth format: invalid mode"); + if (freenect_set_depth_mode(m_dev, mode) < 0) throw std::runtime_error("Cannot set depth format"); + freenect_start_depth(m_dev); + m_depth_format = requested_format; + m_depth_resolution = requested_resolution; + } + } + freenect_depth_format getDepthFormat() { + return m_depth_format; + } + freenect_resolution getDepthResolution() { + return m_depth_resolution; + } + int getDepthBufferSize() { + return freenect_get_current_depth_mode(m_dev).bytes; + } + // Do not call directly even in child + virtual void VideoCallback(void *video, uint32_t timestamp) = 0; + // Do not call directly even in child + virtual void DepthCallback(void *depth, uint32_t timestamp) = 0; + private: + freenect_device *m_dev; + freenect_video_format m_video_format; + freenect_depth_format m_depth_format; + freenect_resolution m_video_resolution; + freenect_resolution m_depth_resolution; + static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) { + FreenectDevice* device = static_cast(freenect_get_user(dev)); + device->DepthCallback(depth, timestamp); + } + static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) { + FreenectDevice* device = static_cast(freenect_get_user(dev)); + device->VideoCallback(video, timestamp); + } + }; + + class Freenect : Noncopyable { + private: + typedef std::map DeviceMap; + public: + Freenect() : m_stop(false) { + if(freenect_init(&m_ctx, NULL) < 0) throw std::runtime_error("Cannot initialize freenect library"); + // We claim both the motor and camera devices, since this class exposes both. + // It does not support audio, so we do not claim it. + freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); + if(pthread_create(&m_thread, NULL, pthread_callback, (void*)this) != 0) throw std::runtime_error("Cannot initialize freenect thread"); + } + ~Freenect() { + for(DeviceMap::iterator it = m_devices.begin() ; it != m_devices.end() ; ++it) { + delete it->second; + } + m_stop = true; + pthread_join(m_thread, NULL); + if(freenect_shutdown(m_ctx) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion"); + } + template + ConcreteDevice& createDevice(int _index) { + DeviceMap::iterator it = m_devices.find(_index); + if (it != m_devices.end()) delete it->second; + ConcreteDevice * device = new ConcreteDevice(m_ctx, _index); + m_devices.insert(std::make_pair(_index, device)); + return *device; + } + void deleteDevice(int _index) { + DeviceMap::iterator it = m_devices.find(_index); + if (it == m_devices.end()) return; + delete it->second; + m_devices.erase(it); + } + int deviceCount() { + return freenect_num_devices(m_ctx); + } + // Do not call directly, thread runs here + void operator()() { + while(!m_stop) { + int res = freenect_process_events(m_ctx); + if (res < 0) + { + // libusb signals an error has occurred + if (res == LIBUSB_ERROR_INTERRUPTED) + { + // This happens sometimes, it means that a system call in libusb was interrupted somehow (perhaps due to a signal) + // The simple solution seems to be just ignore it. + continue; + } + std::stringstream ss; + ss << "Cannot process freenect events (libusb error code: " << res << ")"; + throw std::runtime_error(ss.str()); + } + } + } + static void *pthread_callback(void *user_data) { + Freenect* freenect = static_cast(user_data); + (*freenect)(); + return NULL; + } + protected: + freenect_context *m_ctx; + private: + volatile bool m_stop; + pthread_t m_thread; + DeviceMap m_devices; + }; + +} +