diff --git a/CMakeLists.txt b/CMakeLists.txt index 729d778b2..8c943acd4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ SET(DEPENDS_DIR "${MY_DIR}/depends" CACHE STRING "dependency directory must be s OPTION(BUILD_SHARED_LIBS "Build shared (ON) or static (OFF) libraries" ON) OPTION(BUILD_EXAMPLES "Build examples" ON) +OPTION(BUILD_OPENNI2_DRIVER "Build OpenNI2 driver" OFF) OPTION(ENABLE_CXX11 "Enable C++11 support" OFF) OPTION(ENABLE_OPENCL "Enable OpenCL support" ON) OPTION(ENABLE_OPENGL "Enable OpenGL support" ON) @@ -268,3 +269,8 @@ IF(BUILD_EXAMPLES) MESSAGE(STATUS "Configurating examples") ADD_SUBDIRECTORY(${MY_DIR}/examples) ENDIF() + +IF(BUILD_OPENNI2_DRIVER) + MESSAGE(STATUS "Configuring OpenNI2 driver") + ADD_SUBDIRECTORY(${MY_DIR}/drivers/OpenNI2-Freenect2Driver) +ENDIF() diff --git a/cmake_modules/FindOpenNI2.cmake b/cmake_modules/FindOpenNI2.cmake new file mode 100644 index 000000000..4fb747779 --- /dev/null +++ b/cmake_modules/FindOpenNI2.cmake @@ -0,0 +1,53 @@ +# - Find OpenNI2 +# +# OpenNI2_FOUND: true if OpenNI2 was found +# OpenNI2_INCLUDE_DIRS: the directory that contains the include file +# OpenNI2_INSTALL_DIR: the directory that contains the librariy file + +IF(PKG_CONFIG_FOUND) + IF(CMAKE_SYSTEM_NAME MATCHES "Linux") + SET(MODULE "libopenni2") + IF(OpenNI2_FIND_REQUIRED) + SET(OpenNI2_REQUIRED "REQUIRED") + ENDIF() + PKG_CHECK_MODULES(OpenNI2 ${OpenNI2_REQUIRED} ${MODULE}) + FIND_PATH(OpenNI2_INSTALL_DIR + NAMES ${OpenNI2_LIBRARIES} + HINTS ${OpenNI2_LIBRARY_DIRS} + ) + SET(OpenNI2_INSTALL_DIR ${OpenNI2_LIBRARY}) + + RETURN() + ENDIF() +ENDIF() + +FIND_PATH(OpenNI2_INCLUDE_DIRS + NAMES Driver/OniDriverAPI.h + PATHS + "/opt/include" + "/opt/local/include" + "/usr/include" + "/usr/local/include" + ENV OPENNI2_INCLUDE + PATH_SUFFIXES + "ni2" + "openni2" +) + +FIND_PATH(OpenNI2_INSTALL_DIR + NAMES + libOniFile.so + libOniFile.dylib + PATHS + "/opt/lib" + "/opt/local/lib" + "/usr/lib" + "/usr/local/lib" + ENV OPENNI2_REDIST + PATH_SUFFIXES + ni2/OpenNI2/Drivers + OpenNI2/Drivers/lib +) + +INCLUDE(FindPackageHandleStandardArgs) +FIND_PACKAGE_HANDLE_STANDARD_ARGS(OpenNI2 DEFAULT_MSG OpenNI2_INSTALL_DIR OpenNI2_INCLUDE_DIRS) diff --git a/drivers/OpenNI2-Freenect2Driver/CMakeLists.txt b/drivers/OpenNI2-Freenect2Driver/CMakeLists.txt new file mode 100644 index 000000000..b53d97ebc --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/CMakeLists.txt @@ -0,0 +1,59 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 2.8.12.1) + +if(WIN32 AND NOT MINGW) + if(NOT DEFINED CMAKE_DEBUG_POSTFIX) + set(CMAKE_DEBUG_POSTFIX "d") + endif() +endif() + +IF(NOT DEFINED CMAKE_BUILD_TYPE) + # No effect for multi-configuration generators (e.g. for Visual Studio) + SET(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "Choose: RelWithDebInfo Release Debug MinSizeRel None") +ENDIF() + +PROJECT(openni2_freenect2driver) + +SET(MY_DIR ${openni2_freenect2driver_SOURCE_DIR}) +SET(freenect2_ROOT_DIR ${MY_DIR}/../..) + +FIND_PACKAGE(PkgConfig) # try find PKGConfig as it will be used if found +FIND_PACKAGE(OpenNI2 REQUIRED) + +IF(TARGET freenect2) + MESSAGE(STATUS "Using in-tree freenect2 target") + SET(freenect2_LIBRARIES freenect2) + SET(freenect2_DLLS ${LIBFREENECT2_DLLS}) +ELSE() + FIND_PACKAGE(freenect2 REQUIRED) + # Out-of-tree build will have to have DLLs manually copied. +ENDIF() + +INCLUDE_DIRECTORIES( + ${freenect2_INCLUDE_DIR} + ${OpenNI2_INCLUDE_DIRS} +) + +SET(OpenNI2-Freenect2Driver_src + src/ColorStream.cpp + src/DepthStream.cpp + src/IrStream.cpp + src/DeviceDriver.cpp + src/Registration.cpp + ${freenect2_ROOT_DIR}/${LIBFREENECT2_THREADING_SOURCE} +) + +SET(OpenNI2-Freenect2Driver_LIBRARIES + ${freenect2_LIBRARIES} +) + +SET(OpenNI2-Freenect2Driver_DLLS + ${freenect2_DLLS} +) + +ADD_LIBRARY(OpenNI2-Freenect2Driver ${OpenNI2-Freenect2Driver_src}) + +TARGET_LINK_LIBRARIES(OpenNI2-Freenect2Driver + ${OpenNI2-Freenect2Driver_LIBRARIES} +) + +INSTALL(TARGETS OpenNI2-Freenect2Driver DESTINATION "${OpenNI2_INSTALL_DIR}") diff --git a/drivers/OpenNI2-Freenect2Driver/README.md b/drivers/OpenNI2-Freenect2Driver/README.md new file mode 100644 index 000000000..6556c2526 --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/README.md @@ -0,0 +1,30 @@ +OpenNI2-Freenect2Driver +====================== + +OpenNI2-Freenect2Driver is a bridge to libfreenect2 implemented as an OpenNI2 driver. +It allows OpenNI2 to use Kinect for Windows v2 (K4W2) devices on Mac OS X. (and on Linux?) +OpenNI2-Freenect2Driver is derived from OpenNI2-FreenectDriver (https://github.com/OpenKinect/libfreenect/tree/master/OpenNI2-FreenectDriver). + +Install +------- +Please refer libfreenect2 install documentation at first. This description assumes that you are familiar with the libfreenect2 build instruction. + +1. You need [OpenNI](http://structure.io/openni) 2.2.0.33 or higher installed on your system and set the environment variables properly. You can use homebrew to install OpenNI if you use Mac OS X. And you will need to make sure target systems have libusb and all other dependencies also. + + $ brew tap homebrew/science + $ brew install openni2 + + $ export OPENNI2_REDIST=/usr/local/lib/ni2 + $ export OPENNI2_INCLUDE=/usr/local/include/ni2 + +2. Go to the top libfreenect2 directory and build it. The build option which enables to build this driver is OFF by default. You must specify -DBUILD_OPENNI2_DRIVER=ON in cmake argument. And you may specify additional -DENABLE_OPENGL=NO in cmake argument to use OpenNI2's NiViewer. + + $ cd /some/where/libfreenect2 + $ mkdir build + $ cd build + $ cmake -DBUILD_OPENNI2_DRIVER=ON .. + $ make + +3. 'make install' copies the driver to your OpenNI2 driver repository (${OPENNI2_REDIST}/OpenNI2/Drivers) + + $ make install diff --git a/drivers/OpenNI2-Freenect2Driver/src/ColorStream.cpp b/drivers/OpenNI2-Freenect2Driver/src/ColorStream.cpp new file mode 100644 index 000000000..43e196f99 --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/ColorStream.cpp @@ -0,0 +1,115 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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. + */ + +#include +#include "ColorStream.hpp" + +using namespace Freenect2Driver; + +// from NUI library & converted to radians +const float ColorStream::DIAGONAL_FOV = 73.9 * (M_PI / 180); +const float ColorStream::HORIZONTAL_FOV = 62 * (M_PI / 180); +const float ColorStream::VERTICAL_FOV = 48.6 * (M_PI / 180); + +ColorStream::ColorStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg) : VideoStream(pDevice, reg) +{ + video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30); + setVideoMode(video_mode); +} + +// Add video modes here as you implement them +ColorStream::FreenectVideoModeMap ColorStream::getSupportedVideoModes() const +{ + FreenectVideoModeMap modes; + // pixelFormat, resolutionX, resolutionY, fps + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30)] = 0; + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30)] = 1; + + return modes; +} + +void ColorStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const +{ + dstFrame->sensorType = getSensorType(); + dstFrame->stride = dstFrame->width * 3; + + // copy stream buffer from freenect + switch (video_mode.pixelFormat) + { + default: + LogError("Pixel format " + to_string(video_mode.pixelFormat) + " not supported by populateFrame()"); + return; + + case ONI_PIXEL_FORMAT_RGB888: + if (reg->isEnabled()) { + libfreenect2::Frame registered(512, 424, 4); + + reg->colorFrameRGB888(srcFrame, ®istered); + + copyFrame(static_cast(registered.data), srcX, srcY, registered.width * registered.bytes_per_pixel, + static_cast(dstFrame->data), dstX, dstY, dstFrame->stride, + width, height, mirroring); + } else { + copyFrame(static_cast(srcFrame->data), srcX, srcY, srcFrame->width * srcFrame->bytes_per_pixel, + static_cast(dstFrame->data), dstX, dstY, dstFrame->stride, + width, height, mirroring); + } + return; + } +} + +void ColorStream::copyFrame(uint8_t* srcPix, int srcX, int srcY, int srcStride, uint8_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring) +{ + srcPix += srcX + srcY * srcStride; + dstPix += dstX + dstY * dstStride; + + for (int y = 0; y < height; y++) { + uint8_t* dst = dstPix + y * dstStride; + uint8_t* src = srcPix + y * srcStride; + if (mirroring) { + dst += dstStride - 1; + for (int x = 0; x < srcStride; ++x) + { + if (x % 4 != 3) + { + *dst-- = *src++; + } + else + { + ++src; + } + } + } else { + for (int x = 0; x < dstStride-2; x += 3) + { + *dst++ = src[2]; + *dst++ = src[1]; + *dst++ = src[0]; + src += 4; + } + } + } +} diff --git a/drivers/OpenNI2-Freenect2Driver/src/ColorStream.hpp b/drivers/OpenNI2-Freenect2Driver/src/ColorStream.hpp new file mode 100644 index 000000000..ae0bfacbb --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/ColorStream.hpp @@ -0,0 +1,149 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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 // for transform() +#include // for M_PI +#include +#include +#include +#include "VideoStream.hpp" + + +namespace Freenect2Driver +{ + class ColorStream : public VideoStream + { + public: + // from NUI library & converted to radians + static const float DIAGONAL_FOV; + static const float HORIZONTAL_FOV; + static const float VERTICAL_FOV; + + private: + typedef std::map< OniVideoMode, int > FreenectVideoModeMap; + OniSensorType getSensorType() const { return ONI_SENSOR_COLOR; } + VideoModeMap getSupportedVideoModes() const; + void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; + + static void copyFrame(uint8_t* srcPix, int srcX, int srcY, int srcStride, uint8_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring); + + bool auto_white_balance; + bool auto_exposure; + + public: + ColorStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); + //~ColorStream() { } + + OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) + { + if (mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) { + // XXX, switch color resolution to 512x424 for registrarion here + OniVideoMode video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30); + setProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &video_mode, sizeof(video_mode)); + } + return ONI_STATUS_OK; + } + + // from StreamBase + OniBool isPropertySupported(int propertyId) + { + switch(propertyId) + { + default: + return VideoStream::isPropertySupported(propertyId); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: + return true; + } + } + + OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + return VideoStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) + { + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = HORIZONTAL_FOV; + return ONI_STATUS_OK; + } + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) + { + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = VERTICAL_FOV; + return ONI_STATUS_OK; + } + + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + { + if (*pDataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = auto_white_balance; + return ONI_STATUS_OK; + } + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + { + if (*pDataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_EXPOSURE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = auto_exposure; + return ONI_STATUS_OK; + } + } + } + + OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + switch (propertyId) + { + default: + return VideoStream::setProperty(propertyId, data, dataSize); + } + } + }; +} diff --git a/drivers/OpenNI2-Freenect2Driver/src/DepthStream.cpp b/drivers/OpenNI2-Freenect2Driver/src/DepthStream.cpp new file mode 100644 index 000000000..f96a5d0d6 --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/DepthStream.cpp @@ -0,0 +1,84 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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. + */ + +#include +#include "DepthStream.hpp" + +using namespace Freenect2Driver; + +// from NUI library and converted to radians +const float DepthStream::DIAGONAL_FOV = 70 * (M_PI / 180); +const float DepthStream::HORIZONTAL_FOV = 58.5 * (M_PI / 180); +const float DepthStream::VERTICAL_FOV = 45.6 * (M_PI / 180); +// from DepthKinectStream.cpp +const int DepthStream::MAX_VALUE; +const unsigned long long DepthStream::GAIN_VAL; +const unsigned long long DepthStream::CONST_SHIFT_VAL; +const unsigned long long DepthStream::MAX_SHIFT_VAL; +const unsigned long long DepthStream::PARAM_COEFF_VAL; +const unsigned long long DepthStream::SHIFT_SCALE_VAL; +const unsigned long long DepthStream::ZERO_PLANE_DISTANCE_VAL; +const double DepthStream::ZERO_PLANE_PIXEL_SIZE_VAL = 0.10520000010728836; +const double DepthStream::EMITTER_DCMOS_DISTANCE_VAL = 7.5; + +DepthStream::DepthStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg) : VideoStream(pDevice, reg) +{ + //video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 512, 424, 30); + video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30); + setVideoMode(video_mode); + setImageRegistrationMode(ONI_IMAGE_REGISTRATION_OFF); +} + +// Add video modes here as you implement them +// Note: if image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR, +// setVideoFormat() will try FREENECT_DEPTH_REGISTERED first then fall back on what is set here. +DepthStream::VideoModeMap DepthStream::getSupportedVideoModes() const +{ + VideoModeMap modes; + // pixelFormat, resolutionX, resolutionY, fps + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30)] = 0; + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 512, 424, 30)] = 1; + + return modes; +} + +void DepthStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const +{ + dstFrame->sensorType = getSensorType(); + dstFrame->stride = dstFrame->width * sizeof(uint16_t); + + // XXX, save depth map for registration + if (reg->isEnabled()) + reg->depthFrame(srcFrame); + + if (srcFrame->width < dstFrame->width || srcFrame->height < dstFrame->height) + memset(dstFrame->data, 0x00, dstFrame->width * dstFrame->height * 2); + + // copy stream buffer from freenect + copyFrame(static_cast((void*)srcFrame->data), srcX, srcY, srcFrame->width, + static_cast(dstFrame->data), dstX, dstY, dstFrame->width, + width, height, mirroring); +} diff --git a/drivers/OpenNI2-Freenect2Driver/src/DepthStream.hpp b/drivers/OpenNI2-Freenect2Driver/src/DepthStream.hpp new file mode 100644 index 000000000..5d6b0f39e --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/DepthStream.hpp @@ -0,0 +1,231 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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 // for transform() +#include // for M_PI +#include // for memcpy +#include +#include +#include "PS1080.h" +#include "VideoStream.hpp" + + +namespace Freenect2Driver +{ + class DepthStream : public VideoStream + { + public: + // from NUI library and converted to radians + static const float DIAGONAL_FOV; + static const float HORIZONTAL_FOV; + static const float VERTICAL_FOV; + // from DepthKinectStream.cpp + static const int MAX_VALUE = 10000; + static const unsigned long long GAIN_VAL = 42; + static const unsigned long long CONST_SHIFT_VAL = 200; + static const unsigned long long MAX_SHIFT_VAL = 2047; + static const unsigned long long PARAM_COEFF_VAL = 4; + static const unsigned long long SHIFT_SCALE_VAL = 10; + static const unsigned long long ZERO_PLANE_DISTANCE_VAL = 120; + static const double ZERO_PLANE_PIXEL_SIZE_VAL; + static const double EMITTER_DCMOS_DISTANCE_VAL; + + private: + OniSensorType getSensorType() const { return ONI_SENSOR_DEPTH; } + OniImageRegistrationMode image_registration_mode; + VideoModeMap getSupportedVideoModes() const; + void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; + + public: + DepthStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); + //~DepthStream() { } + + OniImageRegistrationMode getImageRegistrationMode() const { return image_registration_mode; } + OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) + { + if (!isImageRegistrationModeSupported(mode)) + return ONI_STATUS_NOT_SUPPORTED; + image_registration_mode = mode; + reg->setEnable(image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); + return setVideoMode(video_mode); + } + + // from StreamBase + OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF || mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); } + + OniBool isPropertySupported(int propertyId) + { + switch(propertyId) + { + default: + return VideoStream::isPropertySupported(propertyId); + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + case ONI_STREAM_PROPERTY_MAX_VALUE: + case XN_STREAM_PROPERTY_GAIN: + case XN_STREAM_PROPERTY_CONST_SHIFT: + case XN_STREAM_PROPERTY_MAX_SHIFT: + case XN_STREAM_PROPERTY_PARAM_COEFF: + case XN_STREAM_PROPERTY_SHIFT_SCALE: + case XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE: + case XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE: + case XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE: + case XN_STREAM_PROPERTY_S2D_TABLE: + case XN_STREAM_PROPERTY_D2S_TABLE: + return true; + } + } + + OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + return VideoStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = HORIZONTAL_FOV; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = VERTICAL_FOV; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + if (*pDataSize != sizeof(int)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MAX_VALUE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = MAX_VALUE; + return ONI_STATUS_OK; + + case XN_STREAM_PROPERTY_PIXEL_REGISTRATION: // XnPixelRegistration (get only) + case XN_STREAM_PROPERTY_WHITE_BALANCE_ENABLED: // unsigned long long + case XN_STREAM_PROPERTY_HOLE_FILTER: // unsigned long long + case XN_STREAM_PROPERTY_REGISTRATION_TYPE: // XnProcessingType + case XN_STREAM_PROPERTY_AGC_BIN: // XnDepthAGCBin* + case XN_STREAM_PROPERTY_PIXEL_SIZE_FACTOR: // unsigned long long + case XN_STREAM_PROPERTY_DCMOS_RCMOS_DISTANCE: // double + case XN_STREAM_PROPERTY_CLOSE_RANGE: // unsigned long long + return ONI_STATUS_NOT_SUPPORTED; + + case XN_STREAM_PROPERTY_GAIN: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_GAIN"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = GAIN_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_CONST_SHIFT: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_CONST_SHIFT"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = CONST_SHIFT_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_MAX_SHIFT: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_MAX_SHIFT"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = MAX_SHIFT_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_PARAM_COEFF: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_PARAM_COEFF"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = PARAM_COEFF_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_SHIFT_SCALE: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_SHIFT_SCALE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = SHIFT_SCALE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = ZERO_PLANE_DISTANCE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE: // double + if (*pDataSize != sizeof(double)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = ZERO_PLANE_PIXEL_SIZE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE: // double + if (*pDataSize != sizeof(double)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = EMITTER_DCMOS_DISTANCE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_S2D_TABLE: // OniDepthPixel[] + { + uint16_t *s2d = (uint16_t *)data; + *pDataSize = sizeof(*s2d) * 2048; + memset(data, 0, *pDataSize); + for (int i = 1; i <= 1052; i++) + s2d[i] = 342205.0/(1086.671 - i); + } + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_D2S_TABLE: // unsigned short[] + { + uint16_t *d2s = (uint16_t *)data; + *pDataSize = sizeof(*d2s) * 10001; + memset(data, 0, *pDataSize); + for (int i = 315; i <= 10000; i++) + d2s[i] = 1086.671 - 342205.0/(i + 1); + } + return ONI_STATUS_OK; + } + } + }; +} diff --git a/drivers/OpenNI2-Freenect2Driver/src/DeviceDriver.cpp b/drivers/OpenNI2-Freenect2Driver/src/DeviceDriver.cpp new file mode 100644 index 000000000..c5b92c071 --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/DeviceDriver.cpp @@ -0,0 +1,541 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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. + */ +/** +* FreenectDriver +* Copyright 2013 Benn Snyder +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "DepthStream.hpp" +#include "ColorStream.hpp" +#include "IrStream.hpp" + + +namespace Freenect2Driver +{ + typedef std::map ConfigStrings; + + class Device : public oni::driver::DeviceBase + { + private: + libfreenect2::Freenect2Device *dev; + ColorStream* color; + DepthStream* depth; + IrStream* ir; + Registration *reg; + ConfigStrings config; + bool device_stop; + libfreenect2::SyncMultiFrameListener listener; + libfreenect2::thread* thread; + + static void static_run(void* cookie) + { + static_cast(cookie)->run(); + } + + VideoStream* getStream(libfreenect2::Frame::Type type) + { + if (type == libfreenect2::Frame::Depth) + return depth; + if (type == libfreenect2::Frame::Ir) + return ir; + if (type == libfreenect2::Frame::Color) + return color; + return NULL; + } + + void run() + { + libfreenect2::FrameMap frames; + uint32_t seqNum = 0; + libfreenect2::Frame::Type seqType; + + struct streams { + const char* name; + libfreenect2::Frame::Type type; + } streams[] = { + { "Ir", libfreenect2::Frame::Ir }, + { "Depth", libfreenect2::Frame::Depth }, + { "Color", libfreenect2::Frame::Color } + }; + while(!device_stop) + { + listener.waitForNewFrame(frames); + + for (int i = 0; i < sizeof(streams)/sizeof(*streams); i++) { + struct streams& s = streams[i]; + VideoStream* stream = getStream(s.type); + libfreenect2::Frame *frame = frames[s.type]; + if (stream) { + if (seqNum == 0) + seqType = s.type; + if (s.type == seqType) + seqNum++; + frame->timestamp = seqNum * 33369; + stream->buildFrame(frame); + } + } + + listener.release(frames); + } + } + + OniStatus setStreamProperties(VideoStream* stream, std::string pfx) + { + pfx += '-'; + OniStatus res = ONI_STATUS_OK, tmp_res; + if (config.find(pfx + "size") != config.end()) { + WriteMessage("setStreamProperty: " + pfx + "size: " + config[pfx + "size"]); + std::string size(config[pfx + "size"]); + int i = size.find("x"); + OniVideoMode video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, atoi(size.substr(0, i).c_str()), atoi(size.substr(i + 1).c_str()), 30); + tmp_res = stream->setProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, (void*)&video_mode, sizeof(video_mode)); + if (tmp_res != ONI_STATUS_OK) + res = tmp_res; + } + + return res; + } + + public: + Device(int index) : //libfreenect2::Freenect2Device(fn_ctx, index), + dev(NULL), + reg(NULL), + color(NULL), + ir(NULL), + depth(NULL), + device_stop(false), + listener(libfreenect2::Frame::Depth | libfreenect2::Frame::Ir | libfreenect2::Frame::Color), + thread(NULL) + { + thread = new libfreenect2::thread(&Device::static_run, this); + } + ~Device() + { + destroyStream(color); + destroyStream(ir); + destroyStream(depth); + close(); + if (reg) { + delete reg; + reg = NULL; + } + } + + // for Freenect2Device + void setFreenect2Device(libfreenect2::Freenect2Device *dev) { + this->dev = dev; + dev->setColorFrameListener(&listener); + dev->setIrAndDepthFrameListener(&listener); + reg = new Registration(dev); + } + void setConfigStrings(ConfigStrings& config) { + this->config = config; + } + void start() { + //TODO: start thread executing the run() method + //device_stop = false; + //thread = new libfreenect2::thread(&Device::static_run, this); + if (! color) { + color = new ColorStream(dev, reg); + setStreamProperties(color, "color"); + } + if (! depth) { + depth = new DepthStream(dev, reg); + setStreamProperties(depth, "depth"); + } + if (! ir) { + ir = new IrStream(dev, reg); + setStreamProperties(ir, "ir"); + } + dev->start(); + } + void stop() { + if (!device_stop) { + device_stop = true; + thread->join(); + dev->stop(); + } + if (color != NULL) + { + delete color; + color = NULL; + } + if (depth != NULL) + { + delete depth; + depth = NULL; + } + if (ir != NULL) + { + delete ir; + ir = NULL; + } + } + void close() { + if (this->dev) { + stop(); + dev->close(); + } + this->dev = NULL; + } + + // for DeviceBase + + OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return depth->isImageRegistrationModeSupported(mode); } + + OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) + { + *numSensors = 3; + OniSensorInfo * sensors = new OniSensorInfo[*numSensors]; + sensors[0] = depth->getSensorInfo(); + sensors[1] = color->getSensorInfo(); + sensors[2] = ir->getSensorInfo(); + *pSensors = sensors; + return ONI_STATUS_OK; + } + + oni::driver::StreamBase* createStream(OniSensorType sensorType) + { + switch (sensorType) + { + default: + LogError("Cannot create a stream of type " + to_string(sensorType)); + return NULL; + case ONI_SENSOR_COLOR: + WriteMessage("Device: createStream(color)"); + return color; + case ONI_SENSOR_DEPTH: + WriteMessage("Device: createStream(depth)"); + return depth; + case ONI_SENSOR_IR: + WriteMessage("Device: createStream(ir)"); + return ir; + } + } + + void destroyStream(oni::driver::StreamBase* pStream) + { + if (pStream == color) + WriteMessage("Device: destroyStream(color)"); + if (pStream == depth) + WriteMessage("Device: destroyStream(depth)"); + if (pStream == ir) + WriteMessage("Device: destroyStream(ir)"); + } + + // todo: fill out properties + OniBool isPropertySupported(int propertyId) + { + if (propertyId == ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION) + return true; + return false; + } + + 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: // ? + // files + case ONI_DEVICE_PROPERTY_PLAYBACK_SPEED: // float + case ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED: // OniBool + // xn + case XN_MODULE_PROPERTY_USB_INTERFACE: // XnSensorUsbInterface + case XN_MODULE_PROPERTY_MIRROR: // bool + case XN_MODULE_PROPERTY_RESET_SENSOR_ON_STARTUP: // unsigned long long + case XN_MODULE_PROPERTY_LEAN_INIT: // unsigned long long + case XN_MODULE_PROPERTY_SERIAL_NUMBER: // unsigned long long + case XN_MODULE_PROPERTY_VERSION: // XnVersions + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode + if (*pDataSize != sizeof(OniImageRegistrationMode)) + { + LogError("Unexpected size for ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = depth->getImageRegistrationMode(); + return ONI_STATUS_OK; + } + } + + 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: // ? + // files + case ONI_DEVICE_PROPERTY_PLAYBACK_SPEED: // float + case ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED: // OniBool + // xn + case XN_MODULE_PROPERTY_USB_INTERFACE: // XnSensorUsbInterface + case XN_MODULE_PROPERTY_MIRROR: // bool + case XN_MODULE_PROPERTY_RESET_SENSOR_ON_STARTUP: // unsigned long long + case XN_MODULE_PROPERTY_LEAN_INIT: // unsigned long long + case XN_MODULE_PROPERTY_SERIAL_NUMBER: // unsigned long long + case XN_MODULE_PROPERTY_VERSION: // XnVersions + // xn commands + case XN_MODULE_PROPERTY_FIRMWARE_PARAM: // XnInnerParam + case XN_MODULE_PROPERTY_RESET: // unsigned long long + case XN_MODULE_PROPERTY_IMAGE_CONTROL: // XnControlProcessingData + case XN_MODULE_PROPERTY_DEPTH_CONTROL: // XnControlProcessingData + case XN_MODULE_PROPERTY_AHB: // XnAHBData + case XN_MODULE_PROPERTY_LED_STATE: // XnLedState + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode + if (dataSize != sizeof(OniImageRegistrationMode)) + { + LogError("Unexpected size for ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION"); + return ONI_STATUS_ERROR; + } + OniImageRegistrationMode mode = *(static_cast(data)); + color->setImageRegistrationMode(mode); + return depth->setImageRegistrationMode(mode); + } + } + + OniBool isCommandSupported(int commandId) + { + switch (commandId) + { + default: + case ONI_DEVICE_COMMAND_SEEK: + return false; + } + } + + OniStatus invoke(int commandId, void* data, int dataSize) + { + switch (commandId) + { + default: + case ONI_DEVICE_COMMAND_SEEK: // OniSeek + return ONI_STATUS_NOT_SUPPORTED; + } + } + + + /* todo: for DeviceBase + virtual OniStatus tryManualTrigger() {return ONI_STATUS_OK;} + */ + }; + + + class Driver : public oni::driver::DriverBase + { + private: + typedef std::map OniDeviceMap; + OniDeviceMap devices; + std::string uriScheme; + ConfigStrings config; + libfreenect2::Freenect2 freenect2; + + std::string devid_to_uri(int id) { + return uriScheme + "://" + to_string(id); + } + + int uri_to_devid(const std::string uri) { + int id; + std::istringstream is(uri); + is.seekg((uriScheme + "://").length()); + is >> id; + return id; + } + + void register_uri(std::string uri) { + OniDeviceInfo info; + strncpy(info.uri, uri.c_str(), ONI_MAX_STR); + strncpy(info.vendor, "Microsoft", ONI_MAX_STR); + //strncpy(info.name, "Kinect 2", ONI_MAX_STR); // XXX, NiTE does not accept new name + strncpy(info.name, "Kinect", ONI_MAX_STR); + if (devices.find(info) == devices.end()) { + WriteMessage("Driver: register new uri: " + uri); + devices[info] = NULL; + deviceConnected(&info); + deviceStateChanged(&info, 0); + } + } + + public: + Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices), + uriScheme("freenect2") + { + //WriteMessage("Using libfreenect v" + to_string(PROJECT_VER)); + WriteMessage("Using libfreenect2"); + + DriverServices = &getServices(); + } + ~Driver() { shutdown(); } + + // for DriverBase + + OniStatus initialize(oni::driver::DeviceConnectedCallback connectedCallback, oni::driver::DeviceDisconnectedCallback disconnectedCallback, oni::driver::DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) + { + DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); + for (int i = 0; i < freenect2.enumerateDevices(); i++) + { + std::string uri = devid_to_uri(i); + std::vector modes; + + modes.push_back(""); + modes.push_back("?depth-size=640x480"); + modes.push_back("?depth-size=512x424"); + + WriteMessage("Found device " + uri); + + for (int i = 0; i < modes.size(); i++) { + register_uri(uri + modes[i]); + } + } + return ONI_STATUS_OK; + } + + oni::driver::DeviceBase* deviceOpen(const char* c_uri, const char* c_mode = NULL) + { + std::string uri(c_uri); + std::string mode(c_mode ? c_mode : ""); + if (uri.find("?") != -1) { + mode += "&"; + mode += uri.substr(uri.find("?") + 1); + uri = uri.substr(0, uri.find("?")); + } + std::stringstream ss(mode); + std::string buf; + while(std::getline(ss, buf, '&')) { + if (buf.find("=") != -1) { + config[buf.substr(0, buf.find("="))] = buf.substr(buf.find("=")+1); + } else { + if (0 < buf.length()) + config[buf] = ""; + } + } + WriteMessage("deiveOpen: " + uri); + for (std::map::iterator it = config.begin(); it != config.end(); it++) { + WriteMessage(" " + it->first + " = " + it->second); + } + + for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++) + { + std::string iter_uri(iter->first.uri); + if (iter_uri.substr(0, iter_uri.find("?")) == uri) // found + { + if (iter->second) // already open + { + return iter->second; + } + else + { + WriteMessage("Opening device " + std::string(uri)); + int id = uri_to_devid(iter->first.uri); + Device* device = new Device(id); + device->setFreenect2Device(freenect2.openDevice(id)); // XXX, detault pipeline // const PacketPipeline *factory); + device->setConfigStrings(config); + device->start(); + iter->second = device; + return device; + } + } + } + + LogError("Could not find device " + std::string(uri)); + return NULL; + } + + void deviceClose(oni::driver::DeviceBase* pDevice) + { + for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++) + { + if (iter->second == pDevice) + { + WriteMessage("Closing device " + std::string(iter->first.uri)); + int id = uri_to_devid(iter->first.uri); + + Device* device = (Device*)iter->second; + device->stop(); + device->close(); + + devices.erase(iter); + return; + } + } + + LogError("Could not close unrecognized device"); + } + + OniStatus tryDevice(const char* uri) + { + oni::driver::DeviceBase* device = deviceOpen(uri); + if (! device) + return ONI_STATUS_ERROR; + deviceClose(device); + register_uri(std::string(uri)); // XXX, register new uri here. + return ONI_STATUS_OK; + } + + void shutdown() + { + for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++) + { + if (iter->second) { + deviceClose(iter->second); + } + } + } + + + /* todo: for DriverBase + virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); + virtual void disableFrameSync(void* frameSyncGroup); + */ + }; +} + + +// macros defined in XnLib (not included) - workaround +#define XN_NEW(type, arg...) new type(arg) +#define XN_DELETE(p) delete(p) +ONI_EXPORT_DRIVER(Freenect2Driver::Driver); +#undef XN_NEW +#undef XN_DELETE diff --git a/drivers/OpenNI2-Freenect2Driver/src/IrStream.cpp b/drivers/OpenNI2-Freenect2Driver/src/IrStream.cpp new file mode 100644 index 000000000..2427c3d1d --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/IrStream.cpp @@ -0,0 +1,61 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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. + */ + +#include +#include "IrStream.hpp" + +using namespace Freenect2Driver; + +// from NUI library and converted to radians +const float IrStream::HORIZONTAL_FOV = 58.5 * (M_PI / 180); +const float IrStream::VERTICAL_FOV = 45.6 * (M_PI / 180); + +IrStream::IrStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg) : VideoStream(pDevice, reg) +{ + video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_GRAY16, 512, 424, 30); + setVideoMode(video_mode); +} + +// Add video modes here as you implement them +IrStream::VideoModeMap IrStream::getSupportedVideoModes() const +{ + VideoModeMap modes; + // pixelFormat, resolutionX, resolutionY, fps + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_GRAY16, 512, 424, 30)] = 0; + + return modes; +} + +void IrStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const +{ + dstFrame->sensorType = getSensorType(); + dstFrame->stride = dstFrame->width * sizeof(uint16_t); + + // copy stream buffer from freenect + copyFrame(static_cast((void*)srcFrame->data), srcX, srcY, srcFrame->width, + static_cast(dstFrame->data), dstX, dstY, dstFrame->width, + width, height, mirroring); +} diff --git a/drivers/OpenNI2-Freenect2Driver/src/IrStream.hpp b/drivers/OpenNI2-Freenect2Driver/src/IrStream.hpp new file mode 100644 index 000000000..cdd5b3b0c --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/IrStream.hpp @@ -0,0 +1,94 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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 // for transform() +#include // for M_PI +#include // for memcpy +#include +#include +#include "PS1080.h" +#include "VideoStream.hpp" + +namespace Freenect2Driver +{ + class IrStream : public VideoStream + { + public: + // from NUI library and converted to radians + static const float HORIZONTAL_FOV; + static const float VERTICAL_FOV; + + private: + OniSensorType getSensorType() const { return ONI_SENSOR_IR; } + VideoModeMap getSupportedVideoModes() const; + void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; + + public: + IrStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); + //~IrStream() { } + + // from StreamBase + OniBool isPropertySupported(int propertyId) + { + switch(propertyId) + { + default: + return VideoStream::isPropertySupported(propertyId); + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + return true; + } + } + + OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + return VideoStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = HORIZONTAL_FOV; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = VERTICAL_FOV; + return ONI_STATUS_OK; + } + } + }; +} diff --git a/drivers/OpenNI2-Freenect2Driver/src/Registration.cpp b/drivers/OpenNI2-Freenect2Driver/src/Registration.cpp new file mode 100644 index 000000000..c3d6fac9c --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/Registration.cpp @@ -0,0 +1,69 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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. + */ + +#include +#include "Driver/OniDriverAPI.h" +#include "Registration.hpp" + +using namespace Freenect2Driver; + +Registration::Registration(libfreenect2::Freenect2Device* dev) : + dev(dev), + reg(NULL), + enabled(false) +{ + } + +Registration::~Registration() { + delete reg; +} + +void Registration::depthFrame(libfreenect2::Frame* frame) { + lastDepthFrame = frame; + } + +void Registration::colorFrameRGB888(libfreenect2::Frame* colorFrame, libfreenect2::Frame* registeredFrame) +{ + if (!reg) { + libfreenect2::Freenect2Device::ColorCameraParams colCamParams = dev->getColorCameraParams(); + libfreenect2::Freenect2Device::IrCameraParams irCamParams = dev->getIrCameraParams(); + { + libfreenect2::Freenect2Device::ColorCameraParams cp = colCamParams; + std::cout << "fx=" << cp.fx << ",fy=" << cp.fy << + ",cx=" << cp.cx << ",cy=" << cp.cy << std::endl; + libfreenect2::Freenect2Device::IrCameraParams ip = irCamParams; + std::cout << "fx=" << ip.fx << ",fy=" << ip.fy << + ",ix=" << ip.cx << ",iy=" << ip.cy << + ",k1=" << ip.k1 << ",k2=" << ip.k2 << ",k3=" << ip.k3 << + ",p1=" << ip.p1 << ",p2=" << ip.p2 << std::endl; + } + reg = new libfreenect2::Registration(irCamParams, colCamParams); + } + + libfreenect2::Frame undistorted(lastDepthFrame->width, lastDepthFrame->height, lastDepthFrame->bytes_per_pixel); + + reg->apply(colorFrame, lastDepthFrame, &undistorted, registeredFrame); + } diff --git a/drivers/OpenNI2-Freenect2Driver/src/Registration.hpp b/drivers/OpenNI2-Freenect2Driver/src/Registration.hpp new file mode 100644 index 000000000..8aa4a5356 --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/Registration.hpp @@ -0,0 +1,46 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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. + */ + +#include + +namespace Freenect2Driver { + class Registration { + private: + libfreenect2::Freenect2Device* dev; + libfreenect2::Registration* reg; + libfreenect2::Frame* lastDepthFrame; + bool enabled; + + public: + Registration(libfreenect2::Freenect2Device* dev); + ~Registration(); + + void depthFrame(libfreenect2::Frame* frame); + void colorFrameRGB888(libfreenect2::Frame* srcFrame, libfreenect2::Frame* dstFrame); + void setEnable(bool enable = true) { enabled = enable; } + bool isEnabled() { return enabled; } + }; +} diff --git a/drivers/OpenNI2-Freenect2Driver/src/Utility.hpp b/drivers/OpenNI2-Freenect2Driver/src/Utility.hpp new file mode 100644 index 000000000..443d8cbe7 --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/Utility.hpp @@ -0,0 +1,103 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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. + */ + +// This file contains symbols that may be used by any class or don't really go anywhere else. +#pragma once + +#include +#include "Driver/OniDriverAPI.h" + + +// Oni helpers + +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); +} + +static bool operator<(const OniDeviceInfo& left, const OniDeviceInfo& right) +{ + return (strcmp(left.uri, right.uri) < 0); +} + + +/// Extracts `first` from `pair`, for transforming a map into its keys. +struct ExtractKey +{ + template typename T::first_type + operator()(T pair) const + { + return pair.first; + } +}; + + +// holding out on C++11 +template +static std::string to_string(const T& n) +{ + std::ostringstream oss; + oss << n; + return oss.str(); +} + + +// global logging +namespace Freenect2Driver +{ + // DriverServices is set in DeviceDriver.cpp so all files can call errorLoggerAppend() + static oni::driver::DriverServices* DriverServices; + + // from XnLog.h + typedef enum XnLogSeverity { + XN_LOG_VERBOSE = 0, + XN_LOG_INFO = 1, + XN_LOG_WARNING = 2, + XN_LOG_ERROR = 3, + XN_LOG_SEVERITY_NONE = 10, + } XnLogSeverity; +} +#define FN2DRV_LOG_MASK "Freenect2Driver" +#define WriteVerbose(str) do { if (DriverServices != NULL) DriverServices->log(XN_LOG_VERBOSE, __FILE__, __LINE__, FN2DRV_LOG_MASK, std::string(str).c_str()); } while(0) +#define WriteInfo(str) do { if (DriverServices != NULL) DriverServices->log(XN_LOG_INFO, __FILE__, __LINE__, FN2DRV_LOG_MASK, std::string(str).c_str()); } while(0) +#define WriteWarning(str) do { if (DriverServices != NULL) DriverServices->log(XN_LOG_WARNING, __FILE__, __LINE__, FN2DRV_LOG_MASK, std::string(str).c_str()); } while(0) +#define WriteError(str) do { if (DriverServices != NULL) DriverServices->log(XN_LOG_ERROR, __FILE__, __LINE__, FN2DRV_LOG_MASK, std::string(str).c_str()); } while(0) +#define WriteMessage(str) WriteInfo(str) +#define LogError(str) WriteError(str) diff --git a/drivers/OpenNI2-Freenect2Driver/src/VideoStream.hpp b/drivers/OpenNI2-Freenect2Driver/src/VideoStream.hpp new file mode 100644 index 000000000..baff4c1a8 --- /dev/null +++ b/drivers/OpenNI2-Freenect2Driver/src/VideoStream.hpp @@ -0,0 +1,283 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 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 "PS1080.h" +#include "Utility.hpp" +#include "Registration.hpp" + +namespace Freenect2Driver +{ + class VideoStream : public oni::driver::StreamBase + { + private: + unsigned int frame_id; // number each frame + virtual void populateFrame(libfreenect2::Frame* lf2Frame, int srcX, int srcY, OniFrame* oniFrame, int tgtX, int tgtY, int width, int height) const = 0; + + protected: + virtual OniSensorType getSensorType() const = 0; + libfreenect2::Freenect2Device* device; + bool running; // buildFrame() does something iff true + OniVideoMode video_mode; + OniCropping cropping; + bool mirroring; + Freenect2Driver::Registration* reg; + bool callPropertyChangedCallback; + typedef std::map< OniVideoMode, int > VideoModeMap; + virtual VideoModeMap getSupportedVideoModes() const = 0; + + OniStatus setVideoMode(OniVideoMode requested_mode) + { + VideoModeMap supported_video_modes = getSupportedVideoModes(); + VideoModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode); + if (matched_mode_iter == supported_video_modes.end()) + return ONI_STATUS_NOT_SUPPORTED; + + video_mode = requested_mode; + return ONI_STATUS_OK; + } + + static void copyFrame(float* srcPix, int srcX, int srcY, int srcStride, uint16_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring) + { + srcPix += srcX + srcY * srcStride; + dstPix += dstX + dstY * dstStride; + + for (int y = 0; y < height; y++) { + uint16_t* dst = dstPix + y * dstStride; + float* src = srcPix + y * srcStride; + if (mirroring) { + dst += width; + for (int x = 0; x < width; x++) + *dst-- = *src++; + } else { + for (int x = 0; x < width; x++) + *dst++ = *src++; + } + } + } + void raisePropertyChanged(int propertyId, const void* data, int dataSize) { + if (callPropertyChangedCallback) + StreamBase::raisePropertyChanged(propertyId, data, dataSize); + } + + public: + VideoStream(libfreenect2::Freenect2Device* device, Freenect2Driver::Registration* reg) : + frame_id(1), + device(device), + reg(reg), + callPropertyChangedCallback(false), + mirroring(false), + running(false) + { + // joy of structs + memset(&cropping, 0, sizeof(cropping)); + memset(&video_mode, 0, sizeof(video_mode)); + } + //~VideoStream() { stop(); } + + OniSensorInfo getSensorInfo() + { + VideoModeMap supported_modes = getSupportedVideoModes(); + OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; + std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); + OniSensorInfo sensors = { getSensorType(), static_cast(supported_modes.size()), modes }; + return sensors; + } + + void setPropertyChangedCallback(oni::driver::PropertyChangedCallback handler, void* pCookie) { + callPropertyChangedCallback = true; + StreamBase::setPropertyChangedCallback(handler, pCookie); + } + + bool buildFrame(libfreenect2::Frame* lf2Frame) + { + if (!running) + return false; + + OniFrame* oniFrame = getServices().acquireFrame(); + oniFrame->frameIndex = frame_id++; + oniFrame->timestamp = lf2Frame->timestamp; + oniFrame->videoMode = video_mode; + oniFrame->width = video_mode.resolutionX; + oniFrame->height = video_mode.resolutionY; + + if (cropping.enabled) + { + oniFrame->height = cropping.height; + oniFrame->width = cropping.width; + oniFrame->cropOriginX = cropping.originX; + oniFrame->cropOriginY = cropping.originY; + oniFrame->croppingEnabled = true; + } + else + { + oniFrame->cropOriginX = 0; + oniFrame->cropOriginY = 0; + oniFrame->croppingEnabled = false; + } + int width = std::min(oniFrame->width, (int)lf2Frame->width); + int height = std::min(oniFrame->height, (int)lf2Frame->height); + + populateFrame(lf2Frame, oniFrame->cropOriginX, oniFrame->cropOriginY, oniFrame, 0, 0, width, height); + raiseNewFrame(oniFrame); + getServices().releaseFrame(oniFrame); + + return false; + } + + // from StreamBase + + OniStatus start() + { + running = true; + return ONI_STATUS_OK; + } + void stop() { running = false; } + + // only add to property handlers if the property is generic to all children + // otherwise, implement in child and call these in default case + OniBool isPropertySupported(int propertyId) + { + switch(propertyId) + { + case ONI_STREAM_PROPERTY_VIDEO_MODE: + case ONI_STREAM_PROPERTY_CROPPING: + case ONI_STREAM_PROPERTY_MIRRORING: + return true; + default: + return false; + } + } + + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + case ONI_STREAM_PROPERTY_MIN_VALUE: // int + case ONI_STREAM_PROPERTY_STRIDE: // int + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + // xn + case XN_STREAM_PROPERTY_INPUT_FORMAT: // unsigned long long + case XN_STREAM_PROPERTY_CROPPING_MODE: // XnCroppingMode + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (*pDataSize != sizeof(OniVideoMode)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VIDEO_MODE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = video_mode; + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + if (*pDataSize != sizeof(OniCropping)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_CROPPING"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = cropping; + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (*pDataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); + 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: + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + case ONI_STREAM_PROPERTY_MIN_VALUE: // int + case ONI_STREAM_PROPERTY_STRIDE: // int + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + // xn + case XN_STREAM_PROPERTY_INPUT_FORMAT: // unsigned long long + case XN_STREAM_PROPERTY_CROPPING_MODE: // XnCroppingMode + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (dataSize != sizeof(OniVideoMode)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VIDEO_MODE"); + 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; + + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + if (dataSize != sizeof(OniCropping)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_CROPPING"); + return ONI_STATUS_ERROR; + } + cropping = *(static_cast(data)); + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (dataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); + return ONI_STATUS_ERROR; + } + mirroring = *(static_cast(data)); + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + } + } + + + /* todo : from StreamBase + virtual OniStatus convertDepthToColorCoordinates(StreamBase* colorStream, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY) { return ONI_STATUS_NOT_SUPPORTED; } + */ + }; +}