From 1bf2a279df32c2c6ef69c997f924fd01df34e9c4 Mon Sep 17 00:00:00 2001 From: Zou Hanya Date: Sat, 9 May 2015 21:40:06 +0900 Subject: [PATCH 01/12] openni2: Copy OpenNI2-FreenectDriver From libfreenect 89f77f6d2c23876936af65766a4c140898bc3ea8 --- src/openni2/ColorStream.cpp | 84 +++++++++ src/openni2/ColorStream.hpp | 156 ++++++++++++++++ src/openni2/DepthStream.cpp | 146 +++++++++++++++ src/openni2/DepthStream.hpp | 210 +++++++++++++++++++++ src/openni2/DeviceDriver.cpp | 351 +++++++++++++++++++++++++++++++++++ src/openni2/Utility.hpp | 74 ++++++++ src/openni2/VideoStream.hpp | 202 ++++++++++++++++++++ 7 files changed, 1223 insertions(+) create mode 100644 src/openni2/ColorStream.cpp create mode 100644 src/openni2/ColorStream.hpp create mode 100644 src/openni2/DepthStream.cpp create mode 100644 src/openni2/DepthStream.hpp create mode 100644 src/openni2/DeviceDriver.cpp create mode 100644 src/openni2/Utility.hpp create mode 100644 src/openni2/VideoStream.hpp diff --git a/src/openni2/ColorStream.cpp b/src/openni2/ColorStream.cpp new file mode 100644 index 000000000..6ac7444d5 --- /dev/null +++ b/src/openni2/ColorStream.cpp @@ -0,0 +1,84 @@ +#include +#include "ColorStream.hpp" + +using namespace FreenectDriver; + + +ColorStream::ColorStream(Freenect::FreenectDevice* pDevice) : VideoStream(pDevice) +{ + video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30); + setVideoMode(video_mode); + pDevice->startVideo(); +} + +// Add video modes here as you implement them +ColorStream::FreenectVideoModeMap ColorStream::getSupportedVideoModes() +{ + FreenectVideoModeMap modes; + // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30)] = std::pair(FREENECT_VIDEO_RGB, FREENECT_RESOLUTION_MEDIUM); + + + return modes; + + /* working format possiblities + FREENECT_VIDEO_RGB + FREENECT_VIDEO_YUV_RGB + FREENECT_VIDEO_YUV_RAW + */ +} + +OniStatus ColorStream::setVideoMode(OniVideoMode requested_mode) +{ + FreenectVideoModeMap supported_video_modes = getSupportedVideoModes(); + FreenectVideoModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode); + if (matched_mode_iter == supported_video_modes.end()) + return ONI_STATUS_NOT_SUPPORTED; + + 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) + { + LogError("Format " + to_string(format) + " and resolution " + to_string(resolution) + " combination not supported by libfreenect"); + return ONI_STATUS_NOT_SUPPORTED; + } + video_mode = requested_mode; + return ONI_STATUS_OK; +} + +void ColorStream::populateFrame(void* data, OniFrame* frame) const +{ + frame->sensorType = sensor_type; + frame->stride = video_mode.resolutionX * 3; + frame->cropOriginX = 0; + frame->cropOriginY = 0; + frame->croppingEnabled = false; + + // copy stream buffer from freenect + switch (video_mode.pixelFormat) + { + default: + LogError("Pixel format " + to_string(video_mode.pixelFormat) + " not supported by populateFrame()"); + return; + + case ONI_PIXEL_FORMAT_RGB888: + uint8_t* source = static_cast(data); + uint8_t* target = static_cast(frame->data); + std::copy(source, source + frame->dataSize, target); + return; + } +} + +/* 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/src/openni2/ColorStream.hpp b/src/openni2/ColorStream.hpp new file mode 100644 index 000000000..daf38becf --- /dev/null +++ b/src/openni2/ColorStream.hpp @@ -0,0 +1,156 @@ +#pragma once + +#include // for transform() +#include // for M_PI +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +#include "VideoStream.hpp" + + +namespace FreenectDriver +{ + class ColorStream : public VideoStream + { + public: + // from NUI library & converted to radians + static const float DIAGONAL_FOV = 73.9 * (M_PI / 180); + static const float HORIZONTAL_FOV = 62 * (M_PI / 180); + static const float VERTICAL_FOV = 48.6 * (M_PI / 180); + + private: + typedef std::map< OniVideoMode, std::pair > FreenectVideoModeMap; + static const OniSensorType sensor_type = ONI_SENSOR_COLOR; + + static FreenectVideoModeMap getSupportedVideoModes(); + OniStatus setVideoMode(OniVideoMode requested_mode); + void populateFrame(void* data, OniFrame* frame) const; + + bool auto_white_balance; + bool auto_exposure; + + public: + ColorStream(Freenect::FreenectDevice* pDevice); + //~ColorStream() { } + + static OniSensorInfo getSensorInfo() + { + FreenectVideoModeMap supported_modes = getSupportedVideoModes(); + OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; + std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); + OniSensorInfo sensors = { sensor_type, static_cast(supported_modes.size()), modes }; + return sensors; + } + + // from StreamBase + OniBool isPropertySupported(int propertyId) + { + switch(propertyId) + { + default: + return VideoStream::isPropertySupported(propertyId); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: + return true; + } + } + + OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + return VideoStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) + { + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = HORIZONTAL_FOV; + return ONI_STATUS_OK; + } + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) + { + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = VERTICAL_FOV; + return ONI_STATUS_OK; + } + + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + { + if (*pDataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = auto_white_balance; + return ONI_STATUS_OK; + } + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + { + if (*pDataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_EXPOSURE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = auto_exposure; + return ONI_STATUS_OK; + } + } + } + + OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + switch (propertyId) + { + default: + return VideoStream::setProperty(propertyId, data, dataSize); + + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + { + if (dataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE"); + return ONI_STATUS_ERROR; + } + auto_white_balance = *(static_cast(data)); + int ret = device->setFlag(FREENECT_AUTO_WHITE_BALANCE, auto_white_balance); + return (ret == 0) ? ONI_STATUS_OK : ONI_STATUS_ERROR; + } + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + { + if (dataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_EXPOSURE"); + return ONI_STATUS_ERROR; + } + auto_exposure = *(static_cast(data)); + int ret = device->setFlag(FREENECT_AUTO_WHITE_BALANCE, auto_exposure); + return (ret == 0) ? ONI_STATUS_OK : ONI_STATUS_ERROR; + } + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + { + if (dataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); + return ONI_STATUS_ERROR; + } + mirroring = *(static_cast(data)); + int ret = device->setFlag(FREENECT_MIRROR_VIDEO, mirroring); + return (ret == 0) ? ONI_STATUS_OK : ONI_STATUS_ERROR; + } + } + } + }; +} diff --git a/src/openni2/DepthStream.cpp b/src/openni2/DepthStream.cpp new file mode 100644 index 000000000..7159003db --- /dev/null +++ b/src/openni2/DepthStream.cpp @@ -0,0 +1,146 @@ +#include +#include "DepthStream.hpp" + +using namespace FreenectDriver; + + +DepthStream::DepthStream(Freenect::FreenectDevice* pDevice) : VideoStream(pDevice) +{ + video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30); + image_registration_mode = ONI_IMAGE_REGISTRATION_OFF; + setVideoMode(video_mode); + pDevice->startDepth(); +} + +// Add video modes here as you implement them +// Note: if image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR, +// setVideoFormat() will try FREENECT_DEPTH_REGISTERED first then fall back on what is set here. +DepthStream::FreenectDepthModeMap DepthStream::getSupportedVideoModes() +{ + FreenectDepthModeMap modes; + // pixelFormat, resolutionX, resolutionY, fps + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30)] = std::pair(FREENECT_DEPTH_MM, FREENECT_RESOLUTION_MEDIUM); + + + return modes; +} + +OniStatus DepthStream::setVideoMode(OniVideoMode requested_mode) +{ + 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; + if (image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) // try forcing registration mode + format = FREENECT_DEPTH_REGISTERED; + + try { device->setDepthFormat(format, resolution); } + catch (std::runtime_error e) + { + LogError("Format " + to_string(format) + " and resolution " + to_string(resolution) + " combination not supported by libfreenect"); + if (image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) + { + LogError("Could not enable image registration format; falling back to format defined in getSupportedVideoModes()"); + image_registration_mode = ONI_IMAGE_REGISTRATION_OFF; + return setVideoMode(requested_mode); + } + return ONI_STATUS_NOT_SUPPORTED; + } + video_mode = requested_mode; + return ONI_STATUS_OK; +} + +void DepthStream::populateFrame(void* data, OniFrame* frame) const +{ + frame->sensorType = sensor_type; + frame->stride = video_mode.resolutionX * sizeof(uint16_t); + + if (cropping.enabled) + { + frame->height = cropping.height; + frame->width = cropping.width; + frame->cropOriginX = cropping.originX; + frame->cropOriginY = cropping.originY; + frame->croppingEnabled = true; + } + else + { + frame->cropOriginX = 0; + frame->cropOriginY = 0; + frame->croppingEnabled = false; + } + + + // copy stream buffer from freenect + + uint16_t* source = static_cast(data) + frame->cropOriginX + frame->cropOriginY * video_mode.resolutionX; + uint16_t* target = static_cast(frame->data); + const unsigned int skipWidth = video_mode.resolutionX - frame->width; + + if (mirroring) + { + target += frame->width; + + for (int y = 0; y < frame->height; y++) + { + for (int x = 0; x < frame->width; x++) + { + *target-- = *source++; + } + + source += skipWidth; + target += 2 * frame->width; + } + } + else + { + for (int y = 0; y < frame->height; y++) + { + for (int x = 0; x < frame->width; x++) + { + *target++ = *source++; + } + + source += skipWidth; + } + } + + /* + uint16_t* data_ptr = static_cast(data); + uint16_t* frame_data = static_cast(frame->data); + if (mirroring) + { + for (unsigned int i = 0; i < frame->dataSize / 2; 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_ptr[target]; + } + } + else + std::copy(data_ptr, data_ptr+frame->dataSize / 2, frame_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/src/openni2/DepthStream.hpp b/src/openni2/DepthStream.hpp new file mode 100644 index 000000000..303449ecd --- /dev/null +++ b/src/openni2/DepthStream.hpp @@ -0,0 +1,210 @@ +#pragma once + +#include // for transform() +#include // for M_PI +#include // for memcpy +#include "libfreenect.hpp" +#include "Driver/OniDriverAPI.h" +#include "PS1080.h" +#include "VideoStream.hpp" +#include "S2D.h" +#include "D2S.h" + + +namespace FreenectDriver +{ + class DepthStream : public VideoStream + { + public: + // from NUI library and converted to radians + static const float DIAGONAL_FOV = 70 * (M_PI / 180); + static const float HORIZONTAL_FOV = 58.5 * (M_PI / 180); + static const float VERTICAL_FOV = 45.6 * (M_PI / 180); + // from DepthKinectStream.cpp + static const int MAX_VALUE = 10000; + static const unsigned long long GAIN_VAL = 42; + static const unsigned long long CONST_SHIFT_VAL = 200; + static const unsigned long long MAX_SHIFT_VAL = 2047; + static const unsigned long long PARAM_COEFF_VAL = 4; + static const unsigned long long SHIFT_SCALE_VAL = 10; + static const unsigned long long ZERO_PLANE_DISTANCE_VAL = 120; + static const double ZERO_PLANE_PIXEL_SIZE_VAL = 0.10520000010728836; + static const double EMITTER_DCMOS_DISTANCE_VAL = 7.5; + + private: + typedef std::map< OniVideoMode, std::pair > FreenectDepthModeMap; + static const OniSensorType sensor_type = ONI_SENSOR_DEPTH; + OniImageRegistrationMode image_registration_mode; + + static FreenectDepthModeMap getSupportedVideoModes(); + OniStatus setVideoMode(OniVideoMode requested_mode); + void populateFrame(void* data, OniFrame* frame) const; + + public: + DepthStream(Freenect::FreenectDevice* pDevice); + //~DepthStream() { } + + static OniSensorInfo getSensorInfo() + { + FreenectDepthModeMap supported_modes = getSupportedVideoModes(); + OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; + std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); + OniSensorInfo sensors = { sensor_type, static_cast(supported_modes.size()), modes }; + return sensors; + } + + OniImageRegistrationMode getImageRegistrationMode() const { return image_registration_mode; } + OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) + { + if (!isImageRegistrationModeSupported(mode)) + return ONI_STATUS_NOT_SUPPORTED; + image_registration_mode = mode; + return setVideoMode(video_mode); + } + + // from StreamBase + OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF || mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); } + + OniBool isPropertySupported(int propertyId) + { + switch(propertyId) + { + default: + return VideoStream::isPropertySupported(propertyId); + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + case ONI_STREAM_PROPERTY_MAX_VALUE: + case XN_STREAM_PROPERTY_GAIN: + case XN_STREAM_PROPERTY_CONST_SHIFT: + case XN_STREAM_PROPERTY_MAX_SHIFT: + case XN_STREAM_PROPERTY_PARAM_COEFF: + case XN_STREAM_PROPERTY_SHIFT_SCALE: + case XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE: + case XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE: + case XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE: + case XN_STREAM_PROPERTY_S2D_TABLE: + case XN_STREAM_PROPERTY_D2S_TABLE: + return true; + } + } + + OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + return VideoStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = HORIZONTAL_FOV; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = VERTICAL_FOV; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + if (*pDataSize != sizeof(int)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MAX_VALUE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = MAX_VALUE; + return ONI_STATUS_OK; + + case XN_STREAM_PROPERTY_PIXEL_REGISTRATION: // XnPixelRegistration (get only) + case XN_STREAM_PROPERTY_WHITE_BALANCE_ENABLED: // unsigned long long + case XN_STREAM_PROPERTY_HOLE_FILTER: // unsigned long long + case XN_STREAM_PROPERTY_REGISTRATION_TYPE: // XnProcessingType + case XN_STREAM_PROPERTY_AGC_BIN: // XnDepthAGCBin* + case XN_STREAM_PROPERTY_PIXEL_SIZE_FACTOR: // unsigned long long + case XN_STREAM_PROPERTY_DCMOS_RCMOS_DISTANCE: // double + case XN_STREAM_PROPERTY_CLOSE_RANGE: // unsigned long long + return ONI_STATUS_NOT_SUPPORTED; + + case XN_STREAM_PROPERTY_GAIN: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_GAIN"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = GAIN_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_CONST_SHIFT: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_CONST_SHIFT"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = CONST_SHIFT_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_MAX_SHIFT: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_MAX_SHIFT"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = MAX_SHIFT_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_PARAM_COEFF: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_PARAM_COEFF"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = PARAM_COEFF_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_SHIFT_SCALE: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_SHIFT_SCALE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = SHIFT_SCALE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = ZERO_PLANE_DISTANCE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE: // double + if (*pDataSize != sizeof(double)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = ZERO_PLANE_PIXEL_SIZE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE: // double + if (*pDataSize != sizeof(double)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = EMITTER_DCMOS_DISTANCE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_S2D_TABLE: // OniDepthPixel[] + *pDataSize = sizeof(S2D); + //std::copy(S2D, S2D+1, static_cast(data)); + memcpy(data, S2D, *pDataSize); + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_D2S_TABLE: // unsigned short[] + *pDataSize = sizeof(D2S); + //std::copy(D2S, D2S+1, static_cast(data)); + memcpy(data, D2S, *pDataSize); + return ONI_STATUS_OK; + } + } + }; +} diff --git a/src/openni2/DeviceDriver.cpp b/src/openni2/DeviceDriver.cpp new file mode 100644 index 000000000..6a52ff58d --- /dev/null +++ b/src/openni2/DeviceDriver.cpp @@ -0,0 +1,351 @@ +/** +* FreenectDriver +* Copyright 2013 Benn Snyder +* +* 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. +*/ +#include +#include +#include "Driver/OniDriverAPI.h" +#include "freenect_internal.h" +#include "libfreenect.hpp" +#include "DepthStream.hpp" +#include "ColorStream.hpp" + + +namespace FreenectDriver +{ + class Device : public oni::driver::DeviceBase, public Freenect::FreenectDevice + { + private: + ColorStream* color; + DepthStream* depth; + + // for Freenect::FreenectDevice + void DepthCallback(void* data, uint32_t timestamp) { + depth->buildFrame(data, timestamp); + } + void VideoCallback(void* data, uint32_t timestamp) { + color->buildFrame(data, timestamp); + } + + public: + Device(freenect_context* fn_ctx, int index) : Freenect::FreenectDevice(fn_ctx, index), + color(NULL), + depth(NULL) { } + ~Device() + { + destroyStream(color); + destroyStream(depth); + } + + // for DeviceBase + + OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return depth->isImageRegistrationModeSupported(mode); } + + OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) + { + *numSensors = 2; + OniSensorInfo * sensors = new OniSensorInfo[*numSensors]; + sensors[0] = depth->getSensorInfo(); + sensors[1] = color->getSensorInfo(); + *pSensors = sensors; + return ONI_STATUS_OK; + } + + oni::driver::StreamBase* createStream(OniSensorType sensorType) + { + switch (sensorType) + { + default: + LogError("Cannot create a stream of type " + to_string(sensorType)); + return NULL; + case ONI_SENSOR_COLOR: + if (! color) + color = new ColorStream(this); + return color; + case ONI_SENSOR_DEPTH: + if (! depth) + depth = new DepthStream(this); + return depth; + // todo: IR + } + } + + void destroyStream(oni::driver::StreamBase* pStream) + { + if (pStream == NULL) + return; + + if (pStream == color) + { + Freenect::FreenectDevice::stopVideo(); + delete color; + color = NULL; + } + if (pStream == depth) + { + Freenect::FreenectDevice::stopDepth(); + delete depth; + depth = NULL; + } + } + + // todo: fill out properties + OniBool isPropertySupported(int propertyId) + { + if (propertyId == ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION) + return true; + return false; + } + + OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + case ONI_DEVICE_PROPERTY_FIRMWARE_VERSION: // string + case ONI_DEVICE_PROPERTY_DRIVER_VERSION: // OniVersion + case ONI_DEVICE_PROPERTY_HARDWARE_VERSION: // int + case ONI_DEVICE_PROPERTY_SERIAL_NUMBER: // string + case ONI_DEVICE_PROPERTY_ERROR_STATE: // ? + // files + case ONI_DEVICE_PROPERTY_PLAYBACK_SPEED: // float + case ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED: // OniBool + // xn + case XN_MODULE_PROPERTY_USB_INTERFACE: // XnSensorUsbInterface + case XN_MODULE_PROPERTY_MIRROR: // bool + case XN_MODULE_PROPERTY_RESET_SENSOR_ON_STARTUP: // unsigned long long + case XN_MODULE_PROPERTY_LEAN_INIT: // unsigned long long + case XN_MODULE_PROPERTY_SERIAL_NUMBER: // unsigned long long + case XN_MODULE_PROPERTY_VERSION: // XnVersions + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode + if (*pDataSize != sizeof(OniImageRegistrationMode)) + { + LogError("Unexpected size for ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = depth->getImageRegistrationMode(); + return ONI_STATUS_OK; + } + } + + OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + switch (propertyId) + { + default: + case ONI_DEVICE_PROPERTY_FIRMWARE_VERSION: // By implementation + case ONI_DEVICE_PROPERTY_DRIVER_VERSION: // OniVersion + case ONI_DEVICE_PROPERTY_HARDWARE_VERSION: // int + case ONI_DEVICE_PROPERTY_SERIAL_NUMBER: // string + case ONI_DEVICE_PROPERTY_ERROR_STATE: // ? + // files + case ONI_DEVICE_PROPERTY_PLAYBACK_SPEED: // float + case ONI_DEVICE_PROPERTY_PLAYBACK_REPEAT_ENABLED: // OniBool + // xn + case XN_MODULE_PROPERTY_USB_INTERFACE: // XnSensorUsbInterface + case XN_MODULE_PROPERTY_MIRROR: // bool + case XN_MODULE_PROPERTY_RESET_SENSOR_ON_STARTUP: // unsigned long long + case XN_MODULE_PROPERTY_LEAN_INIT: // unsigned long long + case XN_MODULE_PROPERTY_SERIAL_NUMBER: // unsigned long long + case XN_MODULE_PROPERTY_VERSION: // XnVersions + // xn commands + case XN_MODULE_PROPERTY_FIRMWARE_PARAM: // XnInnerParam + case XN_MODULE_PROPERTY_RESET: // unsigned long long + case XN_MODULE_PROPERTY_IMAGE_CONTROL: // XnControlProcessingData + case XN_MODULE_PROPERTY_DEPTH_CONTROL: // XnControlProcessingData + case XN_MODULE_PROPERTY_AHB: // XnAHBData + case XN_MODULE_PROPERTY_LED_STATE: // XnLedState + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION: // OniImageRegistrationMode + if (dataSize != sizeof(OniImageRegistrationMode)) + { + LogError("Unexpected size for ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION"); + return ONI_STATUS_ERROR; + } + return depth->setImageRegistrationMode(*(static_cast(data))); + } + } + + OniBool isCommandSupported(int commandId) + { + switch (commandId) + { + default: + case ONI_DEVICE_COMMAND_SEEK: + return false; + } + } + + OniStatus invoke(int commandId, void* data, int dataSize) + { + switch (commandId) + { + default: + case ONI_DEVICE_COMMAND_SEEK: // OniSeek + return ONI_STATUS_NOT_SUPPORTED; + } + } + + + /* todo: for DeviceBase + virtual OniStatus tryManualTrigger() {return ONI_STATUS_OK;} + */ + }; + + + class Driver : public oni::driver::DriverBase, private Freenect::Freenect + { + private: + typedef std::map OniDeviceMap; + OniDeviceMap devices; + + static std::string devid_to_uri(int id) { + return "freenect://" + to_string(id); + } + + static int uri_to_devid(const std::string uri) { + int id; + std::istringstream is(uri); + is.seekg(strlen("freenect://")); + is >> id; + return id; + } + + public: + Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) + { + WriteMessage("Using libfreenect v" + to_string(PROJECT_VER)); + + freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG); + freenect_select_subdevices(m_ctx, FREENECT_DEVICE_CAMERA); // OpenNI2 doesn't use MOTOR or AUDIO + DriverServices = &getServices(); + } + ~Driver() { shutdown(); } + + // for DriverBase + + OniStatus initialize(oni::driver::DeviceConnectedCallback connectedCallback, oni::driver::DeviceDisconnectedCallback disconnectedCallback, oni::driver::DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) + { + DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); + for (int i = 0; i < Freenect::deviceCount(); i++) + { + std::string uri = devid_to_uri(i); + + WriteMessage("Found device " + uri); + + OniDeviceInfo info; + strncpy(info.uri, uri.c_str(), ONI_MAX_STR); + strncpy(info.vendor, "Microsoft", ONI_MAX_STR); + strncpy(info.name, "Kinect", ONI_MAX_STR); + devices[info] = NULL; + deviceConnected(&info); + deviceStateChanged(&info, 0); + + freenect_device* dev; + if (freenect_open_device(m_ctx, &dev, i) == 0) + { + info.usbVendorId = dev->usb_cam.VID; + info.usbProductId = dev->usb_cam.PID; + freenect_close_device(dev); + } + else + { + WriteMessage("Unable to open device to query VID/PID"); + } + } + return ONI_STATUS_OK; + } + + oni::driver::DeviceBase* deviceOpen(const char* uri, const char* mode = NULL) + { + for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++) + { + if (strcmp(iter->first.uri, uri) == 0) // found + { + if (iter->second) // already open + { + return iter->second; + } + else + { + WriteMessage("Opening device " + std::string(uri)); + int id = uri_to_devid(iter->first.uri); + Device* device = &createDevice(id); + iter->second = device; + return device; + } + } + } + + LogError("Could not find device " + std::string(uri)); + return NULL; + } + + void deviceClose(oni::driver::DeviceBase* pDevice) + { + for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++) + { + if (iter->second == pDevice) + { + WriteMessage("Closing device " + std::string(iter->first.uri)); + int id = uri_to_devid(iter->first.uri); + devices.erase(iter); + deleteDevice(id); + return; + } + } + + LogError("Could not close unrecognized device"); + } + + OniStatus tryDevice(const char* uri) + { + oni::driver::DeviceBase* device = deviceOpen(uri); + if (! device) + return ONI_STATUS_ERROR; + deviceClose(device); + return ONI_STATUS_OK; + } + + void shutdown() + { + for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++) + { + WriteMessage("Closing device " + std::string(iter->first.uri)); + int id = uri_to_devid(iter->first.uri); + deleteDevice(id); + } + + devices.clear(); + } + + + /* todo: for DriverBase + virtual void* enableFrameSync(oni::driver::StreamBase** pStreams, int streamCount); + virtual void disableFrameSync(void* frameSyncGroup); + */ + }; +} + + +// macros defined in XnLib (not included) - workaround +#define XN_NEW(type, arg...) new type(arg) +#define XN_DELETE(p) delete(p) +ONI_EXPORT_DRIVER(FreenectDriver::Driver); +#undef XN_NEW +#undef XN_DELETE diff --git a/src/openni2/Utility.hpp b/src/openni2/Utility.hpp new file mode 100644 index 000000000..531be112d --- /dev/null +++ b/src/openni2/Utility.hpp @@ -0,0 +1,74 @@ +// This file contains symbols that may be used by any class or don't really go anywhere else. +#pragma once + +#include +#include "Driver/OniDriverAPI.h" + + +// Oni helpers + +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); +} + +static bool operator<(const OniDeviceInfo& left, const OniDeviceInfo& right) +{ + return (strcmp(left.uri, right.uri) < 0); +} + + +/// Extracts `first` from `pair`, for transforming a map into its keys. +struct ExtractKey +{ + template typename T::first_type + operator()(T pair) const + { + return pair.first; + } +}; + + +// holding out on C++11 +template +static std::string to_string(const T& n) +{ + std::ostringstream oss; + oss << n; + return oss.str(); +} + + +// global logging +namespace FreenectDriver +{ + static void WriteMessage(std::string info) + { + std::cout << "OpenNI2-FreenectDriver: " << info << std::endl; + } + + // DriverServices is set in DeviceDriver.cpp so all files can call errorLoggerAppend() + static oni::driver::DriverServices* DriverServices; + static void LogError(std::string error) + { + // errorLoggerAppend() doesn't seem to go anywhere, so WriteMessage also + WriteMessage("(ERROR) " + error); + + if (DriverServices != NULL) + DriverServices->errorLoggerAppend(std::string("OpenNI2-FreenectDriver: " + error).c_str()); + } +} diff --git a/src/openni2/VideoStream.hpp b/src/openni2/VideoStream.hpp new file mode 100644 index 000000000..0823e7e7f --- /dev/null +++ b/src/openni2/VideoStream.hpp @@ -0,0 +1,202 @@ +#pragma once + +#include "libfreenect.hpp" +#include "PS1080.h" +#include "Utility.hpp" + + +namespace FreenectDriver +{ + class VideoStream : public oni::driver::StreamBase + { + private: + unsigned int frame_id; // number each frame + + virtual OniStatus setVideoMode(OniVideoMode requested_mode) = 0; + virtual void populateFrame(void* data, OniFrame* frame) const = 0; + + protected: + static const OniSensorType sensor_type; + Freenect::FreenectDevice* device; + bool running; // buildFrame() does something iff true + OniVideoMode video_mode; + OniCropping cropping; + bool mirroring; + + public: + VideoStream(Freenect::FreenectDevice* device) : + frame_id(1), + device(device), + mirroring(false) + { + // joy of structs + memset(&cropping, 0, sizeof(cropping)); + memset(&video_mode, 0, sizeof(video_mode)); + } + //~VideoStream() { stop(); } + + void buildFrame(void* data, uint32_t timestamp) + { + if (!running) + return; + + OniFrame* frame = getServices().acquireFrame(); + frame->frameIndex = frame_id++; + frame->timestamp = timestamp; + frame->videoMode = video_mode; + frame->width = video_mode.resolutionX; + frame->height = video_mode.resolutionY; + + populateFrame(data, frame); + raiseNewFrame(frame); + getServices().releaseFrame(frame); + } + + // from StreamBase + + OniStatus start() + { + running = true; + return ONI_STATUS_OK; + } + void stop() { running = false; } + + // only add to property handlers if the property is generic to all children + // otherwise, implement in child and call these in default case + OniBool isPropertySupported(int propertyId) + { + switch(propertyId) + { + case ONI_STREAM_PROPERTY_VIDEO_MODE: + case ONI_STREAM_PROPERTY_CROPPING: + case ONI_STREAM_PROPERTY_MIRRORING: + return true; + default: + return false; + } + } + + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + case ONI_STREAM_PROPERTY_MIN_VALUE: // int + case ONI_STREAM_PROPERTY_STRIDE: // int + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + // xn + case XN_STREAM_PROPERTY_INPUT_FORMAT: // unsigned long long + case XN_STREAM_PROPERTY_CROPPING_MODE: // XnCroppingMode + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (*pDataSize != sizeof(OniVideoMode)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VIDEO_MODE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = video_mode; + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + if (*pDataSize != sizeof(OniCropping)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_CROPPING"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = cropping; + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (*pDataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = mirroring; + return ONI_STATUS_OK; + } + } + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) + { + switch (propertyId) + { + default: + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + case ONI_STREAM_PROPERTY_MIN_VALUE: // int + case ONI_STREAM_PROPERTY_STRIDE: // int + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + // xn + case XN_STREAM_PROPERTY_INPUT_FORMAT: // unsigned long long + case XN_STREAM_PROPERTY_CROPPING_MODE: // XnCroppingMode + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (dataSize != sizeof(OniVideoMode)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VIDEO_MODE"); + return ONI_STATUS_ERROR; + } + if (ONI_STATUS_OK != setVideoMode(*(static_cast(data)))) + return ONI_STATUS_NOT_SUPPORTED; + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + if (dataSize != sizeof(OniCropping)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_CROPPING"); + return ONI_STATUS_ERROR; + } + cropping = *(static_cast(data)); + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (dataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); + return ONI_STATUS_ERROR; + } + mirroring = *(static_cast(data)); + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + } + } + + + /* todo : from StreamBase + virtual OniStatus convertDepthToColorCoordinates(StreamBase* colorStream, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY) { return ONI_STATUS_NOT_SUPPORTED; } + */ + }; +} + + +/* 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, +*/ From b5b1aa039b4d917b0753d618bd7664e972c95b44 Mon Sep 17 00:00:00 2001 From: Zou Hanya Date: Sun, 10 May 2015 12:52:28 +0900 Subject: [PATCH 02/12] openni2: Adapt to libfreenect2 API Test with /opt/OpenNI2/Tools/NiViewer. --- src/openni2/ColorStream.cpp | 40 +++++----------- src/openni2/ColorStream.hpp | 46 +++---------------- src/openni2/DepthStream.cpp | 47 +++---------------- src/openni2/DepthStream.hpp | 32 +++++++------ src/openni2/DeviceDriver.cpp | 88 +++++++++++++++++++++++++++--------- src/openni2/Utility.hpp | 6 +-- src/openni2/VideoStream.hpp | 30 +++--------- 7 files changed, 117 insertions(+), 172 deletions(-) diff --git a/src/openni2/ColorStream.cpp b/src/openni2/ColorStream.cpp index 6ac7444d5..195472a49 100644 --- a/src/openni2/ColorStream.cpp +++ b/src/openni2/ColorStream.cpp @@ -1,14 +1,14 @@ #include #include "ColorStream.hpp" -using namespace FreenectDriver; +using namespace Freenect2Driver; -ColorStream::ColorStream(Freenect::FreenectDevice* pDevice) : VideoStream(pDevice) +ColorStream::ColorStream(libfreenect2::Freenect2Device* pDevice) : VideoStream(pDevice) { - video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30); + video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30); setVideoMode(video_mode); - pDevice->startVideo(); + pDevice->start(); } // Add video modes here as you implement them @@ -16,11 +16,10 @@ ColorStream::FreenectVideoModeMap ColorStream::getSupportedVideoModes() { FreenectVideoModeMap modes; // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution - modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 640, 480, 30)] = std::pair(FREENECT_VIDEO_RGB, FREENECT_RESOLUTION_MEDIUM); - + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30)] = 1; return modes; - + /* working format possiblities FREENECT_VIDEO_RGB FREENECT_VIDEO_YUV_RGB @@ -35,15 +34,6 @@ OniStatus ColorStream::setVideoMode(OniVideoMode 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) - { - LogError("Format " + to_string(format) + " and resolution " + to_string(resolution) + " combination not supported by libfreenect"); - return ONI_STATUS_NOT_SUPPORTED; - } video_mode = requested_mode; return ONI_STATUS_OK; } @@ -66,19 +56,11 @@ void ColorStream::populateFrame(void* data, OniFrame* frame) const case ONI_PIXEL_FORMAT_RGB888: uint8_t* source = static_cast(data); uint8_t* target = static_cast(frame->data); - std::copy(source, source + frame->dataSize, target); + for (uint8_t* p = source; p < source + frame->dataSize; p+=3) { + *target++ = p[2]; + *target++ = p[1]; + *target++ = p[0]; + } return; } } - -/* 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/src/openni2/ColorStream.hpp b/src/openni2/ColorStream.hpp index daf38becf..6a4a5351e 100644 --- a/src/openni2/ColorStream.hpp +++ b/src/openni2/ColorStream.hpp @@ -2,12 +2,13 @@ #include // for transform() #include // for M_PI -#include "libfreenect.hpp" -#include "Driver/OniDriverAPI.h" +#include +#include +#include #include "VideoStream.hpp" -namespace FreenectDriver +namespace Freenect2Driver { class ColorStream : public VideoStream { @@ -18,7 +19,7 @@ namespace FreenectDriver static const float VERTICAL_FOV = 48.6 * (M_PI / 180); private: - typedef std::map< OniVideoMode, std::pair > FreenectVideoModeMap; + typedef std::map< OniVideoMode, int > FreenectVideoModeMap; static const OniSensorType sensor_type = ONI_SENSOR_COLOR; static FreenectVideoModeMap getSupportedVideoModes(); @@ -29,7 +30,7 @@ namespace FreenectDriver bool auto_exposure; public: - ColorStream(Freenect::FreenectDevice* pDevice); + ColorStream(libfreenect2::Freenect2Device* pDevice); //~ColorStream() { } static OniSensorInfo getSensorInfo() @@ -115,41 +116,6 @@ namespace FreenectDriver { default: return VideoStream::setProperty(propertyId, data, dataSize); - - // camera - case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool - { - if (dataSize != sizeof(OniBool)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE"); - return ONI_STATUS_ERROR; - } - auto_white_balance = *(static_cast(data)); - int ret = device->setFlag(FREENECT_AUTO_WHITE_BALANCE, auto_white_balance); - return (ret == 0) ? ONI_STATUS_OK : ONI_STATUS_ERROR; - } - case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool - { - if (dataSize != sizeof(OniBool)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_EXPOSURE"); - return ONI_STATUS_ERROR; - } - auto_exposure = *(static_cast(data)); - int ret = device->setFlag(FREENECT_AUTO_WHITE_BALANCE, auto_exposure); - return (ret == 0) ? ONI_STATUS_OK : ONI_STATUS_ERROR; - } - case ONI_STREAM_PROPERTY_MIRRORING: // OniBool - { - if (dataSize != sizeof(OniBool)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); - return ONI_STATUS_ERROR; - } - mirroring = *(static_cast(data)); - int ret = device->setFlag(FREENECT_MIRROR_VIDEO, mirroring); - return (ret == 0) ? ONI_STATUS_OK : ONI_STATUS_ERROR; - } } } }; diff --git a/src/openni2/DepthStream.cpp b/src/openni2/DepthStream.cpp index 7159003db..5eec2f90e 100644 --- a/src/openni2/DepthStream.cpp +++ b/src/openni2/DepthStream.cpp @@ -1,15 +1,15 @@ #include #include "DepthStream.hpp" -using namespace FreenectDriver; +using namespace Freenect2Driver; -DepthStream::DepthStream(Freenect::FreenectDevice* pDevice) : VideoStream(pDevice) +DepthStream::DepthStream(libfreenect2::Freenect2Device* pDevice) : VideoStream(pDevice) { - video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30); + video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 512, 424, 30); image_registration_mode = ONI_IMAGE_REGISTRATION_OFF; setVideoMode(video_mode); - pDevice->startDepth(); + pDevice->start(); } // Add video modes here as you implement them @@ -19,8 +19,7 @@ DepthStream::FreenectDepthModeMap DepthStream::getSupportedVideoModes() { FreenectDepthModeMap modes; // pixelFormat, resolutionX, resolutionY, fps - modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30)] = std::pair(FREENECT_DEPTH_MM, FREENECT_RESOLUTION_MEDIUM); - + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 512, 424, 30)] = 1; return modes; } @@ -32,23 +31,6 @@ OniStatus DepthStream::setVideoMode(OniVideoMode 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; - if (image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) // try forcing registration mode - format = FREENECT_DEPTH_REGISTERED; - - try { device->setDepthFormat(format, resolution); } - catch (std::runtime_error e) - { - LogError("Format " + to_string(format) + " and resolution " + to_string(resolution) + " combination not supported by libfreenect"); - if (image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) - { - LogError("Could not enable image registration format; falling back to format defined in getSupportedVideoModes()"); - image_registration_mode = ONI_IMAGE_REGISTRATION_OFF; - return setVideoMode(requested_mode); - } - return ONI_STATUS_NOT_SUPPORTED; - } video_mode = requested_mode; return ONI_STATUS_OK; } @@ -76,7 +58,7 @@ void DepthStream::populateFrame(void* data, OniFrame* frame) const // copy stream buffer from freenect - uint16_t* source = static_cast(data) + frame->cropOriginX + frame->cropOriginY * video_mode.resolutionX; + float* source = static_cast(data) + frame->cropOriginX + frame->cropOriginY * video_mode.resolutionX; uint16_t* target = static_cast(frame->data); const unsigned int skipWidth = video_mode.resolutionX - frame->width; @@ -127,20 +109,3 @@ void DepthStream::populateFrame(void* data, OniFrame* frame) const std::copy(data_ptr, data_ptr+frame->dataSize / 2, frame_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/src/openni2/DepthStream.hpp b/src/openni2/DepthStream.hpp index 303449ecd..c72424f2d 100644 --- a/src/openni2/DepthStream.hpp +++ b/src/openni2/DepthStream.hpp @@ -3,15 +3,13 @@ #include // for transform() #include // for M_PI #include // for memcpy -#include "libfreenect.hpp" -#include "Driver/OniDriverAPI.h" +#include +#include #include "PS1080.h" #include "VideoStream.hpp" -#include "S2D.h" -#include "D2S.h" -namespace FreenectDriver +namespace Freenect2Driver { class DepthStream : public VideoStream { @@ -32,7 +30,7 @@ namespace FreenectDriver static const double EMITTER_DCMOS_DISTANCE_VAL = 7.5; private: - typedef std::map< OniVideoMode, std::pair > FreenectDepthModeMap; + typedef std::map< OniVideoMode, int > FreenectDepthModeMap; static const OniSensorType sensor_type = ONI_SENSOR_DEPTH; OniImageRegistrationMode image_registration_mode; @@ -41,7 +39,7 @@ namespace FreenectDriver void populateFrame(void* data, OniFrame* frame) const; public: - DepthStream(Freenect::FreenectDevice* pDevice); + DepthStream(libfreenect2::Freenect2Device* pDevice); //~DepthStream() { } static OniSensorInfo getSensorInfo() @@ -195,14 +193,22 @@ namespace FreenectDriver *(static_cast(data)) = EMITTER_DCMOS_DISTANCE_VAL; return ONI_STATUS_OK; case XN_STREAM_PROPERTY_S2D_TABLE: // OniDepthPixel[] - *pDataSize = sizeof(S2D); - //std::copy(S2D, S2D+1, static_cast(data)); - memcpy(data, S2D, *pDataSize); + { + uint16_t *s2d = (uint16_t *)data; + *pDataSize = sizeof(*s2d) * 2048; + memset(data, 0, *pDataSize); + for (int i = 1; i <= 1052; i++) + s2d[i] = 342205.0/(1086.671 - i); + } return ONI_STATUS_OK; case XN_STREAM_PROPERTY_D2S_TABLE: // unsigned short[] - *pDataSize = sizeof(D2S); - //std::copy(D2S, D2S+1, static_cast(data)); - memcpy(data, D2S, *pDataSize); + { + uint16_t *d2s = (uint16_t *)data; + *pDataSize = sizeof(*d2s) * 10001; + memset(data, 0, *pDataSize); + for (int i = 315; i <= 10000; i++) + d2s[i] = 1086.671 - 342205.0/(i + 1); + } return ONI_STATUS_OK; } } diff --git a/src/openni2/DeviceDriver.cpp b/src/openni2/DeviceDriver.cpp index 6a52ff58d..23608afc7 100644 --- a/src/openni2/DeviceDriver.cpp +++ b/src/openni2/DeviceDriver.cpp @@ -14,33 +14,45 @@ * See the License for the specific language governing permissions and * limitations under the License. */ +/** +* OpenNI2 Freenect2 Driver +* Copyright 2015 hanyazou@gmail.com +*/ #include #include -#include "Driver/OniDriverAPI.h" -#include "freenect_internal.h" -#include "libfreenect.hpp" +#include +#include +#include #include "DepthStream.hpp" #include "ColorStream.hpp" -namespace FreenectDriver +namespace Freenect2Driver { - class Device : public oni::driver::DeviceBase, public Freenect::FreenectDevice + class Device : public oni::driver::DeviceBase, public libfreenect2::FrameListener { private: + libfreenect2::Freenect2Device *dev; ColorStream* color; DepthStream* depth; // for Freenect::FreenectDevice - void DepthCallback(void* data, uint32_t timestamp) { - depth->buildFrame(data, timestamp); - } - void VideoCallback(void* data, uint32_t timestamp) { - color->buildFrame(data, timestamp); + bool onNewFrame(libfreenect2::Frame::Type type, libfreenect2::Frame *frame) { + if (type == libfreenect2::Frame::Color) { + if (color) + color->buildFrame(frame->data, 0); // timestamp); + } else + if (type == libfreenect2::Frame::Ir) { + } else + if (type == libfreenect2::Frame::Depth) { + if (depth) + depth->buildFrame(frame->data, 0); // timestamp); + } } public: - Device(freenect_context* fn_ctx, int index) : Freenect::FreenectDevice(fn_ctx, index), + Device(int index) : //libfreenect2::Freenect2Device(fn_ctx, index), + dev(NULL), color(NULL), depth(NULL) { } ~Device() @@ -49,6 +61,16 @@ namespace FreenectDriver destroyStream(depth); } + // for Freenect2Device + void setFreenect2Device(libfreenect2::Freenect2Device *dev) { + this->dev = dev; + dev->setColorFrameListener(this); + dev->setIrAndDepthFrameListener(this); + } + void start() { dev->start(); } + void stop() { dev->stop(); } + void close() { dev->close(); } + // for DeviceBase OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return depth->isImageRegistrationModeSupported(mode); } @@ -72,13 +94,15 @@ namespace FreenectDriver return NULL; case ONI_SENSOR_COLOR: if (! color) - color = new ColorStream(this); + color = new ColorStream(dev); return color; case ONI_SENSOR_DEPTH: if (! depth) - depth = new DepthStream(this); + depth = new DepthStream(dev); return depth; // todo: IR + case ONI_SENSOR_IR: + std::cout << "## FreenectDriver::Device::createStream(IR): TODO" << std::endl; } } @@ -87,6 +111,18 @@ namespace FreenectDriver if (pStream == NULL) return; +#if 1 + // destroy them all :-) + dev->stop(); + if (color != NULL) { + delete color; + color = NULL; + } + if (depth != NULL) { + delete depth; + depth = NULL; + } +#else // 0 if (pStream == color) { Freenect::FreenectDevice::stopVideo(); @@ -99,6 +135,7 @@ namespace FreenectDriver delete depth; depth = NULL; } +#endif // 0 } // todo: fill out properties @@ -208,11 +245,12 @@ namespace FreenectDriver }; - class Driver : public oni::driver::DriverBase, private Freenect::Freenect + class Driver : public oni::driver::DriverBase { private: typedef std::map OniDeviceMap; OniDeviceMap devices; + libfreenect2::Freenect2 freenect2; static std::string devid_to_uri(int id) { return "freenect://" + to_string(id); @@ -229,10 +267,9 @@ namespace FreenectDriver public: Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) { - WriteMessage("Using libfreenect v" + to_string(PROJECT_VER)); + //WriteMessage("Using libfreenect v" + to_string(PROJECT_VER)); + WriteMessage("Using libfreenect2"); - freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG); - freenect_select_subdevices(m_ctx, FREENECT_DEVICE_CAMERA); // OpenNI2 doesn't use MOTOR or AUDIO DriverServices = &getServices(); } ~Driver() { shutdown(); } @@ -242,7 +279,7 @@ namespace FreenectDriver OniStatus initialize(oni::driver::DeviceConnectedCallback connectedCallback, oni::driver::DeviceDisconnectedCallback disconnectedCallback, oni::driver::DeviceStateChangedCallback deviceStateChangedCallback, void* pCookie) { DriverBase::initialize(connectedCallback, disconnectedCallback, deviceStateChangedCallback, pCookie); - for (int i = 0; i < Freenect::deviceCount(); i++) + for (int i = 0; i < freenect2.enumerateDevices(); i++) { std::string uri = devid_to_uri(i); @@ -256,6 +293,7 @@ namespace FreenectDriver deviceConnected(&info); deviceStateChanged(&info, 0); +#if 0 freenect_device* dev; if (freenect_open_device(m_ctx, &dev, i) == 0) { @@ -267,6 +305,7 @@ namespace FreenectDriver { WriteMessage("Unable to open device to query VID/PID"); } +#endif // 0 } return ONI_STATUS_OK; } @@ -285,7 +324,8 @@ namespace FreenectDriver { WriteMessage("Opening device " + std::string(uri)); int id = uri_to_devid(iter->first.uri); - Device* device = &createDevice(id); + Device* device = new Device(id); + device->setFreenect2Device(freenect2.openDevice(id)); // XXX, detault pipeline // const PacketPipeline *factory); iter->second = device; return device; } @@ -305,7 +345,9 @@ namespace FreenectDriver WriteMessage("Closing device " + std::string(iter->first.uri)); int id = uri_to_devid(iter->first.uri); devices.erase(iter); - deleteDevice(id); + Device* device = (Device*)iter->second; + device->stop(); + device->close(); return; } } @@ -328,7 +370,9 @@ namespace FreenectDriver { WriteMessage("Closing device " + std::string(iter->first.uri)); int id = uri_to_devid(iter->first.uri); - deleteDevice(id); + Device* device = (Device*)iter->second; + device->stop(); + device->close(); } devices.clear(); @@ -346,6 +390,6 @@ namespace FreenectDriver // macros defined in XnLib (not included) - workaround #define XN_NEW(type, arg...) new type(arg) #define XN_DELETE(p) delete(p) -ONI_EXPORT_DRIVER(FreenectDriver::Driver); +ONI_EXPORT_DRIVER(Freenect2Driver::Driver); #undef XN_NEW #undef XN_DELETE diff --git a/src/openni2/Utility.hpp b/src/openni2/Utility.hpp index 531be112d..f9d5e39ac 100644 --- a/src/openni2/Utility.hpp +++ b/src/openni2/Utility.hpp @@ -54,11 +54,11 @@ static std::string to_string(const T& n) // global logging -namespace FreenectDriver +namespace Freenect2Driver { static void WriteMessage(std::string info) { - std::cout << "OpenNI2-FreenectDriver: " << info << std::endl; + std::cout << "OpenNI2-Freenect2Driver: " << info << std::endl; } // DriverServices is set in DeviceDriver.cpp so all files can call errorLoggerAppend() @@ -69,6 +69,6 @@ namespace FreenectDriver WriteMessage("(ERROR) " + error); if (DriverServices != NULL) - DriverServices->errorLoggerAppend(std::string("OpenNI2-FreenectDriver: " + error).c_str()); + DriverServices->errorLoggerAppend(std::string("OpenNI2-Freenect2Driver: " + error).c_str()); } } diff --git a/src/openni2/VideoStream.hpp b/src/openni2/VideoStream.hpp index 0823e7e7f..d2950fa28 100644 --- a/src/openni2/VideoStream.hpp +++ b/src/openni2/VideoStream.hpp @@ -1,11 +1,12 @@ #pragma once -#include "libfreenect.hpp" +#include +#include +#include #include "PS1080.h" #include "Utility.hpp" - -namespace FreenectDriver +namespace Freenect2Driver { class VideoStream : public oni::driver::StreamBase { @@ -17,14 +18,14 @@ namespace FreenectDriver protected: static const OniSensorType sensor_type; - Freenect::FreenectDevice* device; + libfreenect2::Freenect2Device* device; bool running; // buildFrame() does something iff true OniVideoMode video_mode; OniCropping cropping; bool mirroring; public: - VideoStream(Freenect::FreenectDevice* device) : + VideoStream(libfreenect2::Freenect2Device* device) : frame_id(1), device(device), mirroring(false) @@ -181,22 +182,3 @@ namespace FreenectDriver */ }; } - - -/* 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, -*/ From d2dadbdc54c865800b3c47228fbd7154236a1a59 Mon Sep 17 00:00:00 2001 From: Lingzhu Xiang Date: Sun, 3 Jan 2016 13:55:07 -0500 Subject: [PATCH 03/12] openni2: Add proper build system make install to copy libfreenect2-openni2* to lib/OpenNI2/Drivers. make install-openni2 to cmake -E copy_directory OpenNI2/Drivers --- CMakeLists.txt | 21 ++++++++++++++ cmake_modules/FindOpenNI2.cmake | 51 +++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 cmake_modules/FindOpenNI2.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index d44a65f0c..5d10ec1aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ SET(DEPENDS_DIR "${MY_DIR}/depends" CACHE STRING "dependency directory must be s OPTION(BUILD_SHARED_LIBS "Build shared (ON) or static (OFF) libraries" ON) OPTION(BUILD_EXAMPLES "Build examples" ON) +OPTION(BUILD_OPENNI2_DRIVER "Build OpenNI2 driver" ON) OPTION(ENABLE_CXX11 "Enable C++11 support" OFF) OPTION(ENABLE_OPENCL "Enable OpenCL support" ON) OPTION(ENABLE_OPENGL "Enable OpenGL support" ON) @@ -272,3 +273,23 @@ IF(BUILD_EXAMPLES) MESSAGE(STATUS "Configurating examples") ADD_SUBDIRECTORY(${MY_DIR}/examples) ENDIF() + +IF(BUILD_OPENNI2_DRIVER) + FIND_PACKAGE(OpenNI2) + IF(OpenNI2_FOUND) + FILE(GLOB OPENNI2_DRIVER_SOURCES src/openni2/*.cpp) + ADD_LIBRARY(freenect2-openni2 ${OPENNI2_DRIVER_SOURCES} ${LIBFREENECT2_THREADING_SOURCE}) + TARGET_INCLUDE_DIRECTORIES(freenect2-openni2 PRIVATE ${OpenNI2_INCLUDE_DIRS}) + TARGET_LINK_LIBRARIES(freenect2-openni2 freenect2 ${LIBFREENECT2_THREADING_LIBRARIES}) + SET_TARGET_PROPERTIES(freenect2-openni2 PROPERTIES SOVERSION 0) + IF(NOT ${CMAKE_INSTALL_PREFIX} MATCHES "^/usr") + SET_TARGET_PROPERTIES(freenect2-openni2 PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") + ENDIF() + INSTALL(TARGETS freenect2-openni2 DESTINATION lib/OpenNI2/Drivers RUNTIME DESTINATION bin) + ADD_CUSTOM_TARGET(install-openni2 + DEPENDS freenect2-openni2 + COMMAND ${CMAKE_COMMAND} -E copy_directory "${CMAKE_INSTALL_PREFIX}/lib/OpenNI2/Drivers/" "${OpenNI2_LIBRARY_DIR}/OpenNI2/Drivers/" + VERBATIM + ) + ENDIF() +ENDIF() diff --git a/cmake_modules/FindOpenNI2.cmake b/cmake_modules/FindOpenNI2.cmake new file mode 100644 index 000000000..74a68e73d --- /dev/null +++ b/cmake_modules/FindOpenNI2.cmake @@ -0,0 +1,51 @@ +# - Find OpenNI2 +# +# If the OPENNI2_INCLUDE and OPENNI2_REDIST environment variables +# are defined, they will be used as search path. +# The following standard variables get defined: +# OpenNI2_FOUND: true if found +# OpenNI2_INCLUDE_DIRS: the directory that contains the include file +# OpenNI2_LIBRARY_DIR: the directory that contains the library + +IF(PKG_CONFIG_FOUND) + PKG_CHECK_MODULES(OpenNI2 libopenni2) +ENDIF() + +FIND_PATH(OpenNI2_INCLUDE_DIRS + NAMES Driver/OniDriverAPI.h + PATHS + "/opt/include" + "/opt/local/include" + "/usr/include" + "/usr/local/include" + ENV OPENNI2_INCLUDE + ENV PROGRAMFILES + ENV ProgramW6432 + HINTS ${OpenNI2_INCLUDE_DIRS} + PATH_SUFFIXES + ni2 + openni2 + OpenNI2/Include +) + +FIND_LIBRARY(OpenNI2_LIBRARY + NAMES OpenNI2 ${OpenNI2_LIBRARIES} + PATHS + "/opt/lib" + "/opt/local/lib" + "/usr/lib" + "/usr/local/lib" + ENV OPENNI2_REDIST + ENV PROGRAMFILES + ENV ProgramW6432 + HINTS ${OpenNI2_LIBRARY_DIRS} + PATH_SUFFIXES + ni2/OpenNI2/Drivers + OpenNI2/Drivers/lib + OpenNI2/Lib +) + +GET_FILENAME_COMPONENT(OpenNI2_LIBRARY_DIR ${OpenNI2_LIBRARY} DIRECTORY) + +INCLUDE(FindPackageHandleStandardArgs) +FIND_PACKAGE_HANDLE_STANDARD_ARGS(OpenNI2 DEFAULT_MSG OpenNI2_LIBRARY_DIR OpenNI2_INCLUDE_DIRS) From 8dc55c5c70908fcefbb553d46b6d946ed075baac Mon Sep 17 00:00:00 2001 From: Zou Hanya Date: Sat, 16 May 2015 07:57:04 +0900 Subject: [PATCH 04/12] openni2: Add IrStream class --- src/openni2/DeviceDriver.cpp | 34 ++++++------- src/openni2/IrStream.cpp | 92 ++++++++++++++++++++++++++++++++++++ src/openni2/IrStream.hpp | 92 ++++++++++++++++++++++++++++++++++++ 3 files changed, 201 insertions(+), 17 deletions(-) create mode 100644 src/openni2/IrStream.cpp create mode 100644 src/openni2/IrStream.hpp diff --git a/src/openni2/DeviceDriver.cpp b/src/openni2/DeviceDriver.cpp index 23608afc7..9ed6e1143 100644 --- a/src/openni2/DeviceDriver.cpp +++ b/src/openni2/DeviceDriver.cpp @@ -25,6 +25,7 @@ #include #include "DepthStream.hpp" #include "ColorStream.hpp" +#include "IrStream.hpp" namespace Freenect2Driver @@ -35,6 +36,7 @@ namespace Freenect2Driver libfreenect2::Freenect2Device *dev; ColorStream* color; DepthStream* depth; + IrStream* ir; // for Freenect::FreenectDevice bool onNewFrame(libfreenect2::Frame::Type type, libfreenect2::Frame *frame) { @@ -43,6 +45,8 @@ namespace Freenect2Driver color->buildFrame(frame->data, 0); // timestamp); } else if (type == libfreenect2::Frame::Ir) { + if (ir) + ir->buildFrame(frame->data, 0); // timestamp); } else if (type == libfreenect2::Frame::Depth) { if (depth) @@ -54,10 +58,12 @@ namespace Freenect2Driver Device(int index) : //libfreenect2::Freenect2Device(fn_ctx, index), dev(NULL), color(NULL), + ir(NULL), depth(NULL) { } ~Device() { destroyStream(color); + destroyStream(ir); destroyStream(depth); } @@ -77,10 +83,11 @@ namespace Freenect2Driver OniStatus getSensorInfoList(OniSensorInfo** pSensors, int* numSensors) { - *numSensors = 2; + *numSensors = 3; OniSensorInfo * sensors = new OniSensorInfo[*numSensors]; sensors[0] = depth->getSensorInfo(); sensors[1] = color->getSensorInfo(); + sensors[2] = ir->getSensorInfo(); *pSensors = sensors; return ONI_STATUS_OK; } @@ -100,9 +107,10 @@ namespace Freenect2Driver if (! depth) depth = new DepthStream(dev); return depth; - // todo: IR case ONI_SENSOR_IR: - std::cout << "## FreenectDriver::Device::createStream(IR): TODO" << std::endl; + if (! ir) + ir = new IrStream(dev); + return ir; } } @@ -111,31 +119,23 @@ namespace Freenect2Driver if (pStream == NULL) return; -#if 1 - // destroy them all :-) + // stop them all dev->stop(); - if (color != NULL) { - delete color; - color = NULL; - } - if (depth != NULL) { - delete depth; - depth = NULL; - } -#else // 0 if (pStream == color) { - Freenect::FreenectDevice::stopVideo(); delete color; color = NULL; } if (pStream == depth) { - Freenect::FreenectDevice::stopDepth(); delete depth; depth = NULL; } -#endif // 0 + if (pStream == ir) + { + delete ir; + ir = NULL; + } } // todo: fill out properties diff --git a/src/openni2/IrStream.cpp b/src/openni2/IrStream.cpp new file mode 100644 index 000000000..06358feb6 --- /dev/null +++ b/src/openni2/IrStream.cpp @@ -0,0 +1,92 @@ +#include +#include "IrStream.hpp" + +using namespace Freenect2Driver; + + +IrStream::IrStream(libfreenect2::Freenect2Device* pDevice) : VideoStream(pDevice) +{ + video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_GRAY16, 512, 424, 30); + image_registration_mode = ONI_IMAGE_REGISTRATION_OFF; + setVideoMode(video_mode); + pDevice->start(); +} + +// Add video modes here as you implement them +// Note: if image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR, +// setVideoFormat() will try FREENECT_DEPTH_REGISTERED first then fall back on what is set here. +IrStream::FreenectIrModeMap IrStream::getSupportedVideoModes() +{ + FreenectIrModeMap modes; + // pixelFormat, resolutionX, resolutionY, fps + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_GRAY16, 512, 424, 30)] = 0; + + return modes; +} + +OniStatus IrStream::setVideoMode(OniVideoMode requested_mode) +{ + FreenectIrModeMap supported_video_modes = getSupportedVideoModes(); + FreenectIrModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode); + if (matched_mode_iter == supported_video_modes.end()) + return ONI_STATUS_NOT_SUPPORTED; + + video_mode = requested_mode; + return ONI_STATUS_OK; +} + +void IrStream::populateFrame(void* data, OniFrame* frame) const +{ + frame->sensorType = sensor_type; + frame->stride = video_mode.resolutionX * sizeof(uint16_t); + + if (cropping.enabled) + { + frame->height = cropping.height; + frame->width = cropping.width; + frame->cropOriginX = cropping.originX; + frame->cropOriginY = cropping.originY; + frame->croppingEnabled = true; + } + else + { + frame->cropOriginX = 0; + frame->cropOriginY = 0; + frame->croppingEnabled = false; + } + + + // copy stream buffer from freenect + + float* source = static_cast(data) + frame->cropOriginX + frame->cropOriginY * video_mode.resolutionX; + uint16_t* target = static_cast(frame->data); + const unsigned int skipWidth = video_mode.resolutionX - frame->width; + + if (mirroring) + { + target += frame->width; + + for (int y = 0; y < frame->height; y++) + { + for (int x = 0; x < frame->width; x++) + { + *target-- = *source++; + } + + source += skipWidth; + target += 2 * frame->width; + } + } + else + { + for (int y = 0; y < frame->height; y++) + { + for (int x = 0; x < frame->width; x++) + { + *target++ = *source++; + } + + source += skipWidth; + } + } +} diff --git a/src/openni2/IrStream.hpp b/src/openni2/IrStream.hpp new file mode 100644 index 000000000..c0b16432c --- /dev/null +++ b/src/openni2/IrStream.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include // for transform() +#include // for M_PI +#include // for memcpy +#include +#include +#include "PS1080.h" +#include "VideoStream.hpp" + +namespace Freenect2Driver +{ + class IrStream : public VideoStream + { + public: + // from NUI library and converted to radians + static const float HORIZONTAL_FOV = 58.5 * (M_PI / 180); + static const float VERTICAL_FOV = 45.6 * (M_PI / 180); + + private: + typedef std::map< OniVideoMode, int > FreenectIrModeMap; + static const OniSensorType sensor_type = ONI_SENSOR_IR; + OniImageRegistrationMode image_registration_mode; + + static FreenectIrModeMap getSupportedVideoModes(); + OniStatus setVideoMode(OniVideoMode requested_mode); + void populateFrame(void* data, OniFrame* frame) const; + + public: + IrStream(libfreenect2::Freenect2Device* pDevice); + //~IrStream() { } + + static OniSensorInfo getSensorInfo() + { + FreenectIrModeMap supported_modes = getSupportedVideoModes(); + OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; + std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); + OniSensorInfo sensors = { sensor_type, static_cast(supported_modes.size()), modes }; + return sensors; + } + + OniImageRegistrationMode getImageRegistrationMode() const { return image_registration_mode; } + OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) + { + if (!isImageRegistrationModeSupported(mode)) + return ONI_STATUS_NOT_SUPPORTED; + image_registration_mode = mode; + return setVideoMode(video_mode); + } + + // from StreamBase + OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF || mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); } + + OniBool isPropertySupported(int propertyId) + { + switch(propertyId) + { + default: + return VideoStream::isPropertySupported(propertyId); + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + return true; + } + } + + OniStatus getProperty(int propertyId, void* data, int* pDataSize) + { + switch (propertyId) + { + default: + return VideoStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = HORIZONTAL_FOV; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = VERTICAL_FOV; + return ONI_STATUS_OK; + } + } + }; +} From fe7557582e0f19accbd275774bad05eb0a78ed2d Mon Sep 17 00:00:00 2001 From: Zou Hanya Date: Sun, 31 May 2015 22:12:21 +0900 Subject: [PATCH 05/12] openni2: Add registration @HenningJ has the following contribution to this commit: Change copying of color images to reflect the change from BGR to BGRX color format. --- src/openni2/ColorStream.cpp | 67 ++++++++--- src/openni2/ColorStream.hpp | 22 +++- src/openni2/DepthStream.cpp | 101 +++++------------ src/openni2/DepthStream.hpp | 15 +-- src/openni2/DeviceDriver.cpp | 211 ++++++++++++++++++++++++++++------- src/openni2/IrStream.cpp | 65 ++--------- src/openni2/IrStream.hpp | 20 +--- src/openni2/Registration.cpp | 43 +++++++ src/openni2/Registration.hpp | 20 ++++ src/openni2/VideoStream.hpp | 82 +++++++++++--- 10 files changed, 423 insertions(+), 223 deletions(-) create mode 100644 src/openni2/Registration.cpp create mode 100644 src/openni2/Registration.hpp diff --git a/src/openni2/ColorStream.cpp b/src/openni2/ColorStream.cpp index 195472a49..72c830c18 100644 --- a/src/openni2/ColorStream.cpp +++ b/src/openni2/ColorStream.cpp @@ -3,8 +3,12 @@ using namespace Freenect2Driver; +// from NUI library & converted to radians +const float ColorStream::DIAGONAL_FOV = 73.9 * (M_PI / 180); +const float ColorStream::HORIZONTAL_FOV = 62 * (M_PI / 180); +const float ColorStream::VERTICAL_FOV = 48.6 * (M_PI / 180); -ColorStream::ColorStream(libfreenect2::Freenect2Device* pDevice) : VideoStream(pDevice) +ColorStream::ColorStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg) : VideoStream(pDevice, reg) { video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30); setVideoMode(video_mode); @@ -16,6 +20,7 @@ ColorStream::FreenectVideoModeMap ColorStream::getSupportedVideoModes() { FreenectVideoModeMap modes; // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30)] = 0; modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30)] = 1; return modes; @@ -38,13 +43,10 @@ OniStatus ColorStream::setVideoMode(OniVideoMode requested_mode) return ONI_STATUS_OK; } -void ColorStream::populateFrame(void* data, OniFrame* frame) const +void ColorStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const { - frame->sensorType = sensor_type; - frame->stride = video_mode.resolutionX * 3; - frame->cropOriginX = 0; - frame->cropOriginY = 0; - frame->croppingEnabled = false; + dstFrame->sensorType = sensor_type; + dstFrame->stride = dstFrame->width * 3; // copy stream buffer from freenect switch (video_mode.pixelFormat) @@ -54,13 +56,52 @@ void ColorStream::populateFrame(void* data, OniFrame* frame) const return; case ONI_PIXEL_FORMAT_RGB888: - uint8_t* source = static_cast(data); - uint8_t* target = static_cast(frame->data); - for (uint8_t* p = source; p < source + frame->dataSize; p+=3) { - *target++ = p[2]; - *target++ = p[1]; - *target++ = p[0]; + if (reg->isEnabled()) { + libfreenect2::Frame registered(512, 424, 4); + + reg->colorFrameRGB888(srcFrame, ®istered); + + copyFrame(static_cast(registered.data), srcX, srcY, registered.width * registered.bytes_per_pixel, + static_cast(dstFrame->data), dstX, dstY, dstFrame->stride, + width, height, mirroring); + } else { + copyFrame(static_cast(srcFrame->data), srcX, srcY, srcFrame->width * srcFrame->bytes_per_pixel, + static_cast(dstFrame->data), dstX, dstY, dstFrame->stride, + width, height, mirroring); } return; } } + +void ColorStream::copyFrame(uint8_t* srcPix, int srcX, int srcY, int srcStride, uint8_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring) +{ + srcPix += srcX + srcY * srcStride; + dstPix += dstX + dstY * dstStride; + + for (int y = 0; y < height; y++) { + uint8_t* dst = dstPix + y * dstStride; + uint8_t* src = srcPix + y * srcStride; + if (mirroring) { + dst += dstStride - 1; + for (int x = 0; x < srcStride; ++x) + { + if (x % 4 != 3) + { + *dst-- = *src++; + } + else + { + ++src; + } + } + } else { + for (int x = 0; x < dstStride-2; x += 3) + { + *dst++ = src[2]; + *dst++ = src[1]; + *dst++ = src[0]; + src += 4; + } + } + } +} diff --git a/src/openni2/ColorStream.hpp b/src/openni2/ColorStream.hpp index 6a4a5351e..74a6637b9 100644 --- a/src/openni2/ColorStream.hpp +++ b/src/openni2/ColorStream.hpp @@ -14,9 +14,9 @@ namespace Freenect2Driver { public: // from NUI library & converted to radians - static const float DIAGONAL_FOV = 73.9 * (M_PI / 180); - static const float HORIZONTAL_FOV = 62 * (M_PI / 180); - static const float VERTICAL_FOV = 48.6 * (M_PI / 180); + static const float DIAGONAL_FOV; + static const float HORIZONTAL_FOV; + static const float VERTICAL_FOV; private: typedef std::map< OniVideoMode, int > FreenectVideoModeMap; @@ -24,13 +24,15 @@ namespace Freenect2Driver static FreenectVideoModeMap getSupportedVideoModes(); OniStatus setVideoMode(OniVideoMode requested_mode); - void populateFrame(void* data, OniFrame* frame) const; + void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; + static void copyFrame(uint8_t* srcPix, int srcX, int srcY, int srcStride, uint8_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring); + bool auto_white_balance; bool auto_exposure; public: - ColorStream(libfreenect2::Freenect2Device* pDevice); + ColorStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); //~ColorStream() { } static OniSensorInfo getSensorInfo() @@ -42,6 +44,16 @@ namespace Freenect2Driver return sensors; } + OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) + { + if (mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) { + // XXX, switch color resolution to 512x424 for registrarion here + OniVideoMode video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30); + setProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &video_mode, sizeof(video_mode)); + } + return ONI_STATUS_OK; + } + // from StreamBase OniBool isPropertySupported(int propertyId) { diff --git a/src/openni2/DepthStream.cpp b/src/openni2/DepthStream.cpp index 5eec2f90e..46f7aafc0 100644 --- a/src/openni2/DepthStream.cpp +++ b/src/openni2/DepthStream.cpp @@ -3,10 +3,25 @@ using namespace Freenect2Driver; - -DepthStream::DepthStream(libfreenect2::Freenect2Device* pDevice) : VideoStream(pDevice) +// from NUI library and converted to radians +const float DepthStream::DIAGONAL_FOV = 70 * (M_PI / 180); +const float DepthStream::HORIZONTAL_FOV = 58.5 * (M_PI / 180); +const float DepthStream::VERTICAL_FOV = 45.6 * (M_PI / 180); +// from DepthKinectStream.cpp +const int DepthStream::MAX_VALUE; +const unsigned long long DepthStream::GAIN_VAL; +const unsigned long long DepthStream::CONST_SHIFT_VAL; +const unsigned long long DepthStream::MAX_SHIFT_VAL; +const unsigned long long DepthStream::PARAM_COEFF_VAL; +const unsigned long long DepthStream::SHIFT_SCALE_VAL; +const unsigned long long DepthStream::ZERO_PLANE_DISTANCE_VAL; +const double DepthStream::ZERO_PLANE_PIXEL_SIZE_VAL = 0.10520000010728836; +const double DepthStream::EMITTER_DCMOS_DISTANCE_VAL = 7.5; + +DepthStream::DepthStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg) : VideoStream(pDevice, reg) { - video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 512, 424, 30); + //video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 512, 424, 30); + video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30); image_registration_mode = ONI_IMAGE_REGISTRATION_OFF; setVideoMode(video_mode); pDevice->start(); @@ -19,6 +34,7 @@ DepthStream::FreenectDepthModeMap DepthStream::getSupportedVideoModes() { FreenectDepthModeMap modes; // pixelFormat, resolutionX, resolutionY, fps + modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30)] = 0; modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 512, 424, 30)] = 1; return modes; @@ -35,77 +51,20 @@ OniStatus DepthStream::setVideoMode(OniVideoMode requested_mode) return ONI_STATUS_OK; } -void DepthStream::populateFrame(void* data, OniFrame* frame) const +void DepthStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const { - frame->sensorType = sensor_type; - frame->stride = video_mode.resolutionX * sizeof(uint16_t); + dstFrame->sensorType = sensor_type; + dstFrame->stride = dstFrame->width * sizeof(uint16_t); - if (cropping.enabled) - { - frame->height = cropping.height; - frame->width = cropping.width; - frame->cropOriginX = cropping.originX; - frame->cropOriginY = cropping.originY; - frame->croppingEnabled = true; - } - else - { - frame->cropOriginX = 0; - frame->cropOriginY = 0; - frame->croppingEnabled = false; - } + // XXX, save depth map for registration + if (reg->isEnabled()) + reg->depthFrame(srcFrame); + if (srcFrame->width < dstFrame->width || srcFrame->height < dstFrame->height) + memset(dstFrame->data, 0x00, dstFrame->width * dstFrame->height * 2); // copy stream buffer from freenect - - float* source = static_cast(data) + frame->cropOriginX + frame->cropOriginY * video_mode.resolutionX; - uint16_t* target = static_cast(frame->data); - const unsigned int skipWidth = video_mode.resolutionX - frame->width; - - if (mirroring) - { - target += frame->width; - - for (int y = 0; y < frame->height; y++) - { - for (int x = 0; x < frame->width; x++) - { - *target-- = *source++; - } - - source += skipWidth; - target += 2 * frame->width; - } - } - else - { - for (int y = 0; y < frame->height; y++) - { - for (int x = 0; x < frame->width; x++) - { - *target++ = *source++; - } - - source += skipWidth; - } - } - - /* - uint16_t* data_ptr = static_cast(data); - uint16_t* frame_data = static_cast(frame->data); - if (mirroring) - { - for (unsigned int i = 0; i < frame->dataSize / 2; 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_ptr[target]; - } - } - else - std::copy(data_ptr, data_ptr+frame->dataSize / 2, frame_data); - */ + copyFrame(static_cast((void*)srcFrame->data), srcX, srcY, srcFrame->width, + static_cast(dstFrame->data), dstX, dstY, dstFrame->width, + width, height, mirroring); } diff --git a/src/openni2/DepthStream.hpp b/src/openni2/DepthStream.hpp index c72424f2d..cd693927b 100644 --- a/src/openni2/DepthStream.hpp +++ b/src/openni2/DepthStream.hpp @@ -15,9 +15,9 @@ namespace Freenect2Driver { public: // from NUI library and converted to radians - static const float DIAGONAL_FOV = 70 * (M_PI / 180); - static const float HORIZONTAL_FOV = 58.5 * (M_PI / 180); - static const float VERTICAL_FOV = 45.6 * (M_PI / 180); + static const float DIAGONAL_FOV; + static const float HORIZONTAL_FOV; + static const float VERTICAL_FOV; // from DepthKinectStream.cpp static const int MAX_VALUE = 10000; static const unsigned long long GAIN_VAL = 42; @@ -26,8 +26,8 @@ namespace Freenect2Driver static const unsigned long long PARAM_COEFF_VAL = 4; static const unsigned long long SHIFT_SCALE_VAL = 10; static const unsigned long long ZERO_PLANE_DISTANCE_VAL = 120; - static const double ZERO_PLANE_PIXEL_SIZE_VAL = 0.10520000010728836; - static const double EMITTER_DCMOS_DISTANCE_VAL = 7.5; + static const double ZERO_PLANE_PIXEL_SIZE_VAL; + static const double EMITTER_DCMOS_DISTANCE_VAL; private: typedef std::map< OniVideoMode, int > FreenectDepthModeMap; @@ -36,10 +36,10 @@ namespace Freenect2Driver static FreenectDepthModeMap getSupportedVideoModes(); OniStatus setVideoMode(OniVideoMode requested_mode); - void populateFrame(void* data, OniFrame* frame) const; + void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; public: - DepthStream(libfreenect2::Freenect2Device* pDevice); + DepthStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); //~DepthStream() { } static OniSensorInfo getSensorInfo() @@ -57,6 +57,7 @@ namespace Freenect2Driver if (!isImageRegistrationModeSupported(mode)) return ONI_STATUS_NOT_SUPPORTED; image_registration_mode = mode; + reg->setEnable(image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); return setVideoMode(video_mode); } diff --git a/src/openni2/DeviceDriver.cpp b/src/openni2/DeviceDriver.cpp index 9ed6e1143..29b5697d7 100644 --- a/src/openni2/DeviceDriver.cpp +++ b/src/openni2/DeviceDriver.cpp @@ -20,9 +20,14 @@ */ #include #include +#include +#include +#include #include #include #include +#include +#include #include "DepthStream.hpp" #include "ColorStream.hpp" #include "IrStream.hpp" @@ -30,52 +35,124 @@ namespace Freenect2Driver { - class Device : public oni::driver::DeviceBase, public libfreenect2::FrameListener + typedef std::map ConfigStrings; + + class Device : public oni::driver::DeviceBase { private: libfreenect2::Freenect2Device *dev; ColorStream* color; DepthStream* depth; IrStream* ir; + Registration *reg; + ConfigStrings config; + bool device_stop; + libfreenect2::SyncMultiFrameListener listener; + libfreenect2::thread* thread; + + struct timeval ts_epoc; + long getTimestamp() { + struct timeval ts; + gettimeofday(&ts, NULL); + return (ts.tv_sec - ts_epoc.tv_sec) * 1000 + ts.tv_usec / 1000; // XXX, ignoring nsec of the epoc. + } - // for Freenect::FreenectDevice - bool onNewFrame(libfreenect2::Frame::Type type, libfreenect2::Frame *frame) { - if (type == libfreenect2::Frame::Color) { - if (color) - color->buildFrame(frame->data, 0); // timestamp); - } else - if (type == libfreenect2::Frame::Ir) { - if (ir) - ir->buildFrame(frame->data, 0); // timestamp); - } else - if (type == libfreenect2::Frame::Depth) { + static void static_run(void* cookie) + { + static_cast(cookie)->run(); + } + + void run() + { + libfreenect2::FrameMap frames; + while(!device_stop) + { + listener.waitForNewFrame(frames); + + libfreenect2::Frame *irFrame = frames[libfreenect2::Frame::Ir]; + libfreenect2::Frame *depthFrame = frames[libfreenect2::Frame::Depth]; + libfreenect2::Frame *colorFrame = frames[libfreenect2::Frame::Color]; + const long timeStamp = getTimestamp(); if (depth) - depth->buildFrame(frame->data, 0); // timestamp); + depth->buildFrame(depthFrame, timeStamp); + if (ir) + ir->buildFrame(irFrame, timeStamp); + if (color) + color->buildFrame(colorFrame, timeStamp); + + listener.release(frames); + } + } + + OniStatus setStreamProperties(VideoStream* stream, std::string pfx) + { + pfx += '-'; + OniStatus res = ONI_STATUS_OK, tmp_res; + if (config.find(pfx + "size") != config.end()) { + WriteMessage("setStreamProperty: " + pfx + "size: " + config[pfx + "size"]); + std::string size(config[pfx + "size"]); + int i = size.find("x"); + OniVideoMode video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, atoi(size.substr(0, i).c_str()), atoi(size.substr(i + 1).c_str()), 30); + tmp_res = stream->setProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, (void*)&video_mode, sizeof(video_mode)); + if (tmp_res != ONI_STATUS_OK) + res = tmp_res; } + + return res; } public: Device(int index) : //libfreenect2::Freenect2Device(fn_ctx, index), dev(NULL), + reg(NULL), color(NULL), ir(NULL), - depth(NULL) { } + depth(NULL), + device_stop(false), + listener(libfreenect2::Frame::Depth | libfreenect2::Frame::Ir | libfreenect2::Frame::Color), + thread(NULL) + { + gettimeofday(&ts_epoc, NULL); + thread = new libfreenect2::thread(&Device::static_run, this); + } ~Device() { + close(); destroyStream(color); destroyStream(ir); destroyStream(depth); + if (reg) { + delete reg; + reg = NULL; + } } // for Freenect2Device void setFreenect2Device(libfreenect2::Freenect2Device *dev) { this->dev = dev; - dev->setColorFrameListener(this); - dev->setIrAndDepthFrameListener(this); + dev->setColorFrameListener(&listener); + dev->setIrAndDepthFrameListener(&listener); + reg = new Registration(dev); + } + void setConfigStrings(ConfigStrings& config) { + this->config = config; + } + void start() { + //TODO: start thread executing the run() method + //device_stop = false; + //thread = new libfreenect2::thread(&Device::static_run, this); + dev->start(); + } + void stop() { + device_stop = true; + thread->join(); + + dev->stop(); + } + void close() { + stop(); + dev->close(); } - void start() { dev->start(); } - void stop() { dev->stop(); } - void close() { dev->close(); } // for DeviceBase @@ -100,16 +177,22 @@ namespace Freenect2Driver LogError("Cannot create a stream of type " + to_string(sensorType)); return NULL; case ONI_SENSOR_COLOR: - if (! color) - color = new ColorStream(dev); + if (! color) { + color = new ColorStream(dev, reg); + setStreamProperties(color, "color"); + } return color; case ONI_SENSOR_DEPTH: - if (! depth) - depth = new DepthStream(dev); + if (! depth) { + depth = new DepthStream(dev, reg); + setStreamProperties(depth, "depth"); + } return depth; case ONI_SENSOR_IR: - if (! ir) - ir = new IrStream(dev); + if (! ir) { + ir = new IrStream(dev, reg); + setStreamProperties(ir, "ir"); + } return ir; } } @@ -214,7 +297,9 @@ namespace Freenect2Driver LogError("Unexpected size for ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION"); return ONI_STATUS_ERROR; } - return depth->setImageRegistrationMode(*(static_cast(data))); + OniImageRegistrationMode mode = *(static_cast(data)); + color->setImageRegistrationMode(mode); + return depth->setImageRegistrationMode(mode); } } @@ -250,22 +335,39 @@ namespace Freenect2Driver private: typedef std::map OniDeviceMap; OniDeviceMap devices; + std::string uriScheme; + ConfigStrings config; libfreenect2::Freenect2 freenect2; - static std::string devid_to_uri(int id) { - return "freenect://" + to_string(id); + std::string devid_to_uri(int id) { + return uriScheme + "://" + to_string(id); } - static int uri_to_devid(const std::string uri) { + int uri_to_devid(const std::string uri) { int id; std::istringstream is(uri); - is.seekg(strlen("freenect://")); + is.seekg((uriScheme + "://").length()); is >> id; return id; } + void register_uri(std::string uri) { + OniDeviceInfo info; + strncpy(info.uri, uri.c_str(), ONI_MAX_STR); + strncpy(info.vendor, "Microsoft", ONI_MAX_STR); + //strncpy(info.name, "Kinect 2", ONI_MAX_STR); // XXX, NiTE does not accept new name + strncpy(info.name, "Kinect", ONI_MAX_STR); + if (devices.find(info) == devices.end()) { + WriteMessage("Driver: register new uri: " + uri); + devices[info] = NULL; + deviceConnected(&info); + deviceStateChanged(&info, 0); + } + } + public: - Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices) + Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices), + uriScheme("freenect2") { //WriteMessage("Using libfreenect v" + to_string(PROJECT_VER)); WriteMessage("Using libfreenect2"); @@ -282,16 +384,18 @@ namespace Freenect2Driver for (int i = 0; i < freenect2.enumerateDevices(); i++) { std::string uri = devid_to_uri(i); + const char* modes_c[] = { + "", + "?depth-size=640x480", + "?depth-size=512x424", + }; + std::vector modes(modes_c, modes_c + 3); WriteMessage("Found device " + uri); - - OniDeviceInfo info; - strncpy(info.uri, uri.c_str(), ONI_MAX_STR); - strncpy(info.vendor, "Microsoft", ONI_MAX_STR); - strncpy(info.name, "Kinect", ONI_MAX_STR); - devices[info] = NULL; - deviceConnected(&info); - deviceStateChanged(&info, 0); + + for (int i = 0; i < modes.size(); i++) { + register_uri(uri + modes[i]); + } #if 0 freenect_device* dev; @@ -310,11 +414,34 @@ namespace Freenect2Driver return ONI_STATUS_OK; } - oni::driver::DeviceBase* deviceOpen(const char* uri, const char* mode = NULL) + oni::driver::DeviceBase* deviceOpen(const char* c_uri, const char* c_mode = NULL) { + std::string uri(c_uri); + std::string mode(c_mode ? c_mode : ""); + if (uri.find("?") != -1) { + mode += "&"; + mode += uri.substr(uri.find("?") + 1); + uri = uri.substr(0, uri.find("?")); + } + std::stringstream ss(mode); + std::string buf; + while(std::getline(ss, buf, '&')) { + if (buf.find("=") != -1) { + config[buf.substr(0, buf.find("="))] = buf.substr(buf.find("=")+1); + } else { + if (0 < buf.length()) + config[buf] = ""; + } + } + WriteMessage("deiveOpen: " + uri); + for (std::map::iterator it = config.begin(); it != config.end(); it++) { + WriteMessage(" " + it->first + " = " + it->second); + } + for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++) { - if (strcmp(iter->first.uri, uri) == 0) // found + std::string iter_uri(iter->first.uri); + if (iter_uri.substr(0, iter_uri.find("?")) == uri) // found { if (iter->second) // already open { @@ -326,6 +453,7 @@ namespace Freenect2Driver int id = uri_to_devid(iter->first.uri); Device* device = new Device(id); device->setFreenect2Device(freenect2.openDevice(id)); // XXX, detault pipeline // const PacketPipeline *factory); + device->setConfigStrings(config); iter->second = device; return device; } @@ -361,6 +489,7 @@ namespace Freenect2Driver if (! device) return ONI_STATUS_ERROR; deviceClose(device); + register_uri(std::string(uri)); // XXX, register new uri here. return ONI_STATUS_OK; } diff --git a/src/openni2/IrStream.cpp b/src/openni2/IrStream.cpp index 06358feb6..2bed4a397 100644 --- a/src/openni2/IrStream.cpp +++ b/src/openni2/IrStream.cpp @@ -3,18 +3,18 @@ using namespace Freenect2Driver; +// from NUI library and converted to radians +const float IrStream::HORIZONTAL_FOV = 58.5 * (M_PI / 180); +const float IrStream::VERTICAL_FOV = 45.6 * (M_PI / 180); -IrStream::IrStream(libfreenect2::Freenect2Device* pDevice) : VideoStream(pDevice) +IrStream::IrStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg) : VideoStream(pDevice, reg) { video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_GRAY16, 512, 424, 30); - image_registration_mode = ONI_IMAGE_REGISTRATION_OFF; setVideoMode(video_mode); pDevice->start(); } // Add video modes here as you implement them -// Note: if image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR, -// setVideoFormat() will try FREENECT_DEPTH_REGISTERED first then fall back on what is set here. IrStream::FreenectIrModeMap IrStream::getSupportedVideoModes() { FreenectIrModeMap modes; @@ -35,58 +35,13 @@ OniStatus IrStream::setVideoMode(OniVideoMode requested_mode) return ONI_STATUS_OK; } -void IrStream::populateFrame(void* data, OniFrame* frame) const +void IrStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const { - frame->sensorType = sensor_type; - frame->stride = video_mode.resolutionX * sizeof(uint16_t); - - if (cropping.enabled) - { - frame->height = cropping.height; - frame->width = cropping.width; - frame->cropOriginX = cropping.originX; - frame->cropOriginY = cropping.originY; - frame->croppingEnabled = true; - } - else - { - frame->cropOriginX = 0; - frame->cropOriginY = 0; - frame->croppingEnabled = false; - } - + dstFrame->sensorType = sensor_type; + dstFrame->stride = dstFrame->width * sizeof(uint16_t); // copy stream buffer from freenect - - float* source = static_cast(data) + frame->cropOriginX + frame->cropOriginY * video_mode.resolutionX; - uint16_t* target = static_cast(frame->data); - const unsigned int skipWidth = video_mode.resolutionX - frame->width; - - if (mirroring) - { - target += frame->width; - - for (int y = 0; y < frame->height; y++) - { - for (int x = 0; x < frame->width; x++) - { - *target-- = *source++; - } - - source += skipWidth; - target += 2 * frame->width; - } - } - else - { - for (int y = 0; y < frame->height; y++) - { - for (int x = 0; x < frame->width; x++) - { - *target++ = *source++; - } - - source += skipWidth; - } - } + copyFrame(static_cast((void*)srcFrame->data), srcX, srcY, srcFrame->width, + static_cast(dstFrame->data), dstX, dstY, dstFrame->width, + width, height, mirroring); } diff --git a/src/openni2/IrStream.hpp b/src/openni2/IrStream.hpp index c0b16432c..36bdc23af 100644 --- a/src/openni2/IrStream.hpp +++ b/src/openni2/IrStream.hpp @@ -14,20 +14,19 @@ namespace Freenect2Driver { public: // from NUI library and converted to radians - static const float HORIZONTAL_FOV = 58.5 * (M_PI / 180); - static const float VERTICAL_FOV = 45.6 * (M_PI / 180); + static const float HORIZONTAL_FOV; + static const float VERTICAL_FOV; private: typedef std::map< OniVideoMode, int > FreenectIrModeMap; static const OniSensorType sensor_type = ONI_SENSOR_IR; - OniImageRegistrationMode image_registration_mode; static FreenectIrModeMap getSupportedVideoModes(); OniStatus setVideoMode(OniVideoMode requested_mode); - void populateFrame(void* data, OniFrame* frame) const; + void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; public: - IrStream(libfreenect2::Freenect2Device* pDevice); + IrStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); //~IrStream() { } static OniSensorInfo getSensorInfo() @@ -39,18 +38,7 @@ namespace Freenect2Driver return sensors; } - OniImageRegistrationMode getImageRegistrationMode() const { return image_registration_mode; } - OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) - { - if (!isImageRegistrationModeSupported(mode)) - return ONI_STATUS_NOT_SUPPORTED; - image_registration_mode = mode; - return setVideoMode(video_mode); - } - // from StreamBase - OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF || mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); } - OniBool isPropertySupported(int propertyId) { switch(propertyId) diff --git a/src/openni2/Registration.cpp b/src/openni2/Registration.cpp new file mode 100644 index 000000000..4ef46bcf7 --- /dev/null +++ b/src/openni2/Registration.cpp @@ -0,0 +1,43 @@ +#include +#include "Driver/OniDriverAPI.h" +#include "Registration.hpp" + +using namespace Freenect2Driver; + +Registration::Registration(libfreenect2::Freenect2Device* dev) : + dev(dev), + reg(NULL), + enabled(false) +{ + } + +Registration::~Registration() { + delete reg; +} + +void Registration::depthFrame(libfreenect2::Frame* frame) { + lastDepthFrame = frame; + } + +void Registration::colorFrameRGB888(libfreenect2::Frame* colorFrame, libfreenect2::Frame* registeredFrame) +{ + if (!reg) { + libfreenect2::Freenect2Device::ColorCameraParams colCamParams = dev->getColorCameraParams(); + libfreenect2::Freenect2Device::IrCameraParams irCamParams = dev->getIrCameraParams(); + { + libfreenect2::Freenect2Device::ColorCameraParams cp = colCamParams; + std::cout << "fx=" << cp.fx << ",fy=" << cp.fy << + ",cx=" << cp.cx << ",cy=" << cp.cy << std::endl; + libfreenect2::Freenect2Device::IrCameraParams ip = irCamParams; + std::cout << "fx=" << ip.fx << ",fy=" << ip.fy << + ",ix=" << ip.cx << ",iy=" << ip.cy << + ",k1=" << ip.k1 << ",k2=" << ip.k2 << ",k3=" << ip.k3 << + ",p1=" << ip.p1 << ",p2=" << ip.p2 << std::endl; + } + reg = new libfreenect2::Registration(irCamParams, colCamParams); + } + + libfreenect2::Frame undistorted(lastDepthFrame->width, lastDepthFrame->height, lastDepthFrame->bytes_per_pixel); + + reg->apply(colorFrame, lastDepthFrame, &undistorted, registeredFrame); + } diff --git a/src/openni2/Registration.hpp b/src/openni2/Registration.hpp new file mode 100644 index 000000000..69e2affe1 --- /dev/null +++ b/src/openni2/Registration.hpp @@ -0,0 +1,20 @@ +#include + +namespace Freenect2Driver { + class Registration { + private: + libfreenect2::Freenect2Device* dev; + libfreenect2::Registration* reg; + libfreenect2::Frame* lastDepthFrame; + bool enabled; + + public: + Registration(libfreenect2::Freenect2Device* dev); + ~Registration(); + + void depthFrame(libfreenect2::Frame* frame); + void colorFrameRGB888(libfreenect2::Frame* srcFrame, libfreenect2::Frame* dstFrame); + void setEnable(bool enable = true) { enabled = enable; } + bool isEnabled() { return enabled; } + }; +} diff --git a/src/openni2/VideoStream.hpp b/src/openni2/VideoStream.hpp index d2950fa28..1172270bf 100644 --- a/src/openni2/VideoStream.hpp +++ b/src/openni2/VideoStream.hpp @@ -5,6 +5,7 @@ #include #include "PS1080.h" #include "Utility.hpp" +#include "Registration.hpp" namespace Freenect2Driver { @@ -14,7 +15,7 @@ namespace Freenect2Driver unsigned int frame_id; // number each frame virtual OniStatus setVideoMode(OniVideoMode requested_mode) = 0; - virtual void populateFrame(void* data, OniFrame* frame) const = 0; + virtual void populateFrame(libfreenect2::Frame* lf2Frame, int srcX, int srcY, OniFrame* oniFrame, int tgtX, int tgtY, int width, int height) const = 0; protected: static const OniSensorType sensor_type; @@ -23,11 +24,38 @@ namespace Freenect2Driver OniVideoMode video_mode; OniCropping cropping; bool mirroring; + Freenect2Driver::Registration *reg; + bool callPropertyChangedCallback; + + static void copyFrame(float* srcPix, int srcX, int srcY, int srcStride, uint16_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring) + { + srcPix += srcX + srcY * srcStride; + dstPix += dstX + dstY * dstStride; + + for (int y = 0; y < height; y++) { + uint16_t* dst = dstPix + y * dstStride; + float* src = srcPix + y * srcStride; + if (mirroring) { + dst += width; + for (int x = 0; x < width; x++) + *dst-- = *src++; + } else { + for (int x = 0; x < width; x++) + *dst++ = *src++; + } + } + } + void raisePropertyChanged(int propertyId, const void* data, int dataSize) { + if (callPropertyChangedCallback) + StreamBase::raisePropertyChanged(propertyId, data, dataSize); + } public: - VideoStream(libfreenect2::Freenect2Device* device) : + VideoStream(libfreenect2::Freenect2Device* device, Freenect2Driver::Registration *reg) : frame_id(1), device(device), + reg(reg), + callPropertyChangedCallback(false), mirroring(false) { // joy of structs @@ -36,21 +64,45 @@ namespace Freenect2Driver } //~VideoStream() { stop(); } - void buildFrame(void* data, uint32_t timestamp) + void setPropertyChangedCallback(oni::driver::PropertyChangedCallback handler, void* pCookie) { + callPropertyChangedCallback = true; + StreamBase::setPropertyChangedCallback(handler, pCookie); + } + + bool buildFrame(libfreenect2::Frame* lf2Frame, uint32_t timestamp) { if (!running) - return; - - OniFrame* frame = getServices().acquireFrame(); - frame->frameIndex = frame_id++; - frame->timestamp = timestamp; - frame->videoMode = video_mode; - frame->width = video_mode.resolutionX; - frame->height = video_mode.resolutionY; - - populateFrame(data, frame); - raiseNewFrame(frame); - getServices().releaseFrame(frame); + return false; + + OniFrame* oniFrame = getServices().acquireFrame(); + oniFrame->frameIndex = frame_id++; + oniFrame->timestamp = timestamp; + oniFrame->videoMode = video_mode; + oniFrame->width = video_mode.resolutionX; + oniFrame->height = video_mode.resolutionY; + + if (cropping.enabled) + { + oniFrame->height = cropping.height; + oniFrame->width = cropping.width; + oniFrame->cropOriginX = cropping.originX; + oniFrame->cropOriginY = cropping.originY; + oniFrame->croppingEnabled = true; + } + else + { + oniFrame->cropOriginX = 0; + oniFrame->cropOriginY = 0; + oniFrame->croppingEnabled = false; + } + int width = std::min(oniFrame->width, (int)lf2Frame->width); + int height = std::min(oniFrame->height, (int)lf2Frame->height); + + populateFrame(lf2Frame, oniFrame->cropOriginX, oniFrame->cropOriginY, oniFrame, 0, 0, width, height); + raiseNewFrame(oniFrame); + getServices().releaseFrame(oniFrame); + + return false; } // from StreamBase From fc2ee563d787280b857726fef91cab1bda11bd80 Mon Sep 17 00:00:00 2001 From: Zou Hanya Date: Sat, 23 May 2015 17:05:46 +0900 Subject: [PATCH 06/12] openni2: Add timestamp on the frames --- src/openni2/DepthStream.cpp | 2 +- src/openni2/DeviceDriver.cpp | 62 +++++++++++++++++++++++------------- src/openni2/VideoStream.hpp | 4 +-- 3 files changed, 43 insertions(+), 25 deletions(-) diff --git a/src/openni2/DepthStream.cpp b/src/openni2/DepthStream.cpp index 46f7aafc0..6b7246843 100644 --- a/src/openni2/DepthStream.cpp +++ b/src/openni2/DepthStream.cpp @@ -22,8 +22,8 @@ DepthStream::DepthStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver { //video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 512, 424, 30); video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30); - image_registration_mode = ONI_IMAGE_REGISTRATION_OFF; setVideoMode(video_mode); + setImageRegistrationMode(ONI_IMAGE_REGISTRATION_OFF); pDevice->start(); } diff --git a/src/openni2/DeviceDriver.cpp b/src/openni2/DeviceDriver.cpp index 29b5697d7..60dacbc02 100644 --- a/src/openni2/DeviceDriver.cpp +++ b/src/openni2/DeviceDriver.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include @@ -50,38 +49,56 @@ namespace Freenect2Driver libfreenect2::SyncMultiFrameListener listener; libfreenect2::thread* thread; - struct timeval ts_epoc; - long getTimestamp() { - struct timeval ts; - gettimeofday(&ts, NULL); - return (ts.tv_sec - ts_epoc.tv_sec) * 1000 + ts.tv_usec / 1000; // XXX, ignoring nsec of the epoc. - } - static void static_run(void* cookie) { static_cast(cookie)->run(); - } + } + + VideoStream* getStream(libfreenect2::Frame::Type type) + { + if (type == libfreenect2::Frame::Depth) + return depth; + if (type == libfreenect2::Frame::Ir) + return ir; + if (type == libfreenect2::Frame::Color) + return color; + return NULL; + } void run() { libfreenect2::FrameMap frames; + uint32_t seqNum = 0; + libfreenect2::Frame::Type seqType; + + struct streams { + const char* name; + libfreenect2::Frame::Type type; + } streams[] = { + { "Ir", libfreenect2::Frame::Ir }, + { "Depth", libfreenect2::Frame::Depth }, + { "Color", libfreenect2::Frame::Color } + }; while(!device_stop) { listener.waitForNewFrame(frames); - libfreenect2::Frame *irFrame = frames[libfreenect2::Frame::Ir]; - libfreenect2::Frame *depthFrame = frames[libfreenect2::Frame::Depth]; - libfreenect2::Frame *colorFrame = frames[libfreenect2::Frame::Color]; - const long timeStamp = getTimestamp(); - if (depth) - depth->buildFrame(depthFrame, timeStamp); - if (ir) - ir->buildFrame(irFrame, timeStamp); - if (color) - color->buildFrame(colorFrame, timeStamp); + for (int i = 0; i < sizeof(streams)/sizeof(*streams); i++) { + struct streams& s = streams[i]; + VideoStream* stream = getStream(s.type); + libfreenect2::Frame *frame = frames[s.type]; + if (stream) { + if (seqNum == 0) + seqType = s.type; + if (s.type == seqType) + seqNum++; + frame->timestamp = seqNum * 33369; + stream->buildFrame(frame); + } + } listener.release(frames); - } + } } OniStatus setStreamProperties(VideoStream* stream, std::string pfx) @@ -112,7 +129,6 @@ namespace Freenect2Driver listener(libfreenect2::Frame::Depth | libfreenect2::Frame::Ir | libfreenect2::Frame::Color), thread(NULL) { - gettimeofday(&ts_epoc, NULL); thread = new libfreenect2::thread(&Device::static_run, this); } ~Device() @@ -472,10 +488,12 @@ namespace Freenect2Driver { WriteMessage("Closing device " + std::string(iter->first.uri)); int id = uri_to_devid(iter->first.uri); - devices.erase(iter); + Device* device = (Device*)iter->second; device->stop(); device->close(); + + devices.erase(iter); return; } } diff --git a/src/openni2/VideoStream.hpp b/src/openni2/VideoStream.hpp index 1172270bf..a5fccc4cd 100644 --- a/src/openni2/VideoStream.hpp +++ b/src/openni2/VideoStream.hpp @@ -69,14 +69,14 @@ namespace Freenect2Driver StreamBase::setPropertyChangedCallback(handler, pCookie); } - bool buildFrame(libfreenect2::Frame* lf2Frame, uint32_t timestamp) + bool buildFrame(libfreenect2::Frame* lf2Frame) { if (!running) return false; OniFrame* oniFrame = getServices().acquireFrame(); oniFrame->frameIndex = frame_id++; - oniFrame->timestamp = timestamp; + oniFrame->timestamp = lf2Frame->timestamp; oniFrame->videoMode = video_mode; oniFrame->width = video_mode.resolutionX; oniFrame->height = video_mode.resolutionY; From 11b748bb52c762afdf40dd2331be05ed482b9969 Mon Sep 17 00:00:00 2001 From: Zou Hanya Date: Sat, 4 Jul 2015 23:23:59 +0900 Subject: [PATCH 07/12] openni2: Use OpenNI2 logging functions/classes --- src/openni2/Utility.hpp | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/openni2/Utility.hpp b/src/openni2/Utility.hpp index f9d5e39ac..21e04da32 100644 --- a/src/openni2/Utility.hpp +++ b/src/openni2/Utility.hpp @@ -56,19 +56,22 @@ static std::string to_string(const T& n) // global logging namespace Freenect2Driver { - static void WriteMessage(std::string info) - { - std::cout << "OpenNI2-Freenect2Driver: " << info << std::endl; - } - // DriverServices is set in DeviceDriver.cpp so all files can call errorLoggerAppend() static oni::driver::DriverServices* DriverServices; - static void LogError(std::string error) - { - // errorLoggerAppend() doesn't seem to go anywhere, so WriteMessage also - WriteMessage("(ERROR) " + error); - - if (DriverServices != NULL) - DriverServices->errorLoggerAppend(std::string("OpenNI2-Freenect2Driver: " + error).c_str()); - } + + // from XnLog.h + typedef enum XnLogSeverity { + XN_LOG_VERBOSE = 0, + XN_LOG_INFO = 1, + XN_LOG_WARNING = 2, + XN_LOG_ERROR = 3, + XN_LOG_SEVERITY_NONE = 10, + } XnLogSeverity; } +#define FN2DRV_LOG_MASK "Freenect2Driver" +#define WriteVerbose(str) do { if (DriverServices != NULL) DriverServices->log(XN_LOG_VERBOSE, __FILE__, __LINE__, FN2DRV_LOG_MASK, std::string(str).c_str()); } while(0) +#define WriteInfo(str) do { if (DriverServices != NULL) DriverServices->log(XN_LOG_INFO, __FILE__, __LINE__, FN2DRV_LOG_MASK, std::string(str).c_str()); } while(0) +#define WriteWarning(str) do { if (DriverServices != NULL) DriverServices->log(XN_LOG_WARNING, __FILE__, __LINE__, FN2DRV_LOG_MASK, std::string(str).c_str()); } while(0) +#define WriteError(str) do { if (DriverServices != NULL) DriverServices->log(XN_LOG_ERROR, __FILE__, __LINE__, FN2DRV_LOG_MASK, std::string(str).c_str()); } while(0) +#define WriteMessage(str) WriteInfo(str) +#define LogError(str) WriteError(str) From 5d394d669d496a6fc5f7082ec66a2c5d9023cafc Mon Sep 17 00:00:00 2001 From: Zou Hanya Date: Sat, 4 Jul 2015 23:43:47 +0900 Subject: [PATCH 08/12] openni2: Add OpenKinect Project's license headers --- src/openni2/ColorStream.cpp | 26 ++++++++++++++++++++++ src/openni2/ColorStream.hpp | 26 ++++++++++++++++++++++ src/openni2/DepthStream.cpp | 26 ++++++++++++++++++++++ src/openni2/DepthStream.hpp | 26 ++++++++++++++++++++++ src/openni2/DeviceDriver.cpp | 42 ++++++++++++++++++++++-------------- src/openni2/IrStream.cpp | 26 ++++++++++++++++++++++ src/openni2/IrStream.hpp | 26 ++++++++++++++++++++++ src/openni2/Registration.cpp | 26 ++++++++++++++++++++++ src/openni2/Registration.hpp | 26 ++++++++++++++++++++++ src/openni2/Utility.hpp | 26 ++++++++++++++++++++++ src/openni2/VideoStream.hpp | 26 ++++++++++++++++++++++ 11 files changed, 286 insertions(+), 16 deletions(-) diff --git a/src/openni2/ColorStream.cpp b/src/openni2/ColorStream.cpp index 72c830c18..a9499caa3 100644 --- a/src/openni2/ColorStream.cpp +++ b/src/openni2/ColorStream.cpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. + * See the CONTRIB file for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + #include #include "ColorStream.hpp" diff --git a/src/openni2/ColorStream.hpp b/src/openni2/ColorStream.hpp index 74a6637b9..860ab9da9 100644 --- a/src/openni2/ColorStream.hpp +++ b/src/openni2/ColorStream.hpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. + * See the CONTRIB file for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + #pragma once #include // for transform() diff --git a/src/openni2/DepthStream.cpp b/src/openni2/DepthStream.cpp index 6b7246843..af368efc0 100644 --- a/src/openni2/DepthStream.cpp +++ b/src/openni2/DepthStream.cpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. + * See the CONTRIB file for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + #include #include "DepthStream.hpp" diff --git a/src/openni2/DepthStream.hpp b/src/openni2/DepthStream.hpp index cd693927b..078a08257 100644 --- a/src/openni2/DepthStream.hpp +++ b/src/openni2/DepthStream.hpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. + * See the CONTRIB file for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + #pragma once #include // for transform() diff --git a/src/openni2/DeviceDriver.cpp b/src/openni2/DeviceDriver.cpp index 60dacbc02..7e75ad345 100644 --- a/src/openni2/DeviceDriver.cpp +++ b/src/openni2/DeviceDriver.cpp @@ -1,23 +1,33 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. + * See the CONTRIB file for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ /** * FreenectDriver * Copyright 2013 Benn Snyder -* -* 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. -*/ -/** -* OpenNI2 Freenect2 Driver -* Copyright 2015 hanyazou@gmail.com */ + #include #include #include diff --git a/src/openni2/IrStream.cpp b/src/openni2/IrStream.cpp index 2bed4a397..74f5ff0c8 100644 --- a/src/openni2/IrStream.cpp +++ b/src/openni2/IrStream.cpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 individual OpenKinect contributors. See the CONTRIB file + * for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + #include #include "IrStream.hpp" diff --git a/src/openni2/IrStream.hpp b/src/openni2/IrStream.hpp index 36bdc23af..ea35adb99 100644 --- a/src/openni2/IrStream.hpp +++ b/src/openni2/IrStream.hpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 individual OpenKinect contributors. See the CONTRIB file + * for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + #pragma once #include // for transform() diff --git a/src/openni2/Registration.cpp b/src/openni2/Registration.cpp index 4ef46bcf7..c3d6fac9c 100644 --- a/src/openni2/Registration.cpp +++ b/src/openni2/Registration.cpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 individual OpenKinect contributors. See the CONTRIB file + * for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + #include #include "Driver/OniDriverAPI.h" #include "Registration.hpp" diff --git a/src/openni2/Registration.hpp b/src/openni2/Registration.hpp index 69e2affe1..8aa4a5356 100644 --- a/src/openni2/Registration.hpp +++ b/src/openni2/Registration.hpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2015 individual OpenKinect contributors. See the CONTRIB file + * for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + #include namespace Freenect2Driver { diff --git a/src/openni2/Utility.hpp b/src/openni2/Utility.hpp index 21e04da32..f8b84a76e 100644 --- a/src/openni2/Utility.hpp +++ b/src/openni2/Utility.hpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. + * See the CONTRIB file for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + // This file contains symbols that may be used by any class or don't really go anywhere else. #pragma once diff --git a/src/openni2/VideoStream.hpp b/src/openni2/VideoStream.hpp index a5fccc4cd..bb02d0767 100644 --- a/src/openni2/VideoStream.hpp +++ b/src/openni2/VideoStream.hpp @@ -1,3 +1,29 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. + * See the CONTRIB file for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + #pragma once #include From 446cde5b052b54404c19db3532d6ba3606fbac8a Mon Sep 17 00:00:00 2001 From: Zou Hanya Date: Sat, 2 Jan 2016 06:43:54 +0900 Subject: [PATCH 09/12] openni2: Refactor setVideoMode() and getSensorInfo() in VideoStream class --- src/openni2/ColorStream.cpp | 16 +------ src/openni2/ColorStream.hpp | 15 +----- src/openni2/DepthStream.cpp | 18 ++----- src/openni2/DepthStream.hpp | 16 +------ src/openni2/DeviceDriver.cpp | 93 +++++++++++++++++++----------------- src/openni2/IrStream.cpp | 18 ++----- src/openni2/IrStream.hpp | 16 +------ src/openni2/VideoStream.hpp | 29 +++++++++-- 8 files changed, 88 insertions(+), 133 deletions(-) diff --git a/src/openni2/ColorStream.cpp b/src/openni2/ColorStream.cpp index a9499caa3..bc6e8000e 100644 --- a/src/openni2/ColorStream.cpp +++ b/src/openni2/ColorStream.cpp @@ -38,11 +38,10 @@ ColorStream::ColorStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver { video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30); setVideoMode(video_mode); - pDevice->start(); } // Add video modes here as you implement them -ColorStream::FreenectVideoModeMap ColorStream::getSupportedVideoModes() +ColorStream::FreenectVideoModeMap ColorStream::getSupportedVideoModes() const { FreenectVideoModeMap modes; // pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution @@ -58,20 +57,9 @@ ColorStream::FreenectVideoModeMap ColorStream::getSupportedVideoModes() */ } -OniStatus ColorStream::setVideoMode(OniVideoMode requested_mode) -{ - FreenectVideoModeMap supported_video_modes = getSupportedVideoModes(); - FreenectVideoModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode); - if (matched_mode_iter == supported_video_modes.end()) - return ONI_STATUS_NOT_SUPPORTED; - - video_mode = requested_mode; - return ONI_STATUS_OK; -} - void ColorStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const { - dstFrame->sensorType = sensor_type; + dstFrame->sensorType = getSensorType(); dstFrame->stride = dstFrame->width * 3; // copy stream buffer from freenect diff --git a/src/openni2/ColorStream.hpp b/src/openni2/ColorStream.hpp index 860ab9da9..e614a218b 100644 --- a/src/openni2/ColorStream.hpp +++ b/src/openni2/ColorStream.hpp @@ -46,10 +46,8 @@ namespace Freenect2Driver private: typedef std::map< OniVideoMode, int > FreenectVideoModeMap; - static const OniSensorType sensor_type = ONI_SENSOR_COLOR; - - static FreenectVideoModeMap getSupportedVideoModes(); - OniStatus setVideoMode(OniVideoMode requested_mode); + OniSensorType getSensorType() const { return ONI_SENSOR_COLOR; } + VideoModeMap getSupportedVideoModes() const; void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; static void copyFrame(uint8_t* srcPix, int srcX, int srcY, int srcStride, uint8_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring); @@ -61,15 +59,6 @@ namespace Freenect2Driver ColorStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); //~ColorStream() { } - static OniSensorInfo getSensorInfo() - { - FreenectVideoModeMap supported_modes = getSupportedVideoModes(); - OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; - std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); - OniSensorInfo sensors = { sensor_type, static_cast(supported_modes.size()), modes }; - return sensors; - } - OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) { if (mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) { diff --git a/src/openni2/DepthStream.cpp b/src/openni2/DepthStream.cpp index af368efc0..edbe5dc2c 100644 --- a/src/openni2/DepthStream.cpp +++ b/src/openni2/DepthStream.cpp @@ -50,15 +50,14 @@ DepthStream::DepthStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30); setVideoMode(video_mode); setImageRegistrationMode(ONI_IMAGE_REGISTRATION_OFF); - pDevice->start(); } // Add video modes here as you implement them // Note: if image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR, // setVideoFormat() will try FREENECT_DEPTH_REGISTERED first then fall back on what is set here. -DepthStream::FreenectDepthModeMap DepthStream::getSupportedVideoModes() +DepthStream::VideoModeMap DepthStream::getSupportedVideoModes() const { - FreenectDepthModeMap modes; + VideoModeMap modes; // pixelFormat, resolutionX, resolutionY, fps modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 640, 480, 30)] = 0; modes[makeOniVideoMode(ONI_PIXEL_FORMAT_DEPTH_1_MM, 512, 424, 30)] = 1; @@ -66,20 +65,9 @@ DepthStream::FreenectDepthModeMap DepthStream::getSupportedVideoModes() return modes; } -OniStatus DepthStream::setVideoMode(OniVideoMode requested_mode) -{ - 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; - - video_mode = requested_mode; - return ONI_STATUS_OK; -} - void DepthStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const { - dstFrame->sensorType = sensor_type; + dstFrame->sensorType = getSensorType(); dstFrame->stride = dstFrame->width * sizeof(uint16_t); // XXX, save depth map for registration diff --git a/src/openni2/DepthStream.hpp b/src/openni2/DepthStream.hpp index 078a08257..b14e7e369 100644 --- a/src/openni2/DepthStream.hpp +++ b/src/openni2/DepthStream.hpp @@ -56,27 +56,15 @@ namespace Freenect2Driver static const double EMITTER_DCMOS_DISTANCE_VAL; private: - typedef std::map< OniVideoMode, int > FreenectDepthModeMap; - static const OniSensorType sensor_type = ONI_SENSOR_DEPTH; + OniSensorType getSensorType() const { return ONI_SENSOR_DEPTH; } OniImageRegistrationMode image_registration_mode; - - static FreenectDepthModeMap getSupportedVideoModes(); - OniStatus setVideoMode(OniVideoMode requested_mode); + VideoModeMap getSupportedVideoModes() const; void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; public: DepthStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); //~DepthStream() { } - static OniSensorInfo getSensorInfo() - { - FreenectDepthModeMap supported_modes = getSupportedVideoModes(); - OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; - std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); - OniSensorInfo sensors = { sensor_type, static_cast(supported_modes.size()), modes }; - return sensors; - } - OniImageRegistrationMode getImageRegistrationMode() const { return image_registration_mode; } OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) { diff --git a/src/openni2/DeviceDriver.cpp b/src/openni2/DeviceDriver.cpp index 7e75ad345..e7d98136b 100644 --- a/src/openni2/DeviceDriver.cpp +++ b/src/openni2/DeviceDriver.cpp @@ -143,10 +143,10 @@ namespace Freenect2Driver } ~Device() { - close(); destroyStream(color); destroyStream(ir); destroyStream(depth); + close(); if (reg) { delete reg; reg = NULL; @@ -167,17 +167,48 @@ namespace Freenect2Driver //TODO: start thread executing the run() method //device_stop = false; //thread = new libfreenect2::thread(&Device::static_run, this); - dev->start(); + if (! color) { + color = new ColorStream(dev, reg); + setStreamProperties(color, "color"); + } + if (! depth) { + depth = new DepthStream(dev, reg); + setStreamProperties(depth, "depth"); + } + if (! ir) { + ir = new IrStream(dev, reg); + setStreamProperties(ir, "ir"); + } + dev->start(); } void stop() { - device_stop = true; - thread->join(); - - dev->stop(); + if (!device_stop) { + device_stop = true; + thread->join(); + dev->stop(); + } + if (color != NULL) + { + delete color; + color = NULL; + } + if (depth != NULL) + { + delete depth; + depth = NULL; + } + if (ir != NULL) + { + delete ir; + ir = NULL; + } } void close() { - stop(); - dev->close(); + if (this->dev) { + stop(); + dev->close(); + } + this->dev = NULL; } // for DeviceBase @@ -203,48 +234,25 @@ namespace Freenect2Driver LogError("Cannot create a stream of type " + to_string(sensorType)); return NULL; case ONI_SENSOR_COLOR: - if (! color) { - color = new ColorStream(dev, reg); - setStreamProperties(color, "color"); - } + WriteMessage("Device: createStream(color)"); return color; case ONI_SENSOR_DEPTH: - if (! depth) { - depth = new DepthStream(dev, reg); - setStreamProperties(depth, "depth"); - } + WriteMessage("Device: createStream(depth)"); return depth; case ONI_SENSOR_IR: - if (! ir) { - ir = new IrStream(dev, reg); - setStreamProperties(ir, "ir"); - } + WriteMessage("Device: createStream(ir)"); return ir; } } void destroyStream(oni::driver::StreamBase* pStream) { - if (pStream == NULL) - return; - - // stop them all - dev->stop(); if (pStream == color) - { - delete color; - color = NULL; - } + WriteMessage("Device: destroyStream(color)"); if (pStream == depth) - { - delete depth; - depth = NULL; - } + WriteMessage("Device: destroyStream(depth)"); if (pStream == ir) - { - delete ir; - ir = NULL; - } + WriteMessage("Device: destroyStream(ir)"); } // todo: fill out properties @@ -480,6 +488,7 @@ namespace Freenect2Driver Device* device = new Device(id); device->setFreenect2Device(freenect2.openDevice(id)); // XXX, detault pipeline // const PacketPipeline *factory); device->setConfigStrings(config); + device->start(); iter->second = device; return device; } @@ -525,14 +534,10 @@ namespace Freenect2Driver { for (OniDeviceMap::iterator iter = devices.begin(); iter != devices.end(); iter++) { - WriteMessage("Closing device " + std::string(iter->first.uri)); - int id = uri_to_devid(iter->first.uri); - Device* device = (Device*)iter->second; - device->stop(); - device->close(); + if (iter->second) { + deviceClose(iter->second); + } } - - devices.clear(); } diff --git a/src/openni2/IrStream.cpp b/src/openni2/IrStream.cpp index 74f5ff0c8..2427c3d1d 100644 --- a/src/openni2/IrStream.cpp +++ b/src/openni2/IrStream.cpp @@ -37,33 +37,21 @@ IrStream::IrStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Regi { video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_GRAY16, 512, 424, 30); setVideoMode(video_mode); - pDevice->start(); } // Add video modes here as you implement them -IrStream::FreenectIrModeMap IrStream::getSupportedVideoModes() +IrStream::VideoModeMap IrStream::getSupportedVideoModes() const { - FreenectIrModeMap modes; + VideoModeMap modes; // pixelFormat, resolutionX, resolutionY, fps modes[makeOniVideoMode(ONI_PIXEL_FORMAT_GRAY16, 512, 424, 30)] = 0; return modes; } -OniStatus IrStream::setVideoMode(OniVideoMode requested_mode) -{ - FreenectIrModeMap supported_video_modes = getSupportedVideoModes(); - FreenectIrModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode); - if (matched_mode_iter == supported_video_modes.end()) - return ONI_STATUS_NOT_SUPPORTED; - - video_mode = requested_mode; - return ONI_STATUS_OK; -} - void IrStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const { - dstFrame->sensorType = sensor_type; + dstFrame->sensorType = getSensorType(); dstFrame->stride = dstFrame->width * sizeof(uint16_t); // copy stream buffer from freenect diff --git a/src/openni2/IrStream.hpp b/src/openni2/IrStream.hpp index ea35adb99..cdd5b3b0c 100644 --- a/src/openni2/IrStream.hpp +++ b/src/openni2/IrStream.hpp @@ -44,26 +44,14 @@ namespace Freenect2Driver static const float VERTICAL_FOV; private: - typedef std::map< OniVideoMode, int > FreenectIrModeMap; - static const OniSensorType sensor_type = ONI_SENSOR_IR; - - static FreenectIrModeMap getSupportedVideoModes(); - OniStatus setVideoMode(OniVideoMode requested_mode); + OniSensorType getSensorType() const { return ONI_SENSOR_IR; } + VideoModeMap getSupportedVideoModes() const; void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; public: IrStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); //~IrStream() { } - static OniSensorInfo getSensorInfo() - { - FreenectIrModeMap supported_modes = getSupportedVideoModes(); - OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; - std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); - OniSensorInfo sensors = { sensor_type, static_cast(supported_modes.size()), modes }; - return sensors; - } - // from StreamBase OniBool isPropertySupported(int propertyId) { diff --git a/src/openni2/VideoStream.hpp b/src/openni2/VideoStream.hpp index bb02d0767..06680e956 100644 --- a/src/openni2/VideoStream.hpp +++ b/src/openni2/VideoStream.hpp @@ -39,12 +39,10 @@ namespace Freenect2Driver { private: unsigned int frame_id; // number each frame - - virtual OniStatus setVideoMode(OniVideoMode requested_mode) = 0; virtual void populateFrame(libfreenect2::Frame* lf2Frame, int srcX, int srcY, OniFrame* oniFrame, int tgtX, int tgtY, int width, int height) const = 0; protected: - static const OniSensorType sensor_type; + virtual OniSensorType getSensorType() const = 0; libfreenect2::Freenect2Device* device; bool running; // buildFrame() does something iff true OniVideoMode video_mode; @@ -52,6 +50,19 @@ namespace Freenect2Driver bool mirroring; Freenect2Driver::Registration *reg; bool callPropertyChangedCallback; + typedef std::map< OniVideoMode, int > VideoModeMap; + virtual VideoModeMap getSupportedVideoModes() const = 0; + + OniStatus setVideoMode(OniVideoMode requested_mode) + { + VideoModeMap supported_video_modes = getSupportedVideoModes(); + VideoModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode); + if (matched_mode_iter == supported_video_modes.end()) + return ONI_STATUS_NOT_SUPPORTED; + + video_mode = requested_mode; + return ONI_STATUS_OK; + } static void copyFrame(float* srcPix, int srcX, int srcY, int srcStride, uint16_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring) { @@ -82,7 +93,8 @@ namespace Freenect2Driver device(device), reg(reg), callPropertyChangedCallback(false), - mirroring(false) + mirroring(false), + running(false) { // joy of structs memset(&cropping, 0, sizeof(cropping)); @@ -90,6 +102,15 @@ namespace Freenect2Driver } //~VideoStream() { stop(); } + OniSensorInfo getSensorInfo() + { + VideoModeMap supported_modes = getSupportedVideoModes(); + OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; + std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); + OniSensorInfo sensors = { getSensorType(), static_cast(supported_modes.size()), modes }; + return sensors; + } + void setPropertyChangedCallback(oni::driver::PropertyChangedCallback handler, void* pCookie) { callPropertyChangedCallback = true; StreamBase::setPropertyChangedCallback(handler, pCookie); From 93c6f20be44c8de6eed76a1bd8b772585f52ee80 Mon Sep 17 00:00:00 2001 From: Lingzhu Xiang Date: Sun, 3 Jan 2016 14:26:26 -0500 Subject: [PATCH 10/12] openni2: Add build instructions --- README.md | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 9ecb23249..9b2fa1458 100644 --- a/README.md +++ b/README.md @@ -40,6 +40,7 @@ Virtual machines likely do not work, because USB 3.0 isochronous transfer is qui The following minimum requirements must be met in order to enable the optional features: * OpenGL depth processing: OpenGL 3.1 (Windows, Linux, Mac OS X). No OpenGL ES support at the moment. * OpenCL depth processing: OpenCL 1.1 +* OpenNI2 driver: OpenNI2 2.2.0.33 ## FAQ @@ -165,6 +166,10 @@ Or `install_libusb_vs2015.cmd`. If you see some errors, you can always open the * Intel GPU: Download `intel_sdk_for_ocl_applications_2014_x64_setup.msi` from http://www.softpedia.com/get/Programming/SDK-DDK/Intel-SDK-for-OpenCL-Applications.shtml (SDK official download is replaced by $$$ and no longer available) and install it. Then verify `INTELOCLSDKROOT` is set as an environment variable. You may need to download and install additional OpenCL runtime. +#### OpenNI2 (optional) + +* Download OpenNI 2.2.0.33 (x64) from http://structure.io/openni, install it to default locations (`C:\Program Files...`). + #### Build ``` @@ -174,13 +179,15 @@ cmake --build . --config RelWithDebInfo --target install ``` Or `-G "Visual Studio 14 2015 Win64"`. Then you can run the program with `.\install\bin\Protonect.exe`, or start debugging in Visual Studio. `RelWithDebInfo` provides debug symbols with release performance. The default installation path is `install`, you may change it by editing `CMAKE_INSTALL_PREFIX`. +To try OpenNI2, copy freenect2-openni2.dll, and other dll files (libusb-1.0.dll, glfw.dll, etc.) in `install\bin` to `C:\Program Files\OpenNI2\Tools\OpenNI2\Drivers`. Then run `C:\Program Files\OpenNI\Tools\NiViewer.exe`. + ### Mac OSX Use your favorite package managers (brew, ports, etc.) to install most if not all dependencies: 1. ``cd`` into a directory where you want to keep libfreenect2 stuff in 1. Make sure these build tools are available: wget, git, cmake, pkg-config. Xcode may provide some of them. Install the rest via package managers. -1. Install dependencies: libusb, TurboJPEG, GLFW. +1. Install dependencies: libusb, TurboJPEG, GLFW, OpenNI2 (optional). ``` brew update @@ -189,6 +196,9 @@ brew tap homebrew/science brew install jpeg-turbo brew tap homebrew/versions brew install glfw3 +brew install openni2 +export OPENNI2_REDIST=/usr/local/lib/ni2 +export OPENNI2_INCLUDE=/usr/local/include/ni2 ``` It **is** now recommended to install libusb from package managers instead of building from source locally. Previously it was not recommended but that is no longer the case. @@ -209,6 +219,7 @@ mkdir build && cd build cmake .. make make install +make install-openni2 # may need sudo ``` 1. Run the program @@ -217,6 +228,8 @@ make install ./bin/Protonect ``` + Use `NiViewer` to test OpenNI2. + ### Debian/Ubuntu 14.04 1. Install libfreenect2 @@ -252,6 +265,8 @@ sudo dpkg -i libglfw3*_3.0.4-1_*.deb * Mali GPU (e.g. Odroid XU4): (with root) `mkdir -p /etc/OpenCL/vendors; echo /usr/lib/arm-linux-gnueabihf/mali-egl/libmali.so >/etc/OpenCL/vendors/mali.icd; apt-get install opencl-headers`. * Verify: You can install `clinfo` to verify if you have correctly set up the OpenCL stack. +1. OpenNI2 dependency (optional): `sudo apt-get install libopenni2-dev`. + 1. Build the actual protonect executable ``` @@ -260,6 +275,7 @@ mkdir build && cd build cmake .. make sudo make install # without sudo if cmake -DCMAKE_INSTALL_PREFIX=$HOME/... +sudo make install-openni2 # if OpenNI2 support is enabled ``` 1. Run the program @@ -268,6 +284,8 @@ sudo make install # without sudo if cmake -DCMAKE_INSTALL_PREFIX=$HOME/... ./bin/Protonect ``` +1. Run OpenNI2: `sudo apt-get install openni2-utils && NiViewer2`. + ### Other operating systems I'm not sure, but look for libusbx installation instructions for your OS. Figure out how to attach the driver to the Xbox NUI Sensor composite parent device, VID 045E PID 02C4, then contribute your procedure. From 41330567e252742e9ed996e75922f5cf191c5855 Mon Sep 17 00:00:00 2001 From: Lingzhu Xiang Date: Sat, 23 Jan 2016 17:52:54 -0500 Subject: [PATCH 11/12] openni2: Move method definitions out of headers --- src/openni2/ColorStream.cpp | 90 ++++++++++++ src/openni2/ColorStream.hpp | 90 +----------- src/openni2/DepthStream.cpp | 166 +++++++++++++++++++++ src/openni2/DepthStream.hpp | 167 +-------------------- src/openni2/IrStream.cpp | 41 ++++++ src/openni2/IrStream.hpp | 41 +----- src/openni2/Registration.cpp | 10 +- src/openni2/Registration.hpp | 6 +- src/openni2/Utility.cpp | 58 ++++++++ src/openni2/Utility.hpp | 38 +---- src/openni2/VideoStream.cpp | 273 +++++++++++++++++++++++++++++++++++ src/openni2/VideoStream.hpp | 224 ++-------------------------- 12 files changed, 670 insertions(+), 534 deletions(-) create mode 100644 src/openni2/Utility.cpp create mode 100644 src/openni2/VideoStream.cpp diff --git a/src/openni2/ColorStream.cpp b/src/openni2/ColorStream.cpp index bc6e8000e..6a7e00a2a 100644 --- a/src/openni2/ColorStream.cpp +++ b/src/openni2/ColorStream.cpp @@ -119,3 +119,93 @@ void ColorStream::copyFrame(uint8_t* srcPix, int srcX, int srcY, int srcStride, } } } + +OniSensorType ColorStream::getSensorType() const { return ONI_SENSOR_COLOR; } + +OniStatus ColorStream::setImageRegistrationMode(OniImageRegistrationMode mode) +{ + if (mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) { + // XXX, switch color resolution to 512x424 for registrarion here + OniVideoMode video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30); + setProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &video_mode, sizeof(video_mode)); + } + return ONI_STATUS_OK; +} + +// from StreamBase +OniBool ColorStream::isPropertySupported(int propertyId) +{ + switch(propertyId) + { + default: + return VideoStream::isPropertySupported(propertyId); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: + return true; + } +} + +OniStatus ColorStream::getProperty(int propertyId, void* data, int* pDataSize) +{ + switch (propertyId) + { + default: + return VideoStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) + { + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = HORIZONTAL_FOV; + return ONI_STATUS_OK; + } + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) + { + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = VERTICAL_FOV; + return ONI_STATUS_OK; + } + + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + { + if (*pDataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = auto_white_balance; + return ONI_STATUS_OK; + } + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + { + if (*pDataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_EXPOSURE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = auto_exposure; + return ONI_STATUS_OK; + } + } +} + +OniStatus ColorStream::setProperty(int propertyId, const void* data, int dataSize) +{ + switch (propertyId) + { + case 0: + default: + return VideoStream::setProperty(propertyId, data, dataSize); + } +} diff --git a/src/openni2/ColorStream.hpp b/src/openni2/ColorStream.hpp index e614a218b..f163f1e10 100644 --- a/src/openni2/ColorStream.hpp +++ b/src/openni2/ColorStream.hpp @@ -46,7 +46,7 @@ namespace Freenect2Driver private: typedef std::map< OniVideoMode, int > FreenectVideoModeMap; - OniSensorType getSensorType() const { return ONI_SENSOR_COLOR; } + OniSensorType getSensorType() const; VideoModeMap getSupportedVideoModes() const; void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; @@ -59,91 +59,11 @@ namespace Freenect2Driver ColorStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); //~ColorStream() { } - OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) - { - if (mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) { - // XXX, switch color resolution to 512x424 for registrarion here - OniVideoMode video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30); - setProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &video_mode, sizeof(video_mode)); - } - return ONI_STATUS_OK; - } + OniStatus setImageRegistrationMode(OniImageRegistrationMode mode); // from StreamBase - OniBool isPropertySupported(int propertyId) - { - switch(propertyId) - { - default: - return VideoStream::isPropertySupported(propertyId); - - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: - case ONI_STREAM_PROPERTY_VERTICAL_FOV: - case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: - case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: - return true; - } - } - - OniStatus getProperty(int propertyId, void* data, int* pDataSize) - { - switch (propertyId) - { - default: - return VideoStream::getProperty(propertyId, data, pDataSize); - - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) - { - if (*pDataSize != sizeof(float)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = HORIZONTAL_FOV; - return ONI_STATUS_OK; - } - case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) - { - if (*pDataSize != sizeof(float)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = VERTICAL_FOV; - return ONI_STATUS_OK; - } - - // camera - case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool - { - if (*pDataSize != sizeof(OniBool)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = auto_white_balance; - return ONI_STATUS_OK; - } - case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool - { - if (*pDataSize != sizeof(OniBool)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_EXPOSURE"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = auto_exposure; - return ONI_STATUS_OK; - } - } - } - - OniStatus setProperty(int propertyId, const void* data, int dataSize) - { - switch (propertyId) - { - default: - return VideoStream::setProperty(propertyId, data, dataSize); - } - } + OniBool isPropertySupported(int propertyId); + OniStatus getProperty(int propertyId, void* data, int* pDataSize); + OniStatus setProperty(int propertyId, const void* data, int dataSize); }; } diff --git a/src/openni2/DepthStream.cpp b/src/openni2/DepthStream.cpp index edbe5dc2c..429417080 100644 --- a/src/openni2/DepthStream.cpp +++ b/src/openni2/DepthStream.cpp @@ -82,3 +82,169 @@ void DepthStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int src static_cast(dstFrame->data), dstX, dstY, dstFrame->width, width, height, mirroring); } + +OniSensorType DepthStream::getSensorType() const { return ONI_SENSOR_DEPTH; } + +OniImageRegistrationMode DepthStream::getImageRegistrationMode() const { return image_registration_mode; } + +OniStatus DepthStream::setImageRegistrationMode(OniImageRegistrationMode mode) +{ + if (!isImageRegistrationModeSupported(mode)) + return ONI_STATUS_NOT_SUPPORTED; + image_registration_mode = mode; + reg->setEnable(image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); + return setVideoMode(video_mode); +} + +// from StreamBase +OniBool DepthStream::isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF || mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); } + +OniBool DepthStream::isPropertySupported(int propertyId) +{ + switch(propertyId) + { + default: + return VideoStream::isPropertySupported(propertyId); + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + case ONI_STREAM_PROPERTY_MAX_VALUE: + case XN_STREAM_PROPERTY_GAIN: + case XN_STREAM_PROPERTY_CONST_SHIFT: + case XN_STREAM_PROPERTY_MAX_SHIFT: + case XN_STREAM_PROPERTY_PARAM_COEFF: + case XN_STREAM_PROPERTY_SHIFT_SCALE: + case XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE: + case XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE: + case XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE: + case XN_STREAM_PROPERTY_S2D_TABLE: + case XN_STREAM_PROPERTY_D2S_TABLE: + return true; + } +} + +OniStatus DepthStream::getProperty(int propertyId, void* data, int* pDataSize) +{ + switch (propertyId) + { + default: + return VideoStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = HORIZONTAL_FOV; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = VERTICAL_FOV; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + if (*pDataSize != sizeof(int)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MAX_VALUE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = MAX_VALUE; + return ONI_STATUS_OK; + + case XN_STREAM_PROPERTY_PIXEL_REGISTRATION: // XnPixelRegistration (get only) + case XN_STREAM_PROPERTY_WHITE_BALANCE_ENABLED: // unsigned long long + case XN_STREAM_PROPERTY_HOLE_FILTER: // unsigned long long + case XN_STREAM_PROPERTY_REGISTRATION_TYPE: // XnProcessingType + case XN_STREAM_PROPERTY_AGC_BIN: // XnDepthAGCBin* + case XN_STREAM_PROPERTY_PIXEL_SIZE_FACTOR: // unsigned long long + case XN_STREAM_PROPERTY_DCMOS_RCMOS_DISTANCE: // double + case XN_STREAM_PROPERTY_CLOSE_RANGE: // unsigned long long + return ONI_STATUS_NOT_SUPPORTED; + + case XN_STREAM_PROPERTY_GAIN: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_GAIN"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = GAIN_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_CONST_SHIFT: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_CONST_SHIFT"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = CONST_SHIFT_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_MAX_SHIFT: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_MAX_SHIFT"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = MAX_SHIFT_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_PARAM_COEFF: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_PARAM_COEFF"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = PARAM_COEFF_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_SHIFT_SCALE: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_SHIFT_SCALE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = SHIFT_SCALE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE: // unsigned long long + if (*pDataSize != sizeof(unsigned long long)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = ZERO_PLANE_DISTANCE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE: // double + if (*pDataSize != sizeof(double)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = ZERO_PLANE_PIXEL_SIZE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE: // double + if (*pDataSize != sizeof(double)) + { + LogError("Unexpected size for XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = EMITTER_DCMOS_DISTANCE_VAL; + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_S2D_TABLE: // OniDepthPixel[] + { + uint16_t *s2d = (uint16_t *)data; + *pDataSize = sizeof(*s2d) * 2048; + memset(data, 0, *pDataSize); + for (int i = 1; i <= 1052; i++) + s2d[i] = 342205.0/(1086.671 - i); + } + return ONI_STATUS_OK; + case XN_STREAM_PROPERTY_D2S_TABLE: // unsigned short[] + { + uint16_t *d2s = (uint16_t *)data; + *pDataSize = sizeof(*d2s) * 10001; + memset(data, 0, *pDataSize); + for (int i = 315; i <= 10000; i++) + d2s[i] = 1086.671 - 342205.0/(i + 1); + } + return ONI_STATUS_OK; + } +} diff --git a/src/openni2/DepthStream.hpp b/src/openni2/DepthStream.hpp index b14e7e369..349e3d22f 100644 --- a/src/openni2/DepthStream.hpp +++ b/src/openni2/DepthStream.hpp @@ -56,7 +56,7 @@ namespace Freenect2Driver static const double EMITTER_DCMOS_DISTANCE_VAL; private: - OniSensorType getSensorType() const { return ONI_SENSOR_DEPTH; } + OniSensorType getSensorType() const; OniImageRegistrationMode image_registration_mode; VideoModeMap getSupportedVideoModes() const; void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; @@ -65,167 +65,12 @@ namespace Freenect2Driver DepthStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg); //~DepthStream() { } - OniImageRegistrationMode getImageRegistrationMode() const { return image_registration_mode; } - OniStatus setImageRegistrationMode(OniImageRegistrationMode mode) - { - if (!isImageRegistrationModeSupported(mode)) - return ONI_STATUS_NOT_SUPPORTED; - image_registration_mode = mode; - reg->setEnable(image_registration_mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); - return setVideoMode(video_mode); - } + OniImageRegistrationMode getImageRegistrationMode() const; + OniStatus setImageRegistrationMode(OniImageRegistrationMode mode); // from StreamBase - OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode) { return (mode == ONI_IMAGE_REGISTRATION_OFF || mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR); } - - OniBool isPropertySupported(int propertyId) - { - switch(propertyId) - { - default: - return VideoStream::isPropertySupported(propertyId); - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: - case ONI_STREAM_PROPERTY_VERTICAL_FOV: - case ONI_STREAM_PROPERTY_MAX_VALUE: - case XN_STREAM_PROPERTY_GAIN: - case XN_STREAM_PROPERTY_CONST_SHIFT: - case XN_STREAM_PROPERTY_MAX_SHIFT: - case XN_STREAM_PROPERTY_PARAM_COEFF: - case XN_STREAM_PROPERTY_SHIFT_SCALE: - case XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE: - case XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE: - case XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE: - case XN_STREAM_PROPERTY_S2D_TABLE: - case XN_STREAM_PROPERTY_D2S_TABLE: - return true; - } - } - - OniStatus getProperty(int propertyId, void* data, int* pDataSize) - { - switch (propertyId) - { - default: - return VideoStream::getProperty(propertyId, data, pDataSize); - - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) - if (*pDataSize != sizeof(float)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = HORIZONTAL_FOV; - return ONI_STATUS_OK; - case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) - if (*pDataSize != sizeof(float)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = VERTICAL_FOV; - return ONI_STATUS_OK; - case ONI_STREAM_PROPERTY_MAX_VALUE: // int - if (*pDataSize != sizeof(int)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_MAX_VALUE"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = MAX_VALUE; - return ONI_STATUS_OK; - - case XN_STREAM_PROPERTY_PIXEL_REGISTRATION: // XnPixelRegistration (get only) - case XN_STREAM_PROPERTY_WHITE_BALANCE_ENABLED: // unsigned long long - case XN_STREAM_PROPERTY_HOLE_FILTER: // unsigned long long - case XN_STREAM_PROPERTY_REGISTRATION_TYPE: // XnProcessingType - case XN_STREAM_PROPERTY_AGC_BIN: // XnDepthAGCBin* - case XN_STREAM_PROPERTY_PIXEL_SIZE_FACTOR: // unsigned long long - case XN_STREAM_PROPERTY_DCMOS_RCMOS_DISTANCE: // double - case XN_STREAM_PROPERTY_CLOSE_RANGE: // unsigned long long - return ONI_STATUS_NOT_SUPPORTED; - - case XN_STREAM_PROPERTY_GAIN: // unsigned long long - if (*pDataSize != sizeof(unsigned long long)) - { - LogError("Unexpected size for XN_STREAM_PROPERTY_GAIN"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = GAIN_VAL; - return ONI_STATUS_OK; - case XN_STREAM_PROPERTY_CONST_SHIFT: // unsigned long long - if (*pDataSize != sizeof(unsigned long long)) - { - LogError("Unexpected size for XN_STREAM_PROPERTY_CONST_SHIFT"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = CONST_SHIFT_VAL; - return ONI_STATUS_OK; - case XN_STREAM_PROPERTY_MAX_SHIFT: // unsigned long long - if (*pDataSize != sizeof(unsigned long long)) - { - LogError("Unexpected size for XN_STREAM_PROPERTY_MAX_SHIFT"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = MAX_SHIFT_VAL; - return ONI_STATUS_OK; - case XN_STREAM_PROPERTY_PARAM_COEFF: // unsigned long long - if (*pDataSize != sizeof(unsigned long long)) - { - LogError("Unexpected size for XN_STREAM_PROPERTY_PARAM_COEFF"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = PARAM_COEFF_VAL; - return ONI_STATUS_OK; - case XN_STREAM_PROPERTY_SHIFT_SCALE: // unsigned long long - if (*pDataSize != sizeof(unsigned long long)) - { - LogError("Unexpected size for XN_STREAM_PROPERTY_SHIFT_SCALE"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = SHIFT_SCALE_VAL; - return ONI_STATUS_OK; - case XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE: // unsigned long long - if (*pDataSize != sizeof(unsigned long long)) - { - LogError("Unexpected size for XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = ZERO_PLANE_DISTANCE_VAL; - return ONI_STATUS_OK; - case XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE: // double - if (*pDataSize != sizeof(double)) - { - LogError("Unexpected size for XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = ZERO_PLANE_PIXEL_SIZE_VAL; - return ONI_STATUS_OK; - case XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE: // double - if (*pDataSize != sizeof(double)) - { - LogError("Unexpected size for XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = EMITTER_DCMOS_DISTANCE_VAL; - return ONI_STATUS_OK; - case XN_STREAM_PROPERTY_S2D_TABLE: // OniDepthPixel[] - { - uint16_t *s2d = (uint16_t *)data; - *pDataSize = sizeof(*s2d) * 2048; - memset(data, 0, *pDataSize); - for (int i = 1; i <= 1052; i++) - s2d[i] = 342205.0/(1086.671 - i); - } - return ONI_STATUS_OK; - case XN_STREAM_PROPERTY_D2S_TABLE: // unsigned short[] - { - uint16_t *d2s = (uint16_t *)data; - *pDataSize = sizeof(*d2s) * 10001; - memset(data, 0, *pDataSize); - for (int i = 315; i <= 10000; i++) - d2s[i] = 1086.671 - 342205.0/(i + 1); - } - return ONI_STATUS_OK; - } - } + OniBool isImageRegistrationModeSupported(OniImageRegistrationMode mode); + OniBool isPropertySupported(int propertyId); + OniStatus getProperty(int propertyId, void* data, int* pDataSize); }; } diff --git a/src/openni2/IrStream.cpp b/src/openni2/IrStream.cpp index 2427c3d1d..6983f935d 100644 --- a/src/openni2/IrStream.cpp +++ b/src/openni2/IrStream.cpp @@ -59,3 +59,44 @@ void IrStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, static_cast(dstFrame->data), dstX, dstY, dstFrame->width, width, height, mirroring); } + +OniSensorType IrStream::getSensorType() const { return ONI_SENSOR_IR; } + +// from StreamBase +OniBool IrStream::isPropertySupported(int propertyId) +{ + switch(propertyId) + { + default: + return VideoStream::isPropertySupported(propertyId); + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: + case ONI_STREAM_PROPERTY_VERTICAL_FOV: + return true; + } +} + +OniStatus IrStream::getProperty(int propertyId, void* data, int* pDataSize) +{ + switch (propertyId) + { + default: + return VideoStream::getProperty(propertyId, data, pDataSize); + + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = HORIZONTAL_FOV; + return ONI_STATUS_OK; + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) + if (*pDataSize != sizeof(float)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = VERTICAL_FOV; + return ONI_STATUS_OK; + } +} diff --git a/src/openni2/IrStream.hpp b/src/openni2/IrStream.hpp index cdd5b3b0c..ca5ee2d9c 100644 --- a/src/openni2/IrStream.hpp +++ b/src/openni2/IrStream.hpp @@ -44,7 +44,7 @@ namespace Freenect2Driver static const float VERTICAL_FOV; private: - OniSensorType getSensorType() const { return ONI_SENSOR_IR; } + OniSensorType getSensorType() const; VideoModeMap getSupportedVideoModes() const; void populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const; @@ -53,42 +53,7 @@ namespace Freenect2Driver //~IrStream() { } // from StreamBase - OniBool isPropertySupported(int propertyId) - { - switch(propertyId) - { - default: - return VideoStream::isPropertySupported(propertyId); - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: - case ONI_STREAM_PROPERTY_VERTICAL_FOV: - return true; - } - } - - OniStatus getProperty(int propertyId, void* data, int* pDataSize) - { - switch (propertyId) - { - default: - return VideoStream::getProperty(propertyId, data, pDataSize); - - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) - if (*pDataSize != sizeof(float)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = HORIZONTAL_FOV; - return ONI_STATUS_OK; - case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) - if (*pDataSize != sizeof(float)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = VERTICAL_FOV; - return ONI_STATUS_OK; - } - } + OniBool isPropertySupported(int propertyId); + OniStatus getProperty(int propertyId, void* data, int* pDataSize); }; } diff --git a/src/openni2/Registration.cpp b/src/openni2/Registration.cpp index c3d6fac9c..c1b627a0d 100644 --- a/src/openni2/Registration.cpp +++ b/src/openni2/Registration.cpp @@ -35,7 +35,7 @@ Registration::Registration(libfreenect2::Freenect2Device* dev) : reg(NULL), enabled(false) { - } +} Registration::~Registration() { delete reg; @@ -43,7 +43,7 @@ Registration::~Registration() { void Registration::depthFrame(libfreenect2::Frame* frame) { lastDepthFrame = frame; - } +} void Registration::colorFrameRGB888(libfreenect2::Frame* colorFrame, libfreenect2::Frame* registeredFrame) { @@ -66,4 +66,8 @@ void Registration::colorFrameRGB888(libfreenect2::Frame* colorFrame, libfreenect libfreenect2::Frame undistorted(lastDepthFrame->width, lastDepthFrame->height, lastDepthFrame->bytes_per_pixel); reg->apply(colorFrame, lastDepthFrame, &undistorted, registeredFrame); - } +} + +void Registration::setEnable(bool enable) { enabled = enable; } + +bool Registration::isEnabled() { return enabled; } diff --git a/src/openni2/Registration.hpp b/src/openni2/Registration.hpp index 8aa4a5356..e14fd8e05 100644 --- a/src/openni2/Registration.hpp +++ b/src/openni2/Registration.hpp @@ -24,6 +24,8 @@ * either License. */ +#pragma once + #include namespace Freenect2Driver { @@ -40,7 +42,7 @@ namespace Freenect2Driver { void depthFrame(libfreenect2::Frame* frame); void colorFrameRGB888(libfreenect2::Frame* srcFrame, libfreenect2::Frame* dstFrame); - void setEnable(bool enable = true) { enabled = enable; } - bool isEnabled() { return enabled; } + void setEnable(bool enable = true); + bool isEnabled(); }; } diff --git a/src/openni2/Utility.cpp b/src/openni2/Utility.cpp new file mode 100644 index 000000000..7f85ee1b6 --- /dev/null +++ b/src/openni2/Utility.cpp @@ -0,0 +1,58 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. + * See the CONTRIB file for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + +// This file contains symbols that may be used by any class or don't really go anywhere else. +#include +#include "Driver/OniDriverAPI.h" + + +// Oni helpers + +OniVideoMode makeOniVideoMode(OniPixelFormat pixel_format, int resolution_x, int resolution_y, int frames_per_second) +{ + OniVideoMode mode; + mode.pixelFormat = pixel_format; + mode.resolutionX = resolution_x; + mode.resolutionY = resolution_y; + mode.fps = frames_per_second; + return mode; +} + +bool operator==(const OniVideoMode& left, const OniVideoMode& right) +{ + return (left.pixelFormat == right.pixelFormat && left.resolutionX == right.resolutionX + && left.resolutionY == right.resolutionY && left.fps == right.fps); +} + +bool operator<(const OniVideoMode& left, const OniVideoMode& right) +{ + return (left.resolutionX * left.resolutionY < right.resolutionX * right.resolutionY); +} + +bool operator<(const OniDeviceInfo& left, const OniDeviceInfo& right) +{ + return (strcmp(left.uri, right.uri) < 0); +} diff --git a/src/openni2/Utility.hpp b/src/openni2/Utility.hpp index f8b84a76e..bdc1fc661 100644 --- a/src/openni2/Utility.hpp +++ b/src/openni2/Utility.hpp @@ -29,45 +29,17 @@ #include #include "Driver/OniDriverAPI.h" - +#include // Oni helpers -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); -} +OniVideoMode makeOniVideoMode(OniPixelFormat pixel_format, int resolution_x, int resolution_y, int frames_per_second); -static bool operator<(const OniDeviceInfo& left, const OniDeviceInfo& right) -{ - return (strcmp(left.uri, right.uri) < 0); -} +bool operator==(const OniVideoMode& left, const OniVideoMode& right); +bool operator<(const OniVideoMode& left, const OniVideoMode& right); -/// Extracts `first` from `pair`, for transforming a map into its keys. -struct ExtractKey -{ - template typename T::first_type - operator()(T pair) const - { - return pair.first; - } -}; - +bool operator<(const OniDeviceInfo& left, const OniDeviceInfo& right); // holding out on C++11 template diff --git a/src/openni2/VideoStream.cpp b/src/openni2/VideoStream.cpp new file mode 100644 index 000000000..edae8e72b --- /dev/null +++ b/src/openni2/VideoStream.cpp @@ -0,0 +1,273 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. + * See the CONTRIB file for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + +#include +#include +#include "PS1080.h" +#include "VideoStream.hpp" +#include "Utility.hpp" +#include "Registration.hpp" + +struct ExtractKey +{ + template typename T::first_type + operator()(T pair) const + { + return pair.first; + } +}; + +namespace Freenect2Driver +{ +OniStatus VideoStream::setVideoMode(OniVideoMode requested_mode) +{ + VideoModeMap supported_video_modes = getSupportedVideoModes(); + VideoModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode); + if (matched_mode_iter == supported_video_modes.end()) + return ONI_STATUS_NOT_SUPPORTED; + + video_mode = requested_mode; + return ONI_STATUS_OK; +} + +void VideoStream::copyFrame(float* srcPix, int srcX, int srcY, int srcStride, uint16_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring) +{ + srcPix += srcX + srcY * srcStride; + dstPix += dstX + dstY * dstStride; + + for (int y = 0; y < height; y++) { + uint16_t* dst = dstPix + y * dstStride; + float* src = srcPix + y * srcStride; + if (mirroring) { + dst += width; + for (int x = 0; x < width; x++) + *dst-- = *src++; + } else { + for (int x = 0; x < width; x++) + *dst++ = *src++; + } + } +} +void VideoStream::raisePropertyChanged(int propertyId, const void* data, int dataSize) { + if (callPropertyChangedCallback) + StreamBase::raisePropertyChanged(propertyId, data, dataSize); +} + +VideoStream::VideoStream(libfreenect2::Freenect2Device* device, Freenect2Driver::Registration *reg) : + frame_id(1), + device(device), + running(false), + mirroring(false), + reg(reg), + callPropertyChangedCallback(false) + { + // joy of structs + memset(&cropping, 0, sizeof(cropping)); + memset(&video_mode, 0, sizeof(video_mode)); + } +//~VideoStream() { stop(); } + + +OniSensorInfo VideoStream::getSensorInfo() +{ + VideoModeMap supported_modes = getSupportedVideoModes(); + OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; + std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); + OniSensorInfo sensors = { getSensorType(), static_cast(supported_modes.size()), modes }; + return sensors; +} + +void VideoStream::setPropertyChangedCallback(oni::driver::PropertyChangedCallback handler, void* pCookie) { + callPropertyChangedCallback = true; + StreamBase::setPropertyChangedCallback(handler, pCookie); +} + +bool VideoStream::buildFrame(libfreenect2::Frame* lf2Frame) +{ + if (!running) + return false; + + OniFrame* oniFrame = getServices().acquireFrame(); + oniFrame->frameIndex = frame_id++; + oniFrame->timestamp = lf2Frame->timestamp; + oniFrame->videoMode = video_mode; + oniFrame->width = video_mode.resolutionX; + oniFrame->height = video_mode.resolutionY; + + if (cropping.enabled) + { + oniFrame->height = cropping.height; + oniFrame->width = cropping.width; + oniFrame->cropOriginX = cropping.originX; + oniFrame->cropOriginY = cropping.originY; + oniFrame->croppingEnabled = true; + } + else + { + oniFrame->cropOriginX = 0; + oniFrame->cropOriginY = 0; + oniFrame->croppingEnabled = false; + } + //min is a macro with MSVC + #undef min + int width = std::min(oniFrame->width, (int)lf2Frame->width); + int height = std::min(oniFrame->height, (int)lf2Frame->height); + + populateFrame(lf2Frame, oniFrame->cropOriginX, oniFrame->cropOriginY, oniFrame, 0, 0, width, height); + raiseNewFrame(oniFrame); + getServices().releaseFrame(oniFrame); + + return false; +} + +// from StreamBase + +OniStatus VideoStream::start() +{ + running = true; + return ONI_STATUS_OK; +} +void VideoStream::stop() { running = false; } + +// only add to property handlers if the property is generic to all children +// otherwise, implement in child and call these in default case +OniBool VideoStream::isPropertySupported(int propertyId) +{ + switch(propertyId) + { + case ONI_STREAM_PROPERTY_VIDEO_MODE: + case ONI_STREAM_PROPERTY_CROPPING: + case ONI_STREAM_PROPERTY_MIRRORING: + return true; + default: + return false; + } +} + +OniStatus VideoStream::getProperty(int propertyId, void* data, int* pDataSize) +{ + switch (propertyId) + { + default: + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + case ONI_STREAM_PROPERTY_MIN_VALUE: // int + case ONI_STREAM_PROPERTY_STRIDE: // int + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + // xn + case XN_STREAM_PROPERTY_INPUT_FORMAT: // unsigned long long + case XN_STREAM_PROPERTY_CROPPING_MODE: // XnCroppingMode + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (*pDataSize != sizeof(OniVideoMode)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VIDEO_MODE"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = video_mode; + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + if (*pDataSize != sizeof(OniCropping)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_CROPPING"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = cropping; + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (*pDataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); + return ONI_STATUS_ERROR; + } + *(static_cast(data)) = mirroring; + return ONI_STATUS_OK; + } +} +OniStatus VideoStream::setProperty(int propertyId, const void* data, int dataSize) +{ + switch (propertyId) + { + default: + case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians + case ONI_STREAM_PROPERTY_MAX_VALUE: // int + case ONI_STREAM_PROPERTY_MIN_VALUE: // int + case ONI_STREAM_PROPERTY_STRIDE: // int + case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int + // camera + case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool + case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool + // xn + case XN_STREAM_PROPERTY_INPUT_FORMAT: // unsigned long long + case XN_STREAM_PROPERTY_CROPPING_MODE: // XnCroppingMode + return ONI_STATUS_NOT_SUPPORTED; + + case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* + if (dataSize != sizeof(OniVideoMode)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_VIDEO_MODE"); + return ONI_STATUS_ERROR; + } + if (ONI_STATUS_OK != setVideoMode(*(static_cast(data)))) + return ONI_STATUS_NOT_SUPPORTED; + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* + if (dataSize != sizeof(OniCropping)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_CROPPING"); + return ONI_STATUS_ERROR; + } + cropping = *(static_cast(data)); + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + + case ONI_STREAM_PROPERTY_MIRRORING: // OniBool + if (dataSize != sizeof(OniBool)) + { + LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); + return ONI_STATUS_ERROR; + } + mirroring = *(static_cast(data)); + raisePropertyChanged(propertyId, data, dataSize); + return ONI_STATUS_OK; + } +} + + +/* todo : from StreamBase +virtual OniStatus convertDepthToColorCoordinates(StreamBase* colorStream, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY) { return ONI_STATUS_NOT_SUPPORTED; } +*/ +} diff --git a/src/openni2/VideoStream.hpp b/src/openni2/VideoStream.hpp index 06680e956..ad5f04ba0 100644 --- a/src/openni2/VideoStream.hpp +++ b/src/openni2/VideoStream.hpp @@ -26,7 +26,6 @@ #pragma once -#include #include #include #include "PS1080.h" @@ -53,228 +52,29 @@ namespace Freenect2Driver typedef std::map< OniVideoMode, int > VideoModeMap; virtual VideoModeMap getSupportedVideoModes() const = 0; - OniStatus setVideoMode(OniVideoMode requested_mode) - { - VideoModeMap supported_video_modes = getSupportedVideoModes(); - VideoModeMap::const_iterator matched_mode_iter = supported_video_modes.find(requested_mode); - if (matched_mode_iter == supported_video_modes.end()) - return ONI_STATUS_NOT_SUPPORTED; + OniStatus setVideoMode(OniVideoMode requested_mode); - video_mode = requested_mode; - return ONI_STATUS_OK; - } - - static void copyFrame(float* srcPix, int srcX, int srcY, int srcStride, uint16_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring) - { - srcPix += srcX + srcY * srcStride; - dstPix += dstX + dstY * dstStride; - - for (int y = 0; y < height; y++) { - uint16_t* dst = dstPix + y * dstStride; - float* src = srcPix + y * srcStride; - if (mirroring) { - dst += width; - for (int x = 0; x < width; x++) - *dst-- = *src++; - } else { - for (int x = 0; x < width; x++) - *dst++ = *src++; - } - } - } - void raisePropertyChanged(int propertyId, const void* data, int dataSize) { - if (callPropertyChangedCallback) - StreamBase::raisePropertyChanged(propertyId, data, dataSize); - } + static void copyFrame(float* srcPix, int srcX, int srcY, int srcStride, uint16_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring); + void raisePropertyChanged(int propertyId, const void* data, int dataSize); public: - VideoStream(libfreenect2::Freenect2Device* device, Freenect2Driver::Registration *reg) : - frame_id(1), - device(device), - reg(reg), - callPropertyChangedCallback(false), - mirroring(false), - running(false) - { - // joy of structs - memset(&cropping, 0, sizeof(cropping)); - memset(&video_mode, 0, sizeof(video_mode)); - } - //~VideoStream() { stop(); } - - OniSensorInfo getSensorInfo() - { - VideoModeMap supported_modes = getSupportedVideoModes(); - OniVideoMode* modes = new OniVideoMode[supported_modes.size()]; - std::transform(supported_modes.begin(), supported_modes.end(), modes, ExtractKey()); - OniSensorInfo sensors = { getSensorType(), static_cast(supported_modes.size()), modes }; - return sensors; - } - - void setPropertyChangedCallback(oni::driver::PropertyChangedCallback handler, void* pCookie) { - callPropertyChangedCallback = true; - StreamBase::setPropertyChangedCallback(handler, pCookie); - } - - bool buildFrame(libfreenect2::Frame* lf2Frame) - { - if (!running) - return false; - - OniFrame* oniFrame = getServices().acquireFrame(); - oniFrame->frameIndex = frame_id++; - oniFrame->timestamp = lf2Frame->timestamp; - oniFrame->videoMode = video_mode; - oniFrame->width = video_mode.resolutionX; - oniFrame->height = video_mode.resolutionY; - - if (cropping.enabled) - { - oniFrame->height = cropping.height; - oniFrame->width = cropping.width; - oniFrame->cropOriginX = cropping.originX; - oniFrame->cropOriginY = cropping.originY; - oniFrame->croppingEnabled = true; - } - else - { - oniFrame->cropOriginX = 0; - oniFrame->cropOriginY = 0; - oniFrame->croppingEnabled = false; - } - int width = std::min(oniFrame->width, (int)lf2Frame->width); - int height = std::min(oniFrame->height, (int)lf2Frame->height); + VideoStream(libfreenect2::Freenect2Device* device, Freenect2Driver::Registration *reg); - populateFrame(lf2Frame, oniFrame->cropOriginX, oniFrame->cropOriginY, oniFrame, 0, 0, width, height); - raiseNewFrame(oniFrame); - getServices().releaseFrame(oniFrame); + OniSensorInfo getSensorInfo(); - return false; - } + void setPropertyChangedCallback(oni::driver::PropertyChangedCallback handler, void* pCookie); - // from StreamBase + bool buildFrame(libfreenect2::Frame* lf2Frame); - OniStatus start() - { - running = true; - return ONI_STATUS_OK; - } - void stop() { running = false; } + OniStatus start(); + void stop(); // only add to property handlers if the property is generic to all children // otherwise, implement in child and call these in default case - OniBool isPropertySupported(int propertyId) - { - switch(propertyId) - { - case ONI_STREAM_PROPERTY_VIDEO_MODE: - case ONI_STREAM_PROPERTY_CROPPING: - case ONI_STREAM_PROPERTY_MIRRORING: - return true; - default: - return false; - } - } - - virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize) - { - switch (propertyId) - { - default: - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians - case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians - case ONI_STREAM_PROPERTY_MAX_VALUE: // int - case ONI_STREAM_PROPERTY_MIN_VALUE: // int - case ONI_STREAM_PROPERTY_STRIDE: // int - case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int - // camera - case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool - case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool - // xn - case XN_STREAM_PROPERTY_INPUT_FORMAT: // unsigned long long - case XN_STREAM_PROPERTY_CROPPING_MODE: // XnCroppingMode - return ONI_STATUS_NOT_SUPPORTED; - - case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* - if (*pDataSize != sizeof(OniVideoMode)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_VIDEO_MODE"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = video_mode; - return ONI_STATUS_OK; - - case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* - if (*pDataSize != sizeof(OniCropping)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_CROPPING"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = cropping; - return ONI_STATUS_OK; - - case ONI_STREAM_PROPERTY_MIRRORING: // OniBool - if (*pDataSize != sizeof(OniBool)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); - return ONI_STATUS_ERROR; - } - *(static_cast(data)) = mirroring; - return ONI_STATUS_OK; - } - } - virtual OniStatus setProperty(int propertyId, const void* data, int dataSize) - { - switch (propertyId) - { - default: - case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float: radians - case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float: radians - case ONI_STREAM_PROPERTY_MAX_VALUE: // int - case ONI_STREAM_PROPERTY_MIN_VALUE: // int - case ONI_STREAM_PROPERTY_STRIDE: // int - case ONI_STREAM_PROPERTY_NUMBER_OF_FRAMES: // int - // camera - case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool - case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool - // xn - case XN_STREAM_PROPERTY_INPUT_FORMAT: // unsigned long long - case XN_STREAM_PROPERTY_CROPPING_MODE: // XnCroppingMode - return ONI_STATUS_NOT_SUPPORTED; - - case ONI_STREAM_PROPERTY_VIDEO_MODE: // OniVideoMode* - if (dataSize != sizeof(OniVideoMode)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_VIDEO_MODE"); - return ONI_STATUS_ERROR; - } - if (ONI_STATUS_OK != setVideoMode(*(static_cast(data)))) - return ONI_STATUS_NOT_SUPPORTED; - raisePropertyChanged(propertyId, data, dataSize); - return ONI_STATUS_OK; - - case ONI_STREAM_PROPERTY_CROPPING: // OniCropping* - if (dataSize != sizeof(OniCropping)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_CROPPING"); - return ONI_STATUS_ERROR; - } - cropping = *(static_cast(data)); - raisePropertyChanged(propertyId, data, dataSize); - return ONI_STATUS_OK; - - case ONI_STREAM_PROPERTY_MIRRORING: // OniBool - if (dataSize != sizeof(OniBool)) - { - LogError("Unexpected size for ONI_STREAM_PROPERTY_MIRRORING"); - return ONI_STATUS_ERROR; - } - mirroring = *(static_cast(data)); - raisePropertyChanged(propertyId, data, dataSize); - return ONI_STATUS_OK; - } - } + OniBool isPropertySupported(int propertyId); + virtual OniStatus getProperty(int propertyId, void* data, int* pDataSize); + virtual OniStatus setProperty(int propertyId, const void* data, int dataSize); /* todo : from StreamBase virtual OniStatus convertDepthToColorCoordinates(StreamBase* colorStream, int depthX, int depthY, OniDepthPixel depthZ, int* pColorX, int* pColorY) { return ONI_STATUS_NOT_SUPPORTED; } From 98aaf466ed090eca393bb98b176f567b8726c20a Mon Sep 17 00:00:00 2001 From: Lingzhu Xiang Date: Sat, 23 Jan 2016 18:08:18 -0500 Subject: [PATCH 12/12] openni2: Fix compiler warnings and extra headers --- src/openni2/ColorStream.cpp | 3 ++- src/openni2/ColorStream.hpp | 2 -- src/openni2/DepthStream.cpp | 5 +++-- src/openni2/DepthStream.hpp | 4 ---- src/openni2/DeviceDriver.cpp | 16 ++++++++-------- src/openni2/IrStream.cpp | 3 ++- src/openni2/IrStream.hpp | 4 ---- src/openni2/Registration.cpp | 2 +- src/openni2/Utility.cpp | 2 +- src/openni2/Utility.hpp | 3 +-- src/openni2/VideoStream.hpp | 2 +- 11 files changed, 19 insertions(+), 27 deletions(-) diff --git a/src/openni2/ColorStream.cpp b/src/openni2/ColorStream.cpp index 6a7e00a2a..9355bd767 100644 --- a/src/openni2/ColorStream.cpp +++ b/src/openni2/ColorStream.cpp @@ -24,7 +24,8 @@ * either License. */ -#include +#define _USE_MATH_DEFINES +#include // for M_PI #include "ColorStream.hpp" using namespace Freenect2Driver; diff --git a/src/openni2/ColorStream.hpp b/src/openni2/ColorStream.hpp index f163f1e10..62d790fe9 100644 --- a/src/openni2/ColorStream.hpp +++ b/src/openni2/ColorStream.hpp @@ -26,8 +26,6 @@ #pragma once -#include // for transform() -#include // for M_PI #include #include #include diff --git a/src/openni2/DepthStream.cpp b/src/openni2/DepthStream.cpp index 429417080..a1f944eb7 100644 --- a/src/openni2/DepthStream.cpp +++ b/src/openni2/DepthStream.cpp @@ -24,7 +24,8 @@ * either License. */ -#include +#define _USE_MATH_DEFINES +#include // for M_PI #include "DepthStream.hpp" using namespace Freenect2Driver; @@ -74,7 +75,7 @@ void DepthStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int src if (reg->isEnabled()) reg->depthFrame(srcFrame); - if (srcFrame->width < dstFrame->width || srcFrame->height < dstFrame->height) + if (srcFrame->width < (size_t)dstFrame->width || srcFrame->height < (size_t)dstFrame->height) memset(dstFrame->data, 0x00, dstFrame->width * dstFrame->height * 2); // copy stream buffer from freenect diff --git a/src/openni2/DepthStream.hpp b/src/openni2/DepthStream.hpp index 349e3d22f..9055e13eb 100644 --- a/src/openni2/DepthStream.hpp +++ b/src/openni2/DepthStream.hpp @@ -26,12 +26,8 @@ #pragma once -#include // for transform() -#include // for M_PI -#include // for memcpy #include #include -#include "PS1080.h" #include "VideoStream.hpp" diff --git a/src/openni2/DeviceDriver.cpp b/src/openni2/DeviceDriver.cpp index e7d98136b..0feea581c 100644 --- a/src/openni2/DeviceDriver.cpp +++ b/src/openni2/DeviceDriver.cpp @@ -93,7 +93,7 @@ namespace Freenect2Driver { listener.waitForNewFrame(frames); - for (int i = 0; i < sizeof(streams)/sizeof(*streams); i++) { + for (unsigned i = 0; i < sizeof(streams)/sizeof(*streams); i++) { struct streams& s = streams[i]; VideoStream* stream = getStream(s.type); libfreenect2::Frame *frame = frames[s.type]; @@ -131,10 +131,10 @@ namespace Freenect2Driver public: Device(int index) : //libfreenect2::Freenect2Device(fn_ctx, index), dev(NULL), - reg(NULL), color(NULL), - ir(NULL), depth(NULL), + ir(NULL), + reg(NULL), device_stop(false), listener(libfreenect2::Frame::Depth | libfreenect2::Frame::Ir | libfreenect2::Frame::Color), thread(NULL) @@ -427,7 +427,7 @@ namespace Freenect2Driver WriteMessage("Found device " + uri); - for (int i = 0; i < modes.size(); i++) { + for (unsigned i = 0; i < modes.size(); i++) { register_uri(uri + modes[i]); } @@ -452,7 +452,7 @@ namespace Freenect2Driver { std::string uri(c_uri); std::string mode(c_mode ? c_mode : ""); - if (uri.find("?") != -1) { + if (uri.find("?") != std::string::npos) { mode += "&"; mode += uri.substr(uri.find("?") + 1); uri = uri.substr(0, uri.find("?")); @@ -460,7 +460,7 @@ namespace Freenect2Driver std::stringstream ss(mode); std::string buf; while(std::getline(ss, buf, '&')) { - if (buf.find("=") != -1) { + if (buf.find("=") != std::string::npos) { config[buf.substr(0, buf.find("="))] = buf.substr(buf.find("=")+1); } else { if (0 < buf.length()) @@ -506,7 +506,7 @@ namespace Freenect2Driver if (iter->second == pDevice) { WriteMessage("Closing device " + std::string(iter->first.uri)); - int id = uri_to_devid(iter->first.uri); + //int id = uri_to_devid(iter->first.uri); Device* device = (Device*)iter->second; device->stop(); @@ -550,7 +550,7 @@ namespace Freenect2Driver // macros defined in XnLib (not included) - workaround -#define XN_NEW(type, arg...) new type(arg) +#define XN_NEW(type, arg) new type(arg) #define XN_DELETE(p) delete(p) ONI_EXPORT_DRIVER(Freenect2Driver::Driver); #undef XN_NEW diff --git a/src/openni2/IrStream.cpp b/src/openni2/IrStream.cpp index 6983f935d..106110bf6 100644 --- a/src/openni2/IrStream.cpp +++ b/src/openni2/IrStream.cpp @@ -24,7 +24,8 @@ * either License. */ -#include +#define _USE_MATH_DEFINES +#include // for M_PI #include "IrStream.hpp" using namespace Freenect2Driver; diff --git a/src/openni2/IrStream.hpp b/src/openni2/IrStream.hpp index ca5ee2d9c..064f85a4c 100644 --- a/src/openni2/IrStream.hpp +++ b/src/openni2/IrStream.hpp @@ -26,12 +26,8 @@ #pragma once -#include // for transform() -#include // for M_PI -#include // for memcpy #include #include -#include "PS1080.h" #include "VideoStream.hpp" namespace Freenect2Driver diff --git a/src/openni2/Registration.cpp b/src/openni2/Registration.cpp index c1b627a0d..ef394e832 100644 --- a/src/openni2/Registration.cpp +++ b/src/openni2/Registration.cpp @@ -25,7 +25,7 @@ */ #include -#include "Driver/OniDriverAPI.h" +#include #include "Registration.hpp" using namespace Freenect2Driver; diff --git a/src/openni2/Utility.cpp b/src/openni2/Utility.cpp index 7f85ee1b6..5ab7e30a2 100644 --- a/src/openni2/Utility.cpp +++ b/src/openni2/Utility.cpp @@ -25,7 +25,7 @@ */ // This file contains symbols that may be used by any class or don't really go anywhere else. -#include +#include #include "Driver/OniDriverAPI.h" diff --git a/src/openni2/Utility.hpp b/src/openni2/Utility.hpp index bdc1fc661..722596779 100644 --- a/src/openni2/Utility.hpp +++ b/src/openni2/Utility.hpp @@ -27,8 +27,7 @@ // This file contains symbols that may be used by any class or don't really go anywhere else. #pragma once -#include -#include "Driver/OniDriverAPI.h" +#include #include // Oni helpers diff --git a/src/openni2/VideoStream.hpp b/src/openni2/VideoStream.hpp index ad5f04ba0..7137199b7 100644 --- a/src/openni2/VideoStream.hpp +++ b/src/openni2/VideoStream.hpp @@ -28,7 +28,7 @@ #include #include -#include "PS1080.h" +#include #include "Utility.hpp" #include "Registration.hpp"