OpenNI2 driver cleaned up

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()

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.

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)

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 <cmath> // 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, &registered);
+
+        copyFrame(static_cast<uint8_t*>(registered.data), srcX, srcY, registered.width * registered.bytes_per_pixel, 
+                  static_cast<uint8_t*>(dstFrame->data), dstX, dstY, dstFrame->stride, 
+                  width, height, mirroring);
+      } else {
+        copyFrame(static_cast<uint8_t*>(srcFrame->data), srcX, srcY, srcFrame->width * srcFrame->bytes_per_pixel, 
+                  static_cast<uint8_t*>(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<float*>(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<float*>(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<OniBool*>(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<OniBool*>(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);
+  }
+}