diff --git a/CMakeLists.txt b/CMakeLists.txt index d44a65f0c..5d10ec1aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,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" ON) OPTION(ENABLE_CXX11 "Enable C++11 support" OFF) OPTION(ENABLE_OPENCL "Enable OpenCL support" ON) OPTION(ENABLE_OPENGL "Enable OpenGL support" ON) @@ -272,3 +273,23 @@ IF(BUILD_EXAMPLES) MESSAGE(STATUS "Configurating examples") ADD_SUBDIRECTORY(${MY_DIR}/examples) ENDIF() + +IF(BUILD_OPENNI2_DRIVER) + FIND_PACKAGE(OpenNI2) + IF(OpenNI2_FOUND) + FILE(GLOB OPENNI2_DRIVER_SOURCES src/openni2/*.cpp) + ADD_LIBRARY(freenect2-openni2 ${OPENNI2_DRIVER_SOURCES} ${LIBFREENECT2_THREADING_SOURCE}) + TARGET_INCLUDE_DIRECTORIES(freenect2-openni2 PRIVATE ${OpenNI2_INCLUDE_DIRS}) + TARGET_LINK_LIBRARIES(freenect2-openni2 freenect2 ${LIBFREENECT2_THREADING_LIBRARIES}) + SET_TARGET_PROPERTIES(freenect2-openni2 PROPERTIES SOVERSION 0) + IF(NOT ${CMAKE_INSTALL_PREFIX} MATCHES "^/usr") + SET_TARGET_PROPERTIES(freenect2-openni2 PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") + ENDIF() + INSTALL(TARGETS freenect2-openni2 DESTINATION lib/OpenNI2/Drivers RUNTIME DESTINATION bin) + ADD_CUSTOM_TARGET(install-openni2 + DEPENDS freenect2-openni2 + COMMAND ${CMAKE_COMMAND} -E copy_directory "${CMAKE_INSTALL_PREFIX}/lib/OpenNI2/Drivers/" "${OpenNI2_LIBRARY_DIR}/OpenNI2/Drivers/" + VERBATIM + ) + ENDIF() +ENDIF() diff --git a/README.md b/README.md index 9ecb23249..9b2fa1458 100644 --- a/README.md +++ b/README.md @@ -40,6 +40,7 @@ Virtual machines likely do not work, because USB 3.0 isochronous transfer is qui The following minimum requirements must be met in order to enable the optional features: * OpenGL depth processing: OpenGL 3.1 (Windows, Linux, Mac OS X). No OpenGL ES support at the moment. * OpenCL depth processing: OpenCL 1.1 +* OpenNI2 driver: OpenNI2 2.2.0.33 ## FAQ @@ -165,6 +166,10 @@ Or `install_libusb_vs2015.cmd`. If you see some errors, you can always open the * Intel GPU: Download `intel_sdk_for_ocl_applications_2014_x64_setup.msi` from http://www.softpedia.com/get/Programming/SDK-DDK/Intel-SDK-for-OpenCL-Applications.shtml (SDK official download is replaced by $$$ and no longer available) and install it. Then verify `INTELOCLSDKROOT` is set as an environment variable. You may need to download and install additional OpenCL runtime. +#### OpenNI2 (optional) + +* Download OpenNI 2.2.0.33 (x64) from http://structure.io/openni, install it to default locations (`C:\Program Files...`). + #### Build ``` @@ -174,13 +179,15 @@ cmake --build . --config RelWithDebInfo --target install ``` Or `-G "Visual Studio 14 2015 Win64"`. Then you can run the program with `.\install\bin\Protonect.exe`, or start debugging in Visual Studio. `RelWithDebInfo` provides debug symbols with release performance. The default installation path is `install`, you may change it by editing `CMAKE_INSTALL_PREFIX`. +To try OpenNI2, copy freenect2-openni2.dll, and other dll files (libusb-1.0.dll, glfw.dll, etc.) in `install\bin` to `C:\Program Files\OpenNI2\Tools\OpenNI2\Drivers`. Then run `C:\Program Files\OpenNI\Tools\NiViewer.exe`. + ### Mac OSX Use your favorite package managers (brew, ports, etc.) to install most if not all dependencies: 1. ``cd`` into a directory where you want to keep libfreenect2 stuff in 1. Make sure these build tools are available: wget, git, cmake, pkg-config. Xcode may provide some of them. Install the rest via package managers. -1. Install dependencies: libusb, TurboJPEG, GLFW. +1. Install dependencies: libusb, TurboJPEG, GLFW, OpenNI2 (optional). ``` brew update @@ -189,6 +196,9 @@ brew tap homebrew/science brew install jpeg-turbo brew tap homebrew/versions brew install glfw3 +brew install openni2 +export OPENNI2_REDIST=/usr/local/lib/ni2 +export OPENNI2_INCLUDE=/usr/local/include/ni2 ``` It **is** now recommended to install libusb from package managers instead of building from source locally. Previously it was not recommended but that is no longer the case. @@ -209,6 +219,7 @@ mkdir build && cd build cmake .. make make install +make install-openni2 # may need sudo ``` 1. Run the program @@ -217,6 +228,8 @@ make install ./bin/Protonect ``` + Use `NiViewer` to test OpenNI2. + ### Debian/Ubuntu 14.04 1. Install libfreenect2 @@ -252,6 +265,8 @@ sudo dpkg -i libglfw3*_3.0.4-1_*.deb * Mali GPU (e.g. Odroid XU4): (with root) `mkdir -p /etc/OpenCL/vendors; echo /usr/lib/arm-linux-gnueabihf/mali-egl/libmali.so >/etc/OpenCL/vendors/mali.icd; apt-get install opencl-headers`. * Verify: You can install `clinfo` to verify if you have correctly set up the OpenCL stack. +1. OpenNI2 dependency (optional): `sudo apt-get install libopenni2-dev`. + 1. Build the actual protonect executable ``` @@ -260,6 +275,7 @@ mkdir build && cd build cmake .. make sudo make install # without sudo if cmake -DCMAKE_INSTALL_PREFIX=$HOME/... +sudo make install-openni2 # if OpenNI2 support is enabled ``` 1. Run the program @@ -268,6 +284,8 @@ sudo make install # without sudo if cmake -DCMAKE_INSTALL_PREFIX=$HOME/... ./bin/Protonect ``` +1. Run OpenNI2: `sudo apt-get install openni2-utils && NiViewer2`. + ### Other operating systems I'm not sure, but look for libusbx installation instructions for your OS. Figure out how to attach the driver to the Xbox NUI Sensor composite parent device, VID 045E PID 02C4, then contribute your procedure. diff --git a/cmake_modules/FindOpenNI2.cmake b/cmake_modules/FindOpenNI2.cmake new file mode 100644 index 000000000..74a68e73d --- /dev/null +++ b/cmake_modules/FindOpenNI2.cmake @@ -0,0 +1,51 @@ +# - Find OpenNI2 +# +# If the OPENNI2_INCLUDE and OPENNI2_REDIST environment variables +# are defined, they will be used as search path. +# The following standard variables get defined: +# OpenNI2_FOUND: true if found +# OpenNI2_INCLUDE_DIRS: the directory that contains the include file +# OpenNI2_LIBRARY_DIR: the directory that contains the library + +IF(PKG_CONFIG_FOUND) + PKG_CHECK_MODULES(OpenNI2 libopenni2) +ENDIF() + +FIND_PATH(OpenNI2_INCLUDE_DIRS + NAMES Driver/OniDriverAPI.h + PATHS + "/opt/include" + "/opt/local/include" + "/usr/include" + "/usr/local/include" + ENV OPENNI2_INCLUDE + ENV PROGRAMFILES + ENV ProgramW6432 + HINTS ${OpenNI2_INCLUDE_DIRS} + PATH_SUFFIXES + ni2 + openni2 + OpenNI2/Include +) + +FIND_LIBRARY(OpenNI2_LIBRARY + NAMES OpenNI2 ${OpenNI2_LIBRARIES} + PATHS + "/opt/lib" + "/opt/local/lib" + "/usr/lib" + "/usr/local/lib" + ENV OPENNI2_REDIST + ENV PROGRAMFILES + ENV ProgramW6432 + HINTS ${OpenNI2_LIBRARY_DIRS} + PATH_SUFFIXES + ni2/OpenNI2/Drivers + OpenNI2/Drivers/lib + OpenNI2/Lib +) + +GET_FILENAME_COMPONENT(OpenNI2_LIBRARY_DIR ${OpenNI2_LIBRARY} DIRECTORY) + +INCLUDE(FindPackageHandleStandardArgs) +FIND_PACKAGE_HANDLE_STANDARD_ARGS(OpenNI2 DEFAULT_MSG OpenNI2_LIBRARY_DIR OpenNI2_INCLUDE_DIRS) diff --git a/src/openni2/ColorStream.cpp b/src/openni2/ColorStream.cpp new file mode 100644 index 000000000..9355bd767 --- /dev/null +++ b/src/openni2/ColorStream.cpp @@ -0,0 +1,212 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 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. + */ + +#define _USE_MATH_DEFINES +#include // for M_PI +#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 freenect_video_format, freenect_resolution + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30)] = 0; + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30)] = 1; + + return modes; + + /* working format possiblities + FREENECT_VIDEO_RGB + FREENECT_VIDEO_YUV_RGB + FREENECT_VIDEO_YUV_RAW + */ +} + +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; + } + } + } +} + +OniSensorType ColorStream::getSensorType() const { return ONI_SENSOR_COLOR; } + +OniStatus ColorStream::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 ColorStream::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 ColorStream::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 ColorStream::setProperty(int propertyId, const void* data, int dataSize) +{ + switch (propertyId) + { + case 0: + default: + return VideoStream::setProperty(propertyId, data, dataSize); + } +} diff --git a/src/openni2/ColorStream.hpp b/src/openni2/ColorStream.hpp new file mode 100644 index 000000000..62d790fe9 --- /dev/null +++ b/src/openni2/ColorStream.hpp @@ -0,0 +1,67 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 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 "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; + 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); + + // from StreamBase + OniBool isPropertySupported(int propertyId); + OniStatus getProperty(int propertyId, void* data, int* pDataSize); + OniStatus setProperty(int propertyId, const void* data, int dataSize); + }; +} diff --git a/src/openni2/DepthStream.cpp b/src/openni2/DepthStream.cpp new file mode 100644 index 000000000..a1f944eb7 --- /dev/null +++ b/src/openni2/DepthStream.cpp @@ -0,0 +1,251 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 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. + */ + +#define _USE_MATH_DEFINES +#include // for M_PI +#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 < (size_t)dstFrame->width || srcFrame->height < (size_t)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); +} + +OniSensorType DepthStream::getSensorType() const { return ONI_SENSOR_DEPTH; } + +OniImageRegistrationMode DepthStream::getImageRegistrationMode() const { return image_registration_mode; } + +OniStatus DepthStream::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 DepthStream::isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF || mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); } + +OniBool DepthStream::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 DepthStream::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/src/openni2/DepthStream.hpp b/src/openni2/DepthStream.hpp new file mode 100644 index 000000000..9055e13eb --- /dev/null +++ b/src/openni2/DepthStream.hpp @@ -0,0 +1,72 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 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 "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; + 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; + OniStatus setImageRegistrationMode(OniImageRegistrationMode mode); + + // from StreamBase + OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode); + OniBool isPropertySupported(int propertyId); + OniStatus getProperty(int propertyId, void* data, int* pDataSize); + }; +} diff --git a/src/openni2/DeviceDriver.cpp b/src/openni2/DeviceDriver.cpp new file mode 100644 index 000000000..0feea581c --- /dev/null +++ b/src/openni2/DeviceDriver.cpp @@ -0,0 +1,557 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 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 +#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 (unsigned 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), + color(NULL), + depth(NULL), + ir(NULL), + reg(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); + const char* modes_c[] = { + "", + "?depth-size=640x480", + "?depth-size=512x424", + }; + std::vector modes(modes_c, modes_c + 3); + + WriteMessage("Found device " + uri); + + for (unsigned i = 0; i < modes.size(); i++) { + register_uri(uri + modes[i]); + } + +#if 0 + freenect_device* dev; + if (freenect_open_device(m_ctx, &dev, i) == 0) + { + info.usbVendorId = dev->usb_cam.VID; + info.usbProductId = dev->usb_cam.PID; + freenect_close_device(dev); + } + else + { + WriteMessage("Unable to open device to query VID/PID"); + } +#endif // 0 + } + 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("?") != std::string::npos) { + 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("=") != std::string::npos) { + 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/src/openni2/IrStream.cpp b/src/openni2/IrStream.cpp new file mode 100644 index 000000000..106110bf6 --- /dev/null +++ b/src/openni2/IrStream.cpp @@ -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. + */ + +#define _USE_MATH_DEFINES +#include // for M_PI +#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); +} + +OniSensorType IrStream::getSensorType() const { return ONI_SENSOR_IR; } + +// from StreamBase +OniBool IrStream::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 IrStream::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/src/openni2/IrStream.hpp b/src/openni2/IrStream.hpp new file mode 100644 index 000000000..064f85a4c --- /dev/null +++ b/src/openni2/IrStream.hpp @@ -0,0 +1,55 @@ +/* + * 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 "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; + 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); + OniStatus getProperty(int propertyId, void* data, int* pDataSize); + }; +} diff --git a/src/openni2/Registration.cpp b/src/openni2/Registration.cpp new file mode 100644 index 000000000..ef394e832 --- /dev/null +++ b/src/openni2/Registration.cpp @@ -0,0 +1,73 @@ +/* + * 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 +#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); +} + +void Registration::setEnable(bool enable) { enabled = enable; } + +bool Registration::isEnabled() { return enabled; } diff --git a/src/openni2/Registration.hpp b/src/openni2/Registration.hpp new file mode 100644 index 000000000..e14fd8e05 --- /dev/null +++ b/src/openni2/Registration.hpp @@ -0,0 +1,48 @@ +/* + * 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 + +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); + bool isEnabled(); + }; +} diff --git a/src/openni2/Utility.cpp b/src/openni2/Utility.cpp new file mode 100644 index 000000000..5ab7e30a2 --- /dev/null +++ b/src/openni2/Utility.cpp @@ -0,0 +1,58 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 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. +#include +#include "Driver/OniDriverAPI.h" + + +// Oni helpers + +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; +} + +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); +} + +bool operator<(const OniVideoMode& left, const OniVideoMode& right) +{ + return (left.resolutionX * left.resolutionY < right.resolutionX * right.resolutionY); +} + +bool operator<(const OniDeviceInfo& left, const OniDeviceInfo& right) +{ + return (strcmp(left.uri, right.uri) < 0); +} diff --git a/src/openni2/Utility.hpp b/src/openni2/Utility.hpp new file mode 100644 index 000000000..722596779 --- /dev/null +++ b/src/openni2/Utility.hpp @@ -0,0 +1,74 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 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 + +// Oni helpers + +OniVideoMode makeOniVideoMode(OniPixelFormat pixel_format, int resolution_x, int resolution_y, int frames_per_second); + +bool operator==(const OniVideoMode& left, const OniVideoMode& right); + +bool operator<(const OniVideoMode& left, const OniVideoMode& right); + +bool operator<(const OniDeviceInfo& left, const OniDeviceInfo& right); + +// 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/src/openni2/VideoStream.cpp b/src/openni2/VideoStream.cpp new file mode 100644 index 000000000..edae8e72b --- /dev/null +++ b/src/openni2/VideoStream.cpp @@ -0,0 +1,273 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 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 +#include "PS1080.h" +#include "VideoStream.hpp" +#include "Utility.hpp" +#include "Registration.hpp" + +struct ExtractKey +{ + template typename T::first_type + operator()(T pair) const + { + return pair.first; + } +}; + +namespace Freenect2Driver +{ +OniStatus VideoStream::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; +} + +void VideoStream::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 VideoStream::raisePropertyChanged(int propertyId, const void* data, int dataSize) { + if (callPropertyChangedCallback) + StreamBase::raisePropertyChanged(propertyId, data, dataSize); +} + +VideoStream::VideoStream(libfreenect2::Freenect2Device* device, Freenect2Driver::Registration *reg) : + frame_id(1), + device(device), + running(false), + mirroring(false), + reg(reg), + callPropertyChangedCallback(false) + { + // joy of structs + memset(&cropping, 0, sizeof(cropping)); + memset(&video_mode, 0, sizeof(video_mode)); + } +//~VideoStream() { stop(); } + + +OniSensorInfo VideoStream::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 VideoStream::setPropertyChangedCallback(oni::driver::PropertyChangedCallback handler, void* pCookie) { + callPropertyChangedCallback = true; + StreamBase::setPropertyChangedCallback(handler, pCookie); +} + +bool VideoStream::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; + } + //min is a macro with MSVC + #undef min + 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 VideoStream::start() +{ + running = true; + return ONI_STATUS_OK; +} +void VideoStream::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 VideoStream::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; + } +} + +OniStatus VideoStream::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; + } +} +OniStatus VideoStream::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; } +*/ +} diff --git a/src/openni2/VideoStream.hpp b/src/openni2/VideoStream.hpp new file mode 100644 index 000000000..7137199b7 --- /dev/null +++ b/src/openni2/VideoStream.hpp @@ -0,0 +1,83 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 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 "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); + + 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); + void raisePropertyChanged(int propertyId, const void* data, int dataSize); + + public: + VideoStream(libfreenect2::Freenect2Device* device, Freenect2Driver::Registration *reg); + + OniSensorInfo getSensorInfo(); + + void setPropertyChangedCallback(oni::driver::PropertyChangedCallback handler, void* pCookie); + + bool buildFrame(libfreenect2::Frame* lf2Frame); + + OniStatus start(); + void stop(); + + // 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); + + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); + + /* todo : from StreamBase + virtual OniStatus convertDepthToColorCoordinates(StreamBase* colorStream, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY) { return ONI_STATUS_NOT_SUPPORTED; } + */ + }; +}