25 #include "VideoSource_Kinect.hh"
27 #include <Base/Image/ImageIO.hh>
38 int VideoSource_Kinect::deviceCount_ = 0;
39 int VideoSource_Kinect::openDevices_ = 0;
71 VideoSource_Kinect::VideoSource_Kinect()
76 capMode_ = (int)VideoSource_Kinect::DepthRay;
80 VideoSource_Kinect::~VideoSource_Kinect()
82 if (openDevices_ != 0) {
85 DeleteDepthLookupTable();
88 int VideoSource_Kinect::
94 int VideoSource_Kinect::
95 OpenDevice(
int device)
97 if (openDevices_ > 0 && device==0) {
98 BIASWARN(
"Kinect: I guess you try to open the next device (device=0 given)");
99 device = openDevices_;
100 BIASWARN(
"Kinect: Trying to open device " << device <<
" instead");
103 if (freenect_init(&freenectContext_, NULL) < 0)
105 BIASERR(
"Could not init freenect!");
109 deviceCount_ = freenect_num_devices (freenectContext_);
110 cout<<
"Number of devices found: "<< deviceCount_;
112 if (deviceCount_ < 1){
113 BIASERR(
"No devices found, returning");
117 if (freenect_open_device(freenectContext_, &freenectDevice_, device) < 0) {
118 BIASERR(
"Could not open device.");
122 CreateDepthLookupTable();
129 int VideoSource_Kinect::CloseDevice() {
138 uint16_t *depth = (uint16_t*)v_depth;
143 case VideoSource_Kinect::DepthRay: {
144 for (
unsigned int x=0;x<640*480;x++) {
149 case VideoSource_Kinect::DepthEuclidean: {
150 for (
unsigned int x=0;x<640*480;x++) {
155 case VideoSource_Kinect::DepthRaw: {
156 for (
unsigned int x=0;x<640*480;x++) {
162 BIASERR(
"unknown mode")
179 freenect_set_video_buffer(dev,
rgb_back);
222 int VideoSource_Kinect::PreGrab() {
223 cout <<
"Pregrab Device " << deviceId_ << endl;
224 int res = pthread_create(&freenect_thread, NULL, FreenectThreadFunc_, NULL);
226 BIASERR(
"Creating Freenect thread failed");
232 rgb_back =
new unsigned char[640*480*3];
233 rgb_mid =
new unsigned char[640*480*3];
234 rgb_front =
new unsigned char[640*480*3];
236 for (
int i=0; i<2048; i++) {
237 const float k1 = 1.1863f;
238 const float k2 = 2842.5f;
239 const float k3 = 0.1236f;
240 const float depth = k3 * tanf(i/k2 + k1);
249 int VideoSource_Kinect::PostGrab() {
251 pthread_join(freenect_thread, NULL);
261 int VideoSource_Kinect::
265 BIASWARN(
"Image already inited");
268 Image.
Init(640,480,1);
273 int VideoSource_Kinect::
277 BIASWARN(
"Kinect Error: image already inited");
280 Image.
Init(640,480,1,ImageBase::ST_float);
286 int VideoSource_Kinect::
289 pthread_cond_wait(&newColorAvail_, &coloravail_mutex);
303 pthread_cond_wait(&newDepthAvail_, &depthavail_mutex);
316 int VideoSource_Kinect::SetVideoModeColor() {
321 int VideoSource_Kinect::SetVideoModeIR() {
326 int VideoSource_Kinect::SetDepthCaptureMode(CaptureMode mode) {
331 int VideoSource_Kinect::SetSwitchVideoModeAfterGrab() {
332 BIASERR(
"Not implemented!");
336 int VideoSource_Kinect::SetTilt(
double degrees) {
337 freenect_set_tilt_degs(freenectDevice_,degrees);
341 int VideoSource_Kinect::SetLed(
int mode) {
342 freenect_set_led(freenectDevice_,(freenect_led_options)mode);
346 int VideoSource_Kinect::GetAccelerometer(
double &tilt,
double &x,
double &y,
double &z) {
347 freenect_raw_tilt_state* state=NULL;
348 freenect_update_tilt_state(freenectDevice_);
349 state = freenect_get_tilt_state(freenectDevice_);
350 tilt = freenect_get_tilt_degs(state);
351 freenect_get_mks_accel(state, &x, &y, &z);
355 int VideoSource_Kinect::GetCapabilities(VideoSourceCapabilities &caps)
360 int VideoSource_Kinect::GetCapabilities(
const char *device, VideoSourceCapabilities &caps)
365 int VideoSource_Kinect::
366 CreateDepthLookupTable(
double px,
double py,
double fx,
double fy)
368 if (depthLookupTable_ == NULL) {
373 for(
unsigned y=0;y<480;y++){
374 for(
unsigned x=0;x<640;x++){
375 float alpha = atan(
float((x-px)/fx));
376 float beta = atan(
float((py-y)/ sqrt( pow((x-px)*(fy/fx),2) + pow(fy,2))));
377 ida[y][x] = 1.0f / (cos(beta) * cos(alpha));
384 void VideoSource_Kinect::
385 DeleteDepthLookupTable()
387 if (depthLookupTable_ != NULL) {
394 int VideoSource_Kinect::
396 BIASERR(
"ToggleDepthStream not implemented for windows and libfreenect.");
pthread_t freenect_thread
bool IsEmpty() const
check if ImageData_ points to allocated image buffer or not
void CopyIn_NoInit(void *data)
Take some data and fill it into the Image.
void DepthCallBack(freenect_device *dev, void *v_depth, uint32_t timestamp)
freenect_context * freenectContext_
BIAS::Image< float > * depthLookupTable_
void SetColorModel(EColorModel Model)
pthread_mutex_t coloravail_mutex
freenect_video_format current_format
freenect_device * freenectDevice_
pthread_cond_t newColorAvail_
pthread_cond_t gl_frame_cond
void VideoCallback(freenect_device *dev, void *rgb, uint32_t timestamp)
pthread_mutex_t depthavail_mutex
pthread_cond_t newDepthAvail_
const StorageType * GetImageData() const
overloaded GetImageData() from ImageBase
freenect_video_format requested_format
void Init(unsigned int width, unsigned int height, unsigned int nChannels=1, enum EStorageType storageType=ST_unsignedchar, const bool interleaved=true)
Initialize image size and channels.
pthread_mutex_t gl_backbuf_mutex
unsigned char * rgb_front
This is the base class for images in BIAS.
void * FreenectThreadFunc_(void *arg)
const StorageType ** GetImageDataArray() const
overloaded GetImageDataArray() from ImageBase