OpenNI2-Freenect2Driver

Makefile

@@ -37,7 +37,8 @@ ALL_DRIVERS = \
 	Source/Drivers/DummyDevice   \
 	Source/Drivers/PS1080 \
 	Source/Drivers/PSLink \
-	Source/Drivers/OniFile
+	Source/Drivers/OniFile \
+	Source/Drivers/Libfreenect2
 
 # list all wrappers
 ALL_WRAPPERS = \
@@ -48,7 +49,7 @@ ALL_WRAPPERS = \
 ALL_TOOLS = \
 	Source/Drivers/PS1080/PS1080Console \
 	Source/Drivers/PSLink/PSLinkConsole
-	
+
 # list all core projects
 ALL_CORE_PROJS = \
 	$(XNLIB)  \
@@ -152,7 +153,7 @@ $(FINAL_DIR):
 doc:
 	Source/Documentation/Runme.py
 	rm -f Source/Documentation/html/*.md5
-	
+
 release: | all doc $(FINAL_DIR)
 	Packaging/Harvest.py Packaging/$(PRODUCT_STRING) $(PLATFORM)
 	cd Packaging; tar -cjf Final/$(PRODUCT_STRING).tar.bz2 $(PRODUCT_STRING)

Packaging/Harvest.py

@@ -92,6 +92,7 @@ def copyRedistFiles(self, targetDir):
         self.copySharedObject(binDriversDir, 'OniFile', targetDriversDir)
         self.copySharedObject(binDriversDir, 'PS1080', targetDriversDir)
         self.copySharedObject(binDriversDir, 'PSLink', targetDriversDir)
+        self.copySharedObject(binDriversDir, 'Libfreenect2', targetDriversDir)
         shutil.copy(os.path.join(self.rootDir, 'Config', 'OpenNI2', 'Drivers', 'PS1080.ini'), targetDriversDir)
         shutil.copy(os.path.join(self.rootDir, 'Config', 'OpenNI2', 'Drivers', 'PSLink.ini'), targetDriversDir)
         if self.osName == 'Windows':

Source/Drivers/Libfreenect2/LF2Common.h

@@ -0,0 +1,10 @@
+#ifndef LF2COMMON_H
+#define LF2COMMON_H
+
+#define LF2_DEPTH_HORIZONTAL_FOV 70.6
+#define LF2_DEPTH_VERTICAL_FOV 60
+
+#define LF2_COLOR_HORIZONTAL_FOV 84.1
+#define LF2_COLOR_VERTICAL_FOV 53.8
+
+#endif // LF2COMMON_H

Source/Drivers/Libfreenect2/LF2DepthStream.cpp

@@ -0,0 +1,96 @@
+#include "LF2DepthStream.h"
+#include <XnOS.h>
+#include "LF2Common.h"
+#include <XnMath.h>
+
+using namespace LF2;
+
+int
+LF2DepthStream::BuildFrame(libfreenect2::Frame* frame_in,OniFrame* frame_out)
+{
+  // 1 DepthPixel is 2 byte
+  frame_out->dataSize = frame_in->width * frame_in->height * 2;
+
+  frame_out->data = xnOSMalloc (frame_out->dataSize);
+
+  const float* const in_array = (float*) frame_in->data;
+  uint16_t* const out_array = (uint16_t*) frame_out->data;
+  unsigned int t = 0;
+  for (unsigned int y = 0; y < frame_in->height; y++)
+	{
+      for (unsigned int x = 0; x < frame_in->width; x++)
+		{
+          out_array[t] = in_array[t];
+          ++t;
+        }
+    }
+
+
+  frame_out->frameIndex = frame_in->sequence;
+  frame_out->sensorType = ONI_SENSOR_DEPTH;
+
+  frame_out->videoMode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM;
+  frame_out->videoMode.resolutionX = frame_in->width;
+  frame_out->videoMode.resolutionY = frame_in->height;
+  frame_out->videoMode.fps = 30;
+
+  frame_out->width = frame_in->width;
+  frame_out->height = frame_in->height;
+
+  frame_out->cropOriginX = frame_out->cropOriginY = 0;
+  frame_out->croppingEnabled = FALSE;
+
+  frame_out->sensorType = ONI_SENSOR_DEPTH;
+  frame_out->stride = frame_in->width*sizeof(OniDepthPixel);
+  frame_out->timestamp = frame_in->sequence*33369;
+
+  return 1;
+}
+
+OniStatus 
+LF2DepthStream::getProperty(int propertyId, void* data, int* pDataSize)
+{
+  OniStatus status = ONI_STATUS_NOT_SUPPORTED;
+
+  switch (propertyId)
+    {
+    case ONI_STREAM_PROPERTY_HORIZONTAL_FOV:
+      {
+        float* val = (float*)data;
+        XnDouble tmp;
+        tmp =  LF2_DEPTH_HORIZONTAL_FOV * xnl::Math::DTR;
+        *val = (float)tmp;
+        status = ONI_STATUS_OK;
+        break;
+      }
+    case ONI_STREAM_PROPERTY_VERTICAL_FOV:
+      {
+        float* val = (float*)data;
+        XnDouble tmp;
+        tmp =  LF2_DEPTH_VERTICAL_FOV * xnl::Math::DTR;
+        *val = (float)tmp;
+        status = ONI_STATUS_OK;
+        break;
+      }		
+	case ONI_STREAM_PROPERTY_MAX_VALUE:
+      if (*pDataSize != sizeof(int))
+		{
+          return ONI_STATUS_BAD_PARAMETER;
+		}
+
+      *(int*)data = 10000;
+      return ONI_STATUS_OK;
+	case ONI_STREAM_PROPERTY_MIN_VALUE:
+      if (*pDataSize != sizeof(int))
+		{
+          return ONI_STATUS_BAD_PARAMETER;
+		}
+
+      *(int*)data = 0;
+      return ONI_STATUS_OK;
+    default:
+      return LF2Stream::getProperty(propertyId, data, pDataSize);
+    }
+
+  return status;  
+}

Source/Drivers/Libfreenect2/LF2DepthStream.h

@@ -0,0 +1,49 @@
+#ifndef LF2DEPTH_STREAM_H
+#define LF2DEPTH_STREAM_H
+
+#include "LF2Stream.h"
+
+namespace LF2 {
+  class LF2DepthStream :
+    public LF2Stream
+  {
+  public:
+    LF2DepthStream (libfreenect2::Freenect2Device * f2dev) : LF2Stream (f2dev) { 
+      m_videoMode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM;
+      m_videoMode.resolutionX = 512;
+      m_videoMode.resolutionY = 424;
+      m_videoMode.fps = 30;
+
+      m_frameType = libfreenect2::Frame::Depth;
+    }
+
+    ~LF2DepthStream ()
+    {
+      stop ();
+    }
+
+    // function for OpenNI2
+    OniStatus
+    start ()
+    {
+      m_f2dev->setIrAndDepthFrameListener (this);
+      m_f2dev->start();
+      return ONI_STATUS_OK;
+    }
+
+    void
+    stop ()
+    {
+      m_f2dev->setIrAndDepthFrameListener (NULL);
+    }
+
+	OniStatus
+    getProperty(int propertyId, void* data, int* pDataSize);
+
+  protected:
+    int
+    BuildFrame (libfreenect2::Frame*,OniFrame*);
+  };
+}
+
+#endif // LF2DEPTH_STREAM_H

Source/Drivers/Libfreenect2/LF2Device.cpp

@@ -0,0 +1,37 @@
+#include "XnLib.h"
+#include "LF2Device.h"
+#include "LF2DepthStream.h"
+#include "LF2ImageStream.h"
+#include "LF2IrStream.h"
+
+using namespace LF2;
+using namespace oni::driver;
+
+StreamBase*
+LF2Device::createStream (OniSensorType sensorType)
+{
+  if (sensorType == ONI_SENSOR_DEPTH)
+    {
+      LF2DepthStream* pDepth = XN_NEW(LF2DepthStream,m_f2dev);
+      return pDepth;
+    }
+  else if (sensorType == ONI_SENSOR_COLOR)
+    {
+      LF2ImageStream* pImage = XN_NEW(LF2ImageStream,m_f2dev);
+      return pImage;
+    }
+  else if (sensorType == ONI_SENSOR_IR)
+    {
+      LF2IrStream* pIr = XN_NEW(LF2IrStream,m_f2dev);
+      return pIr;
+    }
+
+  return NULL;
+}
+
+void
+LF2Device::destroyStream (StreamBase* pStream)
+{
+  XN_DELETE (pStream);
+}
+

Source/Drivers/Libfreenect2/LF2Device.h

@@ -0,0 +1,74 @@
+#ifndef LF2DEVICE_H
+#define LF2DEVICE_H
+
+#include <string>
+#include "libfreenect2/libfreenect2.hpp"
+#include "Driver/OniDriverAPI.h"
+#include "LF2Driver.h"
+
+
+namespace LF2 {
+  class LF2Driver;
+  class LF2Device : 
+    public oni::driver::DeviceBase 
+  {
+  public :
+    LF2Device(libfreenect2::Freenect2Device* f2dev,LF2Driver* driver)
+    {
+      static const int DEFAULT_FPS = 30;
+      m_f2dev = f2dev;
+      m_driver = driver;
+      // this is actually 3. but now color and ir are not supprted yet.
+
+      m_sensors[0].sensorType = ONI_SENSOR_DEPTH;
+      m_sensors[0].numSupportedVideoModes = 1;
+      m_sensors[0].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 1);
+      m_sensors[0].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM;
+      m_sensors[0].pSupportedVideoModes[0].fps = DEFAULT_FPS;
+      m_sensors[0].pSupportedVideoModes[0].resolutionX = 512;
+      m_sensors[0].pSupportedVideoModes[0].resolutionY = 424;
+
+      m_sensors[1].sensorType = ONI_SENSOR_COLOR;
+      m_sensors[1].numSupportedVideoModes = 2;
+      m_sensors[1].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 1);
+      m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888;
+      m_sensors[1].pSupportedVideoModes[0].fps         = DEFAULT_FPS;
+      m_sensors[1].pSupportedVideoModes[0].resolutionX = 1920;
+      m_sensors[1].pSupportedVideoModes[0].resolutionY = 1080;
+
+      m_sensors[2].sensorType = ONI_SENSOR_IR;
+      m_sensors[2].numSupportedVideoModes = 1;
+      m_sensors[2].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 1);
+      m_sensors[2].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_GRAY16;
+      m_sensors[2].pSupportedVideoModes[0].fps = DEFAULT_FPS;
+      m_sensors[2].pSupportedVideoModes[0].resolutionX = 512;
+      m_sensors[2].pSupportedVideoModes[0].resolutionY = 424;
+
+    }
+
+	OniStatus
+    getSensorInfoList(OniSensorInfo** pSensors, int* numSensors)
+    {
+      *numSensors = m_numSensors;
+      *pSensors = m_sensors;
+      return ONI_STATUS_OK;
+    }
+
+    oni::driver::StreamBase* 
+    createStream (OniSensorType);
+
+    void
+    destroyStream (oni::driver::StreamBase*);
+  protected:
+    static const int m_numSensors = 3;
+    OniSensorInfo m_sensors[m_numSensors];
+    libfreenect2::Freenect2Device* m_f2dev;
+    LF2Driver* m_driver;
+  };
+}
+
+#endif // LF2DEVICE_H
+
+
+
+