/********************************************************************
** Image Component Library (ICL) **
** **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld **
** Neuroinformatics Group **
** Website: www.iclcv.org and **
** http://opensource.cit-ec.de/projects/icl **
** **
** File : ICLIO/src/ICLIO/KinectGrabber.cpp **
** Module : ICLIO **
** Authors: Christof Elbrechter, Viktor Richter **
** **
** **
** GNU LESSER GENERAL PUBLIC LICENSE **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the **
** **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file. Please review the **
** following information to ensure the license requirements will **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt **
** **
** The development of this software was supported by the **
** Excellence Cluster EXC 277 Cognitive Interaction Technology. **
** The Excellence Cluster EXC 277 is a grant of the Deutsche **
** Forschungsgemeinschaft (DFG) in the context of the German **
** Excellence Initiative. **
** **
********************************************************************/
#include <ICLIO/Grabber.h>
#include <ICLIO/KinectGrabber.h>
#include <ICLCore/CCFunctions.h>
#include <ICLUtils/StringUtils.h>
#include <ICLCore/Img.h>
#include <ICLUtils/Mutex.h>
#include <ICLUtils/Thread.h>
#include <ICLFilter/TranslateOp.h>
#include <ICLFilter/MedianOp.h>
#include <ICLUtils/Time.h>
#include <libfreenect.h>
#include <map>
//#define FREENECT_DEBUG_LOGGING
using namespace icl::utils;
using namespace icl::core;
using namespace icl::filter;
namespace icl{
namespace io{
static std::ostream &operator<<(std::ostream &s, const KinectGrabber::Mode &m){
static std::string names[] = { "GRAB_RGB_IMAGE", "GRAB_BAYER_IMAGE", "GRAB_DEPTH_IMAGE",
"GRAB_IR_IMAGE_8BIT", "GRAB_IR_IMAGE_10BIT"};
if((int)m >= 0 && (int)m < 5) return s << names[(int)m];
else return s << "UNDEFINED MODE";
}
class FreenectContext : public Thread{
private:
int started;
int errors;
Mutex startMutex;
Mutex *ctx_mutex;
freenect_context *ctx_ptr;
static const int MAX_ERRORS = 100;
public:
std::vector<std::string> deviceAssociation;
private:
FreenectContext() : started(0), errors(0) {
static Mutex cMutex;
static freenect_context *ctx = NULL;
Mutex::Locker l(cMutex);
if(!ctx && freenect_init(&ctx, NULL) < 0){
throw ICLException("unable to create freenect_context");
}
#ifdef FREENECT_DEBUG_LOGGING
freenect_set_log_level(ctx, FREENECT_LOG_DEBUG);
#endif
ctx_mutex = &cMutex;
ctx_ptr = ctx;
freenect_device_attributes *attribs = 0;
int n = freenect_list_device_attributes(ctx, &attribs);
if(n < 1){
throw ICLException("no Kinect devices found");
}
for(freenect_device_attributes *a = attribs; a ; a=a->next){
if(a->camera_serial){
deviceAssociation.push_back(a->camera_serial);
}else{
DEBUG_LOG("Kinect Device camera serial was null");
deviceAssociation.push_back("null??");
}
}
freenect_free_device_attributes(attribs);
}
public:
static FreenectContext &getFreenectContext(){
static FreenectContext context;
return context;
}
int processEvents(){
Mutex::Locker l(ctx_mutex);
//DEBUG_LOG("calling freenect_process_events");
int x = freenect_process_events(ctx_ptr);
//DEBUG_LOG("calling freenect_process_events done");
return x;
}
int openDevice(freenect_device **dev, int index){
Mutex::Locker l(ctx_mutex);
return freenect_open_device(ctx_ptr, dev, index);
}
int numDevices(){
Mutex::Locker l(ctx_mutex);
return freenect_num_devices(ctx_ptr);
}
void start(){
Mutex::Locker l(startMutex);
if(!started++){
Thread::start();
}
}
void stop(){
Mutex::Locker l(startMutex);
if(!--started){
Thread::stop();
}
}
virtual void run(){
usleep(1000);
while(true){
//DEBUG_LOG("kinect grabber running");
usleep(1);
if(!trylock()){
//DEBUG_LOG("kinect grabber got lock");
if(processEvents() < 0){
errors++;
}
if(errors > MAX_ERRORS){
unlock();
throw ICLException("detected 100th error in freenect event processing");
}
unlock();
//DEBUG_LOG("kinect grabber unlocked");
}
}
}
};
/// just copied form libfreenect.hpp
class FreenectTiltStateICL {
friend struct FreenectDevice;
FreenectTiltStateICL(freenect_raw_tilt_state *_state):
m_code(_state->tilt_status), m_state(_state)
{}
public:
void getAccelerometers(double* x, double* y, double* z) {
freenect_get_mks_accel(m_state, x, y, z);
}
double getTiltDegs() {
return freenect_get_tilt_degs(m_state);
}
public:
freenect_tilt_status_code m_code;
private:
freenect_raw_tilt_state *m_state;
};
struct FreenectDevice{
struct Used{
enum IRShift{ Off=0,Fast,Accurate } irShift;
bool depthImageUnitMM;
int depthImagePostProcessingMedianRadius;
MedianOp postProcessor3x3;
MedianOp postProcessor5x5;
freenect_device *device;
int numColorUsers;
int numDepthUsers;
Mutex colorMutex,depthMutex;
Img8u colorImage,colorImageOut;
Img16s irImage16s,irImage16sOut;
Img8u irImage,irImageOut;
Img32f depthImage,depthImageOut;
Time lastColorTime, lastDepthTime;
Size size;
std::string serial;
Used(const std::string &serial):postProcessor3x3(Size(3,3)),postProcessor5x5(Size(5,5)),serial(serial){
postProcessor3x3.setClipToROI(false);
postProcessor5x5.setClipToROI(false);
}
KinectGrabber::Mode currentColorMode; // this must not be reset as long as the device is used somewhere else
void depth_cb(void *data, uint32_t timestamp){
Mutex::Locker lock(depthMutex);
const int r = depthImagePostProcessingMedianRadius;
MedianOp *pp = (r == 3) ? &postProcessor3x3 : r == 5 ? &postProcessor5x5 : (MedianOp*)0;
#if 0
if(serial == "B00365713743108B"){
// ok this cam needs some manual compensation of the depth values
static bool first = true;
if(first){
first = false;
DEBUG_LOG("detected Kinect Model " << serial << " adapting raw depth values by factor 1.02");
}
icl16s *p = reinterpret_cast<icl16s*>(data);
const icl16s * const e = p + 640*480;
for(;p < e;++p){
if(*p != 2047){
*p = icl16s(1.02f * *p);
if(*p > 2047) *p = 2047;
}
}
}
#endif
if(pp){
const Img16s tmp(Size::VGA, 1, std::vector<icl16s*>(1, (icl16s*)data));
pp->apply(&tmp)->convert(&depthImage);
int b = (r-1)/2;
depthImage.setROI(Rect(b,b,640-2*b, 480-2*b));
depthImage.fillBorder(&tmp);
depthImage.setTime(Time::now());
}else{
depthImage.setTime(Time::now());
std::copy((const icl16s*)data,(const icl16s*)data+640*480,depthImage.begin(0));
}
}
void color_cb(void *data, uint32_t timestamp){
Mutex::Locker lock(colorMutex);
switch(currentColorMode){
case KinectGrabber::GRAB_RGB_IMAGE:
colorImage.setTime(Time::now());
interleavedToPlanar((const icl8u*)data,&colorImage);
break;
case KinectGrabber::GRAB_IR_IMAGE_8BIT:{
irImage.setTime(Time::now());
std::copy((const icl8u*)data,(const icl8u*)data + (size==Size::VGA ? 640*480 : 320*240), irImage.begin(0));
break;}
case KinectGrabber::GRAB_IR_IMAGE_10BIT:
irImage16s.setTime(Time::now());
std::copy((const icl16s*)data,(const icl16s*)data + (size==Size::VGA ? 640*480 : 320*240), irImage16s.begin(0));
break;
default:
// actually, this happens sometimes, when the grabbers 'format' is switched frequently
// at runtime. This is why, we avoid to throw an exception here!
// throw ICLException("processed color callback for depth grabber (this should not happen)");
break;
}
}
static inline float kinect_raw_to_mm(icl16s d){
return 1.046 * (d==2047 ? 0 : 1000. / (d * -0.0030711016 + 3.3309495161));
}
const Img32f &getLastDepthImage(bool avoidDoubleFrames){
Mutex::Locker lock(depthMutex);
if(avoidDoubleFrames){
while(lastDepthTime == depthImage.getTime()){
depthMutex.unlock();
Thread::msleep(1);
depthMutex.lock();
}
}
lastDepthTime = depthImage.getTime();
if(!depthImageUnitMM){
depthImage.deepCopy(&depthImageOut);
}else{
depthImageOut.setSize(depthImage.getSize());
depthImageOut.setChannels(1);
std::transform( depthImage.begin(0), depthImage.end(0), depthImageOut.begin(0), kinect_raw_to_mm);
depthImageOut.setTime(depthImage.getTime());
}
return depthImageOut;
}
const ImgBase &getLastColorImage(bool avoidDoubleFrames){
Mutex::Locker lock(colorMutex);
ImgBase &src = ( currentColorMode == KinectGrabber::GRAB_RGB_IMAGE ? (ImgBase&)colorImage :
currentColorMode == KinectGrabber::GRAB_IR_IMAGE_8BIT ? (ImgBase&)irImage :
(ImgBase&)irImage16s);
ImgBase &dst = ( currentColorMode == KinectGrabber::GRAB_RGB_IMAGE ? (ImgBase&)colorImageOut :
currentColorMode == KinectGrabber::GRAB_IR_IMAGE_8BIT ? (ImgBase&)irImageOut :
(ImgBase&)irImage16sOut);
if(avoidDoubleFrames){
while(lastColorTime == src.getTime()){
// DEBUG_LOG("here!");
colorMutex.unlock();
Thread::msleep(1);
colorMutex.lock();
}
}
lastColorTime = src.getTime();
ImgBase *pDst = &dst;
if((currentColorMode != KinectGrabber::GRAB_RGB_IMAGE) && (irShift != Off)){
// TranslateOp t(-4.8, -3.9, irShift == Fast ? interpolateNN : interpolateLIN);
// found another value in literature, which sounds more plausible !
TranslateOp t(-3, -3, irShift == Fast ? interpolateNN : interpolateLIN);
t.apply(&src, &pDst);
// apply affine warp
}else{
src.deepCopy(&pDst);
}
return dst;
}
void setTiltDegrees(double angle) {
if(freenect_set_tilt_degs(device, angle) < 0){
throw ICLException("Cannot set angle in degrees");
}
}
void setLed(freenect_led_options option) {
if(freenect_set_led(device, option) < 0){
throw ICLException("Cannot set led");
}
}
void updateState() {
if (freenect_update_tilt_state(device) < 0){
throw ICLException("Cannot update device state");
}
}
FreenectTiltStateICL getState() const {
return FreenectTiltStateICL(freenect_get_tilt_state(device));
}
};
static std::map<int,Used*> devices;
Used *used;
KinectGrabber::Mode mode;
int index;
void setMode(KinectGrabber::Mode mode, Used *used, const Size &size){
if(mode != KinectGrabber::GRAB_DEPTH_IMAGE){
if(used->currentColorMode != KinectGrabber::GRAB_DEPTH_IMAGE
&& used->currentColorMode != mode){
WARNING_LOG("the color camera mode was changed even though another device with a different mode does still exist");
used->currentColorMode = mode;
}else if(used->currentColorMode == KinectGrabber::GRAB_DEPTH_IMAGE){
used->currentColorMode = mode;
}
}
#ifndef FREENECTAPI
// note, this is a dirty hack, however, it seems to work
// in the new libfreenect interface, FREENECTAPI is defined to nothing
// for unix/linux system. Therefore we use this hack here, to
// distinguish between the new and the old API
freenect_device *device = used->device;
const bool isVideo = (mode != KinectGrabber::GRAB_DEPTH_IMAGE);
static const freenect_video_format fvf[5] = { FREENECT_VIDEO_RGB,
FREENECT_VIDEO_BAYER,
(freenect_video_format)-1, // this is used for depth
FREENECT_VIDEO_IR_8BIT,
FREENECT_VIDEO_IR_10BIT };
if( size != Size::QVGA){
static bool first = true;
if(first){
first = false;
WARNING_LOG("QVGA size is not supported by the used version of libfreenect (using VGA instead)");
}
}
if(isVideo){
if( freenect_set_video_format(device, fvf[(int)mode]) < 0) throw ICLException("Cannot set video format");
}else{
if( freenect_set_depth_format(device, FREENECT_DEPTH_11BIT) < 0) throw ICLException("Cannot set depth format");
}
used->size = Size::VGA;
#else
freenect_device *device = used->device;
freenect_frame_mode m;
const bool isVideo = (mode != KinectGrabber::GRAB_DEPTH_IMAGE);
static const freenect_video_format fvf[5] = { FREENECT_VIDEO_RGB,
FREENECT_VIDEO_BAYER,
FREENECT_VIDEO_DUMMY,
FREENECT_VIDEO_IR_8BIT,
FREENECT_VIDEO_IR_10BIT };
freenect_resolution res = ( size == Size::VGA ?
FREENECT_RESOLUTION_MEDIUM :
FREENECT_RESOLUTION_LOW);
if(isVideo){
m = freenect_find_video_mode(res, fvf[(int)mode]);
}else{
m = freenect_find_depth_mode(res, FREENECT_DEPTH_11BIT);
}
if (!m.is_valid) throw ICLException("Cannot set video/depth format: invalid mode");
if(isVideo){
if(freenect_set_video_mode(device, m) < 0) throw ICLException("Cannot set video format");
}else{
if(freenect_set_depth_mode(device, m) < 0) throw ICLException("Cannot set depth format");
}
used->size = size;
#endif
}
FreenectDevice(int index, KinectGrabber::Mode mode, Size size):mode(mode),index(index){
FreenectContext &ctx = FreenectContext::getFreenectContext();
std::map<int,Used*>::iterator it = devices.find(index);
if(it == devices.end()){
DEBUG_LOG2("device " << index << " was not used before: creating new one. Mode " << mode);
used = devices[index] = new Used(ctx.deviceAssociation[index]);
used->irShift = Used::Accurate;
used->depthImageUnitMM = true;
used->currentColorMode = mode;
used->depthImagePostProcessingMedianRadius = 0;
if(FreenectContext::getFreenectContext().openDevice(&used->device, index) < 0){
devices.erase(index);
throw ICLException("FreenectDevice:: unable to open kinect device for device " + str(index));
}
used->numColorUsers = used->numDepthUsers = 0;
freenect_set_user(used->device, used);
if(mode != KinectGrabber::GRAB_DEPTH_IMAGE){
used->numColorUsers++;
setMode(mode, used, size);
if(mode == KinectGrabber::GRAB_RGB_IMAGE){
used->colorImage = Img8u(size,formatRGB);
}else if(mode == KinectGrabber::GRAB_IR_IMAGE_8BIT){
used->irImage = Img8u(size,1);
}else if(mode == KinectGrabber::GRAB_IR_IMAGE_10BIT){
used->irImage16s = Img16s(size,1);
}else{
throw ICLException("FreenectDevice:: invalid color mode detected");
}
freenect_set_video_callback(used->device,freenect_video_callback);
if(freenect_start_video(used->device) < 0){
throw ICLException("FreenectDevice:: unable to start video for device" + str(index));
}
}else{
DEBUG_LOG2("device " << index << " was used before");
used->numDepthUsers++;
setMode(mode, used, size);
used->depthImage = Img32f(size,formatMatrix);
freenect_set_depth_callback(used->device,freenect_depth_callback);
if(freenect_start_depth(used->device) < 0){
throw ICLException("FreenectDevice:: unable to start depth for device" + str(index));
}
}
}else{ // reuse old device
// DEBUG_LOG("device " << index << " was used before: using old one. Type" << mode);
used = it->second;
if(used->size != size){
size = used->size;
WARNING_LOG("unable to switch kinect device property \"size\":"
<< " another device with another size is already instantiated");
}
if(mode != KinectGrabber::GRAB_DEPTH_IMAGE){
if(!used->numColorUsers){
freenect_stop_video(used->device);
setMode(mode, used, size);
if(mode == KinectGrabber::GRAB_RGB_IMAGE){
used->colorImage = Img8u(size,formatRGB);
}else if(mode == KinectGrabber::GRAB_IR_IMAGE_8BIT){
used->irImage = Img8u(size,1);
}else if(mode == KinectGrabber::GRAB_IR_IMAGE_10BIT){
used->irImage16s = Img16s(size,1);
}else{
throw ICLException("FreenectDevice:: invalid color mode detected");
}
freenect_set_video_callback(used->device,freenect_video_callback);
if(freenect_start_video(used->device) < 0){
throw ICLException("FreenectDevice:: unable to start video for device" + str(index));
}
}else{
if(used->currentColorMode != mode){
WARNING_LOG("the mode cannot be changed to " << mode
<< " because another grabber instance with mode "
<< used->currentColorMode << " does already exist");
}
}
used->numColorUsers++;
}else{
if(!used->numDepthUsers){
freenect_stop_depth(used->device);
setMode(mode, used, size);
used->depthImage = Img32f(size,formatMatrix);
freenect_set_depth_callback(used->device,freenect_depth_callback);
if(freenect_start_depth(used->device) < 0){
throw ICLException("FreenectDevice:: unable to start depth for device" + str(index));
}
}
}
}
}
~FreenectDevice(){
if(mode != KinectGrabber::GRAB_DEPTH_IMAGE){
used->numColorUsers--;
if(!used->numColorUsers){
used->currentColorMode = KinectGrabber::GRAB_DEPTH_IMAGE;
if(freenect_stop_video(used->device) < 0){
throw ICLException("FreenectDevice:: unable to stop color for device "+ str(index));
}
}
}else{
used->numDepthUsers--;
if(!used->numDepthUsers){
if(freenect_stop_depth(used->device) < 0){
throw ICLException("FreenectDevice:: unable to stop depth for device "+ str(index));
}
}
}
if(!used->numColorUsers && !used->numDepthUsers){
if(freenect_close_device(used->device) < 0){
throw ICLException("FreenectDevice:: unable to close device "+ str(index));
}
devices.erase(devices.find(index));
delete used;
}
}
static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
//SHOW(freenect_get_user(dev));
static_cast<FreenectDevice::Used*>(freenect_get_user(dev))->depth_cb(depth,timestamp);
}
static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
//SHOW(freenect_get_user(dev));
static_cast<FreenectDevice::Used*>(freenect_get_user(dev))->color_cb(video,timestamp);
}
};
std::map<int,FreenectDevice::Used*> FreenectDevice::devices;
struct KinectGrabber::Impl{
private:
SmartPtr<FreenectDevice> device;
public:
int ledColor;
float desiredTiltDegrees;
bool avoidDoubleFrames;
Mutex mutex;
Time lastupdate;
//std::string deviceSerialString;
static int getIndex(std::string idOrSerial){
std::vector<std::string> ts = tok(idOrSerial,"|||",false);
if(ts.size() > 1){
idOrSerial = ts[0];
}
FreenectContext &ctx = FreenectContext::getFreenectContext();
if(idOrSerial.length() < 2){
return parse<int>(idOrSerial);
}else{
for(size_t i=0;i<ctx.deviceAssociation.size();++i){
if(ctx.deviceAssociation[i] == idOrSerial){
return (int)i;
}
}
throw ICLException("Kinect device serial '" + idOrSerial + "' not found\n"
"(found device serials were " + cat(ctx.deviceAssociation,",") + ")");
}
return 0;
}
Impl(KinectGrabber::Mode mode, std::string idOrSerial, const Size &size)
: device(new FreenectDevice(getIndex(idOrSerial),mode, size)),
ledColor(0), desiredTiltDegrees(0), avoidDoubleFrames(true), mutex(Mutex::mutexTypeRecursive), lastupdate(Time::now())
{
}
~Impl(){
device.setNull();
}
void switchMode(KinectGrabber::Mode mode, const Size &size){
if(device->mode != mode || device->used->size != size){
int idx = device->index;
//device = SmartPtr<FreenectDevice>();
device.setNull();
device = SmartPtr<FreenectDevice>(new FreenectDevice(idx,mode,size));
}
}
inline const Img32f &getLastDepthImage(){
return device->used->getLastDepthImage(avoidDoubleFrames);
}
inline const ImgBase &getLastColorImage(){
return device->used->getLastColorImage(avoidDoubleFrames);
}
SmartPtr<FreenectDevice> &getDevice(){
return device;
}
};
KinectGrabber::KinectGrabber(KinectGrabber::Mode mode, int deviceID, const Size &size)
throw (ICLException) {
init(mode, str(deviceID),size);
}
KinectGrabber::KinectGrabber(KinectGrabber::Mode mode, const std::string &idOrSerial, const Size &size)
throw (ICLException) {
init(mode, idOrSerial,size);
}
void KinectGrabber::init(KinectGrabber::Mode mode, const std::string &idOrSerial, const Size &size)
throw (ICLException){
FreenectContext::getFreenectContext().start();
m_impl = new Impl(mode,idOrSerial,size);
// Configurable
static const std::string formats[] = {
"Color Image {24Bit RGB}",
"Bayer Image {8Bit}",
"Depth Image {float}",
"IR Image {8Bit}",
"IR Image {10Bit}"
};
static const std::string values[] = {
"off",
"fast",
"accurate"
};
try{m_impl->getDevice()->used->updateState();}catch(...){}
double degs = m_impl->getDevice()->used->getState().getTiltDegs();
std::string angleval = (degs == -64) ? "moving" : str(degs);
double a[3]={0,0,0};
m_impl->getDevice()->used->getState().getAccelerometers(a,a+1,a+2);
std::string accelval = str(a[0]) + "-" + str(a[1]) + "-" + str(a[2]);
std::string diunit = (m_impl->getDevice()->used->depthImageUnitMM) ? "mm" : "raw";
const int r = m_impl->getDevice()->used->depthImagePostProcessingMedianRadius;
std::string ppvalue = (r == 3) ? "median 3x3" : ((r == 5) ? "median 5x4" : "off");
addProperty("Avoid double frames","flag","",m_impl->avoidDoubleFrames,0,"whether to avoid returning the same frame multiple times");
addProperty("format", "menu", "Color Image {24Bit RGB},Depth Image {float},IR Image {8Bit},IR Image {10Bit}", formats[m_impl->getDevice()->mode], 0, "");
addProperty("size", "menu", "VGA {640x480}", "VGA {640x480}", 0, "");
addProperty("LED", "menu", "off,green,red,yellow,blink yellow,blink green,blink red/yellow", m_impl->ledColor, 0, "");
addProperty("Desired-Tilt-Angle", "range", "[-35,25]", m_impl->desiredTiltDegrees, 0, "");
addProperty("Current-Tilt-Angle", "info", "", angleval, 100, "");
addProperty("Accelerometers", "info", "", accelval, 100, "");
addProperty("shift-IR-image", "menu", "off,fast,accurate", values[(int)(m_impl->getDevice()->used->irShift)], 0, "");
addProperty("depth-image-unit", "menu", "raw,mm", diunit, 0, "");
addProperty("depth-image-post-processing", "menu", "off,median 3x3,median 5x5", ppvalue, 0, "");
Configurable::registerCallback(utils::function(this,&KinectGrabber::processPropertyChange));
}
KinectGrabber::~KinectGrabber(){
ICL_DELETE(m_impl);
FreenectContext::getFreenectContext().stop();
}
const ImgBase* KinectGrabber::acquireImage(){
Mutex::Locker lock(m_impl->mutex);
// update current angle and accelometers every 200ms
if(m_impl -> lastupdate.age() > 200000){
updateState();
}
if(m_impl->getDevice()->mode != GRAB_DEPTH_IMAGE){
return &m_impl->getLastColorImage();
}else{
return &m_impl->getLastDepthImage();
}
}
void KinectGrabber::updateState(){
try{m_impl->getDevice()->used->updateState();}catch(...){DEBUG_LOG("could not update")}
double degs = m_impl->getDevice()->used->getState().getTiltDegs();
std::string angleval = (degs == -64) ? "moving" : str(degs);
double a[3]={0,0,0};
m_impl->getDevice()->used->getState().getAccelerometers(a,a+1,a+2);
std::string accelval = str(a[0]) + "-" + str(a[1]) + "-" + str(a[2]);
prop("Current-Tilt-Angle").value = angleval;
prop("Accelerometers").value = accelval;
m_impl -> lastupdate = Time::now();
}
/// callback for changed configurable properties
void KinectGrabber::processPropertyChange(const utils::Configurable::Property &prop){
Mutex::Locker lock(m_impl->mutex);
if(prop.name == "Avoid double frames"){
m_impl->avoidDoubleFrames = parse<bool>(prop.value);
return;
}
if(prop.name == "format"){
static const std::string formats[] = {
"Color Image {24Bit RGB}",
"Bayer Image {8Bit}",
"Depth Image {float}",
"IR Image {8Bit}",
"IR Image {10Bit}"
};
int idx = (int)(std::find(formats, formats+5, prop.value) - formats);
if(idx == 5){
ERROR_LOG("invalid property value for property 'format'");
return;
}
m_impl->switchMode((Mode)idx, m_impl->getDevice()->used->size);
} else if(prop.name == "size"){
/*
if(value != "VGA {640x480}" && value != "QVGA {320x240}"){
ERROR_LOG("invalid property value for property 'size'");
}else{
m_impl->switchMode(m_impl->device->mode,value == "VGA {640x480}" ?
Size::VGA : Size::QVGA);
}
*/
}else if(prop.name == "LED"){
if(prop.value == "off"){
m_impl->getDevice()->used->setLed((freenect_led_options)0);
}else if(prop.value == "green"){
m_impl->getDevice()->used->setLed((freenect_led_options)1);
}else if(prop.value == "red"){
m_impl->getDevice()->used->setLed((freenect_led_options)2);
}else if(prop.value == "yellow"){
m_impl->getDevice()->used->setLed((freenect_led_options)3);
}else if(prop.value == "blink yellow"){
m_impl->getDevice()->used->setLed((freenect_led_options)4);
}else if(prop.value == "blink green"){
m_impl->getDevice()->used->setLed((freenect_led_options)5);
}else if(prop.value == "blink red/yellow"){
m_impl->getDevice()->used->setLed((freenect_led_options)6);
}else{
ERROR_LOG("invalid property value for property 'LED'" << prop.value);
}
}else if(prop.name == "Desired-Tilt-Angle"){
m_impl->getDevice()->used->setTiltDegrees(parse<double>(prop.value));
}else if(prop.name == "shift-IR-image"){
if(prop.value == "off"){
m_impl->getDevice()->used->irShift = FreenectDevice::Used::Off;
}else if(prop.value == "fast"){
m_impl->getDevice()->used->irShift = FreenectDevice::Used::Fast;
}else if(prop.value == "accurate"){
m_impl->getDevice()->used->irShift = FreenectDevice::Used::Accurate;
}else{
ERROR_LOG("invalid property value for property 'shift-IR-image':" << prop.value);
}
}else if(prop.name == "depth-image-unit"){
if(prop.value == "mm") m_impl->getDevice()->used->depthImageUnitMM = true;
else if(prop.value == "raw") m_impl->getDevice()->used->depthImageUnitMM = false;
else{
ERROR_LOG("invalid property value for property 'depth-image-unit':" << prop.value);
}
}else if(prop.name == "depth-image-post-processing"){
if(prop.value == "off") m_impl->getDevice()->used->depthImagePostProcessingMedianRadius = 0;
else if(prop.value == "median 3x3") m_impl->getDevice()->used->depthImagePostProcessingMedianRadius = 3;
else if(prop.value == "median 5x5") m_impl->getDevice()->used->depthImagePostProcessingMedianRadius = 5;
else{
ERROR_LOG("invalid property value for property 'depth-image-post-processing':" << prop.value);
}
}
}
REGISTER_CONFIGURABLE(KinectGrabber, return new KinectGrabber(KinectGrabber::GRAB_DEPTH_IMAGE, 0, utils::Size::VGA));
/// returns a list of attached kinect devices
const std::vector<GrabberDeviceDescription> &KinectGrabber::getDeviceList(bool rescan){
static std::vector<GrabberDeviceDescription> devices;
if(rescan){
devices.clear();
FreenectContext &ctx = FreenectContext::getFreenectContext();
for(size_t i=0;i<ctx.deviceAssociation.size();++i){
try{
KinectGrabber g(GRAB_RGB_IMAGE,i);
std::string serial = ctx.deviceAssociation[i];
std::string s = str(i);
if(serial != "null"){
s += "|||" + serial;
}
devices.push_back(GrabberDeviceDescription("kinectd",s,"Kinect Depth Camera (ID "+str(i)+")"));
devices.push_back(GrabberDeviceDescription("kinectc",s,"Kinect Color Camera RGB (ID "+str(i)+")"));
devices.push_back(GrabberDeviceDescription("kinecti",s,"Kinect Color Camera IR (ID "+str(i)+")"));
}catch(ICLException &e){
(void)e;//SHOW(e.what());
break;
}
}
}
return devices;
}
Grabber* createDepthGrabber(const std::string ¶m){
return new KinectGrabber(KinectGrabber::GRAB_DEPTH_IMAGE,param);
}
Grabber* createRGBGrabber(const std::string ¶m){
return new KinectGrabber(KinectGrabber::GRAB_RGB_IMAGE,param);
}
Grabber* createIRGrabber(const std::string ¶m){
return new KinectGrabber(KinectGrabber::GRAB_IR_IMAGE_8BIT,param);
}
const std::vector<GrabberDeviceDescription>& getKinectDDeviceList(std::string hint, bool rescan){
static std::vector<GrabberDeviceDescription> devices;
if(rescan){
FreenectContext &ctx = FreenectContext::getFreenectContext();
devices.clear();
int deviceCount = ctx.numDevices();
for(int i = 0; i < deviceCount; ++i){
std::string serial = ctx.deviceAssociation[i];
std::string s = str(i);
if(serial != "null"){
s += "|||" + serial;
}
devices.push_back(GrabberDeviceDescription("kinectd",s,"Kinect Depth Camera (ID "+str(i)+")"));
}
}
return devices;
}
const std::vector<GrabberDeviceDescription>& getKinectCDeviceList(std::string hint, bool rescan){
static std::vector<GrabberDeviceDescription> devices;
if(rescan){
FreenectContext &ctx = FreenectContext::getFreenectContext();
devices.clear();
int deviceCount = ctx.numDevices();
for(int i = 0; i < deviceCount; ++i){
std::string serial = ctx.deviceAssociation[i];
std::string s = str(i);
if(serial != "null"){
s += "|||" + serial;
}
devices.push_back(GrabberDeviceDescription("kinectc",s,"Kinect Color Camera RGB (ID "+str(i)+")"));
}
}
return devices;
}
const std::vector<GrabberDeviceDescription>& getKinectIDeviceList(std::string hint, bool rescan){
static std::vector<GrabberDeviceDescription> devices;
if(rescan){
FreenectContext &ctx = FreenectContext::getFreenectContext();
devices.clear();
int deviceCount = ctx.numDevices();
for(int i = 0; i < deviceCount; ++i){
std::string serial = ctx.deviceAssociation[i];
std::string s = str(i);
if(serial != "null"){
s += "|||" + serial;
}
devices.push_back(GrabberDeviceDescription("kinecti",s,"Kinect Color Camera IR (ID "+str(i)+")"));
}
}
return devices;
}
REGISTER_GRABBER(kinectd,utils::function(createDepthGrabber), utils::function(getKinectDDeviceList), "kinectd:device ID:kinect depth camera source:");
REGISTER_GRABBER(kinectc,utils::function(createRGBGrabber), utils::function(getKinectCDeviceList),"kinectc:device ID:kinect color camera source");
REGISTER_GRABBER(kinecti,utils::function(createIRGrabber), utils::function(getKinectIDeviceList),"kinecti:devide ID:kinect IR camera source");
} // namespace io
} // namespace icl