From 4eb4187d5379d473690a1880362a1bd7fa7f65d9 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Sat, 29 Dec 2012 17:19:45 -0500 Subject: [PATCH 01/18] Initial commit; freenectDevice work in progress --- Makefile | 5 +- Source/Drivers/Kinect/freenectDevice.cpp | 218 +++++++++++++++++++++++ 2 files changed, 221 insertions(+), 2 deletions(-) create mode 100644 Source/Drivers/Kinect/freenectDevice.cpp diff --git a/Makefile b/Makefile index d04a48b1..d738929a 100644 --- a/Makefile +++ b/Makefile @@ -20,9 +20,10 @@ XNLIB = ThirdParty/PSCommon/XnLib/Source # list all drivers ALL_DRIVERS = \ - Source/Drivers/DummyDevice \ + Source/Drivers/DummyDevice \ Source/Drivers/PS1080 \ - Source/Drivers/OniFile + Source/Drivers/OniFile \ + Source/Drivers/Kinect # list all tools ALL_TOOLS = \ diff --git a/Source/Drivers/Kinect/freenectDevice.cpp b/Source/Drivers/Kinect/freenectDevice.cpp new file mode 100644 index 00000000..60b10f22 --- /dev/null +++ b/Source/Drivers/Kinect/freenectDevice.cpp @@ -0,0 +1,218 @@ +/** +* freenectDevice.cpp +* Copyright 2012 Benn Snyder +* +* OpenNI 2.x Alpha +* Copyright (C) 2012 PrimeSense Ltd. +* +* This file is part of OpenNI. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +* +*****************************************************************************/ +/* + * This implementation of KinectDevice serves as a bridge to libfreenect.h + * It allows OpenNI 2.x to use libfreenect for Kinect and Kinect for Windows (k4w) + * This effectively brings Kinect support to OpenNI2 on Linux; please test on OSX! + * + * + */ + + +#include "KinectDevice.h" +#include "DepthKinectStream.h" +#include "ColorKinectStream.h" +#include "IRKinectStream.h" +#include +#include "NuiApi.h" +#include "XnLog.h" + +using namespace kinect_device; +using namespace oni::driver; +#define DEFAULT_FPS 30 +KinectDevice::KinectDevice(INuiSensor * pNuiSensor):m_pDepthStream(NULL), m_pColorStream(NULL),m_pNuiSensor(pNuiSensor) +{ + m_numSensors = 3; + + m_sensors[0].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); + m_sensors[0].sensorType = ONI_SENSOR_DEPTH; + m_sensors[0].numSupportedVideoModes = 3; + 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 = 640; + m_sensors[0].pSupportedVideoModes[0].resolutionY = 480; + + m_sensors[0].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + m_sensors[0].pSupportedVideoModes[1].fps = DEFAULT_FPS; + m_sensors[0].pSupportedVideoModes[1].resolutionX = 320; + m_sensors[0].pSupportedVideoModes[1].resolutionY = 240; + + m_sensors[0].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + m_sensors[0].pSupportedVideoModes[2].fps = DEFAULT_FPS; + m_sensors[0].pSupportedVideoModes[2].resolutionX = 80; + m_sensors[0].pSupportedVideoModes[2].resolutionY = 60; + + m_sensors[1].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); + m_sensors[1].sensorType = ONI_SENSOR_COLOR; + m_sensors[1].numSupportedVideoModes = 3; + m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; + m_sensors[1].pSupportedVideoModes[0].fps = 12; + m_sensors[1].pSupportedVideoModes[0].resolutionX = 1280; + m_sensors[1].pSupportedVideoModes[0].resolutionY = 960; + + m_sensors[1].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_RGB888; + m_sensors[1].pSupportedVideoModes[1].fps = DEFAULT_FPS; + m_sensors[1].pSupportedVideoModes[1].resolutionX = 640; + m_sensors[1].pSupportedVideoModes[1].resolutionY = 480; + + m_sensors[1].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_YUV422; + m_sensors[1].pSupportedVideoModes[2].fps = 15; + m_sensors[1].pSupportedVideoModes[2].resolutionX = 640; + m_sensors[1].pSupportedVideoModes[2].resolutionY = 480; + + m_sensors[2].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); + m_sensors[2].sensorType = ONI_SENSOR_IR; + m_sensors[2].numSupportedVideoModes = 3; + m_sensors[2].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; + m_sensors[2].pSupportedVideoModes[0].fps = DEFAULT_FPS; + m_sensors[2].pSupportedVideoModes[0].resolutionX = 640; + m_sensors[2].pSupportedVideoModes[0].resolutionY = 480; + + m_sensors[2].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_GRAY16; + m_sensors[2].pSupportedVideoModes[1].fps = DEFAULT_FPS; + m_sensors[2].pSupportedVideoModes[1].resolutionX = 640; + m_sensors[2].pSupportedVideoModes[1].resolutionY = 480; + + m_sensors[2].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_GRAY8; + m_sensors[2].pSupportedVideoModes[2].fps = DEFAULT_FPS; + m_sensors[2].pSupportedVideoModes[2].resolutionX = 640; + m_sensors[2].pSupportedVideoModes[2].resolutionY = 480; +} + +KinectDevice::~KinectDevice() +{ + if (m_pNuiSensor) + m_pNuiSensor->NuiShutdown(); + + if (m_pDepthStream != NULL) + XN_DELETE(m_pDepthStream); + + if (m_pColorStream!= NULL) + XN_DELETE(m_pColorStream); + + if (m_pNuiSensor) + m_pNuiSensor->Release(); +} + +OniStatus KinectDevice::getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) +{ + *numSensors = m_numSensors; + *pSensors = m_sensors; + return ONI_STATUS_OK; +} + +StreamBase* KinectDevice::createStream(OniSensorType sensorType) +{ + BaseKinectStream* pImage = NULL; + + if (sensorType == ONI_SENSOR_COLOR ) + { + if (m_pColorStream == NULL) + { + m_pColorStream = XN_NEW(KinectStreamImpl, m_pNuiSensor, sensorType); + pImage = XN_NEW(ColorKinectStream, m_pColorStream); + } + else + { + if (m_pColorStream->getSensorType() != sensorType) + { + if (!m_pColorStream->isRunning()) + { + m_pColorStream->setSensorType(sensorType); + pImage = XN_NEW(ColorKinectStream, m_pColorStream); + } + } + else + { + pImage = XN_NEW(ColorKinectStream, m_pColorStream); + } + } + } + else if (sensorType == ONI_SENSOR_DEPTH ) + { + if (m_pDepthStream == NULL) + { + m_pDepthStream = XN_NEW(KinectStreamImpl, m_pNuiSensor, sensorType); + } + pImage = XN_NEW(DepthKinectStream, m_pDepthStream); + } + if (sensorType == ONI_SENSOR_IR ) + { + if (m_pColorStream == NULL) + { + m_pColorStream = XN_NEW(KinectStreamImpl, m_pNuiSensor, sensorType); + pImage = XN_NEW(IRKinectStream, m_pColorStream); + } + else + { + if (m_pColorStream->getSensorType() != sensorType) + { + if (!m_pColorStream->isRunning()) + { + m_pColorStream->setSensorType(sensorType); + pImage = XN_NEW(IRKinectStream, m_pColorStream); + } + } + else + { + pImage = XN_NEW(IRKinectStream, m_pColorStream); + } + } + } + return pImage; +} + +void kinect_device::KinectDevice::destroyStream(oni::driver::StreamBase* pStream) +{ + XN_DELETE(pStream); +} + +OniStatus KinectDevice::setProperty(int propertyId, const void* data, int dataSize) +{ + return ONI_STATUS_NOT_IMPLEMENTED; +} + +OniStatus KinectDevice::getProperty(int propertyId, void* data, int* pDataSize) +{ + return ONI_STATUS_NOT_IMPLEMENTED; +} + +OniBool KinectDevice::isPropertySupported(int propertyId) +{ + return ONI_STATUS_NOT_IMPLEMENTED; +} + +OniBool KinectDevice::isCommandSupported(int commandId) +{ + return ONI_STATUS_NOT_IMPLEMENTED; +} + +OniStatus KinectDevice::tryManualTrigger() +{ + return ONI_STATUS_NOT_IMPLEMENTED; +} + +void KinectDevice::notifyAllProperties() +{ + return; +} From 10540aac59e7c284812c1ee7b056797a1747eced Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Sat, 29 Dec 2012 18:46:26 -0500 Subject: [PATCH 02/18] Moved to a new directory --- Makefile | 1 + Source/Drivers/freenect/Makefile | 30 +++++++++++++++++++ .../{Kinect => freenect}/freenectDevice.cpp | 16 +++++----- 3 files changed, 40 insertions(+), 7 deletions(-) create mode 100644 Source/Drivers/freenect/Makefile rename Source/Drivers/{Kinect => freenect}/freenectDevice.cpp (96%) diff --git a/Makefile b/Makefile index d738929a..5f7af1ec 100644 --- a/Makefile +++ b/Makefile @@ -97,6 +97,7 @@ Source/Drivers/DummyDevice: $(OPENNI) $(XNLIB) Source/Drivers/RawDevice: $(OPENNI) $(XNLIB) Source/Drivers/PS1080: $(OPENNI) $(XNLIB) Source/Drivers/OniFile: $(OPENNI) $(XNLIB) +Source/Drivers/freenect: $(XNLIB) Source/Tools/NiViewer: $(OPENNI) $(XNLIB) diff --git a/Source/Drivers/freenect/Makefile b/Source/Drivers/freenect/Makefile new file mode 100644 index 00000000..d559710d --- /dev/null +++ b/Source/Drivers/freenect/Makefile @@ -0,0 +1,30 @@ +include ../../../ThirdParty/PSCommon/BuildSystem/CommonDefs.mak + +BIN_DIR = ../../../Bin + +INC_DIRS = \ + ../../../Include \ + ../../../ThirdParty/PSCommon/XnLib/Include + +SRC_FILES = \ + freenectDevice.cpp + +ifeq ("$(OSTYPE)","Darwin") + INC_DIRS += /opt/local/include + LIB_DIRS += /opt/local/lib + LDFLAGS += -framework CoreFoundation -framework IOKit +endif + +LIB_NAME = freenectDriver + +LIB_DIRS = ../../../ThirdParty/PSCommon/XnLib/Bin/$(PLATFORM)-$(CFG) +USED_LIBS = XnLib dl pthread +ifneq ("$(OSTYPE)","Darwin") + USED_LIBS += rt +endif + +CFLAGS += -Wall + +OUT_DIR := $(OUT_DIR)/OpenNI2/Drivers + +include ../../../ThirdParty/PSCommon/BuildSystem/CommonCppMakefile diff --git a/Source/Drivers/Kinect/freenectDevice.cpp b/Source/Drivers/freenect/freenectDevice.cpp similarity index 96% rename from Source/Drivers/Kinect/freenectDevice.cpp rename to Source/Drivers/freenect/freenectDevice.cpp index 60b10f22..d9219cc2 100644 --- a/Source/Drivers/Kinect/freenectDevice.cpp +++ b/Source/Drivers/freenect/freenectDevice.cpp @@ -22,21 +22,23 @@ *****************************************************************************/ /* * This implementation of KinectDevice serves as a bridge to libfreenect.h - * It allows OpenNI 2.x to use libfreenect for Kinect and Kinect for Windows (k4w) + * It allows OpenNI 2.x to use libfreenect as a device for Kinect and Kinect for Windows (k4w) * This effectively brings Kinect support to OpenNI2 on Linux; please test on OSX! * * */ -#include "KinectDevice.h" -#include "DepthKinectStream.h" -#include "ColorKinectStream.h" -#include "IRKinectStream.h" -#include -#include "NuiApi.h" +#include "Drivers/OniDriverAPI.h" + + +//#include "KinectDevice.h" +//#include "DepthKinectStream.h" +//#include "ColorKinectStream.h" +//#include "IRKinectStream.h" #include "XnLog.h" + using namespace kinect_device; using namespace oni::driver; #define DEFAULT_FPS 30 From 7f95ab1edf5fd6b6edb2693406e1437d2ce8fb1c Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Sat, 29 Dec 2012 19:08:53 -0500 Subject: [PATCH 03/18] Started FreenectStream --- ...{freenectDevice.cpp => FreenectDevice.cpp} | 5 +- Source/Drivers/freenect/FreenectStream.cpp | 179 ++++++++++++++++++ Source/Drivers/freenect/FreenectStream.h | 68 +++++++ Source/Drivers/freenect/Makefile | 2 +- 4 files changed, 250 insertions(+), 4 deletions(-) rename Source/Drivers/freenect/{freenectDevice.cpp => FreenectDevice.cpp} (97%) create mode 100644 Source/Drivers/freenect/FreenectStream.cpp create mode 100644 Source/Drivers/freenect/FreenectStream.h diff --git a/Source/Drivers/freenect/freenectDevice.cpp b/Source/Drivers/freenect/FreenectDevice.cpp similarity index 97% rename from Source/Drivers/freenect/freenectDevice.cpp rename to Source/Drivers/freenect/FreenectDevice.cpp index d9219cc2..02746433 100644 --- a/Source/Drivers/freenect/freenectDevice.cpp +++ b/Source/Drivers/freenect/FreenectDevice.cpp @@ -21,9 +21,8 @@ * *****************************************************************************/ /* - * This implementation of KinectDevice serves as a bridge to libfreenect.h - * It allows OpenNI 2.x to use libfreenect as a device for Kinect and Kinect for Windows (k4w) - * This effectively brings Kinect support to OpenNI2 on Linux; please test on OSX! + * This implementation of KinectDevice serves as a bridge to libfreenect + * It brings Kinect and Kinect for Windows (k4w) support to OpenNI 2.x on Linux; please test on OSX! * * */ diff --git a/Source/Drivers/freenect/FreenectStream.cpp b/Source/Drivers/freenect/FreenectStream.cpp new file mode 100644 index 00000000..f9baf465 --- /dev/null +++ b/Source/Drivers/freenect/FreenectStream.cpp @@ -0,0 +1,179 @@ +#include "BaseKinectStream.h" +#include "KinectStreamImpl.h" +#include +#include "XnHash.h" +#include "XnEvent.h" +#include "XnPlatform.h" +#include "NuiApi.h" +#include "PS1080.h" +#include "XnMath.h" + +using namespace oni::driver; +using namespace kinect_device; + +BaseKinectStream::BaseKinectStream(KinectStreamImpl* pStreamImpl): + m_pStreamImpl(pStreamImpl) +{ + m_running = false; + pStreamImpl->addStream(this); +} + +BaseKinectStream::~BaseKinectStream() +{ + destroy(); +} + +OniStatus BaseKinectStream::start() +{ + OniStatus status = m_pStreamImpl->start(); + if (status == ONI_STATUS_OK) + m_running = TRUE; + return status; +} + +void BaseKinectStream::stop() +{ + m_running = FALSE; + m_pStreamImpl->stop(); +} + +void BaseKinectStream::destroy() +{ + stop(); + m_pStreamImpl->removeStream(this); +} + +OniStatus BaseKinectStream::getProperty(int propertyId, void* data, int* pDataSize) +{ + OniStatus status = ONI_STATUS_NOT_SUPPORTED; + switch (propertyId) + { + case ONI_STREAM_PROPERTY_CROPPING: + status = ONI_STATUS_NOT_IMPLEMENTED; + break; + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + { + float* val = (float*)data; + XnDouble tmp; + if (m_videoMode.resolutionX == 640) + tmp = NUI_CAMERA_COLOR_NOMINAL_HORIZONTAL_FOV * xnl::Math::DTR; + else + tmp = NUI_CAMERA_DEPTH_NOMINAL_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; + if (m_videoMode.resolutionY == 480) + tmp = NUI_CAMERA_COLOR_NOMINAL_VERTICAL_FOV * xnl::Math::DTR; + else + tmp = NUI_CAMERA_DEPTH_NOMINAL_VERTICAL_FOV * xnl::Math::DTR; + *val = (float)tmp; + status = ONI_STATUS_OK; + break; + } + case ONI_STREAM_PROPERTY_VIDEO_MODE: + { + if (*pDataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); + status = ONI_STATUS_ERROR; + } + else + { + status = GetVideoMode((OniVideoMode*)data); + } + + break; + } + default: + status = ONI_STATUS_NOT_SUPPORTED; + break; + } + + return status; +} + +OniStatus BaseKinectStream::setProperty(int propertyId, const void* data, int dataSize) +{ + OniStatus status = ONI_STATUS_NOT_SUPPORTED; + if (propertyId == ONI_STREAM_PROPERTY_VIDEO_MODE) + { + if (dataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); + status = ONI_STATUS_ERROR; + } + status = SetVideoMode((OniVideoMode*)data); + } + return status; +} + +OniBool BaseKinectStream::isPropertySupported(int propertyId) +{ + OniBool status = FALSE; + switch (propertyId) + { + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + case ONI_STREAM_PROPERTY_VIDEO_MODE: + status = TRUE; + default: + status = FALSE; + break; + } + return status; +} + +void BaseKinectStream::notifyAllProperties() +{ + XnFloat nDouble; + int size = sizeof(nDouble); + getProperty(ONI_STREAM_PROPERTY_HORIZONTAL_FOV, &nDouble, &size); + raisePropertyChanged(ONI_STREAM_PROPERTY_HORIZONTAL_FOV, &nDouble, size); + + getProperty(ONI_STREAM_PROPERTY_VERTICAL_FOV, &nDouble, &size); + raisePropertyChanged(ONI_STREAM_PROPERTY_VERTICAL_FOV, &nDouble, size); + + OniVideoMode videoMode; + size = sizeof(videoMode); + + getProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &videoMode, &size); + raisePropertyChanged(ONI_STREAM_PROPERTY_VIDEO_MODE, &videoMode, size); +} + +OniStatus BaseKinectStream::SetVideoMode(OniVideoMode* videoMode) +{ + if (!m_pStreamImpl->isRunning()) + { + m_videoMode = *videoMode; + m_pStreamImpl->setVideoMode(videoMode); + return ONI_STATUS_OK; + } + + return ONI_STATUS_NOT_SUPPORTED; +} + +OniStatus BaseKinectStream::GetVideoMode(OniVideoMode* pVideoMode) +{ + *pVideoMode = m_videoMode; + return ONI_STATUS_OK; +} + +void BaseKinectStream::addRefToFrame(OniDriverFrame* pFrame) +{ + ++((KinectStreamFrameCookie*)pFrame->pDriverCookie)->refCount; +} + +void BaseKinectStream::releaseFrame(OniDriverFrame* pFrame) +{ + if (0 == --((KinectStreamFrameCookie*)pFrame->pDriverCookie)->refCount) + { + xnOSFree(pFrame->pDriverCookie); + xnOSFreeAligned(pFrame->frame.data); + xnOSFree(pFrame); + } +} diff --git a/Source/Drivers/freenect/FreenectStream.h b/Source/Drivers/freenect/FreenectStream.h new file mode 100644 index 00000000..9bb96d55 --- /dev/null +++ b/Source/Drivers/freenect/FreenectStream.h @@ -0,0 +1,68 @@ +#ifndef _FREENECT_STREAM_H_ +#define _FREENECT_STREAM_H_ + +#include "Drivers/OniDriverAPI.h" +#include "XnLib.h" + +namespace kinect_device { + +class KinectStreamImpl; + +static const int KINECT_RESOLUTION_X_80 = 80; +static const int KINECT_RESOLUTION_Y_60 = 60; +static const int KINECT_RESOLUTION_X_320 = 320; +static const int KINECT_RESOLUTION_Y_240 = 240; +static const int KINECT_RESOLUTION_X_640 = 640; +static const int KINECT_RESOLUTION_Y_480 = 480; +static const int KINECT_RESOLUTION_X_1280 = 1280; +static const int KINECT_RESOLUTION_Y_960 = 960; + +typedef struct +{ + int refCount; +} KinectStreamFrameCookie; + +class BaseKinectStream : public oni::driver::StreamBase +{ +public: + BaseKinectStream(KinectStreamImpl* pStreamImpl); + + virtual ~BaseKinectStream(); + + OniStatus BaseKinectStream::start(); + + void BaseKinectStream::stop(); + + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); + + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); + + virtual OniBool isPropertySupported(int propertyId); + + virtual void notifyAllProperties(); + + virtual OniStatus SetVideoMode(OniVideoMode* pVideoMode); + + virtual OniStatus GetVideoMode(OniVideoMode* pVideoMode); + + void addRefToFrame(OniDriverFrame* pFrame); + + void releaseFrame(OniDriverFrame* pFrame); + + bool isRunning() { return m_running; } + + virtual void frameReceived(NUI_IMAGE_FRAME& imageFrame, NUI_LOCKED_RECT &LockedRect) = 0; + + +protected: + KinectStreamImpl *m_pStreamImpl; + OniVideoMode m_videoMode; + + bool m_running; + +private: + void destroy(); + +}; +} // namespace kinect_device +#endif //_FREENECT_STREAM_H_ diff --git a/Source/Drivers/freenect/Makefile b/Source/Drivers/freenect/Makefile index d559710d..7fad9d0f 100644 --- a/Source/Drivers/freenect/Makefile +++ b/Source/Drivers/freenect/Makefile @@ -7,7 +7,7 @@ INC_DIRS = \ ../../../ThirdParty/PSCommon/XnLib/Include SRC_FILES = \ - freenectDevice.cpp + *.cpp ifeq ("$(OSTYPE)","Darwin") INC_DIRS += /opt/local/include From 65b901ad8a5e8080ab65e67c93d9d6a961135e15 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Tue, 1 Jan 2013 04:18:02 -0500 Subject: [PATCH 04/18] It compiles! Still lots to do. --- Makefile | 8 +- Samples/freenectTest/Makefile | 20 ++ Samples/freenectTest/freenectTest.cpp | 191 ++++++++++++++ Source/Drivers/freenect/FreenectDepthStream.h | 82 ++++++ ...reenectDevice.cpp => FreenectDeviceNI.cpp} | 14 +- Source/Drivers/freenect/FreenectDeviceNI.h | 152 +++++++++++ Source/Drivers/freenect/FreenectDriver.cpp | 240 ++++++++++++++++++ Source/Drivers/freenect/FreenectImageStream.h | 38 +++ Source/Drivers/freenect/FreenectStream.cpp | 179 ------------- Source/Drivers/freenect/FreenectStream.h | 211 +++++++++++---- Source/Drivers/freenect/Makefile | 5 +- 11 files changed, 903 insertions(+), 237 deletions(-) create mode 100644 Samples/freenectTest/Makefile create mode 100644 Samples/freenectTest/freenectTest.cpp create mode 100644 Source/Drivers/freenect/FreenectDepthStream.h rename Source/Drivers/freenect/{FreenectDevice.cpp => FreenectDeviceNI.cpp} (96%) create mode 100644 Source/Drivers/freenect/FreenectDeviceNI.h create mode 100644 Source/Drivers/freenect/FreenectDriver.cpp create mode 100644 Source/Drivers/freenect/FreenectImageStream.h delete mode 100644 Source/Drivers/freenect/FreenectStream.cpp diff --git a/Makefile b/Makefile index 5f7af1ec..dabe1c88 100644 --- a/Makefile +++ b/Makefile @@ -23,7 +23,7 @@ ALL_DRIVERS = \ Source/Drivers/DummyDevice \ Source/Drivers/PS1080 \ Source/Drivers/OniFile \ - Source/Drivers/Kinect + Source/Drivers/freenect # list all tools ALL_TOOLS = \ @@ -42,7 +42,8 @@ CORE_SAMPLES = \ Samples/EventBasedRead \ Samples/MultipleStreamRead \ Samples/MWClosestPoint \ - Samples/MWClosestPointApp + Samples/MWClosestPointApp \ + Samples/freenectTest ifeq "$(GLUT_SUPPORTED)" "1" CORE_SAMPLES += \ @@ -97,7 +98,7 @@ Source/Drivers/DummyDevice: $(OPENNI) $(XNLIB) Source/Drivers/RawDevice: $(OPENNI) $(XNLIB) Source/Drivers/PS1080: $(OPENNI) $(XNLIB) Source/Drivers/OniFile: $(OPENNI) $(XNLIB) -Source/Drivers/freenect: $(XNLIB) +Source/Drivers/freenect: $(XNLIB) Source/Tools/NiViewer: $(OPENNI) $(XNLIB) @@ -106,6 +107,7 @@ Samples/EventBasedRead: $(OPENNI) Samples/MultipleStreamRead: $(OPENNI) Samples/MWClosestPoint: $(OPENNI) Samples/MWClosestPointApp: $(OPENNI) Samples/MWClosestPoint +Samples/freenectTest: $(OPENNI) Source/Drivers/freenect Samples/SimpleViewer: $(OPENNI) Samples/MultiDepthViewer: $(OPENNI) diff --git a/Samples/freenectTest/Makefile b/Samples/freenectTest/Makefile new file mode 100644 index 00000000..e2a7cea9 --- /dev/null +++ b/Samples/freenectTest/Makefile @@ -0,0 +1,20 @@ +include ../../ThirdParty/PSCommon/BuildSystem/CommonDefs.mak + +BIN_DIR = ../../Bin + +INC_DIRS = \ + /usr/include/libfreenect \ + ../../Source/Drivers/freenect \ + ../../Include \ + ../../ThirdParty/PSCommon/XnLib/Include + +SRC_FILES = *.cpp + +LIB_DIRS = ../../ThirdParty/PSCommon/XnLib/Bin/$(PLATFORM)-$(CFG) +USED_LIBS += OpenNI2 freenect XnLib + +EXE_NAME = freenectTest + +CFLAGS += -Wall + +include ../../ThirdParty/PSCommon/BuildSystem/CommonCppMakefile diff --git a/Samples/freenectTest/freenectTest.cpp b/Samples/freenectTest/freenectTest.cpp new file mode 100644 index 00000000..be95bd10 --- /dev/null +++ b/Samples/freenectTest/freenectTest.cpp @@ -0,0 +1,191 @@ +#include + +#include +#include +#include + +#include "FreenectDriver.cpp" + +#include + + +FreenectDriver* f_driver = new FreenectDriver(NULL); +oni::driver::DeviceBase* f_device; +oni::driver::StreamBase* f_depth; + + +void shutdown(int s) { + std::cout << "Caught signal " << s << std::endl; + + f_device->destroyStream(f_depth); + f_driver->deviceClose(f_device); + + exit(1); +} + +int alt(); + + +int main(int argc, char **argv) +{ + // handle SIGINT (Ctrl+c) + struct sigaction sigIntHandler; + sigIntHandler.sa_handler = shutdown; + sigemptyset(&sigIntHandler.sa_mask); + sigIntHandler.sa_flags = 0; + sigaction(SIGINT, &sigIntHandler, NULL); + + + return alt(); + + + //FreenectDriver* driver = new FreenectDriver(NULL); + f_driver->initialize(NULL, NULL, NULL, NULL); + f_device = f_driver->deviceOpen("freenect:0"); + f_depth = f_device->createStream(ONI_SENSOR_DEPTH); + + + // wait for Ctrl+c + while (true); + + + shutdown(0); + return 0; +} + + +using namespace openni; + +int alt() +{ + Status rc = OpenNI::initialize(); + if (rc != STATUS_OK) + { + printf("Initialize failed\n%s\n", OpenNI::getExtendedError()); + return 1; + } + + std::cout << "NEXT!" << std::endl; + + Device device; + rc = device.open(ANY_DEVICE); + if (rc != STATUS_OK) + { + printf("Couldn't open device\n%s\n", OpenNI::getExtendedError()); + return 2; + } + + std::cout << "got here" << std::endl; + + VideoStream depth, color; + + if (device.getSensorInfo(SENSOR_DEPTH) != NULL) + { + rc = depth.create(device, SENSOR_DEPTH); + if (rc == STATUS_OK) + { + rc = depth.start(); + if (rc != STATUS_OK) + { + printf("Couldn't start depth stream\n%s\n", OpenNI::getExtendedError()); + } + } + else + { + printf("Couldn't create depth stream\n%s\n", OpenNI::getExtendedError()); + } + } + + if (device.getSensorInfo(SENSOR_COLOR) != NULL) + { + rc = color.create(device, SENSOR_COLOR); + if (rc == STATUS_OK) + { + rc = color.start(); + if (rc != STATUS_OK) + { + printf("Couldn't start color stream\n%s\n", OpenNI::getExtendedError()); + } + } + else + { + printf("Couldn't create color stream\n%s\n", OpenNI::getExtendedError()); + } + } + + + + + return 0; +} + +/* +int MSR() +{ + VideoFrameRef frame; + + VideoStream* streams[] = {&depth, &color}; + + while (true) + { + int readyStream = -1; + rc = OpenNI::waitForAnyStream(streams, 2, &readyStream); + if (rc != STATUS_OK) + { + printf("Wait failed\n"); + continue; + } + + switch (readyStream) + { + case 0: + // Depth + depth.readFrame(&frame); + break; + case 1: + // Color + color.readFrame(&frame); + break; + default: + printf("Unxpected stream\n"); + } + + analyzeFrame(frame); + } + + depth.stop(); + color.stop(); + depth.destroy(); + color.destroy(); + device.close(); + OpenNI::shutdown(); +} +*/ + + +void analyzeFrame(const VideoFrameRef& frame) +{ + DepthPixel* pDepth; + RGB888Pixel* pColor; + + int middleIndex = (frame.getHeight()+1)*frame.getWidth()/2; + + switch (frame.getVideoMode().getPixelFormat()) + { + case PIXEL_FORMAT_DEPTH_1_MM: + case PIXEL_FORMAT_DEPTH_100_UM: + pDepth = (DepthPixel*)frame.getData(); + printf("[%08llu] %8d\n", (long long)frame.getTimestamp(), + pDepth[middleIndex]); + break; + case PIXEL_FORMAT_RGB888: + pColor = (RGB888Pixel*)frame.getData(); + printf("[%08llu] 0x%02x%02x%02x\n", (long long)frame.getTimestamp(), + pColor[middleIndex].r&0xff, + pColor[middleIndex].g&0xff, + pColor[middleIndex].b&0xff); + break; + default: + printf("Unknown format\n"); + } +} diff --git a/Source/Drivers/freenect/FreenectDepthStream.h b/Source/Drivers/freenect/FreenectDepthStream.h new file mode 100644 index 00000000..8696057c --- /dev/null +++ b/Source/Drivers/freenect/FreenectDepthStream.h @@ -0,0 +1,82 @@ +#ifndef _FREENECT_DEPTH_STREAM_H_ +#define _FREENECT_DEPTH_STREAM_H_ + +#include "FreenectStream.h" + + +class FreenectDepthStream : public FreenectStream +{ +public: + FreenectDepthStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) + {} + + + int getBytesPerPixel() { return sizeof(OniDepthPixel); } + + OniStatus getVideoMode(OniVideoMode* pVideoMode) + { + //pVideoMode->pixelFormat = format_convert(device->getVideoFormat()); + pVideoMode->pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + pVideoMode->fps = 30; + + xnl::Pair res = resolution_convert(device->getDepthResolution()); + pVideoMode->resolutionX = res.first; + pVideoMode->resolutionY = res.second; + + return ONI_STATUS_OK; + } + + void buildFrame(void* data, uint32_t timestamp) + { + if (!running) + return; + + xnl::Pair res = resolution_convert(device->getDepthResolution()); + int TEST_RESOLUTION_X = res.first; + int TEST_RESOLUTION_Y = res.second; + + + OniDriverFrame* pFrame = (OniDriverFrame*)xnOSCalloc(1, sizeof(OniDriverFrame)); + if (pFrame == NULL) + { + XN_ASSERT(FALSE); + return; + } + + int dataSize = res.first * res.second * getBytesPerPixel(); + pFrame->frame.data = xnOSMallocAligned(dataSize, XN_DEFAULT_MEM_ALIGN); + if (pFrame->frame.data == NULL) + { + XN_ASSERT(FALSE); + return; + } + + pFrame->pDriverCookie = xnOSMalloc(sizeof(FreenectStreamFrameCookie)); + ((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount = 1; + + + pFrame->frame.frameIndex = frameId++; + pFrame->frame.dataSize = dataSize; + pFrame->frame.sensorType = ONI_SENSOR_DEPTH; + pFrame->frame.stride = TEST_RESOLUTION_X*sizeof(OniDepthPixel); + //pFrame->frame.timestamp = m_frameId*33000; + pFrame->frame.timestamp = timestamp; + + pFrame->frame.videoMode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + pFrame->frame.videoMode.resolutionX = TEST_RESOLUTION_X; + pFrame->frame.videoMode.resolutionY = TEST_RESOLUTION_Y; + pFrame->frame.videoMode.fps = 30; + + pFrame->frame.width = TEST_RESOLUTION_X; + pFrame->frame.height = TEST_RESOLUTION_Y; + + pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; + pFrame->frame.croppingEnabled = FALSE; + + + raiseNewFrame(pFrame); + } + +}; + +#endif // _FREENECT_DEPTH_STREAM_H_ diff --git a/Source/Drivers/freenect/FreenectDevice.cpp b/Source/Drivers/freenect/FreenectDeviceNI.cpp similarity index 96% rename from Source/Drivers/freenect/FreenectDevice.cpp rename to Source/Drivers/freenect/FreenectDeviceNI.cpp index 02746433..d3f22765 100644 --- a/Source/Drivers/freenect/FreenectDevice.cpp +++ b/Source/Drivers/freenect/FreenectDeviceNI.cpp @@ -1,5 +1,5 @@ /** -* freenectDevice.cpp +* FreenectDeviceNI.cpp * Copyright 2012 Benn Snyder * * OpenNI 2.x Alpha @@ -27,17 +27,14 @@ * */ +#include "FreenectDeviceNI.h" -#include "Drivers/OniDriverAPI.h" -//#include "KinectDevice.h" -//#include "DepthKinectStream.h" -//#include "ColorKinectStream.h" -//#include "IRKinectStream.h" -#include "XnLog.h" +// REFERENCE FROM KinectDevice.cpp +/* using namespace kinect_device; using namespace oni::driver; #define DEFAULT_FPS 30 @@ -217,3 +214,6 @@ void KinectDevice::notifyAllProperties() { return; } +*/ + +// END REFERENCE diff --git a/Source/Drivers/freenect/FreenectDeviceNI.h b/Source/Drivers/freenect/FreenectDeviceNI.h new file mode 100644 index 00000000..a3df61ef --- /dev/null +++ b/Source/Drivers/freenect/FreenectDeviceNI.h @@ -0,0 +1,152 @@ +#ifndef _FREENECT_DEVICE_NI_H_ +#define _FREENECT_DEVICE_NI_H_ + +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +#include "XnLog.h" +#include "FreenectDepthStream.h" +#include "FreenectImageStream.h" + + +using namespace oni::driver; + +#define DEFAULT_FPS 30 +class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::FreenectDevice +{ +private: + FreenectImageStream* image_stream; + FreenectDepthStream* depth_stream; + + // OpenNI + int m_numSensors; + OniSensorInfo m_sensors[3]; + + +public: + FreenectDeviceNI(freenect_context *_ctx, int _index) : Freenect::FreenectDevice(_ctx, _index) + { + image_stream = NULL; + depth_stream = NULL; + + m_numSensors = 2; + + // DEPTH + m_sensors[0].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 2); + m_sensors[0].sensorType = ONI_SENSOR_DEPTH; + m_sensors[0].numSupportedVideoModes = 2; + + m_sensors[0].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + m_sensors[0].pSupportedVideoModes[0].fps = 30; + m_sensors[0].pSupportedVideoModes[0].resolutionX = 640; + m_sensors[0].pSupportedVideoModes[0].resolutionY = 480; + + m_sensors[0].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + m_sensors[0].pSupportedVideoModes[1].fps = 30; + m_sensors[0].pSupportedVideoModes[1].resolutionX = 320; + m_sensors[0].pSupportedVideoModes[1].resolutionY = 240; + + // COLOR + m_sensors[1].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); + m_sensors[1].sensorType = ONI_SENSOR_COLOR; + m_sensors[1].numSupportedVideoModes = 2; + m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; + m_sensors[1].pSupportedVideoModes[0].fps = 12; // correct? + m_sensors[1].pSupportedVideoModes[0].resolutionX = 1280; + //m_sensors[1].pSupportedVideoModes[0].resolutionY = 960; + m_sensors[1].pSupportedVideoModes[0].resolutionY = 1024; + + m_sensors[1].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_RGB888; + m_sensors[1].pSupportedVideoModes[1].fps = 30; + m_sensors[1].pSupportedVideoModes[1].resolutionX = 640; + m_sensors[1].pSupportedVideoModes[1].resolutionY = 480; + + /* + m_sensors[1].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_YUV422; + m_sensors[1].pSupportedVideoModes[2].fps = 15; + m_sensors[1].pSupportedVideoModes[2].resolutionX = 640; + m_sensors[1].pSupportedVideoModes[2].resolutionY = 480; + */ + + /* + m_sensors[2].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); + m_sensors[2].sensorType = ONI_SENSOR_IR; + m_sensors[2].numSupportedVideoModes = 3; + m_sensors[2].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; + m_sensors[2].pSupportedVideoModes[0].fps = DEFAULT_FPS; + m_sensors[2].pSupportedVideoModes[0].resolutionX = 640; + m_sensors[2].pSupportedVideoModes[0].resolutionY = 480; + + m_sensors[2].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_GRAY16; + m_sensors[2].pSupportedVideoModes[1].fps = DEFAULT_FPS; + m_sensors[2].pSupportedVideoModes[1].resolutionX = 640; + m_sensors[2].pSupportedVideoModes[1].resolutionY = 480; + + m_sensors[2].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_GRAY8; + m_sensors[2].pSupportedVideoModes[2].fps = DEFAULT_FPS; + m_sensors[2].pSupportedVideoModes[2].resolutionX = 640; + m_sensors[2].pSupportedVideoModes[2].resolutionY = 480; + */ + } + + + // OpenNI + OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) + { + *numSensors = m_numSensors; + *pSensors = m_sensors; + return ONI_STATUS_OK; + } + StreamBase* createStream(OniSensorType sensorType) + { + switch(sensorType) + { + case ONI_SENSOR_DEPTH: + if (depth_stream == NULL) + depth_stream = XN_NEW(FreenectDepthStream, this); + startDepth(); + return depth_stream; + case ONI_SENSOR_COLOR: + if (image_stream == NULL) + image_stream = XN_NEW(FreenectImageStream, this); + startVideo(); + return image_stream; + // todo: IR + default: + //m_driverServices.errorLoggerAppend("FreenectDeviceNI: Can't create a stream of type %d", sensorType); + return NULL; + } + } + + void destroyStream(StreamBase* pStream) + { + if (pStream == depth_stream) + { + stopDepth(); + depth_stream = NULL; + } + if (pStream == image_stream) + { + stopVideo(); + image_stream = NULL; + } + + XN_DELETE(pStream); + } + + + // freenect + // Do not call directly even in child + void VideoCallback(void *image, uint32_t timestamp) + { + image_stream->buildFrame(image, timestamp); + } + // Do not call directly even in child + void DepthCallback(void *depth, uint32_t timestamp) + { + depth_stream->buildFrame(depth, timestamp); + } +}; + + + +#endif //_FREENECT_DEVICE_NI_H_ diff --git a/Source/Drivers/freenect/FreenectDriver.cpp b/Source/Drivers/freenect/FreenectDriver.cpp new file mode 100644 index 00000000..d90dcc16 --- /dev/null +++ b/Source/Drivers/freenect/FreenectDriver.cpp @@ -0,0 +1,240 @@ +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +//#include "KinectDriver.h" +#include "FreenectDeviceNI.h" +#include "XnHash.h" + +#include +#include + + +using namespace oni::driver; + +static const char VENDOR_VAL[] = "Microsoft"; +static const char NAME_VAL[] = "Kinect"; + + + +// EXPERIMENTAL with modified libfreenect and double-inheritance + +class FreenectDriver : public DriverBase, private Freenect::Freenect +{ +private: + //Freenect::Freenect freenect; + // from Freenect::Freenect - freenect_context* m_ctx + xnl::Hash devices; + +public: + + FreenectDriver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) + { + freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG); + // MOTOR doesn't work with k4w branch; todo: fix it + //freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); + freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_CAMERA)); + } + ~FreenectDriver() + {} + + OniStatus initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) + { + DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); + std::cout << "number of freenect devices: " << Freenect::deviceCount() << std::endl; + for (int i = 0; i < Freenect::deviceCount(); i++) + { + OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); + std::ostringstream uri; + uri << "freenect:" << i; + xnOSStrCopy(pInfo->uri, uri.str().c_str(), ONI_MAX_STR); + xnOSStrCopy(pInfo->vendor, VENDOR_VAL, ONI_MAX_STR); + xnOSStrCopy(pInfo->name, NAME_VAL, ONI_MAX_STR); + devices[pInfo] = NULL; + //deviceConnected(pInfo); + //deviceStateChanged(pInfo, 0); + } + return ONI_STATUS_OK; + } + + DeviceBase* deviceOpen(const char* uri) + { + for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); ++iter) + { + if (xnOSStrCmp(iter->Key()->uri, uri) == 0) + { + // found + if (iter->Value() != NULL) + { + // already using + return iter->Value(); + } + else + { + int id; + std::istringstream(iter->Key()->uri) >> id; + FreenectDeviceNI * freenect_device = &createDevice(id); + iter->Value() = freenect_device; + return freenect_device; + } + } + } + + getServices().errorLoggerAppend("Looking for '%s'", uri); + return NULL; + } + + void deviceClose(DeviceBase* pDevice) + { + for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); ++iter) + { + if (iter->Value() == pDevice) + { + iter->Value() = NULL; + int id; + std::istringstream(iter->Key()->uri) >> id; + Freenect::deleteDevice(id); + return; + } + } + + // not our device?! + XN_ASSERT(FALSE); + } + + void shutdown() + {} + + /* + + virtual OniStatus tryDevice(const char* uri); + + virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); + virtual void disableFrameSync(void* frameSyncGroup); + void StatusUpdate(const OLECHAR* instanceName, bool isConnected); + */ +}; + + +// PARTIAL REFERENCE FROM TestDevice.cpp +/* +class TestDriver : public oni::driver::DriverBase +{ +public: + virtual OniStatus tryDevice(const char* uri) + { + if (xnOSStrCmp(uri, "Test")) + { + return ONI_STATUS_ERROR; + } + + + OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); + xnOSStrCopy(pInfo->uri, uri, ONI_MAX_STR); + xnOSStrCopy(pInfo->vendor, "Test", ONI_MAX_STR); + m_devices[pInfo] = NULL; + + deviceConnected(pInfo); + + return ONI_STATUS_OK; + } +}; +*/ + +// REFERENCE ENDS HERE + + + + + +/* ORIGINAL + +//class FreenectDriver : public kinect_device::KinectDriver, private Freenect::Freenect +//class FreenectDriver : public kinect_device::KinectDriver +class FreenectDriver : public DriverBase +{ +private: + Freenect::Freenect freenect; + xnl::Hash m_devices; + +public: + + FreenectDriver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) + { + } + ~FreenectDriver() + {} + + OniStatus initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) + { + DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); + std::cout << "number of devices: " << freenect.deviceCount() << std::endl; + for (int i = 0; i < freenect.deviceCount(); i++) + { + OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); + std::ostringstream uri; + uri << "freenect:" << i; + xnOSStrCopy(pInfo->uri, uri.str().c_str(), ONI_MAX_STR); + xnOSStrCopy(pInfo->vendor, VENDOR_VAL, ONI_MAX_STR); + xnOSStrCopy(pInfo->name, NAME_VAL, ONI_MAX_STR); + m_devices[pInfo] = NULL; + //deviceConnected(pInfo); + //deviceStateChanged(pInfo, 0); + } + return ONI_STATUS_OK; + } + + DeviceBase* deviceOpen(const char* uri) + { + for (xnl::Hash::Iterator iter = m_devices.Begin(); iter != m_devices.End(); ++iter) + { + if (xnOSStrCmp(iter->Key()->uri, uri) == 0) + { + // found + if (iter->Value() != NULL) + { + // already using + return iter->Value(); + } + else + { + int id; + std::istringstream(iter->Key()->uri) >> id; + FreenectDeviceNI * freenect_device = &freenect.createDevice(id); + //iter->Value() = freenect_device; + std::cout << "number of devices: " << freenect.deviceCount() << std::endl; + //return freenect_device; + } + } + } + return NULL; + } + + void deviceClose(DeviceBase* pDevice) + { + for (xnl::Hash::Iterator iter = m_devices.Begin(); iter != m_devices.End(); ++iter) + { + if (iter->Value() == pDevice) + { + iter->Value() = NULL; + XN_DELETE(pDevice); + int id; + std::istringstream(iter->Key()->uri) >> id; + freenect.deleteDevice(id); + return; + } + } + // not our device?! + XN_ASSERT(FALSE); + } + + void shutdown() + {} + + /* + + virtual OniStatus tryDevice(const char* uri); + + virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); + virtual void disableFrameSync(void* frameSyncGroup); + void StatusUpdate(const OLECHAR* instanceName, bool isConnected); + */ +//}; diff --git a/Source/Drivers/freenect/FreenectImageStream.h b/Source/Drivers/freenect/FreenectImageStream.h new file mode 100644 index 00000000..eb9c0f9f --- /dev/null +++ b/Source/Drivers/freenect/FreenectImageStream.h @@ -0,0 +1,38 @@ +#ifndef _FREENECT_IMAGE_STREAM_H_ +#define _FREENECT_IMAGE_STREAM_H_ + +#include "FreenectStream.h" + + +class FreenectImageStream : public FreenectStream +{ +public: + FreenectImageStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) + {} + + + int getBytesPerPixel() { return sizeof(OniRGB888Pixel); } + + + OniStatus getVideoMode(OniVideoMode* pVideoMode) + { + //pVideoMode->pixelFormat = format_convert(device->getVideoFormat()); + pVideoMode->pixelFormat = ONI_PIXEL_FORMAT_RGB888; + pVideoMode->fps = 30; + + xnl::Pair res = resolution_convert(device->getDepthResolution()); + pVideoMode->resolutionX = res.first; + pVideoMode->resolutionY = res.second; + + return ONI_STATUS_OK; + } + + + void buildFrame(void* data, uint32_t timestamp) + { + if (!running) + return; + } +}; + +#endif // _FREENECT_IMAGE_STREAM_H_ diff --git a/Source/Drivers/freenect/FreenectStream.cpp b/Source/Drivers/freenect/FreenectStream.cpp deleted file mode 100644 index f9baf465..00000000 --- a/Source/Drivers/freenect/FreenectStream.cpp +++ /dev/null @@ -1,179 +0,0 @@ -#include "BaseKinectStream.h" -#include "KinectStreamImpl.h" -#include -#include "XnHash.h" -#include "XnEvent.h" -#include "XnPlatform.h" -#include "NuiApi.h" -#include "PS1080.h" -#include "XnMath.h" - -using namespace oni::driver; -using namespace kinect_device; - -BaseKinectStream::BaseKinectStream(KinectStreamImpl* pStreamImpl): - m_pStreamImpl(pStreamImpl) -{ - m_running = false; - pStreamImpl->addStream(this); -} - -BaseKinectStream::~BaseKinectStream() -{ - destroy(); -} - -OniStatus BaseKinectStream::start() -{ - OniStatus status = m_pStreamImpl->start(); - if (status == ONI_STATUS_OK) - m_running = TRUE; - return status; -} - -void BaseKinectStream::stop() -{ - m_running = FALSE; - m_pStreamImpl->stop(); -} - -void BaseKinectStream::destroy() -{ - stop(); - m_pStreamImpl->removeStream(this); -} - -OniStatus BaseKinectStream::getProperty(int propertyId, void* data, int* pDataSize) -{ - OniStatus status = ONI_STATUS_NOT_SUPPORTED; - switch (propertyId) - { - case ONI_STREAM_PROPERTY_CROPPING: - status = ONI_STATUS_NOT_IMPLEMENTED; - break; - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: - { - float* val = (float*)data; - XnDouble tmp; - if (m_videoMode.resolutionX == 640) - tmp = NUI_CAMERA_COLOR_NOMINAL_HORIZONTAL_FOV * xnl::Math::DTR; - else - tmp = NUI_CAMERA_DEPTH_NOMINAL_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; - if (m_videoMode.resolutionY == 480) - tmp = NUI_CAMERA_COLOR_NOMINAL_VERTICAL_FOV * xnl::Math::DTR; - else - tmp = NUI_CAMERA_DEPTH_NOMINAL_VERTICAL_FOV * xnl::Math::DTR; - *val = (float)tmp; - status = ONI_STATUS_OK; - break; - } - case ONI_STREAM_PROPERTY_VIDEO_MODE: - { - if (*pDataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); - status = ONI_STATUS_ERROR; - } - else - { - status = GetVideoMode((OniVideoMode*)data); - } - - break; - } - default: - status = ONI_STATUS_NOT_SUPPORTED; - break; - } - - return status; -} - -OniStatus BaseKinectStream::setProperty(int propertyId, const void* data, int dataSize) -{ - OniStatus status = ONI_STATUS_NOT_SUPPORTED; - if (propertyId == ONI_STREAM_PROPERTY_VIDEO_MODE) - { - if (dataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); - status = ONI_STATUS_ERROR; - } - status = SetVideoMode((OniVideoMode*)data); - } - return status; -} - -OniBool BaseKinectStream::isPropertySupported(int propertyId) -{ - OniBool status = FALSE; - switch (propertyId) - { - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: - case ONI_STREAM_PROPERTY_VERTICAL_FOV: - case ONI_STREAM_PROPERTY_VIDEO_MODE: - status = TRUE; - default: - status = FALSE; - break; - } - return status; -} - -void BaseKinectStream::notifyAllProperties() -{ - XnFloat nDouble; - int size = sizeof(nDouble); - getProperty(ONI_STREAM_PROPERTY_HORIZONTAL_FOV, &nDouble, &size); - raisePropertyChanged(ONI_STREAM_PROPERTY_HORIZONTAL_FOV, &nDouble, size); - - getProperty(ONI_STREAM_PROPERTY_VERTICAL_FOV, &nDouble, &size); - raisePropertyChanged(ONI_STREAM_PROPERTY_VERTICAL_FOV, &nDouble, size); - - OniVideoMode videoMode; - size = sizeof(videoMode); - - getProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &videoMode, &size); - raisePropertyChanged(ONI_STREAM_PROPERTY_VIDEO_MODE, &videoMode, size); -} - -OniStatus BaseKinectStream::SetVideoMode(OniVideoMode* videoMode) -{ - if (!m_pStreamImpl->isRunning()) - { - m_videoMode = *videoMode; - m_pStreamImpl->setVideoMode(videoMode); - return ONI_STATUS_OK; - } - - return ONI_STATUS_NOT_SUPPORTED; -} - -OniStatus BaseKinectStream::GetVideoMode(OniVideoMode* pVideoMode) -{ - *pVideoMode = m_videoMode; - return ONI_STATUS_OK; -} - -void BaseKinectStream::addRefToFrame(OniDriverFrame* pFrame) -{ - ++((KinectStreamFrameCookie*)pFrame->pDriverCookie)->refCount; -} - -void BaseKinectStream::releaseFrame(OniDriverFrame* pFrame) -{ - if (0 == --((KinectStreamFrameCookie*)pFrame->pDriverCookie)->refCount) - { - xnOSFree(pFrame->pDriverCookie); - xnOSFreeAligned(pFrame->frame.data); - xnOSFree(pFrame); - } -} diff --git a/Source/Drivers/freenect/FreenectStream.h b/Source/Drivers/freenect/FreenectStream.h index 9bb96d55..9cff848b 100644 --- a/Source/Drivers/freenect/FreenectStream.h +++ b/Source/Drivers/freenect/FreenectStream.h @@ -1,68 +1,187 @@ #ifndef _FREENECT_STREAM_H_ #define _FREENECT_STREAM_H_ -#include "Drivers/OniDriverAPI.h" +#include "libfreenect.h" +#include "Driver/OniDriverAPI.h" #include "XnLib.h" +#include "XnHash.h" +#include "XnEvent.h" +#include "XnPlatform.h" +#include "PS1080.h" +#include "XnMath.h" -namespace kinect_device { - -class KinectStreamImpl; - -static const int KINECT_RESOLUTION_X_80 = 80; -static const int KINECT_RESOLUTION_Y_60 = 60; -static const int KINECT_RESOLUTION_X_320 = 320; -static const int KINECT_RESOLUTION_Y_240 = 240; -static const int KINECT_RESOLUTION_X_640 = 640; -static const int KINECT_RESOLUTION_Y_480 = 480; -static const int KINECT_RESOLUTION_X_1280 = 1280; -static const int KINECT_RESOLUTION_Y_960 = 960; +#include "XnPair.h" typedef struct { int refCount; -} KinectStreamFrameCookie; +} FreenectStreamFrameCookie; -class BaseKinectStream : public oni::driver::StreamBase -{ -public: - BaseKinectStream(KinectStreamImpl* pStreamImpl); - virtual ~BaseKinectStream(); +class FreenectStream : public oni::driver::StreamBase +{ +private: + virtual int getBytesPerPixel() = 0; + virtual OniStatus getVideoMode(OniVideoMode* pVideoMode) = 0; - OniStatus BaseKinectStream::start(); +protected: + Freenect::FreenectDevice* device; + int frameId; // number frames starting with 1 + bool running; + +public: + FreenectStream(Freenect::FreenectDevice* pDevice) + { + device = pDevice; + frameId = 1; + } + ~FreenectStream() + { + stop(); + } + + virtual void buildFrame(void* data, uint32_t timestamp) = 0; + + OniStatus start() + { + running = true; + return ONI_STATUS_OK; + } + void stop() + { + running = false; + } + + + /* + // Stream properties + enum + { + ONI_STREAM_PROPERTY_CROPPING = 0, // OniCropping* + ONI_STREAM_PROPERTY_HORIZONTAL_FOV = 1, // float: radians + ONI_STREAM_PROPERTY_VERTICAL_FOV = 2, // float: radians + ONI_STREAM_PROPERTY_VIDEO_MODE = 3, // OniVideoMode* + + ONI_STREAM_PROPERTY_MAX_VALUE = 4, // int + ONI_STREAM_PROPERTY_MIN_VALUE = 5, // int + + ONI_STREAM_PROPERTY_STRIDE = 6, // int + ONI_STREAM_PROPERTY_MIRRORING = 7, // OniBool + + ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES = 8, // int + + // Camera + ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE = 100, // OniBool + ONI_STREAM_PROPERTY_AUTO_EXPOSURE = 101, // OniBool + }; + */ + OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch(propertyId) + { + default: + return ONI_STATUS_NOT_SUPPORTED; + case ONI_STREAM_PROPERTY_VIDEO_MODE: + if (*pDataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + return getVideoMode((OniVideoMode*)data); + } + } + + // freenect_resolution (libfreenect.h) to (x,y) + //int* resolution_convert(freenect_resolution resolution) { + xnl::Pair resolution_convert(freenect_resolution resolution) + { + switch(resolution) + { + case FREENECT_RESOLUTION_LOW: + return xnl::Pair(320, 240); + case FREENECT_RESOLUTION_MEDIUM: + return xnl::Pair(640, 480); + case FREENECT_RESOLUTION_HIGH: + return xnl::Pair(1280, 1024); + } + } + + + void addRefToFrame(OniDriverFrame* pFrame) + { + ++((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount; + } + void releaseFrame(OniDriverFrame* pFrame) + { + if (0 == --((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount) + { + xnOSFree(pFrame->pDriverCookie); + xnOSFreeAligned(pFrame->frame.data); + xnOSFree(pFrame); + } + } +}; - void BaseKinectStream::stop(); - virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); - - virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); - virtual OniBool isPropertySupported(int propertyId); +// PARTIAL REFERENCE FROM TestDevice.cpp +/* +class TestStream : public oni::driver::StreamBase +{ +public: + TestStream() : oni::driver::StreamBase() + { + m_osEvent.Create(TRUE); + m_sendCount = 0; + } + + virtual OniStatus SetVideoMode(OniVideoMode*) = 0; + virtual OniStatus GetVideoMode(OniVideoMode* pVideoMode) = 0; + + + OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + if (propertyId == ONI_STREAM_PROPERTY_VIDEO_MODE) + { + if (dataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + return SetVideoMode((OniVideoMode*)data); + } + else if (propertyId == 666) + { + if (dataSize != sizeof(int)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(int)); + return ONI_STATUS_ERROR; + } + + // Increment the send count. + m_cs.Lock(); + m_sendCount += *((int*)data); + m_cs.Unlock(); + + // Raise the OS event, to allow thread to start working. + m_osEvent.Set(); + } + + return ONI_STATUS_NOT_IMPLEMENTED; + } - virtual void notifyAllProperties(); +protected: - virtual OniStatus SetVideoMode(OniVideoMode* pVideoMode); + int singleRes(int x, int y) {return y*TEST_RESOLUTION_X+x;} + int m_sendCount; - virtual OniStatus GetVideoMode(OniVideoMode* pVideoMode); + xnl::CriticalSection m_cs; + xnl::OSEvent m_osEvent; +}; +*/ - void addRefToFrame(OniDriverFrame* pFrame); - - void releaseFrame(OniDriverFrame* pFrame); +// END REFERENCE - bool isRunning() { return m_running; } - virtual void frameReceived(NUI_IMAGE_FRAME& imageFrame, NUI_LOCKED_RECT &LockedRect) = 0; - - -protected: - KinectStreamImpl *m_pStreamImpl; - OniVideoMode m_videoMode; - - bool m_running; - -private: - void destroy(); -}; -} // namespace kinect_device -#endif //_FREENECT_STREAM_H_ +#endif // _FREENECT_STREAM_H_ diff --git a/Source/Drivers/freenect/Makefile b/Source/Drivers/freenect/Makefile index 7fad9d0f..237aaeb7 100644 --- a/Source/Drivers/freenect/Makefile +++ b/Source/Drivers/freenect/Makefile @@ -3,11 +3,12 @@ include ../../../ThirdParty/PSCommon/BuildSystem/CommonDefs.mak BIN_DIR = ../../../Bin INC_DIRS = \ + /usr/include/libfreenect \ ../../../Include \ ../../../ThirdParty/PSCommon/XnLib/Include SRC_FILES = \ - *.cpp + FreenectDriver.cpp ifeq ("$(OSTYPE)","Darwin") INC_DIRS += /opt/local/include @@ -18,7 +19,7 @@ endif LIB_NAME = freenectDriver LIB_DIRS = ../../../ThirdParty/PSCommon/XnLib/Bin/$(PLATFORM)-$(CFG) -USED_LIBS = XnLib dl pthread +USED_LIBS = XnLib dl pthread freenect ifneq ("$(OSTYPE)","Darwin") USED_LIBS += rt endif From ed6163433862c5b58cb60af590bbc1b10c8b4a35 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Tue, 1 Jan 2013 19:52:33 -0500 Subject: [PATCH 05/18] Load drivers from alternate paths; fix output --- Samples/MultipleStreamRead/main.cpp | 4 ++-- Source/Core/OniContext.cpp | 33 +++++++++++++++-------------- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/Samples/MultipleStreamRead/main.cpp b/Samples/MultipleStreamRead/main.cpp index c8b736f0..32958477 100644 --- a/Samples/MultipleStreamRead/main.cpp +++ b/Samples/MultipleStreamRead/main.cpp @@ -80,7 +80,7 @@ int main() rc = depth.start(); if (rc != STATUS_OK) { - printf("Couldn't start the color stream\n%s\n", OpenNI::getExtendedError()); + printf("Couldn't start depth stream\n%s\n", OpenNI::getExtendedError()); } } else @@ -97,7 +97,7 @@ int main() rc = color.start(); if (rc != STATUS_OK) { - printf("Couldn't start the color stream\n%s\n", OpenNI::getExtendedError()); + printf("Couldn't start color stream\n%s\n", OpenNI::getExtendedError()); } } else diff --git a/Source/Core/OniContext.cpp b/Source/Core/OniContext.cpp index 8871ec04..c6d6a764 100644 --- a/Source/Core/OniContext.cpp +++ b/Source/Core/OniContext.cpp @@ -112,7 +112,7 @@ OniStatus Context::initialize() // Use path specified in ini file if (repositoryOverridden) { - xnLogVerbose(XN_LOG_MASK_ALL, "Using '%s' as driver path, as configured in file '%s'", ONI_CONFIGURATION_FILE); + xnLogVerbose(XN_LOG_MASK_ALL, "Using '%s' as driver path, as configured in file '%s'", repositoryFromINI, ONI_CONFIGURATION_FILE); rc = loadLibraries(repositoryFromINI); return OniStatusFromXnStatus(rc); } @@ -144,19 +144,20 @@ OniStatus Context::initialize() XnStatus Context::loadLibraries(const char* directoryName) { XnStatus nRetVal; + XnChar cpSearchPath[XN_FILE_MAX_PATH] = ""; + XnChar cpSearchPattern[XN_FILE_MAX_PATH] = ""; XnChar cpSearchString[XN_FILE_MAX_PATH] = ""; - xnLogVerbose(XN_LOG_MASK_ALL, "Looking for drivers in drivers repository '%s'", directoryName); - - // Build the search pattern string - XN_VALIDATE_STR_APPEND(cpSearchString, directoryName, XN_FILE_MAX_PATH, nRetVal); - XN_VALIDATE_STR_APPEND(cpSearchString, XN_FILE_DIR_SEP, XN_FILE_MAX_PATH, nRetVal); - XN_VALIDATE_STR_APPEND(cpSearchString, XN_SHARED_LIBRARY_PREFIX, XN_FILE_MAX_PATH, nRetVal); - XN_VALIDATE_STR_APPEND(cpSearchString, XN_FILE_ALL_WILDCARD, XN_FILE_MAX_PATH, nRetVal); - XN_VALIDATE_STR_APPEND(cpSearchString, XN_SHARED_LIBRARY_POSTFIX, XN_FILE_MAX_PATH, nRetVal); + // Build the search pattern strings + XN_VALIDATE_STR_APPEND(cpSearchPath, directoryName, XN_FILE_MAX_PATH, nRetVal); + XN_VALIDATE_STR_APPEND(cpSearchPath, XN_FILE_DIR_SEP, XN_FILE_MAX_PATH, nRetVal); + XN_VALIDATE_STR_APPEND(cpSearchPattern, XN_SHARED_LIBRARY_PREFIX, XN_FILE_MAX_PATH, nRetVal); + XN_VALIDATE_STR_APPEND(cpSearchPattern, XN_FILE_ALL_WILDCARD, XN_FILE_MAX_PATH, nRetVal); + XN_VALIDATE_STR_APPEND(cpSearchPattern, XN_SHARED_LIBRARY_POSTFIX, XN_FILE_MAX_PATH, nRetVal); + XN_VALIDATE_STR_APPEND(cpSearchString, cpSearchPath, XN_FILE_MAX_PATH, nRetVal); + XN_VALIDATE_STR_APPEND(cpSearchString, cpSearchPattern, XN_FILE_MAX_PATH, nRetVal); // Get a file list of Xiron devices - XnInt32 nFileCount = 0; nRetVal = xnOSCountFiles(cpSearchString, &nFileCount); if (nRetVal != XN_STATUS_OK || nFileCount == 0) @@ -165,16 +166,16 @@ XnStatus Context::loadLibraries(const char* directoryName) m_errorLogger.Append("Found no files matching '%s'", cpSearchString); return XN_STATUS_NO_MODULES_FOUND; } - - typedef XnChar FileName[XN_FILE_MAX_PATH]; - FileName* acsFileList = XN_NEW_ARR(FileName, nFileCount); - nRetVal = xnOSGetFileList(cpSearchString, NULL, acsFileList, nFileCount, &nFileCount); - + // Save directory XnChar workingDir[XN_FILE_MAX_PATH]; xnOSGetCurrentDir(workingDir, XN_FILE_MAX_PATH); // Change directory - xnOSSetCurrentDir(directoryName); + xnOSSetCurrentDir(cpSearchPath); + + typedef XnChar FileName[XN_FILE_MAX_PATH]; + FileName* acsFileList = XN_NEW_ARR(FileName, nFileCount); + nRetVal = xnOSGetFileList(cpSearchPattern, cpSearchPath, acsFileList, nFileCount, &nFileCount); for (int i = 0; i < nFileCount; ++i) { From 07838624f3787202251e5ae8fcfb931c4c373567 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Wed, 2 Jan 2013 14:39:40 -0500 Subject: [PATCH 06/18] Depth and Image streams working; begin experimenting with alternate formats and resolutions --- Samples/freenectTest/freenectTest.cpp | 44 ++++--- Source/Drivers/freenect/FreenectDepthStream.h | 105 ++++++++--------- Source/Drivers/freenect/FreenectDeviceNI.h | 22 ++-- Source/Drivers/freenect/FreenectDriver.cpp | 8 +- Source/Drivers/freenect/FreenectImageStream.h | 78 ++++++++++--- Source/Drivers/freenect/FreenectStream.h | 109 ++++++++++++++---- Source/Drivers/freenect/Makefile | 2 +- 7 files changed, 244 insertions(+), 124 deletions(-) diff --git a/Samples/freenectTest/freenectTest.cpp b/Samples/freenectTest/freenectTest.cpp index be95bd10..4c044e74 100644 --- a/Samples/freenectTest/freenectTest.cpp +++ b/Samples/freenectTest/freenectTest.cpp @@ -17,13 +17,16 @@ oni::driver::StreamBase* f_depth; void shutdown(int s) { std::cout << "Caught signal " << s << std::endl; - f_device->destroyStream(f_depth); - f_driver->deviceClose(f_device); + if (f_device != NULL) { + f_device->destroyStream(f_depth); + f_driver->deviceClose(f_device); + } exit(1); } int alt(); +void analyzeFrame(const openni::VideoFrameRef& frame); int main(int argc, char **argv) @@ -38,8 +41,7 @@ int main(int argc, char **argv) return alt(); - - //FreenectDriver* driver = new FreenectDriver(NULL); + f_driver->initialize(NULL, NULL, NULL, NULL); f_device = f_driver->deviceOpen("freenect:0"); f_depth = f_device->createStream(ONI_SENSOR_DEPTH); @@ -64,19 +66,16 @@ int alt() printf("Initialize failed\n%s\n", OpenNI::getExtendedError()); return 1; } - - std::cout << "NEXT!" << std::endl; Device device; rc = device.open(ANY_DEVICE); + //rc = device.open("freenect:0"); if (rc != STATUS_OK) { printf("Couldn't open device\n%s\n", OpenNI::getExtendedError()); return 2; } - std::cout << "got here" << std::endl; - VideoStream depth, color; if (device.getSensorInfo(SENSOR_DEPTH) != NULL) @@ -112,16 +111,8 @@ int alt() printf("Couldn't create color stream\n%s\n", OpenNI::getExtendedError()); } } - - - return 0; -} - -/* -int MSR() -{ VideoFrameRef frame; VideoStream* streams[] = {&depth, &color}; @@ -153,14 +144,16 @@ int MSR() analyzeFrame(frame); } + depth.stop(); color.stop(); depth.destroy(); color.destroy(); device.close(); - OpenNI::shutdown(); + OpenNI::shutdown(); + + return 0; } -*/ void analyzeFrame(const VideoFrameRef& frame) @@ -169,6 +162,21 @@ void analyzeFrame(const VideoFrameRef& frame) RGB888Pixel* pColor; int middleIndex = (frame.getHeight()+1)*frame.getWidth()/2; + + + + std::cout << "frame width = " << frame.getWidth() << "; frame height = " << frame.getHeight() << std::endl; + /* + pDepth = (DepthPixel*)frame.getData(); + for (int x = 0; x < frame.getWidth(); x++) { + for (int y = 0; y < frame.getHeight(); y++) { + std::cout << pDepth[x*y+x] << " "; + } + std::cout << std::endl; + } + */ + + switch (frame.getVideoMode().getPixelFormat()) { diff --git a/Source/Drivers/freenect/FreenectDepthStream.h b/Source/Drivers/freenect/FreenectDepthStream.h index 8696057c..1c7a721a 100644 --- a/Source/Drivers/freenect/FreenectDepthStream.h +++ b/Source/Drivers/freenect/FreenectDepthStream.h @@ -5,76 +5,73 @@ class FreenectDepthStream : public FreenectStream -{ -public: - FreenectDepthStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) - {} +{ +public: + FreenectDepthStream(Freenect::FreenectDevice* pDevice, freenect_depth_format format = FREENECT_DEPTH_11BIT) : FreenectStream(pDevice) + { + setFormat(format); + + xnl::Pair res = convertResolution(device->getDepthResolution()); + + video_mode.fps = 30; + video_mode.resolutionX = res.first; + video_mode.resolutionY = res.second; + } int getBytesPerPixel() { return sizeof(OniDepthPixel); } - OniStatus getVideoMode(OniVideoMode* pVideoMode) + /* + FREENECT_DEPTH_11BIT = 0, //< 11 bit depth information in one uint16_t/pixel + FREENECT_DEPTH_10BIT = 1, //< 10 bit depth information in one uint16_t/pixel + FREENECT_DEPTH_11BIT_PACKED = 2, //< 11 bit packed depth information + FREENECT_DEPTH_10BIT_PACKED = 3, //< 10 bit packed depth information + FREENECT_DEPTH_REGISTERED = 4, //< processed depth data in mm, aligned to 640x480 RGB + FREENECT_DEPTH_MM = 5, //< depth to each pixel in mm, but left unaligned to RGB image + FREENECT_DEPTH_DUMMY = 2147483647, //< Dummy value to force enum to be 32 bits wide + */ + /* + ONI_PIXEL_FORMAT_DEPTH_1_MM = 100, + ONI_PIXEL_FORMAT_DEPTH_100_UM = 101, + ONI_PIXEL_FORMAT_SHIFT_9_2 = 102, + ONI_PIXEL_FORMAT_SHIFT_9_3 = 103, + */ + freenect_depth_format getFormat() { - //pVideoMode->pixelFormat = format_convert(device->getVideoFormat()); - pVideoMode->pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; - pVideoMode->fps = 30; - - xnl::Pair res = resolution_convert(device->getDepthResolution()); - pVideoMode->resolutionX = res.first; - pVideoMode->resolutionY = res.second; - - return ONI_STATUS_OK; + return device->getDepthFormat(); } - - void buildFrame(void* data, uint32_t timestamp) + OniStatus setFormat(freenect_depth_format format, freenect_resolution resolution = FREENECT_RESOLUTION_MEDIUM) { - if (!running) - return; - - xnl::Pair res = resolution_convert(device->getDepthResolution()); - int TEST_RESOLUTION_X = res.first; - int TEST_RESOLUTION_Y = res.second; - - - OniDriverFrame* pFrame = (OniDriverFrame*)xnOSCalloc(1, sizeof(OniDriverFrame)); - if (pFrame == NULL) + switch(format) { - XN_ASSERT(FALSE); - return; + default: + video_mode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; + break; + case FREENECT_DEPTH_11BIT_PACKED: + case FREENECT_DEPTH_10BIT_PACKED: + return ONI_STATUS_NOT_SUPPORTED; } + + device->setDepthFormat(format, resolution); + return ONI_STATUS_OK; + } + - int dataSize = res.first * res.second * getBytesPerPixel(); - pFrame->frame.data = xnOSMallocAligned(dataSize, XN_DEFAULT_MEM_ALIGN); + void buildFrame(void* depth, OniDriverFrame* pFrame) + { + pFrame->frame.sensorType = ONI_SENSOR_DEPTH; + pFrame->frame.stride = video_mode.resolutionX*sizeof(OniDepthPixel); + pFrame->frame.dataSize = device->getDepthBufferSize(); + pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); if (pFrame->frame.data == NULL) { XN_ASSERT(FALSE); return; } - - pFrame->pDriverCookie = xnOSMalloc(sizeof(FreenectStreamFrameCookie)); - ((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount = 1; - - - pFrame->frame.frameIndex = frameId++; - pFrame->frame.dataSize = dataSize; - pFrame->frame.sensorType = ONI_SENSOR_DEPTH; - pFrame->frame.stride = TEST_RESOLUTION_X*sizeof(OniDepthPixel); - //pFrame->frame.timestamp = m_frameId*33000; - pFrame->frame.timestamp = timestamp; - - pFrame->frame.videoMode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; - pFrame->frame.videoMode.resolutionX = TEST_RESOLUTION_X; - pFrame->frame.videoMode.resolutionY = TEST_RESOLUTION_Y; - pFrame->frame.videoMode.fps = 30; - - pFrame->frame.width = TEST_RESOLUTION_X; - pFrame->frame.height = TEST_RESOLUTION_Y; - - pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; - pFrame->frame.croppingEnabled = FALSE; - - raiseNewFrame(pFrame); + uint8_t* _data = static_cast(depth); + uint8_t* frame_data = static_cast(pFrame->frame.data); + std::copy(_data, _data+pFrame->frame.dataSize, frame_data); } }; diff --git a/Source/Drivers/freenect/FreenectDeviceNI.h b/Source/Drivers/freenect/FreenectDeviceNI.h index a3df61ef..3d3ddf7e 100644 --- a/Source/Drivers/freenect/FreenectDeviceNI.h +++ b/Source/Drivers/freenect/FreenectDeviceNI.h @@ -45,20 +45,24 @@ class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::Freene m_sensors[0].pSupportedVideoModes[1].resolutionX = 320; m_sensors[0].pSupportedVideoModes[1].resolutionY = 240; + // COLOR m_sensors[1].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); m_sensors[1].sensorType = ONI_SENSOR_COLOR; - m_sensors[1].numSupportedVideoModes = 2; + m_sensors[1].numSupportedVideoModes = 1; + + /* m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; m_sensors[1].pSupportedVideoModes[0].fps = 12; // correct? m_sensors[1].pSupportedVideoModes[0].resolutionX = 1280; //m_sensors[1].pSupportedVideoModes[0].resolutionY = 960; m_sensors[1].pSupportedVideoModes[0].resolutionY = 1024; + */ - m_sensors[1].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_RGB888; - m_sensors[1].pSupportedVideoModes[1].fps = 30; - m_sensors[1].pSupportedVideoModes[1].resolutionX = 640; - m_sensors[1].pSupportedVideoModes[1].resolutionY = 480; + m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; + m_sensors[1].pSupportedVideoModes[0].fps = 30; + m_sensors[1].pSupportedVideoModes[0].resolutionX = 640; + m_sensors[1].pSupportedVideoModes[0].resolutionY = 480; /* m_sensors[1].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_YUV422; @@ -101,14 +105,14 @@ class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::Freene switch(sensorType) { case ONI_SENSOR_DEPTH: + startDepth(); if (depth_stream == NULL) depth_stream = XN_NEW(FreenectDepthStream, this); - startDepth(); return depth_stream; case ONI_SENSOR_COLOR: + startVideo(); if (image_stream == NULL) image_stream = XN_NEW(FreenectImageStream, this); - startVideo(); return image_stream; // todo: IR default: @@ -138,12 +142,12 @@ class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::Freene // Do not call directly even in child void VideoCallback(void *image, uint32_t timestamp) { - image_stream->buildFrame(image, timestamp); + image_stream->acquireFrame(image, timestamp); } // Do not call directly even in child void DepthCallback(void *depth, uint32_t timestamp) { - depth_stream->buildFrame(depth, timestamp); + depth_stream->acquireFrame(depth, timestamp); } }; diff --git a/Source/Drivers/freenect/FreenectDriver.cpp b/Source/Drivers/freenect/FreenectDriver.cpp index d90dcc16..60148c69 100644 --- a/Source/Drivers/freenect/FreenectDriver.cpp +++ b/Source/Drivers/freenect/FreenectDriver.cpp @@ -28,7 +28,7 @@ class FreenectDriver : public DriverBase, private Freenect::Freenect FreenectDriver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) { - freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG); + freenect_set_log_level(m_ctx, FREENECT_LOG_NOTICE); // MOTOR doesn't work with k4w branch; todo: fix it //freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_CAMERA)); @@ -49,8 +49,8 @@ class FreenectDriver : public DriverBase, private Freenect::Freenect xnOSStrCopy(pInfo->vendor, VENDOR_VAL, ONI_MAX_STR); xnOSStrCopy(pInfo->name, NAME_VAL, ONI_MAX_STR); devices[pInfo] = NULL; - //deviceConnected(pInfo); - //deviceStateChanged(pInfo, 0); + deviceConnected(pInfo); + deviceStateChanged(pInfo, 0); } return ONI_STATUS_OK; } @@ -238,3 +238,5 @@ class FreenectDriver : public DriverBase void StatusUpdate(const OLECHAR* instanceName, bool isConnected); */ //}; + +ONI_EXPORT_DRIVER(FreenectDriver); diff --git a/Source/Drivers/freenect/FreenectImageStream.h b/Source/Drivers/freenect/FreenectImageStream.h index eb9c0f9f..9a5f070a 100644 --- a/Source/Drivers/freenect/FreenectImageStream.h +++ b/Source/Drivers/freenect/FreenectImageStream.h @@ -4,34 +4,82 @@ #include "FreenectStream.h" + + class FreenectImageStream : public FreenectStream { public: - FreenectImageStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) - {} - + FreenectImageStream(Freenect::FreenectDevice* pDevice, freenect_video_format format = FREENECT_VIDEO_YUV_RAW) : FreenectStream(pDevice) + { + xnl::Pair res = convertResolution(device->getVideoResolution()); + + setFormat(format, FREENECT_RESOLUTION_LOW); + + video_mode.pixelFormat = ONI_PIXEL_FORMAT_RGB888; + video_mode.fps = 30; + video_mode.resolutionX = res.first; + video_mode.resolutionY = res.second; + } int getBytesPerPixel() { return sizeof(OniRGB888Pixel); } - - OniStatus getVideoMode(OniVideoMode* pVideoMode) + /* + FREENECT_VIDEO_RGB = 0, //< Decompressed RGB mode (demosaicing done by libfreenect) + FREENECT_VIDEO_BAYER = 1, //< Bayer compressed mode (raw information from camera) + FREENECT_VIDEO_IR_8BIT = 2, //< 8-bit IR mode + FREENECT_VIDEO_IR_10BIT = 3, //< 10-bit IR mode + FREENECT_VIDEO_IR_10BIT_PACKED = 4, //< 10-bit packed IR mode + FREENECT_VIDEO_YUV_RGB = 5, //< YUV RGB mode + FREENECT_VIDEO_YUV_RAW = 6, //< YUV Raw mode + FREENECT_VIDEO_DUMMY = 2147483647, //< Dummy value to force enum to be 32 bits wide + */ + /* + ONI_PIXEL_FORMAT_RGB888 = 200, + ONI_PIXEL_FORMAT_YUV422 = 201, + ONI_PIXEL_FORMAT_GRAY8 = 202, + ONI_PIXEL_FORMAT_GRAY16 = 203, + ONI_PIXEL_FORMAT_JPEG = 204, + */ + freenect_video_format getFormat() { - //pVideoMode->pixelFormat = format_convert(device->getVideoFormat()); - pVideoMode->pixelFormat = ONI_PIXEL_FORMAT_RGB888; - pVideoMode->fps = 30; - - xnl::Pair res = resolution_convert(device->getDepthResolution()); - pVideoMode->resolutionX = res.first; - pVideoMode->resolutionY = res.second; - + return device->getVideoFormat(); + } + OniStatus setFormat(freenect_video_format format, freenect_resolution resolution = FREENECT_RESOLUTION_MEDIUM) + { + switch(format) + { + default: + video_mode.pixelFormat = ONI_PIXEL_FORMAT_RGB888; + break; + case FREENECT_VIDEO_BAYER: + //video_mode.pixelFormat = ONI_PIXEL_FORMAT_ + //break; + case FREENECT_VIDEO_IR_8BIT: + case FREENECT_VIDEO_IR_10BIT: + case FREENECT_VIDEO_IR_10BIT_PACKED: + case FREENECT_VIDEO_YUV_RAW: + return ONI_STATUS_NOT_IMPLEMENTED; + } + device->setVideoFormat(format, resolution); return ONI_STATUS_OK; } - void buildFrame(void* data, uint32_t timestamp) + void buildFrame(void* image, OniDriverFrame* pFrame) { - if (!running) + pFrame->frame.sensorType = ONI_SENSOR_COLOR; + pFrame->frame.stride = video_mode.resolutionX*3; + pFrame->frame.dataSize = device->getVideoBufferSize(); + pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); + if (pFrame->frame.data == NULL) + { + XN_ASSERT(FALSE); return; + } + + unsigned char* _data = static_cast(image); + unsigned char* frame_data = static_cast(pFrame->frame.data); + std::copy(_data, _data+pFrame->frame.dataSize, frame_data); } }; diff --git a/Source/Drivers/freenect/FreenectStream.h b/Source/Drivers/freenect/FreenectStream.h index 9cff848b..1a427169 100644 --- a/Source/Drivers/freenect/FreenectStream.h +++ b/Source/Drivers/freenect/FreenectStream.h @@ -3,15 +3,19 @@ #include "libfreenect.h" #include "Driver/OniDriverAPI.h" -#include "XnLib.h" -#include "XnHash.h" -#include "XnEvent.h" -#include "XnPlatform.h" -#include "PS1080.h" -#include "XnMath.h" +//#include "XnLib.h" +//#include "XnHash.h" +//#include "XnEvent.h" +//#include "XnPlatform.h" +//#include "PS1080.h" +//#include "XnMath.h" #include "XnPair.h" + +// todo : change video mode on the fly + + typedef struct { int refCount; @@ -22,26 +26,27 @@ class FreenectStream : public oni::driver::StreamBase { private: virtual int getBytesPerPixel() = 0; - virtual OniStatus getVideoMode(OniVideoMode* pVideoMode) = 0; protected: Freenect::FreenectDevice* device; - int frameId; // number frames starting with 1 + OniVideoMode video_mode; // derived classes should set this up in constructor + int frame_id; // number each frame bool running; public: FreenectStream(Freenect::FreenectDevice* pDevice) { device = pDevice; - frameId = 1; + frame_id = 1; } ~FreenectStream() { stop(); } - virtual void buildFrame(void* data, uint32_t timestamp) = 0; + virtual void buildFrame(void* data, OniDriverFrame* pFrame) = 0; + OniStatus start() { running = true; @@ -50,7 +55,21 @@ class FreenectStream : public oni::driver::StreamBase void stop() { running = false; - } + } + + + static xnl::Pair convertResolution(freenect_resolution resolution) + { + switch(resolution) + { + case FREENECT_RESOLUTION_LOW: + return xnl::Pair(320, 240); + case FREENECT_RESOLUTION_MEDIUM: + return xnl::Pair(640, 480); + case FREENECT_RESOLUTION_HIGH: + return xnl::Pair(1280, 1024); + } + } /* @@ -74,7 +93,7 @@ class FreenectStream : public oni::driver::StreamBase ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE = 100, // OniBool ONI_STREAM_PROPERTY_AUTO_EXPOSURE = 101, // OniBool }; - */ + */ OniStatus getProperty(int propertyId, void* data, int* pDataSize) { switch(propertyId) @@ -87,25 +106,67 @@ class FreenectStream : public oni::driver::StreamBase printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); return ONI_STATUS_ERROR; } - return getVideoMode((OniVideoMode*)data); + OniVideoMode* pVideoMode = static_cast(data); + pVideoMode->pixelFormat = video_mode.pixelFormat; + pVideoMode->fps = video_mode.fps; + pVideoMode->resolutionX = video_mode.resolutionX; + pVideoMode->resolutionY = video_mode.resolutionY; + return ONI_STATUS_OK; } } - - // freenect_resolution (libfreenect.h) to (x,y) - //int* resolution_convert(freenect_resolution resolution) { - xnl::Pair resolution_convert(freenect_resolution resolution) + + + + + + OniStatus setProperty(int propertyId, const void* data, int dataSize) { - switch(resolution) + switch(propertyId) { - case FREENECT_RESOLUTION_LOW: - return xnl::Pair(320, 240); - case FREENECT_RESOLUTION_MEDIUM: - return xnl::Pair(640, 480); - case FREENECT_RESOLUTION_HIGH: - return xnl::Pair(1280, 1024); + default: + return ONI_STATUS_NOT_IMPLEMENTED; + case ONI_STREAM_PROPERTY_VIDEO_MODE: + if (dataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + + return ONI_STATUS_NOT_IMPLEMENTED; } } + void acquireFrame(void* data, uint32_t timestamp) + { + if (!running) + return; + + //if (frame_id == 1) + + + OniDriverFrame* pFrame = (OniDriverFrame*)xnOSCalloc(1, sizeof(OniDriverFrame)); + if (pFrame == NULL) + { + XN_ASSERT(FALSE); + return; + } + pFrame->pDriverCookie = xnOSMalloc(sizeof(FreenectStreamFrameCookie)); + ((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount = 1; + + pFrame->frame.frameIndex = frame_id++; + //pFrame->frame.timestamp = m_frameId*33000; + pFrame->frame.timestamp = timestamp; + + pFrame->frame.videoMode = video_mode; + pFrame->frame.width = video_mode.resolutionX; + pFrame->frame.height = video_mode.resolutionY; + pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; + pFrame->frame.croppingEnabled = FALSE; + + + buildFrame(data, pFrame); + raiseNewFrame(pFrame); + } void addRefToFrame(OniDriverFrame* pFrame) { diff --git a/Source/Drivers/freenect/Makefile b/Source/Drivers/freenect/Makefile index 237aaeb7..d007bfb3 100644 --- a/Source/Drivers/freenect/Makefile +++ b/Source/Drivers/freenect/Makefile @@ -8,7 +8,7 @@ INC_DIRS = \ ../../../ThirdParty/PSCommon/XnLib/Include SRC_FILES = \ - FreenectDriver.cpp + *.cpp ifeq ("$(OSTYPE)","Darwin") INC_DIRS += /opt/local/include From 8e6bf2f542b13d072c8e64b91734ae1472e5dc2b Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Thu, 3 Jan 2013 00:45:49 -0500 Subject: [PATCH 07/18] Rounded out implementations; reworking format management --- Source/Drivers/freenect/FreenectDepthStream.h | 75 +++++++- Source/Drivers/freenect/FreenectDeviceNI.h | 76 +++++--- Source/Drivers/freenect/FreenectDriver.cpp | 158 ++------------- Source/Drivers/freenect/FreenectImageStream.h | 49 +++-- Source/Drivers/freenect/FreenectStream.h | 181 +++++------------- 5 files changed, 207 insertions(+), 332 deletions(-) diff --git a/Source/Drivers/freenect/FreenectDepthStream.h b/Source/Drivers/freenect/FreenectDepthStream.h index 1c7a721a..efdf949e 100644 --- a/Source/Drivers/freenect/FreenectDepthStream.h +++ b/Source/Drivers/freenect/FreenectDepthStream.h @@ -3,20 +3,29 @@ #include "FreenectStream.h" +#define SIZE(array) sizeof array / sizeof 0[array] -class FreenectDepthStream : public FreenectStream + +static bool operator==(const OniVideoMode& left, const OniVideoMode& right) { - + return (left.pixelFormat == right.pixelFormat && left.resolutionX == right.resolutionX + && left.resolutionY == right.resolutionY && left.fps == right.fps) ? true : false; +} + + +class FreenectDepthStream : public FreenectStream +{ public: FreenectDepthStream(Freenect::FreenectDevice* pDevice, freenect_depth_format format = FREENECT_DEPTH_11BIT) : FreenectStream(pDevice) { - setFormat(format); + setFormat(format, FREENECT_RESOLUTION_MEDIUM); xnl::Pair res = convertResolution(device->getDepthResolution()); - video_mode.fps = 30; video_mode.resolutionX = res.first; video_mode.resolutionY = res.second; + + printf("FreenectDepthStream: resolutionX = %d; resolutionY = %d\n", video_mode.resolutionX, video_mode.resolutionY); } int getBytesPerPixel() { return sizeof(OniDepthPixel); } @@ -45,15 +54,63 @@ class FreenectDepthStream : public FreenectStream switch(format) { default: - video_mode.pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; - break; case FREENECT_DEPTH_11BIT_PACKED: case FREENECT_DEPTH_10BIT_PACKED: return ONI_STATUS_NOT_SUPPORTED; + + case FREENECT_DEPTH_11BIT: + case FREENECT_DEPTH_10BIT: + case FREENECT_DEPTH_REGISTERED: + case FREENECT_DEPTH_MM: + try { device->setDepthFormat(format, resolution); } + catch (std::runtime_error e) + { + printf("format-resolution combination not supported: %d-%d\ntrying requested format with default freenect resolution\n", format, resolution); + try { device->setDepthFormat(format); } catch (std::runtime_error e) { printf("format not supported %d\n", format); } + return ONI_STATUS_NOT_SUPPORTED; + } + + video_mode.pixelFormat = ONI_PIXEL_FORMAT_RGB888; + return ONI_STATUS_OK; } - - device->setDepthFormat(format, resolution); - return ONI_STATUS_OK; + } + + + OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + switch(propertyId) + { + default: + return ONI_STATUS_NOT_SUPPORTED; + case ONI_STREAM_PROPERTY_VIDEO_MODE: + if (dataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + OniVideoMode requested_mode = *(reinterpret_cast(data)); + + OniVideoMode* supported_modes = getSupportedModes(); + for (unsigned int i = 0; i < SIZE(supported_modes); i++) + { + if (requested_mode == supported_modes[i]) + { + video_mode = requested_mode; + return ONI_STATUS_OK; + } + } + return ONI_STATUS_NOT_SUPPORTED; + + } + } + + // todo : represent this as static const in implementation + static OniVideoMode* getSupportedModes() + { + OniVideoMode * supported_modes = new OniVideoMode[1]; + // pixelFormat, resolutionX, resolutionY, fps + supported_modes[0] = { ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30 }; + return supported_modes; } diff --git a/Source/Drivers/freenect/FreenectDeviceNI.h b/Source/Drivers/freenect/FreenectDeviceNI.h index 3d3ddf7e..c9c67402 100644 --- a/Source/Drivers/freenect/FreenectDeviceNI.h +++ b/Source/Drivers/freenect/FreenectDeviceNI.h @@ -14,8 +14,8 @@ using namespace oni::driver; class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::FreenectDevice { private: - FreenectImageStream* image_stream; FreenectDepthStream* depth_stream; + FreenectImageStream* image_stream; // OpenNI int m_numSensors; @@ -25,46 +25,39 @@ class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::Freene public: FreenectDeviceNI(freenect_context *_ctx, int _index) : Freenect::FreenectDevice(_ctx, _index) { - image_stream = NULL; depth_stream = NULL; + image_stream = NULL; m_numSensors = 2; + // DEPTH - m_sensors[0].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 2); + m_sensors[0].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 1); m_sensors[0].sensorType = ONI_SENSOR_DEPTH; - m_sensors[0].numSupportedVideoModes = 2; + m_sensors[0].numSupportedVideoModes = 1; m_sensors[0].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; m_sensors[0].pSupportedVideoModes[0].fps = 30; m_sensors[0].pSupportedVideoModes[0].resolutionX = 640; m_sensors[0].pSupportedVideoModes[0].resolutionY = 480; - - m_sensors[0].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; - m_sensors[0].pSupportedVideoModes[1].fps = 30; - m_sensors[0].pSupportedVideoModes[1].resolutionX = 320; - m_sensors[0].pSupportedVideoModes[1].resolutionY = 240; - // COLOR - m_sensors[1].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); + m_sensors[1].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 1); m_sensors[1].sensorType = ONI_SENSOR_COLOR; m_sensors[1].numSupportedVideoModes = 1; - - /* - m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; - m_sensors[1].pSupportedVideoModes[0].fps = 12; // correct? - m_sensors[1].pSupportedVideoModes[0].resolutionX = 1280; - //m_sensors[1].pSupportedVideoModes[0].resolutionY = 960; - m_sensors[1].pSupportedVideoModes[0].resolutionY = 1024; - */ - + m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; m_sensors[1].pSupportedVideoModes[0].fps = 30; m_sensors[1].pSupportedVideoModes[0].resolutionX = 640; m_sensors[1].pSupportedVideoModes[0].resolutionY = 480; /* + m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; + m_sensors[1].pSupportedVideoModes[0].fps = 12; // correct? + m_sensors[1].pSupportedVideoModes[0].resolutionX = 1280; + //m_sensors[1].pSupportedVideoModes[0].resolutionY = 960; + m_sensors[1].pSupportedVideoModes[0].resolutionY = 1024; + m_sensors[1].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_YUV422; m_sensors[1].pSupportedVideoModes[2].fps = 15; m_sensors[1].pSupportedVideoModes[2].resolutionX = 640; @@ -100,17 +93,18 @@ class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::Freene *pSensors = m_sensors; return ONI_STATUS_OK; } + StreamBase* createStream(OniSensorType sensorType) { switch(sensorType) { case ONI_SENSOR_DEPTH: - startDepth(); + Freenect::FreenectDevice::startDepth(); if (depth_stream == NULL) depth_stream = XN_NEW(FreenectDepthStream, this); return depth_stream; case ONI_SENSOR_COLOR: - startVideo(); + Freenect::FreenectDevice::startVideo(); if (image_stream == NULL) image_stream = XN_NEW(FreenectImageStream, this); return image_stream; @@ -125,32 +119,56 @@ class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::Freene { if (pStream == depth_stream) { - stopDepth(); + Freenect::FreenectDevice::stopDepth(); depth_stream = NULL; } if (pStream == image_stream) { - stopVideo(); + Freenect::FreenectDevice::stopVideo(); image_stream = NULL; } XN_DELETE(pStream); } - + // freenect // Do not call directly even in child - void VideoCallback(void *image, uint32_t timestamp) + void DepthCallback(void *depth, uint32_t timestamp) { - image_stream->acquireFrame(image, timestamp); + depth_stream->acquireFrame(depth, timestamp); } // Do not call directly even in child - void DepthCallback(void *depth, uint32_t timestamp) + void VideoCallback(void *image, uint32_t timestamp) { - depth_stream->acquireFrame(depth, timestamp); + image_stream->acquireFrame(image, timestamp); } + + + /* unimplemented from DeviceBase + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} + virtual OniBool isPropertySupported(int propertyId) {return FALSE;} + virtual OniStatus invoke(int commandId, const void* data, int dataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} + virtual OniBool isCommandSupported(int commandId) {return FALSE;} + virtual OniStatus tryManualTrigger() {return ONI_STATUS_OK;} + virtual void setPropertyChangedCallback(PropertyChangedCallback handler, void* pCookie) { m_propertyChangedCallback = handler; m_propertyChangedCookie = pCookie; } + virtual void notifyAllProperties() { return; } + virtual OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF); } + */ }; +/* Device Properties + ONI_DEVICE_PROPERTY_FIRMWARE_VERSION = 0, // By implementation + ONI_DEVICE_PROPERTY_DRIVER_VERSION = 1, // OniVersion + ONI_DEVICE_PROPERTY_HARDWARE_VERSION = 2, // int + ONI_DEVICE_PROPERTY_SERIAL_NUMBER = 3, // string + ONI_DEVICE_PROPERTY_ERROR_STATE = 4, // ?? + ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION = 5, // OniImageRegistrationMode + // Files + ONI_DEVICE_PROPERTY_PLAYBACK_SPEED = 100, // float + ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED = 101, // OniBool +*/ #endif //_FREENECT_DEVICE_NI_H_ diff --git a/Source/Drivers/freenect/FreenectDriver.cpp b/Source/Drivers/freenect/FreenectDriver.cpp index 60148c69..1b0731e7 100644 --- a/Source/Drivers/freenect/FreenectDriver.cpp +++ b/Source/Drivers/freenect/FreenectDriver.cpp @@ -1,12 +1,10 @@ +#include #include "libfreenect.hpp" #include "Driver/OniDriverAPI.h" //#include "KinectDriver.h" #include "FreenectDeviceNI.h" #include "XnHash.h" -#include -#include - using namespace oni::driver; @@ -14,9 +12,6 @@ static const char VENDOR_VAL[] = "Microsoft"; static const char NAME_VAL[] = "Kinect"; - -// EXPERIMENTAL with modified libfreenect and double-inheritance - class FreenectDriver : public DriverBase, private Freenect::Freenect { private: @@ -25,7 +20,6 @@ class FreenectDriver : public DriverBase, private Freenect::Freenect xnl::Hash devices; public: - FreenectDriver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) { freenect_set_log_level(m_ctx, FREENECT_LOG_NOTICE); @@ -39,12 +33,11 @@ class FreenectDriver : public DriverBase, private Freenect::Freenect OniStatus initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) { DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); - std::cout << "number of freenect devices: " << Freenect::deviceCount() << std::endl; - for (int i = 0; i < Freenect::deviceCount(); i++) + for (int i = 1; i <= Freenect::deviceCount(); i++) { OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); std::ostringstream uri; - uri << "freenect:" << i; + uri << "freenect://" << i; xnOSStrCopy(pInfo->uri, uri.str().c_str(), ONI_MAX_STR); xnOSStrCopy(pInfo->vendor, VENDOR_VAL, ONI_MAX_STR); xnOSStrCopy(pInfo->name, NAME_VAL, ONI_MAX_STR); @@ -71,9 +64,9 @@ class FreenectDriver : public DriverBase, private Freenect::Freenect { int id; std::istringstream(iter->Key()->uri) >> id; - FreenectDeviceNI * freenect_device = &createDevice(id); - iter->Value() = freenect_device; - return freenect_device; + FreenectDeviceNI * device = &createDevice(id); + iter->Value() = device; + return device; } } } @@ -100,143 +93,24 @@ class FreenectDriver : public DriverBase, private Freenect::Freenect XN_ASSERT(FALSE); } - void shutdown() - {} - - /* - - virtual OniStatus tryDevice(const char* uri); - - virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); - virtual void disableFrameSync(void* frameSyncGroup); - void StatusUpdate(const OLECHAR* instanceName, bool isConnected); - */ -}; - - -// PARTIAL REFERENCE FROM TestDevice.cpp -/* -class TestDriver : public oni::driver::DriverBase -{ -public: - virtual OniStatus tryDevice(const char* uri) - { - if (xnOSStrCmp(uri, "Test")) - { - return ONI_STATUS_ERROR; - } - - - OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); - xnOSStrCopy(pInfo->uri, uri, ONI_MAX_STR); - xnOSStrCopy(pInfo->vendor, "Test", ONI_MAX_STR); - m_devices[pInfo] = NULL; - - deviceConnected(pInfo); - - return ONI_STATUS_OK; - } -}; -*/ - -// REFERENCE ENDS HERE - - - - - -/* ORIGINAL - -//class FreenectDriver : public kinect_device::KinectDriver, private Freenect::Freenect -//class FreenectDriver : public kinect_device::KinectDriver -class FreenectDriver : public DriverBase -{ -private: - Freenect::Freenect freenect; - xnl::Hash m_devices; - -public: - - FreenectDriver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) - { - } - ~FreenectDriver() - {} + void shutdown() { } - OniStatus initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) + OniStatus tryDevice(const char* uri) { - DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); - std::cout << "number of devices: " << freenect.deviceCount() << std::endl; - for (int i = 0; i < freenect.deviceCount(); i++) - { - OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); - std::ostringstream uri; - uri << "freenect:" << i; - xnOSStrCopy(pInfo->uri, uri.str().c_str(), ONI_MAX_STR); - xnOSStrCopy(pInfo->vendor, VENDOR_VAL, ONI_MAX_STR); - xnOSStrCopy(pInfo->name, NAME_VAL, ONI_MAX_STR); - m_devices[pInfo] = NULL; - //deviceConnected(pInfo); - //deviceStateChanged(pInfo, 0); - } + DeviceBase* device = deviceOpen(uri); + if (device == NULL) + return ONI_STATUS_ERROR; + deviceClose(device); + return ONI_STATUS_OK; - } - - DeviceBase* deviceOpen(const char* uri) - { - for (xnl::Hash::Iterator iter = m_devices.Begin(); iter != m_devices.End(); ++iter) - { - if (xnOSStrCmp(iter->Key()->uri, uri) == 0) - { - // found - if (iter->Value() != NULL) - { - // already using - return iter->Value(); - } - else - { - int id; - std::istringstream(iter->Key()->uri) >> id; - FreenectDeviceNI * freenect_device = &freenect.createDevice(id); - //iter->Value() = freenect_device; - std::cout << "number of devices: " << freenect.deviceCount() << std::endl; - //return freenect_device; - } - } - } - return NULL; - } - - void deviceClose(DeviceBase* pDevice) - { - for (xnl::Hash::Iterator iter = m_devices.Begin(); iter != m_devices.End(); ++iter) - { - if (iter->Value() == pDevice) - { - iter->Value() = NULL; - XN_DELETE(pDevice); - int id; - std::istringstream(iter->Key()->uri) >> id; - freenect.deleteDevice(id); - return; - } - } - // not our device?! - XN_ASSERT(FALSE); } - - void shutdown() - {} - /* - - virtual OniStatus tryDevice(const char* uri); + /* unimplemented from DriverBase virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); virtual void disableFrameSync(void* frameSyncGroup); - void StatusUpdate(const OLECHAR* instanceName, bool isConnected); */ -//}; +}; + ONI_EXPORT_DRIVER(FreenectDriver); diff --git a/Source/Drivers/freenect/FreenectImageStream.h b/Source/Drivers/freenect/FreenectImageStream.h index 9a5f070a..d59a53e3 100644 --- a/Source/Drivers/freenect/FreenectImageStream.h +++ b/Source/Drivers/freenect/FreenectImageStream.h @@ -9,16 +9,16 @@ class FreenectImageStream : public FreenectStream { public: - FreenectImageStream(Freenect::FreenectDevice* pDevice, freenect_video_format format = FREENECT_VIDEO_YUV_RAW) : FreenectStream(pDevice) + FreenectImageStream(Freenect::FreenectDevice* pDevice, freenect_video_format format = FREENECT_VIDEO_RGB) : FreenectStream(pDevice) { - xnl::Pair res = convertResolution(device->getVideoResolution()); - - setFormat(format, FREENECT_RESOLUTION_LOW); + setFormat(format, FREENECT_RESOLUTION_MEDIUM); - video_mode.pixelFormat = ONI_PIXEL_FORMAT_RGB888; + xnl::Pair res = convertResolution(device->getVideoResolution()); video_mode.fps = 30; video_mode.resolutionX = res.first; - video_mode.resolutionY = res.second; + video_mode.resolutionY = res.second; + + //printf("FreenectImageStream: resolutionX = %d; resolutionY = %d\n", video_mode.resolutionX, video_mode.resolutionY); } int getBytesPerPixel() { return sizeof(OniRGB888Pixel); } @@ -47,21 +47,42 @@ class FreenectImageStream : public FreenectStream OniStatus setFormat(freenect_video_format format, freenect_resolution resolution = FREENECT_RESOLUTION_MEDIUM) { switch(format) - { + { default: - video_mode.pixelFormat = ONI_PIXEL_FORMAT_RGB888; - break; case FREENECT_VIDEO_BAYER: - //video_mode.pixelFormat = ONI_PIXEL_FORMAT_ - //break; case FREENECT_VIDEO_IR_8BIT: case FREENECT_VIDEO_IR_10BIT: case FREENECT_VIDEO_IR_10BIT_PACKED: - case FREENECT_VIDEO_YUV_RAW: return ONI_STATUS_NOT_IMPLEMENTED; + + case FREENECT_VIDEO_RGB: + case FREENECT_VIDEO_YUV_RGB: + case FREENECT_VIDEO_YUV_RAW: + try { device->setVideoFormat(format, resolution); } + catch (std::runtime_error e) + { + printf("format-resolution combination not supported: %d-%d\ntrying format with default freenect resolution\n", format, resolution); + try { device->setVideoFormat(format); } catch (std::runtime_error e) { printf("format not supported %d\n", format); } + return ONI_STATUS_NOT_SUPPORTED; + } + + video_mode.pixelFormat = ONI_PIXEL_FORMAT_RGB888; + return ONI_STATUS_OK; + } + + switch(resolution) + { + default: + break; + case FREENECT_RESOLUTION_LOW: + break; + case FREENECT_RESOLUTION_MEDIUM: + break; + case FREENECT_RESOLUTION_HIGH: + // video_mode.resolutionX = 1280; + // video_mode.resolutionY = 1024; // 960 ? + break; } - device->setVideoFormat(format, resolution); - return ONI_STATUS_OK; } diff --git a/Source/Drivers/freenect/FreenectStream.h b/Source/Drivers/freenect/FreenectStream.h index 1a427169..a7a2f790 100644 --- a/Source/Drivers/freenect/FreenectStream.h +++ b/Source/Drivers/freenect/FreenectStream.h @@ -3,7 +3,7 @@ #include "libfreenect.h" #include "Driver/OniDriverAPI.h" -//#include "XnLib.h" +#include "XnLib.h" //#include "XnHash.h" //#include "XnEvent.h" //#include "XnPlatform.h" @@ -30,6 +30,7 @@ class FreenectStream : public oni::driver::StreamBase protected: Freenect::FreenectDevice* device; OniVideoMode video_mode; // derived classes should set this up in constructor + static const OniVideoMode* supported_modes; int frame_id; // number each frame bool running; @@ -46,95 +47,56 @@ class FreenectStream : public oni::driver::StreamBase virtual void buildFrame(void* data, OniDriverFrame* pFrame) = 0; - - OniStatus start() - { - running = true; - return ONI_STATUS_OK; - } - void stop() - { - running = false; - } - - - static xnl::Pair convertResolution(freenect_resolution resolution) - { - switch(resolution) - { - case FREENECT_RESOLUTION_LOW: - return xnl::Pair(320, 240); - case FREENECT_RESOLUTION_MEDIUM: - return xnl::Pair(640, 480); - case FREENECT_RESOLUTION_HIGH: - return xnl::Pair(1280, 1024); - } - } + OniStatus start() { running = true; return ONI_STATUS_OK; } + void stop() { running = false; } - /* - // Stream properties - enum - { - ONI_STREAM_PROPERTY_CROPPING = 0, // OniCropping* - ONI_STREAM_PROPERTY_HORIZONTAL_FOV = 1, // float: radians - ONI_STREAM_PROPERTY_VERTICAL_FOV = 2, // float: radians - ONI_STREAM_PROPERTY_VIDEO_MODE = 3, // OniVideoMode* - - ONI_STREAM_PROPERTY_MAX_VALUE = 4, // int - ONI_STREAM_PROPERTY_MIN_VALUE = 5, // int - - ONI_STREAM_PROPERTY_STRIDE = 6, // int - ONI_STREAM_PROPERTY_MIRRORING = 7, // OniBool - - ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES = 8, // int - - // Camera - ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE = 100, // OniBool - ONI_STREAM_PROPERTY_AUTO_EXPOSURE = 101, // OniBool - }; - */ + OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) == ONI_STATUS_NOT_SUPPORTED) ? false : true; } OniStatus getProperty(int propertyId, void* data, int* pDataSize) { switch(propertyId) { default: + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + 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_MIRRORING: // OniBool + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool return ONI_STATUS_NOT_SUPPORTED; - case ONI_STREAM_PROPERTY_VIDEO_MODE: + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* if (*pDataSize != sizeof(OniVideoMode)) { printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); return ONI_STATUS_ERROR; - } - OniVideoMode* pVideoMode = static_cast(data); - pVideoMode->pixelFormat = video_mode.pixelFormat; - pVideoMode->fps = video_mode.fps; - pVideoMode->resolutionX = video_mode.resolutionX; - pVideoMode->resolutionY = video_mode.resolutionY; + } + *(static_cast(data)) = video_mode; return ONI_STATUS_OK; } } - - - - OniStatus setProperty(int propertyId, const void* data, int dataSize) + static xnl::Pair convertResolution(freenect_resolution resolution) { - switch(propertyId) + switch(resolution) { - default: - return ONI_STATUS_NOT_IMPLEMENTED; - case ONI_STREAM_PROPERTY_VIDEO_MODE: - if (dataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); - return ONI_STATUS_ERROR; - } - - return ONI_STATUS_NOT_IMPLEMENTED; + case FREENECT_RESOLUTION_LOW: + return xnl::Pair(320, 240); + case FREENECT_RESOLUTION_MEDIUM: + return xnl::Pair(640, 480); + case FREENECT_RESOLUTION_HIGH: + return xnl::Pair(1280, 1024); } } + + + void acquireFrame(void* data, uint32_t timestamp) { @@ -154,24 +116,18 @@ class FreenectStream : public oni::driver::StreamBase ((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount = 1; pFrame->frame.frameIndex = frame_id++; - //pFrame->frame.timestamp = m_frameId*33000; pFrame->frame.timestamp = timestamp; - pFrame->frame.videoMode = video_mode; pFrame->frame.width = video_mode.resolutionX; pFrame->frame.height = video_mode.resolutionY; pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; pFrame->frame.croppingEnabled = FALSE; - buildFrame(data, pFrame); raiseNewFrame(pFrame); } - void addRefToFrame(OniDriverFrame* pFrame) - { - ++((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount; - } + void addRefToFrame(OniDriverFrame* pFrame) { ++((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount; } void releaseFrame(OniDriverFrame* pFrame) { if (0 == --((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount) @@ -181,68 +137,17 @@ class FreenectStream : public oni::driver::StreamBase xnOSFree(pFrame); } } + + + /* unimplemented from StreamBase + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} + virtual OniStatus invoke(int commandId, const void* data, int dataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} + virtual OniBool isCommandSupported(int commandId) {return FALSE;} + virtual void setNewFrameCallback(NewFrameCallback handler, void* pCookie) { m_newFrameCallback = handler; m_newFrameCallbackCookie = pCookie; } + virtual void setPropertyChangedCallback(PropertyChangedCallback handler, void* pCookie) { m_propertyChangedCallback = handler; m_propertyChangedCookie = pCookie; } + virtual void notifyAllProperties() { return; } + virtual OniStatus convertDepthToColorCoordinates(StreamBase* colorStream, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY) { return ONI_STATUS_NOT_SUPPORTED; } + */ }; - - -// PARTIAL REFERENCE FROM TestDevice.cpp -/* -class TestStream : public oni::driver::StreamBase -{ -public: - TestStream() : oni::driver::StreamBase() - { - m_osEvent.Create(TRUE); - m_sendCount = 0; - } - - virtual OniStatus SetVideoMode(OniVideoMode*) = 0; - virtual OniStatus GetVideoMode(OniVideoMode* pVideoMode) = 0; - - - OniStatus setProperty(int propertyId, const void* data, int dataSize) - { - if (propertyId == ONI_STREAM_PROPERTY_VIDEO_MODE) - { - if (dataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); - return ONI_STATUS_ERROR; - } - return SetVideoMode((OniVideoMode*)data); - } - else if (propertyId == 666) - { - if (dataSize != sizeof(int)) - { - printf("Unexpected size: %d != %d\n", dataSize, sizeof(int)); - return ONI_STATUS_ERROR; - } - - // Increment the send count. - m_cs.Lock(); - m_sendCount += *((int*)data); - m_cs.Unlock(); - - // Raise the OS event, to allow thread to start working. - m_osEvent.Set(); - } - - return ONI_STATUS_NOT_IMPLEMENTED; - } - -protected: - - int singleRes(int x, int y) {return y*TEST_RESOLUTION_X+x;} - int m_sendCount; - - xnl::CriticalSection m_cs; - xnl::OSEvent m_osEvent; -}; -*/ - -// END REFERENCE - - - #endif // _FREENECT_STREAM_H_ From 7aa24b39336877aec87f07312591d82ce5bf5972 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Thu, 3 Jan 2013 01:38:54 -0500 Subject: [PATCH 08/18] Improved format handling --- Source/Drivers/freenect/FreenectDeviceNI.h | 83 +++++-------------- Source/Drivers/freenect/FreenectImageStream.h | 10 +++ Source/Drivers/freenect/FreenectStream.h | 4 +- Source/Drivers/freenect/Makefile | 2 +- 4 files changed, 32 insertions(+), 67 deletions(-) diff --git a/Source/Drivers/freenect/FreenectDeviceNI.h b/Source/Drivers/freenect/FreenectDeviceNI.h index c9c67402..89ffc996 100644 --- a/Source/Drivers/freenect/FreenectDeviceNI.h +++ b/Source/Drivers/freenect/FreenectDeviceNI.h @@ -15,82 +15,37 @@ class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::Freene { private: FreenectDepthStream* depth_stream; - FreenectImageStream* image_stream; - - // OpenNI - int m_numSensors; - OniSensorInfo m_sensors[3]; - + FreenectImageStream* image_stream; public: FreenectDeviceNI(freenect_context *_ctx, int _index) : Freenect::FreenectDevice(_ctx, _index) { depth_stream = NULL; image_stream = NULL; - - m_numSensors = 2; - - - // DEPTH - m_sensors[0].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 1); - m_sensors[0].sensorType = ONI_SENSOR_DEPTH; - m_sensors[0].numSupportedVideoModes = 1; - - m_sensors[0].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; - m_sensors[0].pSupportedVideoModes[0].fps = 30; - m_sensors[0].pSupportedVideoModes[0].resolutionX = 640; - m_sensors[0].pSupportedVideoModes[0].resolutionY = 480; - - // COLOR - m_sensors[1].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 1); - m_sensors[1].sensorType = ONI_SENSOR_COLOR; - m_sensors[1].numSupportedVideoModes = 1; - - m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; - m_sensors[1].pSupportedVideoModes[0].fps = 30; - m_sensors[1].pSupportedVideoModes[0].resolutionX = 640; - m_sensors[1].pSupportedVideoModes[0].resolutionY = 480; - - /* - m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; - m_sensors[1].pSupportedVideoModes[0].fps = 12; // correct? - m_sensors[1].pSupportedVideoModes[0].resolutionX = 1280; - //m_sensors[1].pSupportedVideoModes[0].resolutionY = 960; - m_sensors[1].pSupportedVideoModes[0].resolutionY = 1024; - - m_sensors[1].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_YUV422; - m_sensors[1].pSupportedVideoModes[2].fps = 15; - m_sensors[1].pSupportedVideoModes[2].resolutionX = 640; - m_sensors[1].pSupportedVideoModes[2].resolutionY = 480; - */ - - /* - m_sensors[2].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); - m_sensors[2].sensorType = ONI_SENSOR_IR; - m_sensors[2].numSupportedVideoModes = 3; - m_sensors[2].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; - m_sensors[2].pSupportedVideoModes[0].fps = DEFAULT_FPS; - m_sensors[2].pSupportedVideoModes[0].resolutionX = 640; - m_sensors[2].pSupportedVideoModes[0].resolutionY = 480; - - m_sensors[2].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_GRAY16; - m_sensors[2].pSupportedVideoModes[1].fps = DEFAULT_FPS; - m_sensors[2].pSupportedVideoModes[1].resolutionX = 640; - m_sensors[2].pSupportedVideoModes[1].resolutionY = 480; - - m_sensors[2].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_GRAY8; - m_sensors[2].pSupportedVideoModes[2].fps = DEFAULT_FPS; - m_sensors[2].pSupportedVideoModes[2].resolutionX = 640; - m_sensors[2].pSupportedVideoModes[2].resolutionY = 480; - */ } // OpenNI OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) { - *numSensors = m_numSensors; - *pSensors = m_sensors; + *numSensors = 2; + + OniSensorInfo * sensors = new OniSensorInfo[*numSensors]; + + // DEPTH + OniVideoMode* supported_modes = FreenectDepthStream::getSupportedModes(); + sensors[0].sensorType = ONI_SENSOR_DEPTH; + sensors[0].numSupportedVideoModes = SIZE(supported_modes); + sensors[0].pSupportedVideoModes = supported_modes; + + // COLOR + supported_modes = FreenectImageStream::getSupportedModes(); + sensors[1].sensorType = ONI_SENSOR_COLOR; + sensors[1].numSupportedVideoModes = SIZE(supported_modes); + sensors[1].pSupportedVideoModes = supported_modes; + + + *pSensors = sensors; return ONI_STATUS_OK; } diff --git a/Source/Drivers/freenect/FreenectImageStream.h b/Source/Drivers/freenect/FreenectImageStream.h index d59a53e3..e28ed176 100644 --- a/Source/Drivers/freenect/FreenectImageStream.h +++ b/Source/Drivers/freenect/FreenectImageStream.h @@ -86,6 +86,16 @@ class FreenectImageStream : public FreenectStream } + // todo : represent this as static const in implementation + static OniVideoMode* getSupportedModes() + { + OniVideoMode * supported_modes = new OniVideoMode[1]; + // pixelFormat, resolutionX, resolutionY, fps + supported_modes[0] = { ONI_PIXEL_FORMAT_RGB888, 640, 480, 30 }; + return supported_modes; + } + + void buildFrame(void* image, OniDriverFrame* pFrame) { pFrame->frame.sensorType = ONI_SENSOR_COLOR; diff --git a/Source/Drivers/freenect/FreenectStream.h b/Source/Drivers/freenect/FreenectStream.h index a7a2f790..1cfe9ac6 100644 --- a/Source/Drivers/freenect/FreenectStream.h +++ b/Source/Drivers/freenect/FreenectStream.h @@ -1,7 +1,7 @@ #ifndef _FREENECT_STREAM_H_ #define _FREENECT_STREAM_H_ -#include "libfreenect.h" +#include "libfreenect.hpp" #include "Driver/OniDriverAPI.h" #include "XnLib.h" //#include "XnHash.h" @@ -30,7 +30,7 @@ class FreenectStream : public oni::driver::StreamBase protected: Freenect::FreenectDevice* device; OniVideoMode video_mode; // derived classes should set this up in constructor - static const OniVideoMode* supported_modes; + static const OniVideoMode* supported_modes[]; int frame_id; // number each frame bool running; diff --git a/Source/Drivers/freenect/Makefile b/Source/Drivers/freenect/Makefile index d007bfb3..f4e25f06 100644 --- a/Source/Drivers/freenect/Makefile +++ b/Source/Drivers/freenect/Makefile @@ -24,7 +24,7 @@ ifneq ("$(OSTYPE)","Darwin") USED_LIBS += rt endif -CFLAGS += -Wall +#CFLAGS += -Wall OUT_DIR := $(OUT_DIR)/OpenNI2/Drivers From 3813f222fdce8bfeb52cf243e7b00cdc4df4f94d Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Thu, 3 Jan 2013 04:16:16 -0500 Subject: [PATCH 09/18] Continue reworking format support --- Source/Drivers/freenect/FreenectDepthStream.h | 73 +++++++------------ Source/Drivers/freenect/FreenectDeviceNI.h | 9 ++- Source/Drivers/freenect/FreenectStream.h | 15 +++- 3 files changed, 43 insertions(+), 54 deletions(-) diff --git a/Source/Drivers/freenect/FreenectDepthStream.h b/Source/Drivers/freenect/FreenectDepthStream.h index efdf949e..d0dc32ef 100644 --- a/Source/Drivers/freenect/FreenectDepthStream.h +++ b/Source/Drivers/freenect/FreenectDepthStream.h @@ -3,22 +3,13 @@ #include "FreenectStream.h" -#define SIZE(array) sizeof array / sizeof 0[array] - - -static bool operator==(const OniVideoMode& left, const OniVideoMode& right) -{ - return (left.pixelFormat == right.pixelFormat && left.resolutionX == right.resolutionX - && left.resolutionY == right.resolutionY && left.fps == right.fps) ? true : false; -} - class FreenectDepthStream : public FreenectStream { public: FreenectDepthStream(Freenect::FreenectDevice* pDevice, freenect_depth_format format = FREENECT_DEPTH_11BIT) : FreenectStream(pDevice) { - setFormat(format, FREENECT_RESOLUTION_MEDIUM); + setVideoMode(getSupportedModes()[0]); xnl::Pair res = convertResolution(device->getDepthResolution()); video_mode.fps = 30; @@ -45,34 +36,32 @@ class FreenectDepthStream : public FreenectStream ONI_PIXEL_FORMAT_SHIFT_9_2 = 102, ONI_PIXEL_FORMAT_SHIFT_9_3 = 103, */ - freenect_depth_format getFormat() - { - return device->getDepthFormat(); - } - OniStatus setFormat(freenect_depth_format format, freenect_resolution resolution = FREENECT_RESOLUTION_MEDIUM) + + OniStatus setVideoMode(OniVideoMode requested_mode) { - switch(format) + OniVideoMode* supported_modes = getSupportedModes(); + freenect_depth_format format; + freenect_resolution resolution; + + if (video_mode == supported_modes[0]) { - default: - case FREENECT_DEPTH_11BIT_PACKED: - case FREENECT_DEPTH_10BIT_PACKED: - return ONI_STATUS_NOT_SUPPORTED; - - case FREENECT_DEPTH_11BIT: - case FREENECT_DEPTH_10BIT: - case FREENECT_DEPTH_REGISTERED: - case FREENECT_DEPTH_MM: - try { device->setDepthFormat(format, resolution); } - catch (std::runtime_error e) - { - printf("format-resolution combination not supported: %d-%d\ntrying requested format with default freenect resolution\n", format, resolution); - try { device->setDepthFormat(format); } catch (std::runtime_error e) { printf("format not supported %d\n", format); } - return ONI_STATUS_NOT_SUPPORTED; - } - - video_mode.pixelFormat = ONI_PIXEL_FORMAT_RGB888; - return ONI_STATUS_OK; + format = FREENECT_DEPTH_11BIT; + resolution = FREENECT_RESOLUTION_MEDIUM; } + else + { + return ONI_STATUS_NOT_SUPPORTED; + } + + try { device->setDepthFormat(format, resolution); } + catch (std::runtime_error e) + { + printf("format-resolution combination not supported: %d-%d\n", format, resolution); + return ONI_STATUS_NOT_SUPPORTED; + } + + video_mode = requested_mode; + return ONI_STATUS_OK; } @@ -88,19 +77,7 @@ class FreenectDepthStream : public FreenectStream printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); return ONI_STATUS_ERROR; } - OniVideoMode requested_mode = *(reinterpret_cast(data)); - - OniVideoMode* supported_modes = getSupportedModes(); - for (unsigned int i = 0; i < SIZE(supported_modes); i++) - { - if (requested_mode == supported_modes[i]) - { - video_mode = requested_mode; - return ONI_STATUS_OK; - } - } - return ONI_STATUS_NOT_SUPPORTED; - + return setVideoMode(*(reinterpret_cast(data))); } } diff --git a/Source/Drivers/freenect/FreenectDeviceNI.h b/Source/Drivers/freenect/FreenectDeviceNI.h index 89ffc996..809260c1 100644 --- a/Source/Drivers/freenect/FreenectDeviceNI.h +++ b/Source/Drivers/freenect/FreenectDeviceNI.h @@ -4,9 +4,12 @@ #include "libfreenect.hpp" #include "Driver/OniDriverAPI.h" #include "XnLog.h" + #include "FreenectDepthStream.h" #include "FreenectImageStream.h" +#include "FreenectStream.h" + using namespace oni::driver; @@ -14,8 +17,10 @@ using namespace oni::driver; class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::FreenectDevice { private: - FreenectDepthStream* depth_stream; - FreenectImageStream* image_stream; + //FreenectDepthStream* depth_stream; + //FreenectImageStream* image_stream; + FreenectStream *depth_stream, *image_stream; + public: FreenectDeviceNI(freenect_context *_ctx, int _index) : Freenect::FreenectDevice(_ctx, _index) diff --git a/Source/Drivers/freenect/FreenectStream.h b/Source/Drivers/freenect/FreenectStream.h index 1cfe9ac6..a6f14b85 100644 --- a/Source/Drivers/freenect/FreenectStream.h +++ b/Source/Drivers/freenect/FreenectStream.h @@ -13,19 +13,25 @@ #include "XnPair.h" -// todo : change video mode on the fly - +#define SIZE(array) sizeof array / sizeof 0[array] typedef struct { int refCount; } FreenectStreamFrameCookie; +static bool operator==(const OniVideoMode& left, const OniVideoMode& right) +{ + return (left.pixelFormat == right.pixelFormat && left.resolutionX == right.resolutionX + && left.resolutionY == right.resolutionY && left.fps == right.fps) ? true : false; +} + + class FreenectStream : public oni::driver::StreamBase { private: - virtual int getBytesPerPixel() = 0; + //virtual int getBytesPerPixel() = 0; protected: Freenect::FreenectDevice* device; @@ -45,7 +51,8 @@ class FreenectStream : public oni::driver::StreamBase stop(); } - virtual void buildFrame(void* data, OniDriverFrame* pFrame) = 0; + //virtual void buildFrame(void* data, OniDriverFrame* pFrame) = 0; + virtual void buildFrame(void* data, OniDriverFrame* pFrame) {}; OniStatus start() { running = true; return ONI_STATUS_OK; } From 287fe4294c2f12c7d1dc29132611833ad3a05155 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Fri, 4 Jan 2013 13:47:09 -0500 Subject: [PATCH 10/18] Separated and minimalized Stream implementations --- Makefile | 4 +- Samples/freenectTest/Makefile | 20 -- Samples/freenectTest/freenectTest.cpp | 199 ------------------ .../Drivers/freenect/FreenectDepthStream.cpp | 125 +++++++++++ Source/Drivers/freenect/FreenectDepthStream.h | 111 ++-------- Source/Drivers/freenect/FreenectDeviceNI.h | 6 +- .../Drivers/freenect/FreenectImageStream.cpp | 126 +++++++++++ Source/Drivers/freenect/FreenectImageStream.h | 117 ++-------- Source/Drivers/freenect/FreenectStream.h | 125 +++++------ 9 files changed, 338 insertions(+), 495 deletions(-) delete mode 100644 Samples/freenectTest/Makefile delete mode 100644 Samples/freenectTest/freenectTest.cpp create mode 100644 Source/Drivers/freenect/FreenectDepthStream.cpp create mode 100644 Source/Drivers/freenect/FreenectImageStream.cpp diff --git a/Makefile b/Makefile index dabe1c88..391bcc2d 100644 --- a/Makefile +++ b/Makefile @@ -42,8 +42,7 @@ CORE_SAMPLES = \ Samples/EventBasedRead \ Samples/MultipleStreamRead \ Samples/MWClosestPoint \ - Samples/MWClosestPointApp \ - Samples/freenectTest + Samples/MWClosestPointApp ifeq "$(GLUT_SUPPORTED)" "1" CORE_SAMPLES += \ @@ -107,7 +106,6 @@ Samples/EventBasedRead: $(OPENNI) Samples/MultipleStreamRead: $(OPENNI) Samples/MWClosestPoint: $(OPENNI) Samples/MWClosestPointApp: $(OPENNI) Samples/MWClosestPoint -Samples/freenectTest: $(OPENNI) Source/Drivers/freenect Samples/SimpleViewer: $(OPENNI) Samples/MultiDepthViewer: $(OPENNI) diff --git a/Samples/freenectTest/Makefile b/Samples/freenectTest/Makefile deleted file mode 100644 index e2a7cea9..00000000 --- a/Samples/freenectTest/Makefile +++ /dev/null @@ -1,20 +0,0 @@ -include ../../ThirdParty/PSCommon/BuildSystem/CommonDefs.mak - -BIN_DIR = ../../Bin - -INC_DIRS = \ - /usr/include/libfreenect \ - ../../Source/Drivers/freenect \ - ../../Include \ - ../../ThirdParty/PSCommon/XnLib/Include - -SRC_FILES = *.cpp - -LIB_DIRS = ../../ThirdParty/PSCommon/XnLib/Bin/$(PLATFORM)-$(CFG) -USED_LIBS += OpenNI2 freenect XnLib - -EXE_NAME = freenectTest - -CFLAGS += -Wall - -include ../../ThirdParty/PSCommon/BuildSystem/CommonCppMakefile diff --git a/Samples/freenectTest/freenectTest.cpp b/Samples/freenectTest/freenectTest.cpp deleted file mode 100644 index 4c044e74..00000000 --- a/Samples/freenectTest/freenectTest.cpp +++ /dev/null @@ -1,199 +0,0 @@ -#include - -#include -#include -#include - -#include "FreenectDriver.cpp" - -#include - - -FreenectDriver* f_driver = new FreenectDriver(NULL); -oni::driver::DeviceBase* f_device; -oni::driver::StreamBase* f_depth; - - -void shutdown(int s) { - std::cout << "Caught signal " << s << std::endl; - - if (f_device != NULL) { - f_device->destroyStream(f_depth); - f_driver->deviceClose(f_device); - } - - exit(1); -} - -int alt(); -void analyzeFrame(const openni::VideoFrameRef& frame); - - -int main(int argc, char **argv) -{ - // handle SIGINT (Ctrl+c) - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = shutdown; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); - - - return alt(); - - - f_driver->initialize(NULL, NULL, NULL, NULL); - f_device = f_driver->deviceOpen("freenect:0"); - f_depth = f_device->createStream(ONI_SENSOR_DEPTH); - - - // wait for Ctrl+c - while (true); - - - shutdown(0); - return 0; -} - - -using namespace openni; - -int alt() -{ - Status rc = OpenNI::initialize(); - if (rc != STATUS_OK) - { - printf("Initialize failed\n%s\n", OpenNI::getExtendedError()); - return 1; - } - - Device device; - rc = device.open(ANY_DEVICE); - //rc = device.open("freenect:0"); - if (rc != STATUS_OK) - { - printf("Couldn't open device\n%s\n", OpenNI::getExtendedError()); - return 2; - } - - VideoStream depth, color; - - if (device.getSensorInfo(SENSOR_DEPTH) != NULL) - { - rc = depth.create(device, SENSOR_DEPTH); - if (rc == STATUS_OK) - { - rc = depth.start(); - if (rc != STATUS_OK) - { - printf("Couldn't start depth stream\n%s\n", OpenNI::getExtendedError()); - } - } - else - { - printf("Couldn't create depth stream\n%s\n", OpenNI::getExtendedError()); - } - } - - if (device.getSensorInfo(SENSOR_COLOR) != NULL) - { - rc = color.create(device, SENSOR_COLOR); - if (rc == STATUS_OK) - { - rc = color.start(); - if (rc != STATUS_OK) - { - printf("Couldn't start color stream\n%s\n", OpenNI::getExtendedError()); - } - } - else - { - printf("Couldn't create color stream\n%s\n", OpenNI::getExtendedError()); - } - } - - - VideoFrameRef frame; - - VideoStream* streams[] = {&depth, &color}; - - while (true) - { - int readyStream = -1; - rc = OpenNI::waitForAnyStream(streams, 2, &readyStream); - if (rc != STATUS_OK) - { - printf("Wait failed\n"); - continue; - } - - switch (readyStream) - { - case 0: - // Depth - depth.readFrame(&frame); - break; - case 1: - // Color - color.readFrame(&frame); - break; - default: - printf("Unxpected stream\n"); - } - - analyzeFrame(frame); - } - - - depth.stop(); - color.stop(); - depth.destroy(); - color.destroy(); - device.close(); - OpenNI::shutdown(); - - return 0; -} - - -void analyzeFrame(const VideoFrameRef& frame) -{ - DepthPixel* pDepth; - RGB888Pixel* pColor; - - int middleIndex = (frame.getHeight()+1)*frame.getWidth()/2; - - - - std::cout << "frame width = " << frame.getWidth() << "; frame height = " << frame.getHeight() << std::endl; - /* - pDepth = (DepthPixel*)frame.getData(); - for (int x = 0; x < frame.getWidth(); x++) { - for (int y = 0; y < frame.getHeight(); y++) { - std::cout << pDepth[x*y+x] << " "; - } - std::cout << std::endl; - } - */ - - - - switch (frame.getVideoMode().getPixelFormat()) - { - case PIXEL_FORMAT_DEPTH_1_MM: - case PIXEL_FORMAT_DEPTH_100_UM: - pDepth = (DepthPixel*)frame.getData(); - printf("[%08llu] %8d\n", (long long)frame.getTimestamp(), - pDepth[middleIndex]); - break; - case PIXEL_FORMAT_RGB888: - pColor = (RGB888Pixel*)frame.getData(); - printf("[%08llu] 0x%02x%02x%02x\n", (long long)frame.getTimestamp(), - pColor[middleIndex].r&0xff, - pColor[middleIndex].g&0xff, - pColor[middleIndex].b&0xff); - break; - default: - printf("Unknown format\n"); - } -} diff --git a/Source/Drivers/freenect/FreenectDepthStream.cpp b/Source/Drivers/freenect/FreenectDepthStream.cpp new file mode 100644 index 00000000..66bb4ef9 --- /dev/null +++ b/Source/Drivers/freenect/FreenectDepthStream.cpp @@ -0,0 +1,125 @@ +#include "FreenectDepthStream.h" +#include "XnLib.h" + + +FreenectDepthStream::FreenectDepthStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) +{ + setVideoMode(getSupportedVideoModes()[0]); +} + +OniVideoMode* FreenectDepthStream::getSupportedVideoModes() +{ + OniVideoMode* supported_video_modes = new OniVideoMode[1] { + { ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30 }, + }; + return supported_video_modes; +} + +OniStatus FreenectDepthStream::setVideoMode(OniVideoMode requested_mode) +{ + OniVideoMode* supported_modes = getSupportedVideoModes(); + freenect_depth_format format; + freenect_resolution resolution; + + if (requested_mode == supported_modes[0]) + { + format = FREENECT_DEPTH_11BIT; + resolution = FREENECT_RESOLUTION_MEDIUM; + + /* working format possiblities + FREENECT_DEPTH_10BIT + FREENECT_DEPTH_REGISTERED + FREENECT_DEPTH_MM + */ + } + else + { + return ONI_STATUS_NOT_SUPPORTED; + } + + try { device->setDepthFormat(format, resolution); } + catch (std::runtime_error e) + { + printf("format-resolution combination not supported: %d-%d\n", format, resolution); + return ONI_STATUS_NOT_SUPPORTED; + } + + video_mode = requested_mode; + return ONI_STATUS_OK; +} + +void FreenectDepthStream::buildFrame(void* depth, OniDriverFrame* pFrame) +{ + pFrame->frame.sensorType = ONI_SENSOR_DEPTH; + pFrame->frame.videoMode = video_mode; + pFrame->frame.width = video_mode.resolutionX; + pFrame->frame.height = video_mode.resolutionY; + pFrame->frame.stride = video_mode.resolutionX*sizeof(OniDepthPixel); + pFrame->frame.dataSize = device->getDepthBufferSize(); + + // copy stream buffer from freenect + pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); + if (pFrame->frame.data == NULL) + { + XN_ASSERT(FALSE); + return; + } + uint8_t* _data = static_cast(depth); + uint8_t* frame_data = static_cast(pFrame->frame.data); + std::copy(_data, _data+pFrame->frame.dataSize, frame_data); +} + + +// for oni::driver::StreamBase + +OniStatus FreenectDepthStream::getProperty(int propertyId, void* data, int* pDataSize) +{ + switch(propertyId) + { + default: + return FreenectStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (*pDataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = video_mode; + return ONI_STATUS_OK; + } +} +OniStatus FreenectDepthStream::setProperty(int propertyId, const void* data, int dataSize) +{ + switch(propertyId) + { + default: + return FreenectStream::setProperty(propertyId, data, dataSize); + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (dataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + raisePropertyChanged(propertyId, data, dataSize); + return setVideoMode(*(reinterpret_cast(data))); + } +} + + +/* depth video modes reference + +FREENECT_DEPTH_11BIT = 0, //< 11 bit depth information in one uint16_t/pixel +FREENECT_DEPTH_10BIT = 1, //< 10 bit depth information in one uint16_t/pixel +FREENECT_DEPTH_11BIT_PACKED = 2, //< 11 bit packed depth information +FREENECT_DEPTH_10BIT_PACKED = 3, //< 10 bit packed depth information +FREENECT_DEPTH_REGISTERED = 4, //< processed depth data in mm, aligned to 640x480 RGB +FREENECT_DEPTH_MM = 5, //< depth to each pixel in mm, but left unaligned to RGB image +FREENECT_DEPTH_DUMMY = 2147483647, //< Dummy value to force enum to be 32 bits wide + +ONI_PIXEL_FORMAT_DEPTH_1_MM = 100, +ONI_PIXEL_FORMAT_DEPTH_100_UM = 101, +ONI_PIXEL_FORMAT_SHIFT_9_2 = 102, +ONI_PIXEL_FORMAT_SHIFT_9_3 = 103, +*/ diff --git a/Source/Drivers/freenect/FreenectDepthStream.h b/Source/Drivers/freenect/FreenectDepthStream.h index d0dc32ef..099edac4 100644 --- a/Source/Drivers/freenect/FreenectDepthStream.h +++ b/Source/Drivers/freenect/FreenectDepthStream.h @@ -6,108 +6,21 @@ class FreenectDepthStream : public FreenectStream { +private: + OniVideoMode video_mode; + void buildFrame(void* data, OniDriverFrame* pFrame); + public: - FreenectDepthStream(Freenect::FreenectDevice* pDevice, freenect_depth_format format = FREENECT_DEPTH_11BIT) : FreenectStream(pDevice) - { - setVideoMode(getSupportedModes()[0]); - - xnl::Pair res = convertResolution(device->getDepthResolution()); - video_mode.fps = 30; - video_mode.resolutionX = res.first; - video_mode.resolutionY = res.second; - - printf("FreenectDepthStream: resolutionX = %d; resolutionY = %d\n", video_mode.resolutionX, video_mode.resolutionY); - } + FreenectDepthStream(Freenect::FreenectDevice* pDevice); + ~FreenectDepthStream() { } - int getBytesPerPixel() { return sizeof(OniDepthPixel); } - - /* - FREENECT_DEPTH_11BIT = 0, //< 11 bit depth information in one uint16_t/pixel - FREENECT_DEPTH_10BIT = 1, //< 10 bit depth information in one uint16_t/pixel - FREENECT_DEPTH_11BIT_PACKED = 2, //< 11 bit packed depth information - FREENECT_DEPTH_10BIT_PACKED = 3, //< 10 bit packed depth information - FREENECT_DEPTH_REGISTERED = 4, //< processed depth data in mm, aligned to 640x480 RGB - FREENECT_DEPTH_MM = 5, //< depth to each pixel in mm, but left unaligned to RGB image - FREENECT_DEPTH_DUMMY = 2147483647, //< Dummy value to force enum to be 32 bits wide - */ - /* - ONI_PIXEL_FORMAT_DEPTH_1_MM = 100, - ONI_PIXEL_FORMAT_DEPTH_100_UM = 101, - ONI_PIXEL_FORMAT_SHIFT_9_2 = 102, - ONI_PIXEL_FORMAT_SHIFT_9_3 = 103, - */ - - OniStatus setVideoMode(OniVideoMode requested_mode) - { - OniVideoMode* supported_modes = getSupportedModes(); - freenect_depth_format format; - freenect_resolution resolution; - - if (video_mode == supported_modes[0]) - { - format = FREENECT_DEPTH_11BIT; - resolution = FREENECT_RESOLUTION_MEDIUM; - } - else - { - return ONI_STATUS_NOT_SUPPORTED; - } - - try { device->setDepthFormat(format, resolution); } - catch (std::runtime_error e) - { - printf("format-resolution combination not supported: %d-%d\n", format, resolution); - return ONI_STATUS_NOT_SUPPORTED; - } - - video_mode = requested_mode; - return ONI_STATUS_OK; - } - - - OniStatus setProperty(int propertyId, const void* data, int dataSize) - { - switch(propertyId) - { - default: - return ONI_STATUS_NOT_SUPPORTED; - case ONI_STREAM_PROPERTY_VIDEO_MODE: - if (dataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); - return ONI_STATUS_ERROR; - } - return setVideoMode(*(reinterpret_cast(data))); - } - } - - // todo : represent this as static const in implementation - static OniVideoMode* getSupportedModes() - { - OniVideoMode * supported_modes = new OniVideoMode[1]; - // pixelFormat, resolutionX, resolutionY, fps - supported_modes[0] = { ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30 }; - return supported_modes; - } - + static OniVideoMode* getSupportedVideoModes(); + inline const OniVideoMode getVideoMode() const { return video_mode; } + OniStatus setVideoMode(OniVideoMode requested_mode); - void buildFrame(void* depth, OniDriverFrame* pFrame) - { - pFrame->frame.sensorType = ONI_SENSOR_DEPTH; - pFrame->frame.stride = video_mode.resolutionX*sizeof(OniDepthPixel); - pFrame->frame.dataSize = device->getDepthBufferSize(); - pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); - if (pFrame->frame.data == NULL) - { - XN_ASSERT(FALSE); - return; - } - - uint8_t* _data = static_cast(depth); - uint8_t* frame_data = static_cast(pFrame->frame.data); - std::copy(_data, _data+pFrame->frame.dataSize, frame_data); - } - + // from oni::driver::StreamBase + OniStatus getProperty(int propertyId, void* data, int* pDataSize); + OniStatus setProperty(int propertyId, const void* data, int dataSize); }; #endif // _FREENECT_DEPTH_STREAM_H_ diff --git a/Source/Drivers/freenect/FreenectDeviceNI.h b/Source/Drivers/freenect/FreenectDeviceNI.h index 809260c1..84e8aa47 100644 --- a/Source/Drivers/freenect/FreenectDeviceNI.h +++ b/Source/Drivers/freenect/FreenectDeviceNI.h @@ -38,13 +38,13 @@ class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::Freene OniSensorInfo * sensors = new OniSensorInfo[*numSensors]; // DEPTH - OniVideoMode* supported_modes = FreenectDepthStream::getSupportedModes(); + OniVideoMode* supported_modes = FreenectDepthStream::getSupportedVideoModes(); sensors[0].sensorType = ONI_SENSOR_DEPTH; sensors[0].numSupportedVideoModes = SIZE(supported_modes); sensors[0].pSupportedVideoModes = supported_modes; // COLOR - supported_modes = FreenectImageStream::getSupportedModes(); + supported_modes = FreenectImageStream::getSupportedVideoModes(); sensors[1].sensorType = ONI_SENSOR_COLOR; sensors[1].numSupportedVideoModes = SIZE(supported_modes); sensors[1].pSupportedVideoModes = supported_modes; @@ -92,7 +92,7 @@ class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::Freene } - // freenect + // freenect callbacks // Do not call directly even in child void DepthCallback(void *depth, uint32_t timestamp) { diff --git a/Source/Drivers/freenect/FreenectImageStream.cpp b/Source/Drivers/freenect/FreenectImageStream.cpp new file mode 100644 index 00000000..b371ef7b --- /dev/null +++ b/Source/Drivers/freenect/FreenectImageStream.cpp @@ -0,0 +1,126 @@ +#include "FreenectImageStream.h" + + +FreenectImageStream::FreenectImageStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) +{ + setVideoMode(getSupportedVideoModes()[0]); +} + +OniVideoMode* FreenectImageStream::getSupportedVideoModes() +{ + OniVideoMode* supported_video_modes = new OniVideoMode[1] { + { ONI_PIXEL_FORMAT_RGB888, 640, 480, 30 }, + }; + return supported_video_modes; +} + +OniStatus FreenectImageStream::setVideoMode(OniVideoMode requested_mode) +{ + OniVideoMode* supported_modes = getSupportedVideoModes(); + freenect_video_format format; + freenect_resolution resolution; + + if (requested_mode == supported_modes[0]) + { + format = FREENECT_VIDEO_RGB; + resolution = FREENECT_RESOLUTION_MEDIUM; + + /* working format possiblities + FREENECT_VIDEO_RGB + FREENECT_VIDEO_YUV_RGB + FREENECT_VIDEO_YUV_RAW + */ + } + else + { + return ONI_STATUS_NOT_SUPPORTED; + } + + try { device->setVideoFormat(format, resolution); } + catch (std::runtime_error e) + { + printf("format-resolution combination not supported: %d-%d\n", format, resolution); + return ONI_STATUS_NOT_SUPPORTED; + } + + video_mode = requested_mode; + return ONI_STATUS_OK; +} + +void FreenectImageStream::buildFrame(void* image, OniDriverFrame* pFrame) +{ + pFrame->frame.sensorType = ONI_SENSOR_COLOR; + pFrame->frame.videoMode = video_mode; + pFrame->frame.width = video_mode.resolutionX; + pFrame->frame.height = video_mode.resolutionY; + pFrame->frame.stride = video_mode.resolutionX*3; + pFrame->frame.dataSize = device->getVideoBufferSize(); + + // copy stream buffer from freenect + pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); + if (pFrame->frame.data == NULL) + { + XN_ASSERT(FALSE); + return; + } + unsigned char* _data = static_cast(image); + unsigned char* frame_data = static_cast(pFrame->frame.data); + std::copy(_data, _data+pFrame->frame.dataSize, frame_data); +} + + +// for oni::driver::StreamBase + +OniStatus FreenectImageStream::getProperty(int propertyId, void* data, int* pDataSize) +{ + switch(propertyId) + { + default: + return FreenectStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (*pDataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = video_mode; + return ONI_STATUS_OK; + } +} +OniStatus FreenectImageStream::setProperty(int propertyId, const void* data, int dataSize) +{ + switch(propertyId) + { + default: + return FreenectStream::setProperty(propertyId, data, dataSize); + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (dataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + raisePropertyChanged(propertyId, data, dataSize); + return setVideoMode(*(reinterpret_cast(data))); + } +} + + +/* image video modes reference + +FREENECT_VIDEO_RGB = 0, //< Decompressed RGB mode (demosaicing done by libfreenect) +FREENECT_VIDEO_BAYER = 1, //< Bayer compressed mode (raw information from camera) +FREENECT_VIDEO_IR_8BIT = 2, //< 8-bit IR mode +FREENECT_VIDEO_IR_10BIT = 3, //< 10-bit IR mode +FREENECT_VIDEO_IR_10BIT_PACKED = 4, //< 10-bit packed IR mode +FREENECT_VIDEO_YUV_RGB = 5, //< YUV RGB mode +FREENECT_VIDEO_YUV_RAW = 6, //< YUV Raw mode +FREENECT_VIDEO_DUMMY = 2147483647, //< Dummy value to force enum to be 32 bits wide + +ONI_PIXEL_FORMAT_RGB888 = 200, +ONI_PIXEL_FORMAT_YUV422 = 201, +ONI_PIXEL_FORMAT_GRAY8 = 202, +ONI_PIXEL_FORMAT_GRAY16 = 203, +ONI_PIXEL_FORMAT_JPEG = 204, +*/ diff --git a/Source/Drivers/freenect/FreenectImageStream.h b/Source/Drivers/freenect/FreenectImageStream.h index e28ed176..90ad74bb 100644 --- a/Source/Drivers/freenect/FreenectImageStream.h +++ b/Source/Drivers/freenect/FreenectImageStream.h @@ -4,114 +4,23 @@ #include "FreenectStream.h" - - class FreenectImageStream : public FreenectStream -{ -public: - FreenectImageStream(Freenect::FreenectDevice* pDevice, freenect_video_format format = FREENECT_VIDEO_RGB) : FreenectStream(pDevice) - { - setFormat(format, FREENECT_RESOLUTION_MEDIUM); - - xnl::Pair res = convertResolution(device->getVideoResolution()); - video_mode.fps = 30; - video_mode.resolutionX = res.first; - video_mode.resolutionY = res.second; - - //printf("FreenectImageStream: resolutionX = %d; resolutionY = %d\n", video_mode.resolutionX, video_mode.resolutionY); - } - - int getBytesPerPixel() { return sizeof(OniRGB888Pixel); } +{ +private: + OniVideoMode video_mode; + void buildFrame(void* data, OniDriverFrame* pFrame); - /* - FREENECT_VIDEO_RGB = 0, //< Decompressed RGB mode (demosaicing done by libfreenect) - FREENECT_VIDEO_BAYER = 1, //< Bayer compressed mode (raw information from camera) - FREENECT_VIDEO_IR_8BIT = 2, //< 8-bit IR mode - FREENECT_VIDEO_IR_10BIT = 3, //< 10-bit IR mode - FREENECT_VIDEO_IR_10BIT_PACKED = 4, //< 10-bit packed IR mode - FREENECT_VIDEO_YUV_RGB = 5, //< YUV RGB mode - FREENECT_VIDEO_YUV_RAW = 6, //< YUV Raw mode - FREENECT_VIDEO_DUMMY = 2147483647, //< Dummy value to force enum to be 32 bits wide - */ - /* - ONI_PIXEL_FORMAT_RGB888 = 200, - ONI_PIXEL_FORMAT_YUV422 = 201, - ONI_PIXEL_FORMAT_GRAY8 = 202, - ONI_PIXEL_FORMAT_GRAY16 = 203, - ONI_PIXEL_FORMAT_JPEG = 204, - */ - freenect_video_format getFormat() - { - return device->getVideoFormat(); - } - OniStatus setFormat(freenect_video_format format, freenect_resolution resolution = FREENECT_RESOLUTION_MEDIUM) - { - switch(format) - { - default: - case FREENECT_VIDEO_BAYER: - case FREENECT_VIDEO_IR_8BIT: - case FREENECT_VIDEO_IR_10BIT: - case FREENECT_VIDEO_IR_10BIT_PACKED: - return ONI_STATUS_NOT_IMPLEMENTED; - - case FREENECT_VIDEO_RGB: - case FREENECT_VIDEO_YUV_RGB: - case FREENECT_VIDEO_YUV_RAW: - try { device->setVideoFormat(format, resolution); } - catch (std::runtime_error e) - { - printf("format-resolution combination not supported: %d-%d\ntrying format with default freenect resolution\n", format, resolution); - try { device->setVideoFormat(format); } catch (std::runtime_error e) { printf("format not supported %d\n", format); } - return ONI_STATUS_NOT_SUPPORTED; - } +public: + FreenectImageStream(Freenect::FreenectDevice* pDevice); + ~FreenectImageStream() { } - video_mode.pixelFormat = ONI_PIXEL_FORMAT_RGB888; - return ONI_STATUS_OK; - } - - switch(resolution) - { - default: - break; - case FREENECT_RESOLUTION_LOW: - break; - case FREENECT_RESOLUTION_MEDIUM: - break; - case FREENECT_RESOLUTION_HIGH: - // video_mode.resolutionX = 1280; - // video_mode.resolutionY = 1024; // 960 ? - break; - } - } - - - // todo : represent this as static const in implementation - static OniVideoMode* getSupportedModes() - { - OniVideoMode * supported_modes = new OniVideoMode[1]; - // pixelFormat, resolutionX, resolutionY, fps - supported_modes[0] = { ONI_PIXEL_FORMAT_RGB888, 640, 480, 30 }; - return supported_modes; - } - + static OniVideoMode* getSupportedVideoModes(); + inline const OniVideoMode getVideoMode() const { return video_mode; } + OniStatus setVideoMode(OniVideoMode requested_mode); - void buildFrame(void* image, OniDriverFrame* pFrame) - { - pFrame->frame.sensorType = ONI_SENSOR_COLOR; - pFrame->frame.stride = video_mode.resolutionX*3; - pFrame->frame.dataSize = device->getVideoBufferSize(); - pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); - if (pFrame->frame.data == NULL) - { - XN_ASSERT(FALSE); - return; - } - - unsigned char* _data = static_cast(image); - unsigned char* frame_data = static_cast(pFrame->frame.data); - std::copy(_data, _data+pFrame->frame.dataSize, frame_data); - } + // from oni::driver::StreamBase + OniStatus getProperty(int propertyId, void* data, int* pDataSize); + OniStatus setProperty(int propertyId, const void* data, int dataSize); }; #endif // _FREENECT_IMAGE_STREAM_H_ diff --git a/Source/Drivers/freenect/FreenectStream.h b/Source/Drivers/freenect/FreenectStream.h index a6f14b85..5b20f39d 100644 --- a/Source/Drivers/freenect/FreenectStream.h +++ b/Source/Drivers/freenect/FreenectStream.h @@ -10,8 +10,6 @@ //#include "PS1080.h" //#include "XnMath.h" -#include "XnPair.h" - #define SIZE(array) sizeof array / sizeof 0[array] @@ -27,19 +25,16 @@ static bool operator==(const OniVideoMode& left, const OniVideoMode& right) } - class FreenectStream : public oni::driver::StreamBase { private: - //virtual int getBytesPerPixel() = 0; - + virtual void buildFrame(void* data, OniDriverFrame* pFrame) = 0; + protected: Freenect::FreenectDevice* device; - OniVideoMode video_mode; // derived classes should set this up in constructor - static const OniVideoMode* supported_modes[]; int frame_id; // number each frame - bool running; - + bool running; // acquireFrame() does something iff true + public: FreenectStream(Freenect::FreenectDevice* pDevice) { @@ -51,15 +46,46 @@ class FreenectStream : public oni::driver::StreamBase stop(); } - //virtual void buildFrame(void* data, OniDriverFrame* pFrame) = 0; - virtual void buildFrame(void* data, OniDriverFrame* pFrame) {}; + // local functions + + virtual void acquireFrame(void* data, uint32_t timestamp) + { + if (!running) + return; + + //if (frame_id == 1) + + + OniDriverFrame* pFrame = (OniDriverFrame*)xnOSCalloc(1, sizeof(OniDriverFrame)); + if (pFrame == NULL) + { + XN_ASSERT(FALSE); + return; + } + pFrame->pDriverCookie = xnOSMalloc(sizeof(FreenectStreamFrameCookie)); + ((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount = 1; + + pFrame->frame.frameIndex = frame_id++; + pFrame->frame.timestamp = timestamp; + pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; + pFrame->frame.croppingEnabled = FALSE; + + buildFrame(data, pFrame); + raiseNewFrame(pFrame); + } + + + // partial default implementation inheriting oni::driver::StreamBase OniStatus start() { running = true; return ONI_STATUS_OK; } void stop() { running = false; } - OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) == ONI_STATUS_NOT_SUPPORTED) ? false : true; } - OniStatus getProperty(int propertyId, void* data, int* pDataSize) + OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } + // property handlers are empty skeletons by default + // only add here if the property is generic to all children of FreenectStream + // otherwise, implement in child and call these in default case (see FreenectDepthStream.h) + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) { switch(propertyId) { @@ -67,6 +93,7 @@ class FreenectStream : public oni::driver::StreamBase case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* case ONI_STREAM_PROPERTY_MAX_VALUE: // int case ONI_STREAM_PROPERTY_MIN_VALUE: // int case ONI_STREAM_PROPERTY_STRIDE: // int @@ -76,62 +103,27 @@ class FreenectStream : public oni::driver::StreamBase case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool return ONI_STATUS_NOT_SUPPORTED; - - case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* - if (*pDataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = video_mode; - return ONI_STATUS_OK; - } - } - - - static xnl::Pair convertResolution(freenect_resolution resolution) - { - switch(resolution) - { - case FREENECT_RESOLUTION_LOW: - return xnl::Pair(320, 240); - case FREENECT_RESOLUTION_MEDIUM: - return xnl::Pair(640, 480); - case FREENECT_RESOLUTION_HIGH: - return xnl::Pair(1280, 1024); } } - - - - - void acquireFrame(void* data, uint32_t timestamp) + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) { - if (!running) - return; - - //if (frame_id == 1) - - - OniDriverFrame* pFrame = (OniDriverFrame*)xnOSCalloc(1, sizeof(OniDriverFrame)); - if (pFrame == NULL) + switch(propertyId) { - XN_ASSERT(FALSE); - return; + default: + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + case ONI_STREAM_PROPERTY_MIN_VALUE: // int + case ONI_STREAM_PROPERTY_STRIDE: // int + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + return ONI_STATUS_NOT_SUPPORTED; } - pFrame->pDriverCookie = xnOSMalloc(sizeof(FreenectStreamFrameCookie)); - ((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount = 1; - - pFrame->frame.frameIndex = frame_id++; - pFrame->frame.timestamp = timestamp; - pFrame->frame.videoMode = video_mode; - pFrame->frame.width = video_mode.resolutionX; - pFrame->frame.height = video_mode.resolutionY; - pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; - pFrame->frame.croppingEnabled = FALSE; - - buildFrame(data, pFrame); - raiseNewFrame(pFrame); } void addRefToFrame(OniDriverFrame* pFrame) { ++((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount; } @@ -145,9 +137,8 @@ class FreenectStream : public oni::driver::StreamBase } } - - /* unimplemented from StreamBase - virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} + + /* unimplemented from oni::driver::StreamBase virtual OniStatus invoke(int commandId, const void* data, int dataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} virtual OniBool isCommandSupported(int commandId) {return FALSE;} virtual void setNewFrameCallback(NewFrameCallback handler, void* pCookie) { m_newFrameCallback = handler; m_newFrameCallbackCookie = pCookie; } From 905cba0cc5d8e504bcb6e9c8483b4e8b39543ddb Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Sat, 5 Jan 2013 00:34:59 -0500 Subject: [PATCH 11/18] Finished(?) stream mode handling; continue separating implementations --- .../Drivers/freenect/FreenectDepthStream.cpp | 32 ++- Source/Drivers/freenect/FreenectDepthStream.h | 27 +- Source/Drivers/freenect/FreenectDeviceNI.cpp | 239 +++--------------- Source/Drivers/freenect/FreenectDeviceNI.h | 146 ++++------- Source/Drivers/freenect/FreenectDriver.cpp | 153 +++++------ Source/Drivers/freenect/FreenectDriver.h | 76 ++++++ Source/Drivers/freenect/FreenectImageStream.h | 26 -- Source/Drivers/freenect/FreenectStream.h | 63 ++--- ...mageStream.cpp => FreenectVideoStream.cpp} | 40 +-- Source/Drivers/freenect/FreenectVideoStream.h | 37 +++ 10 files changed, 347 insertions(+), 492 deletions(-) create mode 100644 Source/Drivers/freenect/FreenectDriver.h delete mode 100644 Source/Drivers/freenect/FreenectImageStream.h rename Source/Drivers/freenect/{FreenectImageStream.cpp => FreenectVideoStream.cpp} (75%) create mode 100644 Source/Drivers/freenect/FreenectVideoStream.h diff --git a/Source/Drivers/freenect/FreenectDepthStream.cpp b/Source/Drivers/freenect/FreenectDepthStream.cpp index 66bb4ef9..acb3ef6e 100644 --- a/Source/Drivers/freenect/FreenectDepthStream.cpp +++ b/Source/Drivers/freenect/FreenectDepthStream.cpp @@ -1,27 +1,32 @@ #include "FreenectDepthStream.h" #include "XnLib.h" +#include "libfreenect.hpp" +const OniSensorType FreenectDepthStream::sensor_type = ONI_SENSOR_DEPTH; +// pixelFormat, resolutionX, resolutionY, fps +const OniVideoMode FreenectDepthStream::supported_video_modes[] = { + { ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30 }, +}; + FreenectDepthStream::FreenectDepthStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) -{ +{ setVideoMode(getSupportedVideoModes()[0]); } OniVideoMode* FreenectDepthStream::getSupportedVideoModes() { - OniVideoMode* supported_video_modes = new OniVideoMode[1] { - { ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30 }, - }; - return supported_video_modes; + // make a copy to avoid need for const + OniVideoMode* modes = new OniVideoMode[SIZE(supported_video_modes)]; + std::copy(supported_video_modes, supported_video_modes+SIZE(supported_video_modes), modes); + return modes; } - OniStatus FreenectDepthStream::setVideoMode(OniVideoMode requested_mode) -{ - OniVideoMode* supported_modes = getSupportedVideoModes(); +{ freenect_depth_format format; freenect_resolution resolution; - if (requested_mode == supported_modes[0]) + if (requested_mode == supported_video_modes[0]) { format = FREENECT_DEPTH_11BIT; resolution = FREENECT_RESOLUTION_MEDIUM; @@ -47,15 +52,16 @@ OniStatus FreenectDepthStream::setVideoMode(OniVideoMode requested_mode) video_mode = requested_mode; return ONI_STATUS_OK; } - void FreenectDepthStream::buildFrame(void* depth, OniDriverFrame* pFrame) { pFrame->frame.sensorType = ONI_SENSOR_DEPTH; pFrame->frame.videoMode = video_mode; + pFrame->frame.dataSize = device->getDepthBufferSize(); pFrame->frame.width = video_mode.resolutionX; pFrame->frame.height = video_mode.resolutionY; pFrame->frame.stride = video_mode.resolutionX*sizeof(OniDepthPixel); - pFrame->frame.dataSize = device->getDepthBufferSize(); + pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; + pFrame->frame.croppingEnabled = FALSE; // copy stream buffer from freenect pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); @@ -69,9 +75,7 @@ void FreenectDepthStream::buildFrame(void* depth, OniDriverFrame* pFrame) std::copy(_data, _data+pFrame->frame.dataSize, frame_data); } - -// for oni::driver::StreamBase - +// for StreamBase OniStatus FreenectDepthStream::getProperty(int propertyId, void* data, int* pDataSize) { switch(propertyId) diff --git a/Source/Drivers/freenect/FreenectDepthStream.h b/Source/Drivers/freenect/FreenectDepthStream.h index 099edac4..044d6c7b 100644 --- a/Source/Drivers/freenect/FreenectDepthStream.h +++ b/Source/Drivers/freenect/FreenectDepthStream.h @@ -7,20 +7,31 @@ class FreenectDepthStream : public FreenectStream { private: - OniVideoMode video_mode; + static const OniSensorType sensor_type; + static const OniVideoMode supported_video_modes[]; void buildFrame(void* data, OniDriverFrame* pFrame); -public: - FreenectDepthStream(Freenect::FreenectDevice* pDevice); - ~FreenectDepthStream() { } - +protected: + OniVideoMode video_mode; + static OniSensorType getSensorType() { return sensor_type; } static OniVideoMode* getSupportedVideoModes(); inline const OniVideoMode getVideoMode() const { return video_mode; } OniStatus setVideoMode(OniVideoMode requested_mode); + +public: + FreenectDepthStream(Freenect::FreenectDevice* pDevice); + ~FreenectDepthStream() { } + + static OniSensorInfo getSensorInfo() + { + OniVideoMode* modes = getSupportedVideoModes(); + // sensorType, numSupportedVideoModes, pSupportedVideoModes + return { getSensorType(), SIZE(modes), modes }; + } - // from oni::driver::StreamBase - OniStatus getProperty(int propertyId, void* data, int* pDataSize); - OniStatus setProperty(int propertyId, const void* data, int dataSize); + // from StreamBase + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); }; #endif // _FREENECT_DEPTH_STREAM_H_ diff --git a/Source/Drivers/freenect/FreenectDeviceNI.cpp b/Source/Drivers/freenect/FreenectDeviceNI.cpp index d3f22765..b79719e2 100644 --- a/Source/Drivers/freenect/FreenectDeviceNI.cpp +++ b/Source/Drivers/freenect/FreenectDeviceNI.cpp @@ -1,219 +1,60 @@ -/** -* FreenectDeviceNI.cpp -* Copyright 2012 Benn Snyder -* -* OpenNI 2.x Alpha -* Copyright (C) 2012 PrimeSense Ltd. -* -* This file is part of OpenNI. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -* -*****************************************************************************/ -/* - * This implementation of KinectDevice serves as a bridge to libfreenect - * It brings Kinect and Kinect for Windows (k4w) support to OpenNI 2.x on Linux; please test on OSX! - * - * - */ - #include "FreenectDeviceNI.h" +#include "XnLib.h" - - - -// REFERENCE FROM KinectDevice.cpp -/* -using namespace kinect_device; -using namespace oni::driver; -#define DEFAULT_FPS 30 -KinectDevice::KinectDevice(INuiSensor * pNuiSensor):m_pDepthStream(NULL), m_pColorStream(NULL),m_pNuiSensor(pNuiSensor) +FreenectDeviceNI::FreenectDeviceNI(freenect_context *_ctx, int _index) : Freenect::FreenectDevice(_ctx, _index) { - m_numSensors = 3; - - m_sensors[0].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); - m_sensors[0].sensorType = ONI_SENSOR_DEPTH; - m_sensors[0].numSupportedVideoModes = 3; - 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 = 640; - m_sensors[0].pSupportedVideoModes[0].resolutionY = 480; - - m_sensors[0].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; - m_sensors[0].pSupportedVideoModes[1].fps = DEFAULT_FPS; - m_sensors[0].pSupportedVideoModes[1].resolutionX = 320; - m_sensors[0].pSupportedVideoModes[1].resolutionY = 240; - - m_sensors[0].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_DEPTH_1_MM; - m_sensors[0].pSupportedVideoModes[2].fps = DEFAULT_FPS; - m_sensors[0].pSupportedVideoModes[2].resolutionX = 80; - m_sensors[0].pSupportedVideoModes[2].resolutionY = 60; - - m_sensors[1].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); - m_sensors[1].sensorType = ONI_SENSOR_COLOR; - m_sensors[1].numSupportedVideoModes = 3; - m_sensors[1].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; - m_sensors[1].pSupportedVideoModes[0].fps = 12; - m_sensors[1].pSupportedVideoModes[0].resolutionX = 1280; - m_sensors[1].pSupportedVideoModes[0].resolutionY = 960; - - m_sensors[1].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_RGB888; - m_sensors[1].pSupportedVideoModes[1].fps = DEFAULT_FPS; - m_sensors[1].pSupportedVideoModes[1].resolutionX = 640; - m_sensors[1].pSupportedVideoModes[1].resolutionY = 480; - - m_sensors[1].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_YUV422; - m_sensors[1].pSupportedVideoModes[2].fps = 15; - m_sensors[1].pSupportedVideoModes[2].resolutionX = 640; - m_sensors[1].pSupportedVideoModes[2].resolutionY = 480; - - m_sensors[2].pSupportedVideoModes = XN_NEW_ARR(OniVideoMode, 3); - m_sensors[2].sensorType = ONI_SENSOR_IR; - m_sensors[2].numSupportedVideoModes = 3; - m_sensors[2].pSupportedVideoModes[0].pixelFormat = ONI_PIXEL_FORMAT_RGB888; - m_sensors[2].pSupportedVideoModes[0].fps = DEFAULT_FPS; - m_sensors[2].pSupportedVideoModes[0].resolutionX = 640; - m_sensors[2].pSupportedVideoModes[0].resolutionY = 480; - - m_sensors[2].pSupportedVideoModes[1].pixelFormat = ONI_PIXEL_FORMAT_GRAY16; - m_sensors[2].pSupportedVideoModes[1].fps = DEFAULT_FPS; - m_sensors[2].pSupportedVideoModes[1].resolutionX = 640; - m_sensors[2].pSupportedVideoModes[1].resolutionY = 480; - - m_sensors[2].pSupportedVideoModes[2].pixelFormat = ONI_PIXEL_FORMAT_GRAY8; - m_sensors[2].pSupportedVideoModes[2].fps = DEFAULT_FPS; - m_sensors[2].pSupportedVideoModes[2].resolutionX = 640; - m_sensors[2].pSupportedVideoModes[2].resolutionY = 480; + depth_stream = NULL; + video_stream = NULL; } - -KinectDevice::~KinectDevice() +FreenectDeviceNI::~FreenectDeviceNI() { - if (m_pNuiSensor) - m_pNuiSensor->NuiShutdown(); - - if (m_pDepthStream != NULL) - XN_DELETE(m_pDepthStream); - - if (m_pColorStream!= NULL) - XN_DELETE(m_pColorStream); - - if (m_pNuiSensor) - m_pNuiSensor->Release(); + destroyStream(depth_stream); + destroyStream(video_stream); } -OniStatus KinectDevice::getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) +// for DeviceBase +OniStatus FreenectDeviceNI::getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) { - *numSensors = m_numSensors; - *pSensors = m_sensors; + *numSensors = 2; + OniSensorInfo * sensors = new OniSensorInfo[*numSensors]; + sensors[0] = depth_stream->getSensorInfo(); + sensors[1] = video_stream->getSensorInfo(); + *pSensors = sensors; return ONI_STATUS_OK; } - -StreamBase* KinectDevice::createStream(OniSensorType sensorType) +StreamBase* FreenectDeviceNI::createStream(OniSensorType sensorType) { - BaseKinectStream* pImage = NULL; - - if (sensorType == ONI_SENSOR_COLOR ) + switch(sensorType) { - if (m_pColorStream == NULL) - { - m_pColorStream = XN_NEW(KinectStreamImpl, m_pNuiSensor, sensorType); - pImage = XN_NEW(ColorKinectStream, m_pColorStream); - } - else - { - if (m_pColorStream->getSensorType() != sensorType) - { - if (!m_pColorStream->isRunning()) - { - m_pColorStream->setSensorType(sensorType); - pImage = XN_NEW(ColorKinectStream, m_pColorStream); - } - } - else - { - pImage = XN_NEW(ColorKinectStream, m_pColorStream); - } - } + case ONI_SENSOR_DEPTH: + Freenect::FreenectDevice::startDepth(); + if (depth_stream == NULL) + depth_stream = XN_NEW(FreenectDepthStream, this); + return depth_stream; + case ONI_SENSOR_COLOR: + Freenect::FreenectDevice::startVideo(); + if (video_stream == NULL) + video_stream = XN_NEW(FreenectVideoStream, this); + return video_stream; + // todo: IR + default: + //m_driverServices.errorLoggerAppend("FreenectDeviceNI: Can't create a stream of type %d", sensorType); + return NULL; } - else if (sensorType == ONI_SENSOR_DEPTH ) +} +void FreenectDeviceNI::destroyStream(StreamBase* pStream) +{ + if (pStream == depth_stream) { - if (m_pDepthStream == NULL) - { - m_pDepthStream = XN_NEW(KinectStreamImpl, m_pNuiSensor, sensorType); - } - pImage = XN_NEW(DepthKinectStream, m_pDepthStream); + Freenect::FreenectDevice::stopDepth(); + depth_stream = NULL; } - if (sensorType == ONI_SENSOR_IR ) + if (pStream == video_stream) { - if (m_pColorStream == NULL) - { - m_pColorStream = XN_NEW(KinectStreamImpl, m_pNuiSensor, sensorType); - pImage = XN_NEW(IRKinectStream, m_pColorStream); - } - else - { - if (m_pColorStream->getSensorType() != sensorType) - { - if (!m_pColorStream->isRunning()) - { - m_pColorStream->setSensorType(sensorType); - pImage = XN_NEW(IRKinectStream, m_pColorStream); - } - } - else - { - pImage = XN_NEW(IRKinectStream, m_pColorStream); - } - } + Freenect::FreenectDevice::stopVideo(); + video_stream = NULL; } - return pImage; -} - -void kinect_device::KinectDevice::destroyStream(oni::driver::StreamBase* pStream) -{ + XN_DELETE(pStream); } - -OniStatus KinectDevice::setProperty(int propertyId, const void* data, int dataSize) -{ - return ONI_STATUS_NOT_IMPLEMENTED; -} - -OniStatus KinectDevice::getProperty(int propertyId, void* data, int* pDataSize) -{ - return ONI_STATUS_NOT_IMPLEMENTED; -} - -OniBool KinectDevice::isPropertySupported(int propertyId) -{ - return ONI_STATUS_NOT_IMPLEMENTED; -} - -OniBool KinectDevice::isCommandSupported(int commandId) -{ - return ONI_STATUS_NOT_IMPLEMENTED; -} - -OniStatus KinectDevice::tryManualTrigger() -{ - return ONI_STATUS_NOT_IMPLEMENTED; -} - -void KinectDevice::notifyAllProperties() -{ - return; -} -*/ - -// END REFERENCE diff --git a/Source/Drivers/freenect/FreenectDeviceNI.h b/Source/Drivers/freenect/FreenectDeviceNI.h index 84e8aa47..c1d5521c 100644 --- a/Source/Drivers/freenect/FreenectDeviceNI.h +++ b/Source/Drivers/freenect/FreenectDeviceNI.h @@ -3,132 +3,92 @@ #include "libfreenect.hpp" #include "Driver/OniDriverAPI.h" -#include "XnLog.h" - #include "FreenectDepthStream.h" -#include "FreenectImageStream.h" - -#include "FreenectStream.h" +#include "FreenectVideoStream.h" using namespace oni::driver; -#define DEFAULT_FPS 30 -class FreenectDeviceNI : public oni::driver::DeviceBase, public Freenect::FreenectDevice +class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice { -private: - //FreenectDepthStream* depth_stream; - //FreenectImageStream* image_stream; - FreenectStream *depth_stream, *image_stream; - +protected: + FreenectDepthStream* depth_stream; + FreenectVideoStream* video_stream; public: - FreenectDeviceNI(freenect_context *_ctx, int _index) : Freenect::FreenectDevice(_ctx, _index) - { - depth_stream = NULL; - image_stream = NULL; - } - - - // OpenNI - OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) - { - *numSensors = 2; - - OniSensorInfo * sensors = new OniSensorInfo[*numSensors]; - - // DEPTH - OniVideoMode* supported_modes = FreenectDepthStream::getSupportedVideoModes(); - sensors[0].sensorType = ONI_SENSOR_DEPTH; - sensors[0].numSupportedVideoModes = SIZE(supported_modes); - sensors[0].pSupportedVideoModes = supported_modes; - - // COLOR - supported_modes = FreenectImageStream::getSupportedVideoModes(); - sensors[1].sensorType = ONI_SENSOR_COLOR; - sensors[1].numSupportedVideoModes = SIZE(supported_modes); - sensors[1].pSupportedVideoModes = supported_modes; - - - *pSensors = sensors; - return ONI_STATUS_OK; - } + FreenectDeviceNI(freenect_context *_ctx, int _index); + ~FreenectDeviceNI(); - StreamBase* createStream(OniSensorType sensorType) + // from DeviceBase + OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors); + StreamBase* createStream(OniSensorType sensorType); + void destroyStream(StreamBase* pStream); + // property and command handlers are empty skeletons by default + // only add here if the property is generic to all children + // otherwise, implement in child and call these in default case + OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) { - switch(sensorType) + switch(propertyId) { - case ONI_SENSOR_DEPTH: - Freenect::FreenectDevice::startDepth(); - if (depth_stream == NULL) - depth_stream = XN_NEW(FreenectDepthStream, this); - return depth_stream; - case ONI_SENSOR_COLOR: - Freenect::FreenectDevice::startVideo(); - if (image_stream == NULL) - image_stream = XN_NEW(FreenectImageStream, this); - return image_stream; - // todo: IR default: - //m_driverServices.errorLoggerAppend("FreenectDeviceNI: Can't create a stream of type %d", sensorType); - return NULL; + 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: // ? + case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode + // files + case ONI_DEVICE_PROPERTY_PLAYBACK_SPEED: // float + case ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED: // OniBool + return ONI_STATUS_NOT_SUPPORTED; } } - - void destroyStream(StreamBase* pStream) + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) { - if (pStream == depth_stream) + switch(propertyId) { - Freenect::FreenectDevice::stopDepth(); - depth_stream = NULL; + 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: // ? + case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode + // files + case ONI_DEVICE_PROPERTY_PLAYBACK_SPEED: // float + case ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED: // OniBool + return ONI_STATUS_NOT_SUPPORTED; } - if (pStream == image_stream) + } + virtual OniBool isCommandSupported(int propertyId) { return (invoke(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } + virtual OniStatus invoke(int commandId, const void* data, int dataSize) + { + switch (commandId) { - Freenect::FreenectDevice::stopVideo(); - image_stream = NULL; + default: + case ONI_DEVICE_COMMAND_SEEK: // OniSeek + return ONI_STATUS_NOT_SUPPORTED; } - - XN_DELETE(pStream); } - - - // freenect callbacks - // Do not call directly even in child + + // from Freenect::FreenectDevice + // Do not call these directly, even in child void DepthCallback(void *depth, uint32_t timestamp) { depth_stream->acquireFrame(depth, timestamp); } - // Do not call directly even in child void VideoCallback(void *image, uint32_t timestamp) { - image_stream->acquireFrame(image, timestamp); + video_stream->acquireFrame(image, timestamp); } - /* unimplemented from DeviceBase - virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} - virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} - virtual OniBool isPropertySupported(int propertyId) {return FALSE;} - virtual OniStatus invoke(int commandId, const void* data, int dataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} - virtual OniBool isCommandSupported(int commandId) {return FALSE;} + /* todo : unimplemented from DeviceBase virtual OniStatus tryManualTrigger() {return ONI_STATUS_OK;} - virtual void setPropertyChangedCallback(PropertyChangedCallback handler, void* pCookie) { m_propertyChangedCallback = handler; m_propertyChangedCookie = pCookie; } - virtual void notifyAllProperties() { return; } virtual OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF); } */ }; -/* Device Properties - ONI_DEVICE_PROPERTY_FIRMWARE_VERSION = 0, // By implementation - ONI_DEVICE_PROPERTY_DRIVER_VERSION = 1, // OniVersion - ONI_DEVICE_PROPERTY_HARDWARE_VERSION = 2, // int - ONI_DEVICE_PROPERTY_SERIAL_NUMBER = 3, // string - ONI_DEVICE_PROPERTY_ERROR_STATE = 4, // ?? - ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION = 5, // OniImageRegistrationMode - - // Files - ONI_DEVICE_PROPERTY_PLAYBACK_SPEED = 100, // float - ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED = 101, // OniBool -*/ #endif //_FREENECT_DEVICE_NI_H_ diff --git a/Source/Drivers/freenect/FreenectDriver.cpp b/Source/Drivers/freenect/FreenectDriver.cpp index 1b0731e7..a9dfd60c 100644 --- a/Source/Drivers/freenect/FreenectDriver.cpp +++ b/Source/Drivers/freenect/FreenectDriver.cpp @@ -1,116 +1,81 @@ +#include "FreenectDriver.h" #include -#include "libfreenect.hpp" -#include "Driver/OniDriverAPI.h" -//#include "KinectDriver.h" -#include "FreenectDeviceNI.h" -#include "XnHash.h" +#include "XnLib.h" -using namespace oni::driver; - static const char VENDOR_VAL[] = "Microsoft"; static const char NAME_VAL[] = "Kinect"; - -class FreenectDriver : public DriverBase, private Freenect::Freenect +FreenectDriver::FreenectDriver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) { -private: - //Freenect::Freenect freenect; - // from Freenect::Freenect - freenect_context* m_ctx - xnl::Hash devices; + freenect_set_log_level(m_ctx, FREENECT_LOG_NOTICE); + // MOTOR doesn't work with k4w branch; todo: fix it + //freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); + freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_CAMERA)); +} -public: - FreenectDriver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) +// for DriverBase +OniStatus FreenectDriver::initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) +{ + DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); + for (unsigned int i = 1; i <= Freenect::deviceCount(); i++) { - freenect_set_log_level(m_ctx, FREENECT_LOG_NOTICE); - // MOTOR doesn't work with k4w branch; todo: fix it - //freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); - freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_CAMERA)); + OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); + std::ostringstream uri; + uri << "freenect://" << i; + xnOSStrCopy(pInfo->uri, uri.str().c_str(), ONI_MAX_STR); + xnOSStrCopy(pInfo->vendor, VENDOR_VAL, ONI_MAX_STR); + xnOSStrCopy(pInfo->name, NAME_VAL, ONI_MAX_STR); + devices[pInfo] = NULL; + deviceConnected(pInfo); + deviceStateChanged(pInfo, 0); } - ~FreenectDriver() - {} - - OniStatus initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) - { - DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); - for (int i = 1; i <= Freenect::deviceCount(); i++) - { - OniDeviceInfo* pInfo = XN_NEW(OniDeviceInfo); - std::ostringstream uri; - uri << "freenect://" << i; - xnOSStrCopy(pInfo->uri, uri.str().c_str(), ONI_MAX_STR); - xnOSStrCopy(pInfo->vendor, VENDOR_VAL, ONI_MAX_STR); - xnOSStrCopy(pInfo->name, NAME_VAL, ONI_MAX_STR); - devices[pInfo] = NULL; - deviceConnected(pInfo); - deviceStateChanged(pInfo, 0); - } - return ONI_STATUS_OK; - } - - DeviceBase* deviceOpen(const char* uri) + return ONI_STATUS_OK; +} +DeviceBase* FreenectDriver::deviceOpen(const char* uri) +{ + for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); iter++) { - for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); ++iter) + if (xnOSStrCmp(iter->Key()->uri, uri) == 0) { - if (xnOSStrCmp(iter->Key()->uri, uri) == 0) + // found + if (iter->Value() != NULL) { - // found - if (iter->Value() != NULL) - { - // already using - return iter->Value(); - } - else - { - int id; - std::istringstream(iter->Key()->uri) >> id; - FreenectDeviceNI * device = &createDevice(id); - iter->Value() = device; - return device; - } + // already using + return iter->Value(); } - } - - getServices().errorLoggerAppend("Looking for '%s'", uri); - return NULL; - } - - void deviceClose(DeviceBase* pDevice) - { - for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); ++iter) - { - if (iter->Value() == pDevice) + else { - iter->Value() = NULL; int id; std::istringstream(iter->Key()->uri) >> id; - Freenect::deleteDevice(id); - return; + FreenectDeviceNI * device = &createDevice(id); + iter->Value() = device; + return device; } } - - // not our device?! - XN_ASSERT(FALSE); } - - void shutdown() { } - - OniStatus tryDevice(const char* uri) + getServices().errorLoggerAppend("Looking for '%s'", uri); + return NULL; +} +void FreenectDriver::deviceClose(DeviceBase* pDevice) +{ + for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); iter++) { - DeviceBase* device = deviceOpen(uri); - if (device == NULL) - return ONI_STATUS_ERROR; - deviceClose(device); - - return ONI_STATUS_OK; + if (iter->Value() == pDevice) + { + iter->Value() = NULL; + XN_DELETE(pDevice); + return; + } } - - - /* unimplemented from DriverBase - virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); - virtual void disableFrameSync(void* frameSyncGroup); - */ -}; - - -ONI_EXPORT_DRIVER(FreenectDriver); + // not our device?! + XN_ASSERT(FALSE); +} +OniStatus FreenectDriver::tryDevice(const char* uri) +{ + DeviceBase* device = deviceOpen(uri); + if (device == NULL) + return ONI_STATUS_ERROR; + deviceClose(device); + return ONI_STATUS_OK; +} diff --git a/Source/Drivers/freenect/FreenectDriver.h b/Source/Drivers/freenect/FreenectDriver.h new file mode 100644 index 00000000..e88a28ec --- /dev/null +++ b/Source/Drivers/freenect/FreenectDriver.h @@ -0,0 +1,76 @@ +/** +* FreenectDriver +* Copyright 2012 Benn Snyder +* +* OpenNI 2.x Alpha +* Copyright (C) 2012 PrimeSense Ltd. +* +* This file is part of OpenNI. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +* +*/ +/* + * FreenectDriver serves as a bridge to libfreenect + * It brings Kinect and Kinect for Windows (k4w) support to OpenNI 2.x on Linux; please test on OSX! + * + * + */ + +#ifndef _FREENECT_DRIVER_H_ +#define _FREENECT_DRIVER_H_ + +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +#include "FreenectDeviceNI.h" +#include "XnHash.h" + + +using namespace oni::driver; + +class FreenectDriver : public DriverBase, private Freenect::Freenect +{ +private: + //Freenect::Freenect freenect; + // from Freenect::Freenect - freenect_context* m_ctx + xnl::Hash devices; + +public: + FreenectDriver(OniDriverServices* pDriverServices); + ~FreenectDriver() { shutdown(); } + + // from DriverBase + OniStatus initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie); + DeviceBase* deviceOpen(const char* uri); + void deviceClose(DeviceBase* pDevice); + void shutdown() + { + for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); iter++) + { + deviceClose(iter->Value()); + } + } + OniStatus tryDevice(const char* uri); + + + /* unimplemented from DriverBase + virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); + virtual void disableFrameSync(void* frameSyncGroup); + */ +}; + + +ONI_EXPORT_DRIVER(FreenectDriver); + + +#endif //_FREENECT_DRIVER_H_ diff --git a/Source/Drivers/freenect/FreenectImageStream.h b/Source/Drivers/freenect/FreenectImageStream.h deleted file mode 100644 index 90ad74bb..00000000 --- a/Source/Drivers/freenect/FreenectImageStream.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef _FREENECT_IMAGE_STREAM_H_ -#define _FREENECT_IMAGE_STREAM_H_ - -#include "FreenectStream.h" - - -class FreenectImageStream : public FreenectStream -{ -private: - OniVideoMode video_mode; - void buildFrame(void* data, OniDriverFrame* pFrame); - -public: - FreenectImageStream(Freenect::FreenectDevice* pDevice); - ~FreenectImageStream() { } - - static OniVideoMode* getSupportedVideoModes(); - inline const OniVideoMode getVideoMode() const { return video_mode; } - OniStatus setVideoMode(OniVideoMode requested_mode); - - // from oni::driver::StreamBase - OniStatus getProperty(int propertyId, void* data, int* pDataSize); - OniStatus setProperty(int propertyId, const void* data, int dataSize); -}; - -#endif // _FREENECT_IMAGE_STREAM_H_ diff --git a/Source/Drivers/freenect/FreenectStream.h b/Source/Drivers/freenect/FreenectStream.h index 5b20f39d..235cdc7d 100644 --- a/Source/Drivers/freenect/FreenectStream.h +++ b/Source/Drivers/freenect/FreenectStream.h @@ -4,11 +4,6 @@ #include "libfreenect.hpp" #include "Driver/OniDriverAPI.h" #include "XnLib.h" -//#include "XnHash.h" -//#include "XnEvent.h" -//#include "XnPlatform.h" -//#include "PS1080.h" -//#include "XnMath.h" #define SIZE(array) sizeof array / sizeof 0[array] @@ -21,18 +16,20 @@ typedef struct static bool operator==(const OniVideoMode& left, const OniVideoMode& right) { return (left.pixelFormat == right.pixelFormat && left.resolutionX == right.resolutionX - && left.resolutionY == right.resolutionY && left.fps == right.fps) ? true : false; + && left.resolutionY == right.resolutionY && left.fps == right.fps); } -class FreenectStream : public oni::driver::StreamBase +using namespace oni::driver; + +class FreenectStream : public StreamBase { private: + int frame_id; // number each frame virtual void buildFrame(void* data, OniDriverFrame* pFrame) = 0; protected: Freenect::FreenectDevice* device; - int frame_id; // number each frame bool running; // acquireFrame() does something iff true public: @@ -45,17 +42,11 @@ class FreenectStream : public oni::driver::StreamBase { stop(); } - - - // local functions - + virtual void acquireFrame(void* data, uint32_t timestamp) { if (!running) - return; - - //if (frame_id == 1) - + return; OniDriverFrame* pFrame = (OniDriverFrame*)xnOSCalloc(1, sizeof(OniDriverFrame)); if (pFrame == NULL) @@ -68,23 +59,28 @@ class FreenectStream : public oni::driver::StreamBase pFrame->frame.frameIndex = frame_id++; pFrame->frame.timestamp = timestamp; - pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; - pFrame->frame.croppingEnabled = FALSE; buildFrame(data, pFrame); raiseNewFrame(pFrame); } - - // partial default implementation inheriting oni::driver::StreamBase - + // from StreamBase OniStatus start() { running = true; return ONI_STATUS_OK; } void stop() { running = false; } - - OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } + virtual void addRefToFrame(OniDriverFrame* pFrame) { ++((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount; } + virtual void releaseFrame(OniDriverFrame* pFrame) + { + if (0 == --((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount) + { + xnOSFree(pFrame->pDriverCookie); + xnOSFreeAligned(pFrame->frame.data); + xnOSFree(pFrame); + } + } // property handlers are empty skeletons by default - // only add here if the property is generic to all children of FreenectStream + // only add here if the property is generic to all children // otherwise, implement in child and call these in default case (see FreenectDepthStream.h) + OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) { switch(propertyId) @@ -126,26 +122,11 @@ class FreenectStream : public oni::driver::StreamBase } } - void addRefToFrame(OniDriverFrame* pFrame) { ++((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount; } - void releaseFrame(OniDriverFrame* pFrame) - { - if (0 == --((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount) - { - xnOSFree(pFrame->pDriverCookie); - xnOSFreeAligned(pFrame->frame.data); - xnOSFree(pFrame); - } - } - - /* unimplemented from oni::driver::StreamBase - virtual OniStatus invoke(int commandId, const void* data, int dataSize) {return ONI_STATUS_NOT_IMPLEMENTED;} - virtual OniBool isCommandSupported(int commandId) {return FALSE;} - virtual void setNewFrameCallback(NewFrameCallback handler, void* pCookie) { m_newFrameCallback = handler; m_newFrameCallbackCookie = pCookie; } - virtual void setPropertyChangedCallback(PropertyChangedCallback handler, void* pCookie) { m_propertyChangedCallback = handler; m_propertyChangedCookie = pCookie; } - virtual void notifyAllProperties() { return; } + /* todo : unimplemented from StreamBase virtual OniStatus convertDepthToColorCoordinates(StreamBase* colorStream, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY) { return ONI_STATUS_NOT_SUPPORTED; } */ }; + #endif // _FREENECT_STREAM_H_ diff --git a/Source/Drivers/freenect/FreenectImageStream.cpp b/Source/Drivers/freenect/FreenectVideoStream.cpp similarity index 75% rename from Source/Drivers/freenect/FreenectImageStream.cpp rename to Source/Drivers/freenect/FreenectVideoStream.cpp index b371ef7b..7d12931d 100644 --- a/Source/Drivers/freenect/FreenectImageStream.cpp +++ b/Source/Drivers/freenect/FreenectVideoStream.cpp @@ -1,20 +1,27 @@ -#include "FreenectImageStream.h" +#include "FreenectVideoStream.h" +#include "XnLib.h" +#include "libfreenect.hpp" -FreenectImageStream::FreenectImageStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) +const OniSensorType FreenectVideoStream::sensor_type = ONI_SENSOR_COLOR; +// pixelFormat, resolutionX, resolutionY, fps +const OniVideoMode FreenectVideoStream::supported_video_modes[] = { + { ONI_PIXEL_FORMAT_RGB888, 640, 480, 30 }, +}; + +FreenectVideoStream::FreenectVideoStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) { setVideoMode(getSupportedVideoModes()[0]); } -OniVideoMode* FreenectImageStream::getSupportedVideoModes() +OniVideoMode* FreenectVideoStream::getSupportedVideoModes() { - OniVideoMode* supported_video_modes = new OniVideoMode[1] { - { ONI_PIXEL_FORMAT_RGB888, 640, 480, 30 }, - }; - return supported_video_modes; + // make a copy to avoid need for const + OniVideoMode* modes = new OniVideoMode[SIZE(supported_video_modes)]; + std::copy(supported_video_modes, supported_video_modes+SIZE(supported_video_modes), modes); + return modes; } - -OniStatus FreenectImageStream::setVideoMode(OniVideoMode requested_mode) +OniStatus FreenectVideoStream::setVideoMode(OniVideoMode requested_mode) { OniVideoMode* supported_modes = getSupportedVideoModes(); freenect_video_format format; @@ -46,15 +53,16 @@ OniStatus FreenectImageStream::setVideoMode(OniVideoMode requested_mode) video_mode = requested_mode; return ONI_STATUS_OK; } - -void FreenectImageStream::buildFrame(void* image, OniDriverFrame* pFrame) +void FreenectVideoStream::buildFrame(void* image, OniDriverFrame* pFrame) { pFrame->frame.sensorType = ONI_SENSOR_COLOR; pFrame->frame.videoMode = video_mode; + pFrame->frame.dataSize = device->getVideoBufferSize(); pFrame->frame.width = video_mode.resolutionX; pFrame->frame.height = video_mode.resolutionY; pFrame->frame.stride = video_mode.resolutionX*3; - pFrame->frame.dataSize = device->getVideoBufferSize(); + pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; + pFrame->frame.croppingEnabled = FALSE; // copy stream buffer from freenect pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); @@ -68,10 +76,8 @@ void FreenectImageStream::buildFrame(void* image, OniDriverFrame* pFrame) std::copy(_data, _data+pFrame->frame.dataSize, frame_data); } - -// for oni::driver::StreamBase - -OniStatus FreenectImageStream::getProperty(int propertyId, void* data, int* pDataSize) +// for StreamBase +OniStatus FreenectVideoStream::getProperty(int propertyId, void* data, int* pDataSize) { switch(propertyId) { @@ -88,7 +94,7 @@ OniStatus FreenectImageStream::getProperty(int propertyId, void* data, int* pDat return ONI_STATUS_OK; } } -OniStatus FreenectImageStream::setProperty(int propertyId, const void* data, int dataSize) +OniStatus FreenectVideoStream::setProperty(int propertyId, const void* data, int dataSize) { switch(propertyId) { diff --git a/Source/Drivers/freenect/FreenectVideoStream.h b/Source/Drivers/freenect/FreenectVideoStream.h new file mode 100644 index 00000000..06ca3b49 --- /dev/null +++ b/Source/Drivers/freenect/FreenectVideoStream.h @@ -0,0 +1,37 @@ +#ifndef _FREENECT_IMAGE_STREAM_H_ +#define _FREENECT_IMAGE_STREAM_H_ + +#include "FreenectStream.h" + + +class FreenectVideoStream : public FreenectStream +{ +private: + static const OniSensorType sensor_type; + static const OniVideoMode supported_video_modes[]; + void buildFrame(void* data, OniDriverFrame* pFrame); + +protected: + OniVideoMode video_mode; + static OniSensorType getSensorType() { return sensor_type; } + static OniVideoMode* getSupportedVideoModes(); + inline const OniVideoMode getVideoMode() const { return video_mode; } + OniStatus setVideoMode(OniVideoMode requested_mode); + +public: + FreenectVideoStream(Freenect::FreenectDevice* pDevice); + ~FreenectVideoStream() { } + + static OniSensorInfo getSensorInfo() + { + OniVideoMode* modes = getSupportedVideoModes(); + // sensorType, numSupportedVideoModes, pSupportedVideoModes + return { getSensorType(), SIZE(modes), modes }; + } + + // from StreamBase + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); +}; + +#endif // _FREENECT_IMAGE_STREAM_H_ From 4db99e24da163a1d7b80ed3d6c12d70f55033a0a Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Sat, 5 Jan 2013 03:31:41 -0500 Subject: [PATCH 12/18] Added README and updated Makefile --- Makefile | 12 +- .../FreenectDepthStream.cpp | 1 - .../FreenectDepthStream.h | 6 +- .../FreenectDeviceNI.cpp | 0 .../{freenect => Freenect}/FreenectDeviceNI.h | 4 +- .../{freenect => Freenect}/FreenectDriver.cpp | 0 .../{freenect => Freenect}/FreenectDriver.h | 13 +- .../{freenect => Freenect}/FreenectStream.h | 2 +- .../FreenectVideoStream.cpp | 1 - .../FreenectVideoStream.h | 5 +- .../Drivers/{freenect => Freenect}/Makefile | 4 +- Source/Drivers/Freenect/README | 55 ++++ Source/Drivers/Freenect/libfreenect.hpp | 243 ++++++++++++++++++ 13 files changed, 320 insertions(+), 26 deletions(-) rename Source/Drivers/{freenect => Freenect}/FreenectDepthStream.cpp (98%) rename Source/Drivers/{freenect => Freenect}/FreenectDepthStream.h (83%) rename Source/Drivers/{freenect => Freenect}/FreenectDeviceNI.cpp (100%) rename Source/Drivers/{freenect => Freenect}/FreenectDeviceNI.h (96%) rename Source/Drivers/{freenect => Freenect}/FreenectDriver.cpp (100%) rename Source/Drivers/{freenect => Freenect}/FreenectDriver.h (86%) rename Source/Drivers/{freenect => Freenect}/FreenectStream.h (99%) rename Source/Drivers/{freenect => Freenect}/FreenectVideoStream.cpp (98%) rename Source/Drivers/{freenect => Freenect}/FreenectVideoStream.h (88%) rename Source/Drivers/{freenect => Freenect}/Makefile (93%) create mode 100644 Source/Drivers/Freenect/README create mode 100644 Source/Drivers/Freenect/libfreenect.hpp diff --git a/Makefile b/Makefile index 391bcc2d..c82b4b5f 100644 --- a/Makefile +++ b/Makefile @@ -11,6 +11,8 @@ # To force CLR projects use: # make FORCE_BUILD_CLR=1 # +# To enable the beta FreenectDriver that uses libfreenect for Kinect support: +# make USE_FREENECT=1 ############################################################################# include ThirdParty/PSCommon/BuildSystem/CommonDefs.mak @@ -22,8 +24,12 @@ XNLIB = ThirdParty/PSCommon/XnLib/Source ALL_DRIVERS = \ Source/Drivers/DummyDevice \ Source/Drivers/PS1080 \ - Source/Drivers/OniFile \ - Source/Drivers/freenect + Source/Drivers/OniFile + +ifeq "$(USE_FREENECT)" "1" + ALL_DRIVERS += \ + Source/Drivers/Freenect +endif # list all tools ALL_TOOLS = \ @@ -97,7 +103,7 @@ Source/Drivers/DummyDevice: $(OPENNI) $(XNLIB) Source/Drivers/RawDevice: $(OPENNI) $(XNLIB) Source/Drivers/PS1080: $(OPENNI) $(XNLIB) Source/Drivers/OniFile: $(OPENNI) $(XNLIB) -Source/Drivers/freenect: $(XNLIB) +Source/Drivers/Freenect: $(OPENNI) $(XNLIB) Source/Tools/NiViewer: $(OPENNI) $(XNLIB) diff --git a/Source/Drivers/freenect/FreenectDepthStream.cpp b/Source/Drivers/Freenect/FreenectDepthStream.cpp similarity index 98% rename from Source/Drivers/freenect/FreenectDepthStream.cpp rename to Source/Drivers/Freenect/FreenectDepthStream.cpp index acb3ef6e..0db7a043 100644 --- a/Source/Drivers/freenect/FreenectDepthStream.cpp +++ b/Source/Drivers/Freenect/FreenectDepthStream.cpp @@ -3,7 +3,6 @@ #include "libfreenect.hpp" -const OniSensorType FreenectDepthStream::sensor_type = ONI_SENSOR_DEPTH; // pixelFormat, resolutionX, resolutionY, fps const OniVideoMode FreenectDepthStream::supported_video_modes[] = { { ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30 }, diff --git a/Source/Drivers/freenect/FreenectDepthStream.h b/Source/Drivers/Freenect/FreenectDepthStream.h similarity index 83% rename from Source/Drivers/freenect/FreenectDepthStream.h rename to Source/Drivers/Freenect/FreenectDepthStream.h index 044d6c7b..c6d39732 100644 --- a/Source/Drivers/freenect/FreenectDepthStream.h +++ b/Source/Drivers/Freenect/FreenectDepthStream.h @@ -7,15 +7,15 @@ class FreenectDepthStream : public FreenectStream { private: - static const OniSensorType sensor_type; + static const OniSensorType sensor_type = ONI_SENSOR_DEPTH; static const OniVideoMode supported_video_modes[]; - void buildFrame(void* data, OniDriverFrame* pFrame); + virtual void buildFrame(void* data, OniDriverFrame* pFrame); protected: OniVideoMode video_mode; static OniSensorType getSensorType() { return sensor_type; } static OniVideoMode* getSupportedVideoModes(); - inline const OniVideoMode getVideoMode() const { return video_mode; } + const OniVideoMode getVideoMode() const { return video_mode; } OniStatus setVideoMode(OniVideoMode requested_mode); public: diff --git a/Source/Drivers/freenect/FreenectDeviceNI.cpp b/Source/Drivers/Freenect/FreenectDeviceNI.cpp similarity index 100% rename from Source/Drivers/freenect/FreenectDeviceNI.cpp rename to Source/Drivers/Freenect/FreenectDeviceNI.cpp diff --git a/Source/Drivers/freenect/FreenectDeviceNI.h b/Source/Drivers/Freenect/FreenectDeviceNI.h similarity index 96% rename from Source/Drivers/freenect/FreenectDeviceNI.h rename to Source/Drivers/Freenect/FreenectDeviceNI.h index c1d5521c..952ba6e6 100644 --- a/Source/Drivers/freenect/FreenectDeviceNI.h +++ b/Source/Drivers/Freenect/FreenectDeviceNI.h @@ -32,7 +32,7 @@ class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice switch(propertyId) { default: - case ONI_DEVICE_PROPERTY_FIRMWARE_VERSION: // By implementation + 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 @@ -84,7 +84,7 @@ class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice } - /* todo : unimplemented from DeviceBase + /* todo : from DeviceBase virtual OniStatus tryManualTrigger() {return ONI_STATUS_OK;} virtual OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF); } */ diff --git a/Source/Drivers/freenect/FreenectDriver.cpp b/Source/Drivers/Freenect/FreenectDriver.cpp similarity index 100% rename from Source/Drivers/freenect/FreenectDriver.cpp rename to Source/Drivers/Freenect/FreenectDriver.cpp diff --git a/Source/Drivers/freenect/FreenectDriver.h b/Source/Drivers/Freenect/FreenectDriver.h similarity index 86% rename from Source/Drivers/freenect/FreenectDriver.h rename to Source/Drivers/Freenect/FreenectDriver.h index e88a28ec..b33361a0 100644 --- a/Source/Drivers/freenect/FreenectDriver.h +++ b/Source/Drivers/Freenect/FreenectDriver.h @@ -18,14 +18,7 @@ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. -* */ -/* - * FreenectDriver serves as a bridge to libfreenect - * It brings Kinect and Kinect for Windows (k4w) support to OpenNI 2.x on Linux; please test on OSX! - * - * - */ #ifndef _FREENECT_DRIVER_H_ #define _FREENECT_DRIVER_H_ @@ -41,8 +34,6 @@ using namespace oni::driver; class FreenectDriver : public DriverBase, private Freenect::Freenect { private: - //Freenect::Freenect freenect; - // from Freenect::Freenect - freenect_context* m_ctx xnl::Hash devices; public: @@ -53,6 +44,7 @@ class FreenectDriver : public DriverBase, private Freenect::Freenect OniStatus initialize(DeviceConnectedCallback connectedCallback, DeviceDisconnectedCallback disconnectedCallback, DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie); DeviceBase* deviceOpen(const char* uri); void deviceClose(DeviceBase* pDevice); + OniStatus tryDevice(const char* uri); void shutdown() { for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); iter++) @@ -60,10 +52,9 @@ class FreenectDriver : public DriverBase, private Freenect::Freenect deviceClose(iter->Value()); } } - OniStatus tryDevice(const char* uri); - /* unimplemented from DriverBase + /* todo : from DriverBase virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); virtual void disableFrameSync(void* frameSyncGroup); */ diff --git a/Source/Drivers/freenect/FreenectStream.h b/Source/Drivers/Freenect/FreenectStream.h similarity index 99% rename from Source/Drivers/freenect/FreenectStream.h rename to Source/Drivers/Freenect/FreenectStream.h index 235cdc7d..ffa25ce0 100644 --- a/Source/Drivers/freenect/FreenectStream.h +++ b/Source/Drivers/Freenect/FreenectStream.h @@ -123,7 +123,7 @@ class FreenectStream : public StreamBase } - /* todo : unimplemented from StreamBase + /* 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/Source/Drivers/freenect/FreenectVideoStream.cpp b/Source/Drivers/Freenect/FreenectVideoStream.cpp similarity index 98% rename from Source/Drivers/freenect/FreenectVideoStream.cpp rename to Source/Drivers/Freenect/FreenectVideoStream.cpp index 7d12931d..07194012 100644 --- a/Source/Drivers/freenect/FreenectVideoStream.cpp +++ b/Source/Drivers/Freenect/FreenectVideoStream.cpp @@ -3,7 +3,6 @@ #include "libfreenect.hpp" -const OniSensorType FreenectVideoStream::sensor_type = ONI_SENSOR_COLOR; // pixelFormat, resolutionX, resolutionY, fps const OniVideoMode FreenectVideoStream::supported_video_modes[] = { { ONI_PIXEL_FORMAT_RGB888, 640, 480, 30 }, diff --git a/Source/Drivers/freenect/FreenectVideoStream.h b/Source/Drivers/Freenect/FreenectVideoStream.h similarity index 88% rename from Source/Drivers/freenect/FreenectVideoStream.h rename to Source/Drivers/Freenect/FreenectVideoStream.h index 06ca3b49..99bc8ccc 100644 --- a/Source/Drivers/freenect/FreenectVideoStream.h +++ b/Source/Drivers/Freenect/FreenectVideoStream.h @@ -7,7 +7,7 @@ class FreenectVideoStream : public FreenectStream { private: - static const OniSensorType sensor_type; + static const OniSensorType sensor_type = ONI_SENSOR_COLOR; static const OniVideoMode supported_video_modes[]; void buildFrame(void* data, OniDriverFrame* pFrame); @@ -15,7 +15,7 @@ class FreenectVideoStream : public FreenectStream OniVideoMode video_mode; static OniSensorType getSensorType() { return sensor_type; } static OniVideoMode* getSupportedVideoModes(); - inline const OniVideoMode getVideoMode() const { return video_mode; } + const OniVideoMode getVideoMode() const { return video_mode; } OniStatus setVideoMode(OniVideoMode requested_mode); public: @@ -34,4 +34,5 @@ class FreenectVideoStream : public FreenectStream virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); }; + #endif // _FREENECT_IMAGE_STREAM_H_ diff --git a/Source/Drivers/freenect/Makefile b/Source/Drivers/Freenect/Makefile similarity index 93% rename from Source/Drivers/freenect/Makefile rename to Source/Drivers/Freenect/Makefile index f4e25f06..b6951cd3 100644 --- a/Source/Drivers/freenect/Makefile +++ b/Source/Drivers/Freenect/Makefile @@ -16,7 +16,7 @@ ifeq ("$(OSTYPE)","Darwin") LDFLAGS += -framework CoreFoundation -framework IOKit endif -LIB_NAME = freenectDriver +LIB_NAME = FreenectDriver LIB_DIRS = ../../../ThirdParty/PSCommon/XnLib/Bin/$(PLATFORM)-$(CFG) USED_LIBS = XnLib dl pthread freenect @@ -24,7 +24,7 @@ ifneq ("$(OSTYPE)","Darwin") USED_LIBS += rt endif -#CFLAGS += -Wall +CFLAGS += -Wall OUT_DIR := $(OUT_DIR)/OpenNI2/Drivers diff --git a/Source/Drivers/Freenect/README b/Source/Drivers/Freenect/README new file mode 100644 index 00000000..dbb3e885 --- /dev/null +++ b/Source/Drivers/Freenect/README @@ -0,0 +1,55 @@ +FreenectDriver is a translation layer to libfreenect implemented as an OpenNI2 driver. +It is currently capable of providing depth and video streams at 640x480 @ 30 fps. +Copyright information appears in FreenectDriver.h +The Apache 2.0 license is available in the root of this project. + + + dependencies: +libfreenect - https://github.com/OpenKinect/libfreenect +libfreenect with K4W support - https://github.com/zarvox/libfreenect/tree/k4w-wip + + build: +make USE_FREENECT=1 + + + structure: +This module is modeled heavily on TestDevice.cpp and Drivers/Kinect/ +It ties together the C++ interfaces of OpenNI2 and libfreenect using multiple inheritance. +FreenectDriver inherits publically from DriverBase and privately from Freenect::Freenect. +A custom libfreenect.hpp allows protected access to the Freenect context, so that FreenectDriver can call the Freenect's C API. +As a DriverBase, FreenectDriver manages devices and sets up device state callbacks. + +FreenectDeviceNI inherits publically from DeviceBase and Freenect::FreenectDevice. +Because of this, it can be built by Freenect::Freenect::createDevice() and it can define FreenectDevice's depth and video callbacks. +Those callbacks trigger acquireFrame in FreenectStream. + +FreenectStream is a virtual base class inheriting from StreamBase. +It does generic frame setup in acquireFrame() and then calls pure virtual buildFrame() to let derived classes finish the frame. +It also provides a skeleton for setting and getting properties. +Only properties that are generically true for all streams should be set there. +Derived classes implementing getProperty() should call FreenectStream::getProperty() in their default case. + +FreenectDepthStream and FreenectVideoStream are nearly identical in definition and implementation. +They differ mostly in the formats they use to process data - depth comes in uint8_t and video in sets of 3 unsigned char +These two stream implementations also offer a system to report supported video modes - after implementing a new mode, simply add it to supported_video_modes[] in the corresponding .cpp + + +NOTE: This is beta software, tested as yet by no one but myself. +It's a good, clean start but it leaves a log of libfreenect untouched. +Suggestions, bug reports, and insights on Freenect and OpenNI formats are welcome. + + +TODO: + +provide more OniVideoMode and OniStreamProperty +support more FREENECT_RESOLUTION and FREENECT_VIDEO +mirrored mode support +FreenectIRStream +polish and publish Gentoo ebuilds +acquire and test with original Kinect (within 2 weeks) - please report issues! +PROPER LOGGING! +improve K4W support in libfreenect +implement interesting derived functions +tilt motor support +audio stream +OSX support - please hack! diff --git a/Source/Drivers/Freenect/libfreenect.hpp b/Source/Drivers/Freenect/libfreenect.hpp new file mode 100644 index 00000000..15123c42 --- /dev/null +++ b/Source/Drivers/Freenect/libfreenect.hpp @@ -0,0 +1,243 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2010 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 +#include +#include + +namespace Freenect { + class Noncopyable { + public: + Noncopyable() {} + ~Noncopyable() {} + private: + Noncopyable( const Noncopyable& ); + const Noncopyable& operator=( const Noncopyable& ); + }; + + class FreenectTiltState { + friend class FreenectDevice; + FreenectTiltState(freenect_raw_tilt_state *_state): + m_code(_state->tilt_status), m_state(_state) + {} + public: + void getAccelerometers(double* x, double* y, double* z) { + freenect_get_mks_accel(m_state, x, y, z); + } + double getTiltDegs() { + return freenect_get_tilt_degs(m_state); + } + public: + freenect_tilt_status_code m_code; + private: + freenect_raw_tilt_state *m_state; + }; + + class FreenectDevice : Noncopyable { + public: + FreenectDevice(freenect_context *_ctx, int _index) + : m_video_resolution(FREENECT_RESOLUTION_MEDIUM), m_depth_resolution(FREENECT_RESOLUTION_MEDIUM) + { + if(freenect_open_device(_ctx, &m_dev, _index) < 0) throw std::runtime_error("Cannot open Kinect"); + freenect_set_user(m_dev, this); + freenect_set_video_mode(m_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB)); + freenect_set_depth_mode(m_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT)); + freenect_set_depth_callback(m_dev, freenect_depth_callback); + freenect_set_video_callback(m_dev, freenect_video_callback); + } + virtual ~FreenectDevice() { + if(freenect_close_device(m_dev) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion"); + } + void startVideo() { + if(freenect_start_video(m_dev) < 0) throw std::runtime_error("Cannot start RGB callback"); + } + void stopVideo() { + if(freenect_stop_video(m_dev) < 0) throw std::runtime_error("Cannot stop RGB callback"); + } + void startDepth() { + if(freenect_start_depth(m_dev) < 0) throw std::runtime_error("Cannot start depth callback"); + } + void stopDepth() { + if(freenect_stop_depth(m_dev) < 0) throw std::runtime_error("Cannot stop depth callback"); + } + void setTiltDegrees(double _angle) { + if(freenect_set_tilt_degs(m_dev, _angle) < 0) throw std::runtime_error("Cannot set angle in degrees"); + } + void setLed(freenect_led_options _option) { + if(freenect_set_led(m_dev, _option) < 0) throw std::runtime_error("Cannot set led"); + } + void updateState() { + if (freenect_update_tilt_state(m_dev) < 0) throw std::runtime_error("Cannot update device state"); + } + FreenectTiltState getState() const { + return FreenectTiltState(freenect_get_tilt_state(m_dev)); + } + void setVideoFormat(freenect_video_format requested_format, freenect_resolution requested_resolution = FREENECT_RESOLUTION_MEDIUM) { + if (requested_format != m_video_format || requested_resolution != m_video_resolution) { + freenect_stop_video(m_dev); + freenect_frame_mode mode = freenect_find_video_mode(requested_resolution, requested_format); + if (!mode.is_valid) throw std::runtime_error("Cannot set video format: invalid mode"); + if (freenect_set_video_mode(m_dev, mode) < 0) throw std::runtime_error("Cannot set video format"); + freenect_start_video(m_dev); + m_video_format = requested_format; + m_video_resolution = requested_resolution; + } + } + freenect_video_format getVideoFormat() { + return m_video_format; + } + freenect_resolution getVideoResolution() { + return m_video_resolution; + } + int getVideoBufferSize(){ + switch(m_video_format) { + case FREENECT_VIDEO_RGB: + case FREENECT_VIDEO_BAYER: + case FREENECT_VIDEO_IR_8BIT: + case FREENECT_VIDEO_IR_10BIT: + case FREENECT_VIDEO_IR_10BIT_PACKED: + case FREENECT_VIDEO_YUV_RGB: + case FREENECT_VIDEO_YUV_RAW: + return freenect_find_video_mode(m_video_resolution, m_video_format).bytes; + default: + return 0; + } + } + void setDepthFormat(freenect_depth_format requested_format, freenect_resolution requested_resolution = FREENECT_RESOLUTION_MEDIUM) { + if (requested_format != m_depth_format || requested_resolution != m_depth_resolution) { + freenect_stop_depth(m_dev); + freenect_frame_mode mode = freenect_find_depth_mode(requested_resolution, requested_format); + if (!mode.is_valid) throw std::runtime_error("Cannot set depth format: invalid mode"); + if (freenect_set_depth_mode(m_dev, mode) < 0) throw std::runtime_error("Cannot set depth format"); + freenect_start_depth(m_dev); + m_depth_format = requested_format; + m_depth_resolution = requested_resolution; + } + } + freenect_depth_format getDepthFormat() { + return m_depth_format; + } + freenect_resolution getDepthResolution() { + return m_depth_resolution; + } + int getDepthBufferSize() { + return freenect_get_current_depth_mode(m_dev).bytes; + } + // Do not call directly even in child + virtual void VideoCallback(void *video, uint32_t timestamp) = 0; + // Do not call directly even in child + virtual void DepthCallback(void *depth, uint32_t timestamp) = 0; + private: + freenect_device *m_dev; + freenect_video_format m_video_format; + freenect_depth_format m_depth_format; + freenect_resolution m_video_resolution; + freenect_resolution m_depth_resolution; + static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) { + FreenectDevice* device = static_cast(freenect_get_user(dev)); + device->DepthCallback(depth, timestamp); + } + static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) { + FreenectDevice* device = static_cast(freenect_get_user(dev)); + device->VideoCallback(video, timestamp); + } + }; + + class Freenect : Noncopyable { + private: + typedef std::map DeviceMap; + public: + Freenect() : m_stop(false) { + if(freenect_init(&m_ctx, NULL) < 0) throw std::runtime_error("Cannot initialize freenect library"); + // We claim both the motor and camera devices, since this class exposes both. + // It does not support audio, so we do not claim it. + freenect_select_subdevices(m_ctx, static_cast(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); + if(pthread_create(&m_thread, NULL, pthread_callback, (void*)this) != 0) throw std::runtime_error("Cannot initialize freenect thread"); + } + ~Freenect() { + for(DeviceMap::iterator it = m_devices.begin() ; it != m_devices.end() ; ++it) { + delete it->second; + } + m_stop = true; + pthread_join(m_thread, NULL); + if(freenect_shutdown(m_ctx) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion"); + } + template + ConcreteDevice& createDevice(int _index) { + DeviceMap::iterator it = m_devices.find(_index); + if (it != m_devices.end()) delete it->second; + ConcreteDevice * device = new ConcreteDevice(m_ctx, _index); + m_devices.insert(std::make_pair(_index, device)); + return *device; + } + void deleteDevice(int _index) { + DeviceMap::iterator it = m_devices.find(_index); + if (it == m_devices.end()) return; + delete it->second; + m_devices.erase(it); + } + int deviceCount() { + return freenect_num_devices(m_ctx); + } + // Do not call directly, thread runs here + void operator()() { + while(!m_stop) { + int res = freenect_process_events(m_ctx); + if (res < 0) + { + // libusb signals an error has occurred + if (res == LIBUSB_ERROR_INTERRUPTED) + { + // This happens sometimes, it means that a system call in libusb was interrupted somehow (perhaps due to a signal) + // The simple solution seems to be just ignore it. + continue; + } + std::stringstream ss; + ss << "Cannot process freenect events (libusb error code: " << res << ")"; + throw std::runtime_error(ss.str()); + } + } + } + static void *pthread_callback(void *user_data) { + Freenect* freenect = static_cast(user_data); + (*freenect)(); + return NULL; + } + protected: + freenect_context *m_ctx; + private: + volatile bool m_stop; + pthread_t m_thread; + DeviceMap m_devices; + }; + +} + From e177a3275573d27f7150f4660705806701ca9e8c Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Sat, 5 Jan 2013 22:39:02 -0500 Subject: [PATCH 13/18] Reworked video mode support in FreenectVideoStream; FreenectDepthStream to follow suit --- Source/Drivers/Freenect/FreenectDeviceNI.h | 2 +- Source/Drivers/Freenect/FreenectDriver.h | 4 +- Source/Drivers/Freenect/FreenectStream.h | 13 ++++ .../Drivers/Freenect/FreenectVideoStream.cpp | 60 +++++++++---------- Source/Drivers/Freenect/FreenectVideoStream.h | 27 +++++---- Source/Drivers/Freenect/Makefile | 2 +- Source/Drivers/Freenect/README | 5 +- 7 files changed, 66 insertions(+), 47 deletions(-) diff --git a/Source/Drivers/Freenect/FreenectDeviceNI.h b/Source/Drivers/Freenect/FreenectDeviceNI.h index 952ba6e6..29fc65c5 100644 --- a/Source/Drivers/Freenect/FreenectDeviceNI.h +++ b/Source/Drivers/Freenect/FreenectDeviceNI.h @@ -61,7 +61,7 @@ class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice return ONI_STATUS_NOT_SUPPORTED; } } - virtual OniBool isCommandSupported(int propertyId) { return (invoke(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } + OniBool isCommandSupported(int propertyId) { return (invoke(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } virtual OniStatus invoke(int commandId, const void* data, int dataSize) { switch (commandId) diff --git a/Source/Drivers/Freenect/FreenectDriver.h b/Source/Drivers/Freenect/FreenectDriver.h index b33361a0..5ff0a893 100644 --- a/Source/Drivers/Freenect/FreenectDriver.h +++ b/Source/Drivers/Freenect/FreenectDriver.h @@ -25,8 +25,8 @@ #include "libfreenect.hpp" #include "Driver/OniDriverAPI.h" -#include "FreenectDeviceNI.h" #include "XnHash.h" +#include "FreenectDeviceNI.h" using namespace oni::driver; @@ -47,7 +47,7 @@ class FreenectDriver : public DriverBase, private Freenect::Freenect OniStatus tryDevice(const char* uri); void shutdown() { - for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); iter++) + for (xnl::Hash::Iterator iter = devices.Begin(); iter != devices.End(); ++iter) { deviceClose(iter->Value()); } diff --git a/Source/Drivers/Freenect/FreenectStream.h b/Source/Drivers/Freenect/FreenectStream.h index ffa25ce0..0f00f333 100644 --- a/Source/Drivers/Freenect/FreenectStream.h +++ b/Source/Drivers/Freenect/FreenectStream.h @@ -8,6 +8,15 @@ #define SIZE(array) sizeof array / sizeof 0[array] +struct RetrieveKey +{ + template + typename T::first_type operator()(T pair) const + { + return pair.first; + } +}; + typedef struct { int refCount; @@ -18,6 +27,10 @@ static bool operator==(const OniVideoMode& left, const OniVideoMode& right) return (left.pixelFormat == right.pixelFormat && left.resolutionX == right.resolutionX && left.resolutionY == right.resolutionY && left.fps == right.fps); } +static bool operator<(const OniVideoMode& left, const OniVideoMode& right) +{ + return (left.resolutionX*left.resolutionY < right.resolutionX*right.resolutionY); +} using namespace oni::driver; diff --git a/Source/Drivers/Freenect/FreenectVideoStream.cpp b/Source/Drivers/Freenect/FreenectVideoStream.cpp index 07194012..c303d7cf 100644 --- a/Source/Drivers/Freenect/FreenectVideoStream.cpp +++ b/Source/Drivers/Freenect/FreenectVideoStream.cpp @@ -1,51 +1,51 @@ #include "FreenectVideoStream.h" #include "XnLib.h" #include "libfreenect.hpp" +#include -// pixelFormat, resolutionX, resolutionY, fps -const OniVideoMode FreenectVideoStream::supported_video_modes[] = { - { ONI_PIXEL_FORMAT_RGB888, 640, 480, 30 }, -}; +// Video mode handling - add modes to getSupportedVideoModes() as you implement them +const std::map< OniVideoMode, std::pair > FreenectVideoStream::supported_video_modes = FreenectVideoStream::getSupportedVideoModes(); +FreenectVideoModeMap FreenectVideoStream::getSupportedVideoModes() +{ + FreenectVideoModeMap modes; + // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30)] = { FREENECT_VIDEO_RGB, FREENECT_RESOLUTION_MEDIUM }; + + + return modes; + + /* working format possiblities + FREENECT_VIDEO_RGB + FREENECT_VIDEO_YUV_RGB + FREENECT_VIDEO_YUV_RAW + */ +} + FreenectVideoStream::FreenectVideoStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) { - setVideoMode(getSupportedVideoModes()[0]); + setVideoMode(makeOniVideoMode( ONI_PIXEL_FORMAT_RGB888, 640, 480, 30 )); } -OniVideoMode* FreenectVideoStream::getSupportedVideoModes() +OniSensorInfo FreenectVideoStream::getSensorInfo() { - // make a copy to avoid need for const - OniVideoMode* modes = new OniVideoMode[SIZE(supported_video_modes)]; - std::copy(supported_video_modes, supported_video_modes+SIZE(supported_video_modes), modes); - return modes; + OniVideoMode* modes = new OniVideoMode[supported_video_modes.size()]; + std::transform(supported_video_modes.begin(), supported_video_modes.end(), modes, RetrieveKey()); + // sensorType, numSupportedVideoModes, pSupportedVideoModes + return { sensor_type, SIZE(modes), modes }; } OniStatus FreenectVideoStream::setVideoMode(OniVideoMode requested_mode) { - OniVideoMode* supported_modes = getSupportedVideoModes(); - freenect_video_format format; - freenect_resolution resolution; + FreenectVideoModeMap::const_iterator matched_mode = supported_video_modes.find(requested_mode); - if (requested_mode == supported_modes[0]) - { - format = FREENECT_VIDEO_RGB; - resolution = FREENECT_RESOLUTION_MEDIUM; - - /* working format possiblities - FREENECT_VIDEO_RGB - FREENECT_VIDEO_YUV_RGB - FREENECT_VIDEO_YUV_RAW - */ - } - else - { + if (matched_mode == supported_video_modes.end()) return ONI_STATUS_NOT_SUPPORTED; - } - try { device->setVideoFormat(format, resolution); } + try { device->setVideoFormat(matched_mode->second.first, matched_mode->second.second); } catch (std::runtime_error e) { - printf("format-resolution combination not supported: %d-%d\n", format, resolution); + printf("format-resolution combination not supported by libfreenect: %d-%d\n", matched_mode->second.first, matched_mode->second.second); return ONI_STATUS_NOT_SUPPORTED; } @@ -54,7 +54,7 @@ OniStatus FreenectVideoStream::setVideoMode(OniVideoMode requested_mode) } void FreenectVideoStream::buildFrame(void* image, OniDriverFrame* pFrame) { - pFrame->frame.sensorType = ONI_SENSOR_COLOR; + pFrame->frame.sensorType = sensor_type; pFrame->frame.videoMode = video_mode; pFrame->frame.dataSize = device->getVideoBufferSize(); pFrame->frame.width = video_mode.resolutionX; diff --git a/Source/Drivers/Freenect/FreenectVideoStream.h b/Source/Drivers/Freenect/FreenectVideoStream.h index 99bc8ccc..29327543 100644 --- a/Source/Drivers/Freenect/FreenectVideoStream.h +++ b/Source/Drivers/Freenect/FreenectVideoStream.h @@ -4,31 +4,36 @@ #include "FreenectStream.h" +typedef std::map< OniVideoMode, std::pair > FreenectVideoModeMap; + class FreenectVideoStream : public FreenectStream { private: - static const OniSensorType sensor_type = ONI_SENSOR_COLOR; - static const OniVideoMode supported_video_modes[]; - void buildFrame(void* data, OniDriverFrame* pFrame); + static const FreenectVideoModeMap supported_video_modes; + static FreenectVideoModeMap getSupportedVideoModes(); + virtual void buildFrame(void* data, OniDriverFrame* pFrame); -protected: +protected: + static const OniSensorType sensor_type = ONI_SENSOR_COLOR; OniVideoMode video_mode; - static OniSensorType getSensorType() { return sensor_type; } - static OniVideoMode* getSupportedVideoModes(); - const OniVideoMode getVideoMode() const { return video_mode; } OniStatus setVideoMode(OniVideoMode requested_mode); public: FreenectVideoStream(Freenect::FreenectDevice* pDevice); ~FreenectVideoStream() { } - static OniSensorInfo getSensorInfo() + static OniSensorInfo getSensorInfo(); + static OniVideoMode makeOniVideoMode(OniPixelFormat pixel_format, int resolution_x, int resolution_y, int frames_per_second) { - OniVideoMode* modes = getSupportedVideoModes(); - // sensorType, numSupportedVideoModes, pSupportedVideoModes - return { getSensorType(), SIZE(modes), modes }; + OniVideoMode mode; + mode.pixelFormat = pixel_format; + mode.resolutionX = resolution_x; + mode.resolutionY = resolution_y; + mode.fps = frames_per_second; + return mode; } + // from StreamBase virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); diff --git a/Source/Drivers/Freenect/Makefile b/Source/Drivers/Freenect/Makefile index b6951cd3..689a39c3 100644 --- a/Source/Drivers/Freenect/Makefile +++ b/Source/Drivers/Freenect/Makefile @@ -24,7 +24,7 @@ ifneq ("$(OSTYPE)","Darwin") USED_LIBS += rt endif -CFLAGS += -Wall +CFLAGS += -Wall -w OUT_DIR := $(OUT_DIR)/OpenNI2/Drivers diff --git a/Source/Drivers/Freenect/README b/Source/Drivers/Freenect/README index dbb3e885..bac0713a 100644 --- a/Source/Drivers/Freenect/README +++ b/Source/Drivers/Freenect/README @@ -21,7 +21,7 @@ As a DriverBase, FreenectDriver manages devices and sets up device state callbac FreenectDeviceNI inherits publically from DeviceBase and Freenect::FreenectDevice. Because of this, it can be built by Freenect::Freenect::createDevice() and it can define FreenectDevice's depth and video callbacks. -Those callbacks trigger acquireFrame in FreenectStream. +Those callbacks trigger acquireFrame() in FreenectStream. FreenectStream is a virtual base class inheriting from StreamBase. It does generic frame setup in acquireFrame() and then calls pure virtual buildFrame() to let derived classes finish the frame. @@ -35,7 +35,7 @@ These two stream implementations also offer a system to report supported video m NOTE: This is beta software, tested as yet by no one but myself. -It's a good, clean start but it leaves a log of libfreenect untouched. +It's a good, clean start but it leaves a lot of libfreenect untouched. Suggestions, bug reports, and insights on Freenect and OpenNI formats are welcome. @@ -52,4 +52,5 @@ improve K4W support in libfreenect implement interesting derived functions tilt motor support audio stream +create C++ versions of OniCTypes (with constructors and overloaded comparison operators) OSX support - please hack! From e27942458b40c3e0ef97e14bdade2744a8c58c94 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Sun, 6 Jan 2013 03:06:12 -0500 Subject: [PATCH 14/18] Made FreenectVideoStream virtual; new FreenectColorStream takes its old role --- .../Drivers/Freenect/FreenectColorStream.cpp | 32 +++++ Source/Drivers/Freenect/FreenectColorStream.h | 47 +++++++ .../Drivers/Freenect/FreenectDepthStream.cpp | 104 ++------------ Source/Drivers/Freenect/FreenectDepthStream.h | 56 +++++--- Source/Drivers/Freenect/FreenectDeviceNI.cpp | 16 +-- Source/Drivers/Freenect/FreenectDeviceNI.h | 6 +- Source/Drivers/Freenect/FreenectStream.h | 19 +-- .../Drivers/Freenect/FreenectVideoStream.cpp | 131 ------------------ Source/Drivers/Freenect/FreenectVideoStream.h | 118 +++++++++++++--- Source/Drivers/Freenect/README | 27 ++-- 10 files changed, 242 insertions(+), 314 deletions(-) create mode 100644 Source/Drivers/Freenect/FreenectColorStream.cpp create mode 100644 Source/Drivers/Freenect/FreenectColorStream.h delete mode 100644 Source/Drivers/Freenect/FreenectVideoStream.cpp diff --git a/Source/Drivers/Freenect/FreenectColorStream.cpp b/Source/Drivers/Freenect/FreenectColorStream.cpp new file mode 100644 index 00000000..738aea96 --- /dev/null +++ b/Source/Drivers/Freenect/FreenectColorStream.cpp @@ -0,0 +1,32 @@ +#include "FreenectColorStream.h" + + +const OniVideoMode FreenectColorStream::default_video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30); + +// Add video modes here as you implement them +FreenectVideoModeMap FreenectColorStream::getSupportedVideoModes() +{ + FreenectVideoModeMap modes; + // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30)] = { FREENECT_VIDEO_RGB, FREENECT_RESOLUTION_MEDIUM }; + + + return modes; + + /* working format possiblities + FREENECT_VIDEO_RGB + FREENECT_VIDEO_YUV_RGB + FREENECT_VIDEO_YUV_RAW + */ +} +void FreenectColorStream::populateFrame(void* image, OniDriverFrame* pFrame) const +{ + pFrame->frame.sensorType = sensor_type; + pFrame->frame.stride = video_mode.resolutionX*3; + pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; + pFrame->frame.croppingEnabled = FALSE; + // copy stream buffer from freenect + unsigned char* _data = static_cast(image); + unsigned char* frame_data = static_cast(pFrame->frame.data); + std::copy(_data, _data+pFrame->frame.dataSize, frame_data); +} diff --git a/Source/Drivers/Freenect/FreenectColorStream.h b/Source/Drivers/Freenect/FreenectColorStream.h new file mode 100644 index 00000000..8951c08a --- /dev/null +++ b/Source/Drivers/Freenect/FreenectColorStream.h @@ -0,0 +1,47 @@ +#ifndef _FREENECT_COLOR_STREAM_H_ +#define _FREENECT_COLOR_STREAM_H_ + +#include "FreenectVideoStream.h" +#include "Driver/OniDriverAPI.h" +#include "libfreenect.hpp" + + +typedef std::map< OniVideoMode, std::pair > FreenectVideoModeMap; + +class FreenectColorStream : public FreenectVideoStream +{ +private: + static const OniSensorType sensor_type = ONI_SENSOR_COLOR; + static const OniVideoMode default_video_mode; + static FreenectVideoModeMap getSupportedVideoModes(); + virtual void populateFrame(void* image, OniDriverFrame* pFrame) const; + OniStatus setVideoMode(OniVideoMode requested_mode) + { + FreenectVideoModeMap::const_iterator matched_mode_iter = getSupportedVideoModes().find(requested_mode); + if (matched_mode_iter == getSupportedVideoModes().end()) + return ONI_STATUS_NOT_SUPPORTED; + try { device->setVideoFormat(matched_mode_iter->second.first, matched_mode_iter->second.second); } + catch (std::runtime_error e) + { + printf("format-resolution combination not supported by libfreenect: %d-%d\n", matched_mode_iter->second.first, matched_mode_iter->second.second); + return ONI_STATUS_NOT_SUPPORTED; + } + video_mode = requested_mode; + return ONI_STATUS_OK; + } + +public: + FreenectColorStream(Freenect::FreenectDevice* pDevice) : FreenectVideoStream(pDevice) { setVideoMode(default_video_mode); } + ~FreenectColorStream() { } + + static OniSensorInfo getSensorInfo() + { + FreenectVideoModeMap supported_modes = getSupportedVideoModes(); + OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; + std::transform(supported_modes.begin(), supported_modes.end(), modes, RetrieveKey()); + return { sensor_type, SIZE(modes), modes }; // sensorType, numSupportedVideoModes, pSupportedVideoModes + } +}; + + +#endif // _FREENECT_COLOR_STREAM_H_ diff --git a/Source/Drivers/Freenect/FreenectDepthStream.cpp b/Source/Drivers/Freenect/FreenectDepthStream.cpp index 0db7a043..6c42d6b7 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.cpp +++ b/Source/Drivers/Freenect/FreenectDepthStream.cpp @@ -1,116 +1,30 @@ #include "FreenectDepthStream.h" -#include "XnLib.h" -#include "libfreenect.hpp" -// pixelFormat, resolutionX, resolutionY, fps -const OniVideoMode FreenectDepthStream::supported_video_modes[] = { - { ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30 }, -}; +const OniVideoMode FreenectDepthStream::default_video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30); -FreenectDepthStream::FreenectDepthStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) -{ - setVideoMode(getSupportedVideoModes()[0]); -} - -OniVideoMode* FreenectDepthStream::getSupportedVideoModes() +// Add video modes here as you implement them +FreenectDepthModeMap FreenectDepthStream::getSupportedVideoModes() { - // make a copy to avoid need for const - OniVideoMode* modes = new OniVideoMode[SIZE(supported_video_modes)]; - std::copy(supported_video_modes, supported_video_modes+SIZE(supported_video_modes), modes); - return modes; -} -OniStatus FreenectDepthStream::setVideoMode(OniVideoMode requested_mode) -{ - freenect_depth_format format; - freenect_resolution resolution; + FreenectDepthModeMap modes; + // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30)] = { FREENECT_DEPTH_11BIT, FREENECT_RESOLUTION_MEDIUM }; - if (requested_mode == supported_video_modes[0]) - { - format = FREENECT_DEPTH_11BIT; - resolution = FREENECT_RESOLUTION_MEDIUM; - - /* working format possiblities - FREENECT_DEPTH_10BIT - FREENECT_DEPTH_REGISTERED - FREENECT_DEPTH_MM - */ - } - else - { - return ONI_STATUS_NOT_SUPPORTED; - } - try { device->setDepthFormat(format, resolution); } - catch (std::runtime_error e) - { - printf("format-resolution combination not supported: %d-%d\n", format, resolution); - return ONI_STATUS_NOT_SUPPORTED; - } - - video_mode = requested_mode; - return ONI_STATUS_OK; + return modes; } -void FreenectDepthStream::buildFrame(void* depth, OniDriverFrame* pFrame) +void FreenectDepthStream::populateFrame(void* depth, OniDriverFrame* pFrame) const { - pFrame->frame.sensorType = ONI_SENSOR_DEPTH; - pFrame->frame.videoMode = video_mode; - pFrame->frame.dataSize = device->getDepthBufferSize(); - pFrame->frame.width = video_mode.resolutionX; - pFrame->frame.height = video_mode.resolutionY; + pFrame->frame.sensorType = sensor_type; pFrame->frame.stride = video_mode.resolutionX*sizeof(OniDepthPixel); pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; pFrame->frame.croppingEnabled = FALSE; - // copy stream buffer from freenect - pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); - if (pFrame->frame.data == NULL) - { - XN_ASSERT(FALSE); - return; - } uint8_t* _data = static_cast(depth); uint8_t* frame_data = static_cast(pFrame->frame.data); std::copy(_data, _data+pFrame->frame.dataSize, frame_data); } -// for StreamBase -OniStatus FreenectDepthStream::getProperty(int propertyId, void* data, int* pDataSize) -{ - switch(propertyId) - { - default: - return FreenectStream::getProperty(propertyId, data, pDataSize); - - case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* - if (*pDataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = video_mode; - return ONI_STATUS_OK; - } -} -OniStatus FreenectDepthStream::setProperty(int propertyId, const void* data, int dataSize) -{ - switch(propertyId) - { - default: - return FreenectStream::setProperty(propertyId, data, dataSize); - - case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* - if (dataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); - return ONI_STATUS_ERROR; - } - raisePropertyChanged(propertyId, data, dataSize); - return setVideoMode(*(reinterpret_cast(data))); - } -} - - /* depth video modes reference FREENECT_DEPTH_11BIT = 0, //< 11 bit depth information in one uint16_t/pixel diff --git a/Source/Drivers/Freenect/FreenectDepthStream.h b/Source/Drivers/Freenect/FreenectDepthStream.h index c6d39732..d22996e5 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.h +++ b/Source/Drivers/Freenect/FreenectDepthStream.h @@ -1,37 +1,47 @@ #ifndef _FREENECT_DEPTH_STREAM_H_ #define _FREENECT_DEPTH_STREAM_H_ -#include "FreenectStream.h" +#include "FreenectVideoStream.h" +#include "Driver/OniDriverAPI.h" +#include "libfreenect.hpp" -class FreenectDepthStream : public FreenectStream -{ +typedef std::map< OniVideoMode, std::pair > FreenectDepthModeMap; + +class FreenectDepthStream : public FreenectVideoStream +{ private: static const OniSensorType sensor_type = ONI_SENSOR_DEPTH; - static const OniVideoMode supported_video_modes[]; - virtual void buildFrame(void* data, OniDriverFrame* pFrame); - -protected: - OniVideoMode video_mode; - static OniSensorType getSensorType() { return sensor_type; } - static OniVideoMode* getSupportedVideoModes(); - const OniVideoMode getVideoMode() const { return video_mode; } - OniStatus setVideoMode(OniVideoMode requested_mode); - -public: - FreenectDepthStream(Freenect::FreenectDevice* pDevice); - ~FreenectDepthStream() { } + static const OniVideoMode default_video_mode; + static FreenectDepthModeMap getSupportedVideoModes(); + virtual void populateFrame(void* image, OniDriverFrame* pFrame) const; + OniStatus setVideoMode(OniVideoMode requested_mode) + { + FreenectDepthModeMap::const_iterator matched_mode_iter = getSupportedVideoModes().find(requested_mode); + if (matched_mode_iter == getSupportedVideoModes().end()) + return ONI_STATUS_NOT_SUPPORTED; + try { device->setDepthFormat(matched_mode_iter->second.first, matched_mode_iter->second.second); } + catch (std::runtime_error e) + { + printf("format-resolution combination not supported by libfreenect: %d-%d\n", matched_mode_iter->second.first, matched_mode_iter->second.second); + return ONI_STATUS_NOT_SUPPORTED; + } + video_mode = requested_mode; + return ONI_STATUS_OK; + } +public: + FreenectDepthStream(Freenect::FreenectDevice* pDevice) : FreenectVideoStream(pDevice) { setVideoMode(default_video_mode); } + ~FreenectDepthStream() { } + static OniSensorInfo getSensorInfo() { - OniVideoMode* modes = getSupportedVideoModes(); - // sensorType, numSupportedVideoModes, pSupportedVideoModes - return { getSensorType(), SIZE(modes), modes }; + FreenectDepthModeMap supported_modes = getSupportedVideoModes(); + OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; + std::transform(supported_modes.begin(), supported_modes.end(), modes, RetrieveKey()); + return { sensor_type, SIZE(modes), modes }; // sensorType, numSupportedVideoModes, pSupportedVideoModes } - - // from StreamBase - virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); - virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); }; + #endif // _FREENECT_DEPTH_STREAM_H_ diff --git a/Source/Drivers/Freenect/FreenectDeviceNI.cpp b/Source/Drivers/Freenect/FreenectDeviceNI.cpp index b79719e2..73ee975d 100644 --- a/Source/Drivers/Freenect/FreenectDeviceNI.cpp +++ b/Source/Drivers/Freenect/FreenectDeviceNI.cpp @@ -5,12 +5,12 @@ FreenectDeviceNI::FreenectDeviceNI(freenect_context *_ctx, int _index) : Freenect::FreenectDevice(_ctx, _index) { depth_stream = NULL; - video_stream = NULL; + color_stream = NULL; } FreenectDeviceNI::~FreenectDeviceNI() { destroyStream(depth_stream); - destroyStream(video_stream); + destroyStream(color_stream); } // for DeviceBase @@ -19,7 +19,7 @@ OniStatus FreenectDeviceNI::getSensorInfoList(OniSensorInfo** pSensors, int* num *numSensors = 2; OniSensorInfo * sensors = new OniSensorInfo[*numSensors]; sensors[0] = depth_stream->getSensorInfo(); - sensors[1] = video_stream->getSensorInfo(); + sensors[1] = color_stream->getSensorInfo(); *pSensors = sensors; return ONI_STATUS_OK; } @@ -34,9 +34,9 @@ StreamBase* FreenectDeviceNI::createStream(OniSensorType sensorType) return depth_stream; case ONI_SENSOR_COLOR: Freenect::FreenectDevice::startVideo(); - if (video_stream == NULL) - video_stream = XN_NEW(FreenectVideoStream, this); - return video_stream; + if (color_stream == NULL) + color_stream = XN_NEW(FreenectColorStream, this); + return color_stream; // todo: IR default: //m_driverServices.errorLoggerAppend("FreenectDeviceNI: Can't create a stream of type %d", sensorType); @@ -50,10 +50,10 @@ void FreenectDeviceNI::destroyStream(StreamBase* pStream) Freenect::FreenectDevice::stopDepth(); depth_stream = NULL; } - if (pStream == video_stream) + if (pStream == color_stream) { Freenect::FreenectDevice::stopVideo(); - video_stream = NULL; + color_stream = NULL; } XN_DELETE(pStream); diff --git a/Source/Drivers/Freenect/FreenectDeviceNI.h b/Source/Drivers/Freenect/FreenectDeviceNI.h index 29fc65c5..a85c5592 100644 --- a/Source/Drivers/Freenect/FreenectDeviceNI.h +++ b/Source/Drivers/Freenect/FreenectDeviceNI.h @@ -4,7 +4,7 @@ #include "libfreenect.hpp" #include "Driver/OniDriverAPI.h" #include "FreenectDepthStream.h" -#include "FreenectVideoStream.h" +#include "FreenectColorStream.h" using namespace oni::driver; @@ -13,7 +13,7 @@ class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice { protected: FreenectDepthStream* depth_stream; - FreenectVideoStream* video_stream; + FreenectColorStream* color_stream; public: FreenectDeviceNI(freenect_context *_ctx, int _index); @@ -80,7 +80,7 @@ class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice } void VideoCallback(void *image, uint32_t timestamp) { - video_stream->acquireFrame(image, timestamp); + color_stream->acquireFrame(image, timestamp); } diff --git a/Source/Drivers/Freenect/FreenectStream.h b/Source/Drivers/Freenect/FreenectStream.h index 0f00f333..48c4b4c7 100644 --- a/Source/Drivers/Freenect/FreenectStream.h +++ b/Source/Drivers/Freenect/FreenectStream.h @@ -22,16 +22,6 @@ typedef struct int refCount; } FreenectStreamFrameCookie; -static bool operator==(const OniVideoMode& left, const OniVideoMode& right) -{ - return (left.pixelFormat == right.pixelFormat && left.resolutionX == right.resolutionX - && left.resolutionY == right.resolutionY && left.fps == right.fps); -} -static bool operator<(const OniVideoMode& left, const OniVideoMode& right) -{ - return (left.resolutionX*left.resolutionY < right.resolutionX*right.resolutionY); -} - using namespace oni::driver; @@ -39,9 +29,10 @@ class FreenectStream : public StreamBase { private: int frame_id; // number each frame - virtual void buildFrame(void* data, OniDriverFrame* pFrame) = 0; + virtual void buildFrame(void* data, OniDriverFrame* pFrame) const = 0; protected: + static const OniSensorType sensor_type; Freenect::FreenectDevice* device; bool running; // acquireFrame() does something iff true @@ -51,10 +42,7 @@ class FreenectStream : public StreamBase device = pDevice; frame_id = 1; } - ~FreenectStream() - { - stop(); - } + ~FreenectStream() { stop(); } virtual void acquireFrame(void* data, uint32_t timestamp) { @@ -69,7 +57,6 @@ class FreenectStream : public StreamBase } pFrame->pDriverCookie = xnOSMalloc(sizeof(FreenectStreamFrameCookie)); ((FreenectStreamFrameCookie*)pFrame->pDriverCookie)->refCount = 1; - pFrame->frame.frameIndex = frame_id++; pFrame->frame.timestamp = timestamp; diff --git a/Source/Drivers/Freenect/FreenectVideoStream.cpp b/Source/Drivers/Freenect/FreenectVideoStream.cpp deleted file mode 100644 index c303d7cf..00000000 --- a/Source/Drivers/Freenect/FreenectVideoStream.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include "FreenectVideoStream.h" -#include "XnLib.h" -#include "libfreenect.hpp" -#include - - -// Video mode handling - add modes to getSupportedVideoModes() as you implement them -const std::map< OniVideoMode, std::pair > FreenectVideoStream::supported_video_modes = FreenectVideoStream::getSupportedVideoModes(); -FreenectVideoModeMap FreenectVideoStream::getSupportedVideoModes() -{ - FreenectVideoModeMap modes; - // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution - modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30)] = { FREENECT_VIDEO_RGB, FREENECT_RESOLUTION_MEDIUM }; - - - return modes; - - /* working format possiblities - FREENECT_VIDEO_RGB - FREENECT_VIDEO_YUV_RGB - FREENECT_VIDEO_YUV_RAW - */ -} - - -FreenectVideoStream::FreenectVideoStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) -{ - setVideoMode(makeOniVideoMode( ONI_PIXEL_FORMAT_RGB888, 640, 480, 30 )); -} - -OniSensorInfo FreenectVideoStream::getSensorInfo() -{ - OniVideoMode* modes = new OniVideoMode[supported_video_modes.size()]; - std::transform(supported_video_modes.begin(), supported_video_modes.end(), modes, RetrieveKey()); - // sensorType, numSupportedVideoModes, pSupportedVideoModes - return { sensor_type, SIZE(modes), modes }; -} -OniStatus FreenectVideoStream::setVideoMode(OniVideoMode requested_mode) -{ - FreenectVideoModeMap::const_iterator matched_mode = supported_video_modes.find(requested_mode); - - if (matched_mode == supported_video_modes.end()) - return ONI_STATUS_NOT_SUPPORTED; - - try { device->setVideoFormat(matched_mode->second.first, matched_mode->second.second); } - catch (std::runtime_error e) - { - printf("format-resolution combination not supported by libfreenect: %d-%d\n", matched_mode->second.first, matched_mode->second.second); - return ONI_STATUS_NOT_SUPPORTED; - } - - video_mode = requested_mode; - return ONI_STATUS_OK; -} -void FreenectVideoStream::buildFrame(void* image, OniDriverFrame* pFrame) -{ - pFrame->frame.sensorType = sensor_type; - pFrame->frame.videoMode = video_mode; - pFrame->frame.dataSize = device->getVideoBufferSize(); - pFrame->frame.width = video_mode.resolutionX; - pFrame->frame.height = video_mode.resolutionY; - pFrame->frame.stride = video_mode.resolutionX*3; - pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; - pFrame->frame.croppingEnabled = FALSE; - - // copy stream buffer from freenect - pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); - if (pFrame->frame.data == NULL) - { - XN_ASSERT(FALSE); - return; - } - unsigned char* _data = static_cast(image); - unsigned char* frame_data = static_cast(pFrame->frame.data); - std::copy(_data, _data+pFrame->frame.dataSize, frame_data); -} - -// for StreamBase -OniStatus FreenectVideoStream::getProperty(int propertyId, void* data, int* pDataSize) -{ - switch(propertyId) - { - default: - return FreenectStream::getProperty(propertyId, data, pDataSize); - - case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* - if (*pDataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = video_mode; - return ONI_STATUS_OK; - } -} -OniStatus FreenectVideoStream::setProperty(int propertyId, const void* data, int dataSize) -{ - switch(propertyId) - { - default: - return FreenectStream::setProperty(propertyId, data, dataSize); - - case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* - if (dataSize != sizeof(OniVideoMode)) - { - printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); - return ONI_STATUS_ERROR; - } - raisePropertyChanged(propertyId, data, dataSize); - return setVideoMode(*(reinterpret_cast(data))); - } -} - - -/* image video modes reference - -FREENECT_VIDEO_RGB = 0, //< Decompressed RGB mode (demosaicing done by libfreenect) -FREENECT_VIDEO_BAYER = 1, //< Bayer compressed mode (raw information from camera) -FREENECT_VIDEO_IR_8BIT = 2, //< 8-bit IR mode -FREENECT_VIDEO_IR_10BIT = 3, //< 10-bit IR mode -FREENECT_VIDEO_IR_10BIT_PACKED = 4, //< 10-bit packed IR mode -FREENECT_VIDEO_YUV_RGB = 5, //< YUV RGB mode -FREENECT_VIDEO_YUV_RAW = 6, //< YUV Raw mode -FREENECT_VIDEO_DUMMY = 2147483647, //< Dummy value to force enum to be 32 bits wide - -ONI_PIXEL_FORMAT_RGB888 = 200, -ONI_PIXEL_FORMAT_YUV422 = 201, -ONI_PIXEL_FORMAT_GRAY8 = 202, -ONI_PIXEL_FORMAT_GRAY16 = 203, -ONI_PIXEL_FORMAT_JPEG = 204, -*/ diff --git a/Source/Drivers/Freenect/FreenectVideoStream.h b/Source/Drivers/Freenect/FreenectVideoStream.h index 29327543..790a8802 100644 --- a/Source/Drivers/Freenect/FreenectVideoStream.h +++ b/Source/Drivers/Freenect/FreenectVideoStream.h @@ -1,43 +1,115 @@ -#ifndef _FREENECT_IMAGE_STREAM_H_ -#define _FREENECT_IMAGE_STREAM_H_ +#ifndef _FREENECT_VIDEO_STREAM_H_ +#define _FREENECT_VIDEO_STREAM_H_ #include "FreenectStream.h" +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +#include -typedef std::map< OniVideoMode, std::pair > FreenectVideoModeMap; +static OniVideoMode makeOniVideoMode(OniPixelFormat pixel_format, int resolution_x, int resolution_y, int frames_per_second) +{ + OniVideoMode mode; + mode.pixelFormat = pixel_format; + mode.resolutionX = resolution_x; + mode.resolutionY = resolution_y; + mode.fps = frames_per_second; + return mode; +} +static bool operator==(const OniVideoMode& left, const OniVideoMode& right) +{ + return (left.pixelFormat == right.pixelFormat && left.resolutionX == right.resolutionX + && left.resolutionY == right.resolutionY && left.fps == right.fps); +} +static bool operator<(const OniVideoMode& left, const OniVideoMode& right) +{ + return (left.resolutionX*left.resolutionY < right.resolutionX*right.resolutionY); +} + class FreenectVideoStream : public FreenectStream { private: - static const FreenectVideoModeMap supported_video_modes; - static FreenectVideoModeMap getSupportedVideoModes(); - virtual void buildFrame(void* data, OniDriverFrame* pFrame); + virtual OniStatus setVideoMode(OniVideoMode requested_mode) = 0; + virtual void populateFrame(void* data, OniDriverFrame* pFrame) const = 0; + virtual void buildFrame(void* data, OniDriverFrame* pFrame) const + { + pFrame->frame.videoMode = video_mode; + pFrame->frame.dataSize = device->getVideoBufferSize(); + pFrame->frame.width = video_mode.resolutionX; + pFrame->frame.height = video_mode.resolutionY; + pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); + if (pFrame->frame.data == NULL) + { + XN_ASSERT(FALSE); + return; + } + populateFrame(data, pFrame); + } protected: - static const OniSensorType sensor_type = ONI_SENSOR_COLOR; OniVideoMode video_mode; - OniStatus setVideoMode(OniVideoMode requested_mode); public: - FreenectVideoStream(Freenect::FreenectDevice* pDevice); + FreenectVideoStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) { } ~FreenectVideoStream() { } - static OniSensorInfo getSensorInfo(); - static OniVideoMode makeOniVideoMode(OniPixelFormat pixel_format, int resolution_x, int resolution_y, int frames_per_second) + // from StreamBase + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) { - OniVideoMode mode; - mode.pixelFormat = pixel_format; - mode.resolutionX = resolution_x; - mode.resolutionY = resolution_y; - mode.fps = frames_per_second; - return mode; + switch(propertyId) + { + default: + return FreenectStream::getProperty(propertyId, data, pDataSize); + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (*pDataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = video_mode; + return ONI_STATUS_OK; + } + } + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + switch(propertyId) + { + default: + return FreenectStream::setProperty(propertyId, data, dataSize); + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (dataSize != sizeof(OniVideoMode)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); + return ONI_STATUS_ERROR; + } + if (ONI_STATUS_OK == setVideoMode(*(reinterpret_cast(data)))) + { + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + } + } } - - - // from StreamBase - virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); - virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); }; -#endif // _FREENECT_IMAGE_STREAM_H_ +/* image video modes reference + +FREENECT_VIDEO_RGB = 0, //< Decompressed RGB mode (demosaicing done by libfreenect) +FREENECT_VIDEO_BAYER = 1, //< Bayer compressed mode (raw information from camera) +FREENECT_VIDEO_IR_8BIT = 2, //< 8-bit IR mode +FREENECT_VIDEO_IR_10BIT = 3, //< 10-bit IR mode +FREENECT_VIDEO_IR_10BIT_PACKED = 4, //< 10-bit packed IR mode +FREENECT_VIDEO_YUV_RGB = 5, //< YUV RGB mode +FREENECT_VIDEO_YUV_RAW = 6, //< YUV Raw mode +FREENECT_VIDEO_DUMMY = 2147483647, //< Dummy value to force enum to be 32 bits wide + +ONI_PIXEL_FORMAT_RGB888 = 200, +ONI_PIXEL_FORMAT_YUV422 = 201, +ONI_PIXEL_FORMAT_GRAY8 = 202, +ONI_PIXEL_FORMAT_GRAY16 = 203, +ONI_PIXEL_FORMAT_JPEG = 204, +*/ + + +#endif // _FREENECT_VIDEO_STREAM_H_ diff --git a/Source/Drivers/Freenect/README b/Source/Drivers/Freenect/README index bac0713a..5a48ec08 100644 --- a/Source/Drivers/Freenect/README +++ b/Source/Drivers/Freenect/README @@ -1,12 +1,13 @@ FreenectDriver is a translation layer to libfreenect implemented as an OpenNI2 driver. +It allows use of Kinect hardware as supported by libfreenect. It is currently capable of providing depth and video streams at 640x480 @ 30 fps. Copyright information appears in FreenectDriver.h The Apache 2.0 license is available in the root of this project. dependencies: -libfreenect - https://github.com/OpenKinect/libfreenect -libfreenect with K4W support - https://github.com/zarvox/libfreenect/tree/k4w-wip +libfreenect - https://github.com/OpenKinect/libfreenect +libfreenect with K4W support - https://github.com/zarvox/libfreenect/tree/k4w-wip build: make USE_FREENECT=1 @@ -15,6 +16,7 @@ make USE_FREENECT=1 structure: This module is modeled heavily on TestDevice.cpp and Drivers/Kinect/ It ties together the C++ interfaces of OpenNI2 and libfreenect using multiple inheritance. + FreenectDriver inherits publically from DriverBase and privately from Freenect::Freenect. A custom libfreenect.hpp allows protected access to the Freenect context, so that FreenectDriver can call the Freenect's C API. As a DriverBase, FreenectDriver manages devices and sets up device state callbacks. @@ -25,22 +27,18 @@ Those callbacks trigger acquireFrame() in FreenectStream. FreenectStream is a virtual base class inheriting from StreamBase. It does generic frame setup in acquireFrame() and then calls pure virtual buildFrame() to let derived classes finish the frame. -It also provides a skeleton for setting and getting properties. -Only properties that are generically true for all streams should be set there. -Derived classes implementing getProperty() should call FreenectStream::getProperty() in their default case. - -FreenectDepthStream and FreenectVideoStream are nearly identical in definition and implementation. -They differ mostly in the formats they use to process data - depth comes in uint8_t and video in sets of 3 unsigned char -These two stream implementations also offer a system to report supported video modes - after implementing a new mode, simply add it to supported_video_modes[] in the corresponding .cpp +It also provides the base skeleton for setting and getting properties, which cascades down the inheritance tree. +FreenectVideoStream is another virtual base that extends FreenectStream to be specific to visual data. -NOTE: This is beta software, tested as yet by no one but myself. -It's a good, clean start but it leaves a lot of libfreenect untouched. -Suggestions, bug reports, and insights on Freenect and OpenNI formats are welcome. +FreenectDepthStream and FreenectColorStream are nearly identical in definition and implementation, both inheriting from FreenectVideoStream. +They differ mostly in the formats they use to process data and the video modes they support. +These two implementations offer a system to store and report supported video modes. +To implement a new mode, simply add it to getSupportedVideoModes() and modify populateFrame() if necessary. -TODO: + TODO: provide more OniVideoMode and OniStreamProperty support more FREENECT_RESOLUTION and FREENECT_VIDEO mirrored mode support @@ -51,6 +49,5 @@ PROPER LOGGING! improve K4W support in libfreenect implement interesting derived functions tilt motor support -audio stream -create C++ versions of OniCTypes (with constructors and overloaded comparison operators) OSX support - please hack! +audio stream From 9931a031ef2b87d5fce5960ac96288bb10f68080 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Sun, 6 Jan 2013 17:23:37 -0500 Subject: [PATCH 15/18] Fixed weird results from mode map iteration in setVideoMode() --- .../Drivers/Freenect/FreenectColorStream.cpp | 21 +++++++++++++++---- Source/Drivers/Freenect/FreenectColorStream.h | 17 +++++++++------ .../Drivers/Freenect/FreenectDepthStream.cpp | 10 +++++---- Source/Drivers/Freenect/FreenectDepthStream.h | 17 +++++++++------ 4 files changed, 45 insertions(+), 20 deletions(-) diff --git a/Source/Drivers/Freenect/FreenectColorStream.cpp b/Source/Drivers/Freenect/FreenectColorStream.cpp index 738aea96..a760d606 100644 --- a/Source/Drivers/Freenect/FreenectColorStream.cpp +++ b/Source/Drivers/Freenect/FreenectColorStream.cpp @@ -9,7 +9,7 @@ FreenectVideoModeMap FreenectColorStream::getSupportedVideoModes() FreenectVideoModeMap modes; // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30)] = { FREENECT_VIDEO_RGB, FREENECT_RESOLUTION_MEDIUM }; - + return modes; @@ -19,14 +19,27 @@ FreenectVideoModeMap FreenectColorStream::getSupportedVideoModes() FREENECT_VIDEO_YUV_RAW */ } -void FreenectColorStream::populateFrame(void* image, OniDriverFrame* pFrame) const +void FreenectColorStream::populateFrame(void* data, OniDriverFrame* pFrame) const { pFrame->frame.sensorType = sensor_type; pFrame->frame.stride = video_mode.resolutionX*3; pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; pFrame->frame.croppingEnabled = FALSE; // copy stream buffer from freenect - unsigned char* _data = static_cast(image); + unsigned char* _data = static_cast(data); unsigned char* frame_data = static_cast(pFrame->frame.data); - std::copy(_data, _data+pFrame->frame.dataSize, frame_data); + std::copy(_data, _data+pFrame->frame.dataSize, frame_data); } + + +/* color video modes reference + +FREENECT_VIDEO_RGB = 0, //< Decompressed RGB mode (demosaicing done by libfreenect) +FREENECT_VIDEO_BAYER = 1, //< Bayer compressed mode (raw information from camera) +FREENECT_VIDEO_YUV_RGB = 5, //< YUV RGB mode +FREENECT_VIDEO_YUV_RAW = 6, //< YUV Raw mode + +ONI_PIXEL_FORMAT_RGB888 = 200, +ONI_PIXEL_FORMAT_YUV422 = 201, +ONI_PIXEL_FORMAT_JPEG = 204, +*/ diff --git a/Source/Drivers/Freenect/FreenectColorStream.h b/Source/Drivers/Freenect/FreenectColorStream.h index 8951c08a..9512bd64 100644 --- a/Source/Drivers/Freenect/FreenectColorStream.h +++ b/Source/Drivers/Freenect/FreenectColorStream.h @@ -14,16 +14,21 @@ class FreenectColorStream : public FreenectVideoStream static const OniSensorType sensor_type = ONI_SENSOR_COLOR; static const OniVideoMode default_video_mode; static FreenectVideoModeMap getSupportedVideoModes(); - virtual void populateFrame(void* image, OniDriverFrame* pFrame) const; + virtual void populateFrame(void* data, OniDriverFrame* pFrame) const; OniStatus setVideoMode(OniVideoMode requested_mode) { - FreenectVideoModeMap::const_iterator matched_mode_iter = getSupportedVideoModes().find(requested_mode); - if (matched_mode_iter == getSupportedVideoModes().end()) - return ONI_STATUS_NOT_SUPPORTED; - try { device->setVideoFormat(matched_mode_iter->second.first, matched_mode_iter->second.second); } + 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; + + freenect_video_format format = matched_mode_iter->second.first; + freenect_resolution resolution = matched_mode_iter->second.second; + + try { device->setVideoFormat(format, resolution); } catch (std::runtime_error e) { - printf("format-resolution combination not supported by libfreenect: %d-%d\n", matched_mode_iter->second.first, matched_mode_iter->second.second); + printf("format-resolution combination not supported by libfreenect: %d-%d\n", format, resolution); return ONI_STATUS_NOT_SUPPORTED; } video_mode = requested_mode; diff --git a/Source/Drivers/Freenect/FreenectDepthStream.cpp b/Source/Drivers/Freenect/FreenectDepthStream.cpp index 6c42d6b7..5d6404d8 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.cpp +++ b/Source/Drivers/Freenect/FreenectDepthStream.cpp @@ -8,21 +8,23 @@ FreenectDepthModeMap FreenectDepthStream::getSupportedVideoModes() { FreenectDepthModeMap modes; // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution - modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30)] = { FREENECT_DEPTH_11BIT, FREENECT_RESOLUTION_MEDIUM }; - + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30)] = { FREENECT_DEPTH_REGISTERED, FREENECT_RESOLUTION_MEDIUM }; + return modes; } -void FreenectDepthStream::populateFrame(void* depth, OniDriverFrame* pFrame) const +void FreenectDepthStream::populateFrame(void* data, OniDriverFrame* pFrame) const { pFrame->frame.sensorType = sensor_type; pFrame->frame.stride = video_mode.resolutionX*sizeof(OniDepthPixel); pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; pFrame->frame.croppingEnabled = FALSE; // copy stream buffer from freenect - uint8_t* _data = static_cast(depth); + uint8_t* _data = static_cast(data); uint8_t* frame_data = static_cast(pFrame->frame.data); std::copy(_data, _data+pFrame->frame.dataSize, frame_data); + + //printf("size of frame data = %d\n", pFrame->frame.dataSize); } /* depth video modes reference diff --git a/Source/Drivers/Freenect/FreenectDepthStream.h b/Source/Drivers/Freenect/FreenectDepthStream.h index d22996e5..6faece0a 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.h +++ b/Source/Drivers/Freenect/FreenectDepthStream.h @@ -14,16 +14,21 @@ class FreenectDepthStream : public FreenectVideoStream static const OniSensorType sensor_type = ONI_SENSOR_DEPTH; static const OniVideoMode default_video_mode; static FreenectDepthModeMap getSupportedVideoModes(); - virtual void populateFrame(void* image, OniDriverFrame* pFrame) const; + virtual void populateFrame(void* data, OniDriverFrame* pFrame) const; OniStatus setVideoMode(OniVideoMode requested_mode) { - FreenectDepthModeMap::const_iterator matched_mode_iter = getSupportedVideoModes().find(requested_mode); - if (matched_mode_iter == getSupportedVideoModes().end()) - return ONI_STATUS_NOT_SUPPORTED; - try { device->setDepthFormat(matched_mode_iter->second.first, matched_mode_iter->second.second); } + FreenectDepthModeMap supported_video_modes = getSupportedVideoModes(); + FreenectDepthModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode); + if (matched_mode_iter == supported_video_modes.end()) + return ONI_STATUS_NOT_SUPPORTED; + + freenect_depth_format format = matched_mode_iter->second.first; + freenect_resolution resolution = matched_mode_iter->second.second; + + try { device->setDepthFormat(format, resolution); } catch (std::runtime_error e) { - printf("format-resolution combination not supported by libfreenect: %d-%d\n", matched_mode_iter->second.first, matched_mode_iter->second.second); + printf("format-resolution combination not supported by libfreenect: %d-%d\n", format, resolution); return ONI_STATUS_NOT_SUPPORTED; } video_mode = requested_mode; From 68dbf28720d48d30380375f3d211b46a0d1ca0c1 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Sun, 6 Jan 2013 17:48:57 -0500 Subject: [PATCH 16/18] Moved mode map typedefs to more appropriate scopes --- Source/Drivers/Freenect/FreenectColorStream.cpp | 2 +- Source/Drivers/Freenect/FreenectColorStream.h | 2 -- Source/Drivers/Freenect/FreenectDepthStream.cpp | 2 +- Source/Drivers/Freenect/FreenectDepthStream.h | 5 +++-- Source/Drivers/Freenect/FreenectVideoStream.h | 1 + 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Source/Drivers/Freenect/FreenectColorStream.cpp b/Source/Drivers/Freenect/FreenectColorStream.cpp index a760d606..6ae32f49 100644 --- a/Source/Drivers/Freenect/FreenectColorStream.cpp +++ b/Source/Drivers/Freenect/FreenectColorStream.cpp @@ -4,7 +4,7 @@ const OniVideoMode FreenectColorStream::default_video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30); // Add video modes here as you implement them -FreenectVideoModeMap FreenectColorStream::getSupportedVideoModes() +FreenectVideoStream::FreenectVideoModeMap FreenectColorStream::getSupportedVideoModes() { FreenectVideoModeMap modes; // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution diff --git a/Source/Drivers/Freenect/FreenectColorStream.h b/Source/Drivers/Freenect/FreenectColorStream.h index 9512bd64..51d8fcdf 100644 --- a/Source/Drivers/Freenect/FreenectColorStream.h +++ b/Source/Drivers/Freenect/FreenectColorStream.h @@ -6,8 +6,6 @@ #include "libfreenect.hpp" -typedef std::map< OniVideoMode, std::pair > FreenectVideoModeMap; - class FreenectColorStream : public FreenectVideoStream { private: diff --git a/Source/Drivers/Freenect/FreenectDepthStream.cpp b/Source/Drivers/Freenect/FreenectDepthStream.cpp index 5d6404d8..10b8abab 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.cpp +++ b/Source/Drivers/Freenect/FreenectDepthStream.cpp @@ -4,7 +4,7 @@ const OniVideoMode FreenectDepthStream::default_video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30); // Add video modes here as you implement them -FreenectDepthModeMap FreenectDepthStream::getSupportedVideoModes() +FreenectDepthStream::FreenectDepthModeMap FreenectDepthStream::getSupportedVideoModes() { FreenectDepthModeMap modes; // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution diff --git a/Source/Drivers/Freenect/FreenectDepthStream.h b/Source/Drivers/Freenect/FreenectDepthStream.h index 6faece0a..0bb68d33 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.h +++ b/Source/Drivers/Freenect/FreenectDepthStream.h @@ -6,10 +6,11 @@ #include "libfreenect.hpp" -typedef std::map< OniVideoMode, std::pair > FreenectDepthModeMap; - class FreenectDepthStream : public FreenectVideoStream { +protected: + typedef std::map< OniVideoMode, std::pair > FreenectDepthModeMap; + private: static const OniSensorType sensor_type = ONI_SENSOR_DEPTH; static const OniVideoMode default_video_mode; diff --git a/Source/Drivers/Freenect/FreenectVideoStream.h b/Source/Drivers/Freenect/FreenectVideoStream.h index 790a8802..64511127 100644 --- a/Source/Drivers/Freenect/FreenectVideoStream.h +++ b/Source/Drivers/Freenect/FreenectVideoStream.h @@ -48,6 +48,7 @@ class FreenectVideoStream : public FreenectStream } protected: + typedef std::map< OniVideoMode, std::pair > FreenectVideoModeMap; OniVideoMode video_mode; public: From 75af9b0154c34e08d017acd3d9a201a7a34abba9 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Mon, 7 Jan 2013 13:25:49 -0500 Subject: [PATCH 17/18] Added mirroring support to FreenectColorStream --- .../Drivers/Freenect/FreenectColorStream.cpp | 36 ++++++++++++++++++- Source/Drivers/Freenect/FreenectColorStream.h | 3 ++ .../Drivers/Freenect/FreenectDepthStream.cpp | 28 +++++++++++++-- Source/Drivers/Freenect/FreenectDepthStream.h | 3 ++ Source/Drivers/Freenect/FreenectDeviceNI.h | 6 ++-- Source/Drivers/Freenect/FreenectStream.h | 6 ++-- Source/Drivers/Freenect/FreenectVideoStream.h | 24 +++++++++---- Source/Drivers/Freenect/README | 5 +-- 8 files changed, 92 insertions(+), 19 deletions(-) diff --git a/Source/Drivers/Freenect/FreenectColorStream.cpp b/Source/Drivers/Freenect/FreenectColorStream.cpp index 6ae32f49..f00fcd78 100644 --- a/Source/Drivers/Freenect/FreenectColorStream.cpp +++ b/Source/Drivers/Freenect/FreenectColorStream.cpp @@ -28,7 +28,41 @@ void FreenectColorStream::populateFrame(void* data, OniDriverFrame* pFrame) cons // copy stream buffer from freenect unsigned char* _data = static_cast(data); unsigned char* frame_data = static_cast(pFrame->frame.data); - std::copy(_data, _data+pFrame->frame.dataSize, frame_data); + if (mirroring) + { + for (unsigned int i = 0; i < pFrame->frame.dataSize; i += 3) + { + // find corresponding mirrored pixel + int pixel = i / 3; + int row = pixel / video_mode.resolutionX; + int col = video_mode.resolutionX - (pixel % video_mode.resolutionX); + int target = 3 * (row * video_mode.resolutionX + col); + // copy it to this pixel + frame_data[i] = _data[target]; + frame_data[i+1] = _data[target+1]; + frame_data[i+2] = _data[target+2]; + } + } + else + std::copy(_data, _data+pFrame->frame.dataSize, frame_data); +} + +// for StreamBase +OniStatus FreenectColorStream::setProperty(int propertyId, const void* data, int dataSize) +{ + switch (propertyId) + { + default: + return FreenectVideoStream::setProperty(propertyId, data, dataSize); + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (dataSize != sizeof(OniBool)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniBool)); + return ONI_STATUS_ERROR; + } + mirroring = *(static_cast(data)); + return ONI_STATUS_OK; + } } diff --git a/Source/Drivers/Freenect/FreenectColorStream.h b/Source/Drivers/Freenect/FreenectColorStream.h index 51d8fcdf..b0956c07 100644 --- a/Source/Drivers/Freenect/FreenectColorStream.h +++ b/Source/Drivers/Freenect/FreenectColorStream.h @@ -44,6 +44,9 @@ class FreenectColorStream : public FreenectVideoStream std::transform(supported_modes.begin(), supported_modes.end(), modes, RetrieveKey()); return { sensor_type, SIZE(modes), modes }; // sensorType, numSupportedVideoModes, pSupportedVideoModes } + + // from StreamBase + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); }; diff --git a/Source/Drivers/Freenect/FreenectDepthStream.cpp b/Source/Drivers/Freenect/FreenectDepthStream.cpp index 10b8abab..2c7e6d1e 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.cpp +++ b/Source/Drivers/Freenect/FreenectDepthStream.cpp @@ -14,7 +14,7 @@ FreenectDepthStream::FreenectDepthModeMap FreenectDepthStream::getSupportedVideo return modes; } void FreenectDepthStream::populateFrame(void* data, OniDriverFrame* pFrame) const -{ +{ pFrame->frame.sensorType = sensor_type; pFrame->frame.stride = video_mode.resolutionX*sizeof(OniDepthPixel); pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; @@ -22,11 +22,33 @@ void FreenectDepthStream::populateFrame(void* data, OniDriverFrame* pFrame) cons // copy stream buffer from freenect uint8_t* _data = static_cast(data); uint8_t* frame_data = static_cast(pFrame->frame.data); - std::copy(_data, _data+pFrame->frame.dataSize, frame_data); - + //if (mirroring) + // std::reverse_copy(_data, _data+pFrame->frame.dataSize, frame_data); + //else + std::copy(_data, _data+pFrame->frame.dataSize, frame_data); + //printf("size of frame data = %d\n", pFrame->frame.dataSize); } +// for StreamBase +OniStatus FreenectDepthStream::setProperty(int propertyId, const void* data, int dataSize) +{ + switch (propertyId) + { + default: + return FreenectVideoStream::setProperty(propertyId, data, dataSize); + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (dataSize != sizeof(OniBool)) + { + printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniBool)); + return ONI_STATUS_ERROR; + } + mirroring = *(static_cast(data)); + return ONI_STATUS_OK; + } +} + + /* depth video modes reference FREENECT_DEPTH_11BIT = 0, //< 11 bit depth information in one uint16_t/pixel diff --git a/Source/Drivers/Freenect/FreenectDepthStream.h b/Source/Drivers/Freenect/FreenectDepthStream.h index 0bb68d33..ebae5aa8 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.h +++ b/Source/Drivers/Freenect/FreenectDepthStream.h @@ -47,6 +47,9 @@ class FreenectDepthStream : public FreenectVideoStream std::transform(supported_modes.begin(), supported_modes.end(), modes, RetrieveKey()); return { sensor_type, SIZE(modes), modes }; // sensorType, numSupportedVideoModes, pSupportedVideoModes } + + // from StreamBase + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); }; diff --git a/Source/Drivers/Freenect/FreenectDeviceNI.h b/Source/Drivers/Freenect/FreenectDeviceNI.h index a85c5592..ce380dbd 100644 --- a/Source/Drivers/Freenect/FreenectDeviceNI.h +++ b/Source/Drivers/Freenect/FreenectDeviceNI.h @@ -21,6 +21,7 @@ class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice // from DeviceBase OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors); + virtual OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); } StreamBase* createStream(OniSensorType sensorType); void destroyStream(StreamBase* pStream); // property and command handlers are empty skeletons by default @@ -29,7 +30,7 @@ class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) { - switch(propertyId) + switch (propertyId) { default: case ONI_DEVICE_PROPERTY_FIRMWARE_VERSION: // string @@ -46,7 +47,7 @@ class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice } virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) { - switch(propertyId) + switch (propertyId) { default: case ONI_DEVICE_PROPERTY_FIRMWARE_VERSION: // By implementation @@ -86,7 +87,6 @@ class FreenectDeviceNI : public DeviceBase, public Freenect::FreenectDevice /* todo : from DeviceBase virtual OniStatus tryManualTrigger() {return ONI_STATUS_OK;} - virtual OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF); } */ }; diff --git a/Source/Drivers/Freenect/FreenectStream.h b/Source/Drivers/Freenect/FreenectStream.h index 48c4b4c7..01f64cf0 100644 --- a/Source/Drivers/Freenect/FreenectStream.h +++ b/Source/Drivers/Freenect/FreenectStream.h @@ -79,11 +79,11 @@ class FreenectStream : public StreamBase } // property handlers are empty skeletons by default // only add here if the property is generic to all children - // otherwise, implement in child and call these in default case (see FreenectDepthStream.h) + // otherwise, implement in child and call these in default case (see FreenectVideoStream.h) OniBool isPropertySupported(int propertyId) { return (getProperty(propertyId, NULL, NULL) != ONI_STATUS_NOT_SUPPORTED); } virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) { - switch(propertyId) + switch (propertyId) { default: case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* @@ -103,7 +103,7 @@ class FreenectStream : public StreamBase } virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) { - switch(propertyId) + switch (propertyId) { default: case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* diff --git a/Source/Drivers/Freenect/FreenectVideoStream.h b/Source/Drivers/Freenect/FreenectVideoStream.h index 64511127..82303067 100644 --- a/Source/Drivers/Freenect/FreenectVideoStream.h +++ b/Source/Drivers/Freenect/FreenectVideoStream.h @@ -50,6 +50,7 @@ class FreenectVideoStream : public FreenectStream protected: typedef std::map< OniVideoMode, std::pair > FreenectVideoModeMap; OniVideoMode video_mode; + bool mirroring; public: FreenectVideoStream(Freenect::FreenectDevice* pDevice) : FreenectStream(pDevice) { } @@ -58,11 +59,11 @@ class FreenectVideoStream : public FreenectStream // from StreamBase virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) { - switch(propertyId) + switch (propertyId) { default: return FreenectStream::getProperty(propertyId, data, pDataSize); - case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* if (*pDataSize != sizeof(OniVideoMode)) { printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVideoMode)); @@ -70,25 +71,34 @@ class FreenectVideoStream : public FreenectStream } *(static_cast(data)) = video_mode; return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (*pDataSize != sizeof(OniBool)) + { + printf("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniBool)); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = mirroring; + return ONI_STATUS_OK; } } virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) { - switch(propertyId) + switch (propertyId) { default: return FreenectStream::setProperty(propertyId, data, dataSize); - case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* if (dataSize != sizeof(OniVideoMode)) { printf("Unexpected size: %d != %d\n", dataSize, sizeof(OniVideoMode)); return ONI_STATUS_ERROR; } - if (ONI_STATUS_OK == setVideoMode(*(reinterpret_cast(data)))) + if (ONI_STATUS_OK != setVideoMode(*(static_cast(data)))) { - raisePropertyChanged(propertyId, data, dataSize); - return ONI_STATUS_OK; + return ONI_STATUS_NOT_SUPPORTED; } + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; } } }; diff --git a/Source/Drivers/Freenect/README b/Source/Drivers/Freenect/README index 5a48ec08..8014cefc 100644 --- a/Source/Drivers/Freenect/README +++ b/Source/Drivers/Freenect/README @@ -39,13 +39,14 @@ To implement a new mode, simply add it to getSupportedVideoModes() and modify po TODO: -provide more OniVideoMode and OniStreamProperty -support more FREENECT_RESOLUTION and FREENECT_VIDEO mirrored mode support FreenectIRStream polish and publish Gentoo ebuilds acquire and test with original Kinect (within 2 weeks) - please report issues! +support more FREENECT_RESOLUTION and FREENECT_VIDEO +allow toggling image registration PROPER LOGGING! +provide more OniVideoMode and OniStreamProperty improve K4W support in libfreenect implement interesting derived functions tilt motor support From a53bc59ee72a2ae19226ecdc6549cce895f11b46 Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Tue, 8 Jan 2013 14:32:44 -0500 Subject: [PATCH 18/18] Added mirroring support to FreenectDepthStream; moved buffer malloc to populateFrame() for more correct buffer sizes and data types --- .../Drivers/Freenect/FreenectColorStream.cpp | 51 ++++++++++++------- Source/Drivers/Freenect/FreenectColorStream.h | 2 +- .../Drivers/Freenect/FreenectDepthStream.cpp | 32 +++++++++--- Source/Drivers/Freenect/FreenectDepthStream.h | 2 +- Source/Drivers/Freenect/FreenectVideoStream.h | 7 --- Source/Drivers/Freenect/README | 1 - 6 files changed, 59 insertions(+), 36 deletions(-) diff --git a/Source/Drivers/Freenect/FreenectColorStream.cpp b/Source/Drivers/Freenect/FreenectColorStream.cpp index f00fcd78..c14412a9 100644 --- a/Source/Drivers/Freenect/FreenectColorStream.cpp +++ b/Source/Drivers/Freenect/FreenectColorStream.cpp @@ -24,27 +24,42 @@ void FreenectColorStream::populateFrame(void* data, OniDriverFrame* pFrame) cons pFrame->frame.sensorType = sensor_type; pFrame->frame.stride = video_mode.resolutionX*3; pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; - pFrame->frame.croppingEnabled = FALSE; + pFrame->frame.croppingEnabled = FALSE; + pFrame->frame.dataSize = device->getVideoBufferSize(); + pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); + if (pFrame->frame.data == NULL) + { + XN_ASSERT(FALSE); + return; + } // copy stream buffer from freenect - unsigned char* _data = static_cast(data); - unsigned char* frame_data = static_cast(pFrame->frame.data); - if (mirroring) + switch (video_mode.pixelFormat) { - for (unsigned int i = 0; i < pFrame->frame.dataSize; i += 3) - { - // find corresponding mirrored pixel - int pixel = i / 3; - int row = pixel / video_mode.resolutionX; - int col = video_mode.resolutionX - (pixel % video_mode.resolutionX); - int target = 3 * (row * video_mode.resolutionX + col); - // copy it to this pixel - frame_data[i] = _data[target]; - frame_data[i+1] = _data[target+1]; - frame_data[i+2] = _data[target+2]; - } + default: + printf("pixelFormat %s not supported by populateFrame\n", video_mode.pixelFormat); + return; + case ONI_PIXEL_FORMAT_RGB888: + unsigned char* _data = static_cast(data); + unsigned char* frame_data = static_cast(pFrame->frame.data); + if (mirroring) + { + for (unsigned int i = 0; i < pFrame->frame.dataSize; i += 3) + { + // find corresponding mirrored pixel + unsigned int pixel = i / 3; + unsigned int row = pixel / video_mode.resolutionX; + unsigned int col = video_mode.resolutionX - (pixel % video_mode.resolutionX); + unsigned int target = 3 * (row * video_mode.resolutionX + col); + // copy it to this pixel + frame_data[i] = _data[target]; + frame_data[i+1] = _data[target+1]; + frame_data[i+2] = _data[target+2]; + } + } + else + std::copy(_data, _data+pFrame->frame.dataSize, frame_data); + return; } - else - std::copy(_data, _data+pFrame->frame.dataSize, frame_data); } // for StreamBase diff --git a/Source/Drivers/Freenect/FreenectColorStream.h b/Source/Drivers/Freenect/FreenectColorStream.h index b0956c07..5911dbf1 100644 --- a/Source/Drivers/Freenect/FreenectColorStream.h +++ b/Source/Drivers/Freenect/FreenectColorStream.h @@ -34,7 +34,7 @@ class FreenectColorStream : public FreenectVideoStream } public: - FreenectColorStream(Freenect::FreenectDevice* pDevice) : FreenectVideoStream(pDevice) { setVideoMode(default_video_mode); } + FreenectColorStream(Freenect::FreenectDevice* pDevice) : FreenectVideoStream(pDevice) { mirroring = false; setVideoMode(default_video_mode); } ~FreenectColorStream() { } static OniSensorInfo getSensorInfo() diff --git a/Source/Drivers/Freenect/FreenectDepthStream.cpp b/Source/Drivers/Freenect/FreenectDepthStream.cpp index 2c7e6d1e..4e463b4e 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.cpp +++ b/Source/Drivers/Freenect/FreenectDepthStream.cpp @@ -16,18 +16,34 @@ FreenectDepthStream::FreenectDepthModeMap FreenectDepthStream::getSupportedVideo void FreenectDepthStream::populateFrame(void* data, OniDriverFrame* pFrame) const { pFrame->frame.sensorType = sensor_type; - pFrame->frame.stride = video_mode.resolutionX*sizeof(OniDepthPixel); + pFrame->frame.stride = video_mode.resolutionX*sizeof(uint16_t); pFrame->frame.cropOriginX = pFrame->frame.cropOriginY = 0; pFrame->frame.croppingEnabled = FALSE; + pFrame->frame.dataSize = device->getDepthBufferSize(); + pFrame->frame.data = xnOSMallocAligned(sizeof(uint16_t)*pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); + if (pFrame->frame.data == NULL) + { + XN_ASSERT(FALSE); + return; + } + // copy stream buffer from freenect - uint8_t* _data = static_cast(data); - uint8_t* frame_data = static_cast(pFrame->frame.data); - //if (mirroring) - // std::reverse_copy(_data, _data+pFrame->frame.dataSize, frame_data); - //else + uint16_t* _data = static_cast(data); + uint16_t* frame_data = static_cast(pFrame->frame.data); + if (mirroring) + { + for (unsigned int i = 0; i < pFrame->frame.dataSize; i++) + { + // find corresponding mirrored pixel + unsigned int row = i / video_mode.resolutionX; + unsigned int col = video_mode.resolutionX - (i % video_mode.resolutionX); + unsigned int target = (row * video_mode.resolutionX + col); + // copy it to this pixel + frame_data[i] = _data[target]; + } + } + else std::copy(_data, _data+pFrame->frame.dataSize, frame_data); - - //printf("size of frame data = %d\n", pFrame->frame.dataSize); } // for StreamBase diff --git a/Source/Drivers/Freenect/FreenectDepthStream.h b/Source/Drivers/Freenect/FreenectDepthStream.h index ebae5aa8..8a4ad503 100644 --- a/Source/Drivers/Freenect/FreenectDepthStream.h +++ b/Source/Drivers/Freenect/FreenectDepthStream.h @@ -37,7 +37,7 @@ class FreenectDepthStream : public FreenectVideoStream } public: - FreenectDepthStream(Freenect::FreenectDevice* pDevice) : FreenectVideoStream(pDevice) { setVideoMode(default_video_mode); } + FreenectDepthStream(Freenect::FreenectDevice* pDevice) : FreenectVideoStream(pDevice) { mirroring = false; setVideoMode(default_video_mode); } ~FreenectDepthStream() { } static OniSensorInfo getSensorInfo() diff --git a/Source/Drivers/Freenect/FreenectVideoStream.h b/Source/Drivers/Freenect/FreenectVideoStream.h index 82303067..b040b394 100644 --- a/Source/Drivers/Freenect/FreenectVideoStream.h +++ b/Source/Drivers/Freenect/FreenectVideoStream.h @@ -35,15 +35,8 @@ class FreenectVideoStream : public FreenectStream virtual void buildFrame(void* data, OniDriverFrame* pFrame) const { pFrame->frame.videoMode = video_mode; - pFrame->frame.dataSize = device->getVideoBufferSize(); pFrame->frame.width = video_mode.resolutionX; pFrame->frame.height = video_mode.resolutionY; - pFrame->frame.data = xnOSMallocAligned(pFrame->frame.dataSize, XN_DEFAULT_MEM_ALIGN); - if (pFrame->frame.data == NULL) - { - XN_ASSERT(FALSE); - return; - } populateFrame(data, pFrame); } diff --git a/Source/Drivers/Freenect/README b/Source/Drivers/Freenect/README index 8014cefc..d9dca720 100644 --- a/Source/Drivers/Freenect/README +++ b/Source/Drivers/Freenect/README @@ -39,7 +39,6 @@ To implement a new mode, simply add it to getSupportedVideoModes() and modify po TODO: -mirrored mode support FreenectIRStream polish and publish Gentoo ebuilds acquire and test with original Kinect (within 2 weeks) - please report issues!