-
Notifications
You must be signed in to change notification settings - Fork 746
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #523 from xlz/openni2
OpenNI2 driver
- Loading branch information
Showing
16 changed files
with
2,017 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,212 @@ | ||
/* | ||
* This file is part of the OpenKinect Project. http://www.openkinect.org | ||
* | ||
* Copyright (c) 2014 Benn Snyder, 2015 individual OpenKinect contributors. | ||
* See the CONTRIB file for details. | ||
* | ||
* This code is licensed to you under the terms of the Apache License, version | ||
* 2.0, or, at your option, the terms of the GNU General Public License, | ||
* version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, | ||
* or the following URLs: | ||
* http://www.apache.org/licenses/LICENSE-2.0 | ||
* http://www.gnu.org/licenses/gpl-2.0.txt | ||
* | ||
* If you redistribute this file in source form, modified or unmodified, you | ||
* may: | ||
* 1) Leave this header intact and distribute it under the same terms, | ||
* accompanying it with the APACHE20 and GPL20 files, or | ||
* 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or | ||
* 3) Delete the GPL v2 clause and accompany it with the APACHE20 file | ||
* In all cases you must keep the copyright notice intact and include a copy | ||
* of the CONTRIB file. | ||
* | ||
* Binary distributions must follow the binary distribution requirements of | ||
* either License. | ||
*/ | ||
|
||
#define _USE_MATH_DEFINES | ||
#include <cmath> // for M_PI | ||
#include "ColorStream.hpp" | ||
|
||
using namespace Freenect2Driver; | ||
|
||
// from NUI library & converted to radians | ||
const float ColorStream::DIAGONAL_FOV = 73.9 * (M_PI / 180); | ||
const float ColorStream::HORIZONTAL_FOV = 62 * (M_PI / 180); | ||
const float ColorStream::VERTICAL_FOV = 48.6 * (M_PI / 180); | ||
|
||
ColorStream::ColorStream(libfreenect2::Freenect2Device* pDevice, Freenect2Driver::Registration *reg) : VideoStream(pDevice, reg) | ||
{ | ||
video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30); | ||
setVideoMode(video_mode); | ||
} | ||
|
||
// Add video modes here as you implement them | ||
ColorStream::FreenectVideoModeMap ColorStream::getSupportedVideoModes() const | ||
{ | ||
FreenectVideoModeMap modes; | ||
// pixelFormat, resolutionX, resolutionY, fps freenect_video_format, freenect_resolution | ||
modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30)] = 0; | ||
modes[makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 1920, 1080, 30)] = 1; | ||
|
||
return modes; | ||
|
||
/* working format possiblities | ||
FREENECT_VIDEO_RGB | ||
FREENECT_VIDEO_YUV_RGB | ||
FREENECT_VIDEO_YUV_RAW | ||
*/ | ||
} | ||
|
||
void ColorStream::populateFrame(libfreenect2::Frame* srcFrame, int srcX, int srcY, OniFrame* dstFrame, int dstX, int dstY, int width, int height) const | ||
{ | ||
dstFrame->sensorType = getSensorType(); | ||
dstFrame->stride = dstFrame->width * 3; | ||
|
||
// copy stream buffer from freenect | ||
switch (video_mode.pixelFormat) | ||
{ | ||
default: | ||
LogError("Pixel format " + to_string(video_mode.pixelFormat) + " not supported by populateFrame()"); | ||
return; | ||
|
||
case ONI_PIXEL_FORMAT_RGB888: | ||
if (reg->isEnabled()) { | ||
libfreenect2::Frame registered(512, 424, 4); | ||
|
||
reg->colorFrameRGB888(srcFrame, ®istered); | ||
|
||
copyFrame(static_cast<uint8_t*>(registered.data), srcX, srcY, registered.width * registered.bytes_per_pixel, | ||
static_cast<uint8_t*>(dstFrame->data), dstX, dstY, dstFrame->stride, | ||
width, height, mirroring); | ||
} else { | ||
copyFrame(static_cast<uint8_t*>(srcFrame->data), srcX, srcY, srcFrame->width * srcFrame->bytes_per_pixel, | ||
static_cast<uint8_t*>(dstFrame->data), dstX, dstY, dstFrame->stride, | ||
width, height, mirroring); | ||
} | ||
return; | ||
} | ||
} | ||
|
||
void ColorStream::copyFrame(uint8_t* srcPix, int srcX, int srcY, int srcStride, uint8_t* dstPix, int dstX, int dstY, int dstStride, int width, int height, bool mirroring) | ||
{ | ||
srcPix += srcX + srcY * srcStride; | ||
dstPix += dstX + dstY * dstStride; | ||
|
||
for (int y = 0; y < height; y++) { | ||
uint8_t* dst = dstPix + y * dstStride; | ||
uint8_t* src = srcPix + y * srcStride; | ||
if (mirroring) { | ||
dst += dstStride - 1; | ||
for (int x = 0; x < srcStride; ++x) | ||
{ | ||
if (x % 4 != 3) | ||
{ | ||
*dst-- = *src++; | ||
} | ||
else | ||
{ | ||
++src; | ||
} | ||
} | ||
} else { | ||
for (int x = 0; x < dstStride-2; x += 3) | ||
{ | ||
*dst++ = src[2]; | ||
*dst++ = src[1]; | ||
*dst++ = src[0]; | ||
src += 4; | ||
} | ||
} | ||
} | ||
} | ||
|
||
OniSensorType ColorStream::getSensorType() const { return ONI_SENSOR_COLOR; } | ||
|
||
OniStatus ColorStream::setImageRegistrationMode(OniImageRegistrationMode mode) | ||
{ | ||
if (mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) { | ||
// XXX, switch color resolution to 512x424 for registrarion here | ||
OniVideoMode video_mode = makeOniVideoMode(ONI_PIXEL_FORMAT_RGB888, 512, 424, 30); | ||
setProperty(ONI_STREAM_PROPERTY_VIDEO_MODE, &video_mode, sizeof(video_mode)); | ||
} | ||
return ONI_STATUS_OK; | ||
} | ||
|
||
// from StreamBase | ||
OniBool ColorStream::isPropertySupported(int propertyId) | ||
{ | ||
switch(propertyId) | ||
{ | ||
default: | ||
return VideoStream::isPropertySupported(propertyId); | ||
|
||
case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: | ||
case ONI_STREAM_PROPERTY_VERTICAL_FOV: | ||
case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: | ||
case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: | ||
return true; | ||
} | ||
} | ||
|
||
OniStatus ColorStream::getProperty(int propertyId, void* data, int* pDataSize) | ||
{ | ||
switch (propertyId) | ||
{ | ||
default: | ||
return VideoStream::getProperty(propertyId, data, pDataSize); | ||
|
||
case ONI_STREAM_PROPERTY_HORIZONTAL_FOV: // float (radians) | ||
{ | ||
if (*pDataSize != sizeof(float)) | ||
{ | ||
LogError("Unexpected size for ONI_STREAM_PROPERTY_HORIZONTAL_FOV"); | ||
return ONI_STATUS_ERROR; | ||
} | ||
*(static_cast<float*>(data)) = HORIZONTAL_FOV; | ||
return ONI_STATUS_OK; | ||
} | ||
case ONI_STREAM_PROPERTY_VERTICAL_FOV: // float (radians) | ||
{ | ||
if (*pDataSize != sizeof(float)) | ||
{ | ||
LogError("Unexpected size for ONI_STREAM_PROPERTY_VERTICAL_FOV"); | ||
return ONI_STATUS_ERROR; | ||
} | ||
*(static_cast<float*>(data)) = VERTICAL_FOV; | ||
return ONI_STATUS_OK; | ||
} | ||
|
||
// camera | ||
case ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE: // OniBool | ||
{ | ||
if (*pDataSize != sizeof(OniBool)) | ||
{ | ||
LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_WHITE_BALANCE"); | ||
return ONI_STATUS_ERROR; | ||
} | ||
*(static_cast<OniBool*>(data)) = auto_white_balance; | ||
return ONI_STATUS_OK; | ||
} | ||
case ONI_STREAM_PROPERTY_AUTO_EXPOSURE: // OniBool | ||
{ | ||
if (*pDataSize != sizeof(OniBool)) | ||
{ | ||
LogError("Unexpected size for ONI_STREAM_PROPERTY_AUTO_EXPOSURE"); | ||
return ONI_STATUS_ERROR; | ||
} | ||
*(static_cast<OniBool*>(data)) = auto_exposure; | ||
return ONI_STATUS_OK; | ||
} | ||
} | ||
} | ||
|
||
OniStatus ColorStream::setProperty(int propertyId, const void* data, int dataSize) | ||
{ | ||
switch (propertyId) | ||
{ | ||
case 0: | ||
default: | ||
return VideoStream::setProperty(propertyId, data, dataSize); | ||
} | ||
} |
Oops, something went wrong.