OpenNI2 driver

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "drivers/OpenNI2-Freenect2Driver/extern/OpenNI2"]
+	path = drivers/OpenNI2-Freenect2Driver/extern/OpenNI2
+	url = https://github.com/occipital/OpenNI2.git

drivers/OpenNI2-Freenect2Driver/CMakeLists.txt

@@ -0,0 +1,36 @@
+##############################################################################
+# OpenNI2-Freenect2Driver
+##############################################################################
+
+set(LIBFREENECT2_DIR ../..)
+set(PROTONECT_DIR ${LIBFREENECT2_DIR}/examples/protonect)
+set(MY_DIR ${LIBFREENECT2_DIR}/drivers/OpenNI2-Freenect2Driver)
+LIST(APPEND CMAKE_MODULE_PATH ${PROTONECT_DIR}/cmake_modules)
+# setup threading
+INCLUDE(SetupLibfreenect2Threading)
+
+file(GLOB HEADERS src/*.hpp src/*.h)
+file(GLOB SOURCES src/*.cpp)
+add_library(Freenect2Driver SHARED ${HEADERS} ${SOURCES})
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unused-function")
+
+set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib/OpenNI2-Freenect2Driver)
+set_target_properties(Freenect2Driver PROPERTIES
+  VERSION ${PROJECT_VER}
+  SOVERSION ${PROJECT_APIVER}
+  OUTPUT_NAME Freenect2Driver)
+
+add_definitions(-DPROJECT_VER="${PROJECT_VER}")
+
+include_directories(${LIBFREENECT2_DIR}/include)
+include_directories(${PROTONECT_DIR}/include)
+include_directories(${MY_DIR}/extern/OpenNI2/Include)
+include_directories(${MY_DIR}/src)
+include_directories(${PROTONECT_DIR}/${LIBFREENECT2_THREADING_INCLUDE_DIR})
+
+FIND_LIBRARY(FREENECT2_LIB freenect2 ${PROTONECT_DIR}/lib)
+target_link_libraries(Freenect2Driver ${FREENECT2_LIB} ${MATH_LIB})
+
+install (TARGETS Freenect2Driver
+  DESTINATION "${PROJECT_LIBRARY_INSTALL_DIR}/OpenNI2-Freenect2Driver")

drivers/OpenNI2-Freenect2Driver/README.md

@@ -0,0 +1,26 @@
+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 OSX. (and on Linux?)
+OpenNI2-Freenect2Driver is derived from OpenNI2-FreenectDriver (https://github.com/OpenKinect/libfreenect/tree/master/OpenNI2-FreenectDriver).
+
+Install
+-------
+1. Download and unpack [OpenNI](http://structure.io/openni) 2.2.0.33 or higher.
+2. Go to the top OpenNI2-Freenect2Driver directory and build it.
+
+        cd /some/where/libfreenect2/OpenNI2-Freenect2Driver
+        cmake .
+        make
+
+3. Copy the driver to your OpenNI2 driver repository. You must first change `Repository` to match your project layout.
+
+        Repository="/example/path/to/Samples/Bin/OpenNI2/Drivers/"
+        cp -L lib/OpenNI2-Freenect2Driver/libFreenect2Driver.{so,dylib} ${Repository}
+
+        # you could instead make a symlink to avoid copying after every build
+        # ln -s lib/OpenNI2-Freenect2Driver/libFreenect2Driver.{so,dylib} ${Repository}
+
+OpenNI2-Freenect2Driver is currently not built with a static libfreenect2, so you might need to include libfreenect2 when deploying.
+You will need to make sure target systems have libusb and all other dependencies also.

drivers/OpenNI2-Freenect2Driver/src/ColorStream.cpp

@@ -0,0 +1,130 @@
+/*
+ * 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 <string>
+#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);
+  pDevice->start();
+}
+
+// Add video modes here as you implement them
+ColorStream::FreenectVideoModeMap ColorStream::getSupportedVideoModes()
+{
+  FreenectVideoModeMap modes;
+  //                    pixelFormat, resolutionX, resolutionY, fps    freenect_video_format, freenect_resolution
+  modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30)] = std::pair<freenect2_video_format, freenect2_resolution>(FREENECT2_VIDEO_RGB, FREENECT2_RESOLUTION_1920x1080);
+  modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30)] = std::pair<freenect2_video_format, freenect2_resolution>(FREENECT2_VIDEO_RGB, FREENECT2_RESOLUTION_1920x1080);
+
+  return modes;
+}
+
+OniStatus ColorStream::setVideoMode(OniVideoMode requested_mode)
+{
+  FreenectVideoModeMap supported_video_modes = getSupportedVideoModes();
+  FreenectVideoModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode);
+  if (matched_mode_iter == supported_video_modes.end())
+    return ONI_STATUS_NOT_SUPPORTED;
+
+  freenect2_video_format format = matched_mode_iter->second.first;
+  freenect2_resolution resolution = matched_mode_iter->second.second;
+
+  video_mode = requested_mode;
+  return ONI_STATUS_OK;
+}
+
+void ColorStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const
+{
+  dstFrame->sensorType = sensor_type;
+  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;
+      }
+    }
+  }
+}

drivers/OpenNI2-Freenect2Driver/src/ColorStream.hpp

@@ -0,0 +1,161 @@
+/*
+ * 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 <algorithm> // for transform()
+#include <cmath> // for M_PI
+#include <map>
+#include "libfreenect2.h"
+#include "libfreenect2/libfreenect2.hpp"
+#include "Driver/OniDriverAPI.h"
+#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, std::pair<freenect2_video_format, freenect2_resolution> > FreenectVideoModeMap;
+    static const OniSensorType sensor_type = ONI_SENSOR_COLOR;
+
+    static FreenectVideoModeMap getSupportedVideoModes();
+    OniStatus setVideoMode(OniVideoMode requested_mode);
+    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() { }
+
+    static OniSensorInfo getSensorInfo()
+    {
+      FreenectVideoModeMap supported_modes = getSupportedVideoModes();
+      OniVideoMode* modes = new OniVideoMode[supported_modes.size()];
+      std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey());
+      OniSensorInfo sensors = { sensor_type, static_cast<int>(supported_modes.size()), modes };
+      return sensors;
+    }
+
+    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<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 setProperty(int propertyId, const void* data, int dataSize)
+    {
+      switch (propertyId)
+      {
+        default:
+          return VideoStream::setProperty(propertyId, data, dataSize);      
+      }
+    }
+  };
+}