QKinect.cpp

Go to the documentation of this file.

/***************************************************************************
 *  This file is part of the OpenKinect Project. http://www.openkinect.org *
 *                                                                         *
 *  Copyright (c) 2010 individual OpenKinect contributors.                 *
 *  See the CONTRIB file for details.                                      *
 *                                                                         *
 *  This file is part of direcs.                                           *
 *  www.direcs.de                                                          *
 *                                                                         *
 *  direcs is free software: you can redistribute it and/or modify it      *
 *  under the terms of the GNU General Public License as published         *
 *  by the Free Software Foundation, version 3 of the License.             *
 *                                                                         *
 *  direcs is distributed in the hope that it will be useful,              *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of         *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the           *
 *  GNU General Public License for more details.                           *
 *                                                                         *
 *  You should have received a copy of the GNU General Public License      *
 *  along with direcs. If not, see <http://www.gnu.org/licenses/>.         *
 *                                                                         *
 ***************************************************************************/

//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------

#include <QKinect.h>
#include <QDebug>
#include <cstdlib>
#include <cmath>
#include <cassert>


//----------------------------------------------------------------------------------------------------------------------
QKinect *QKinect::s_instance = 0;// initialize pointer
//----------------------------------------------------------------------------------------------------------------------


void QKinectProcessEvents::run()
{
        // loop while we are active and process the kinect event queue
        while(m_active)
        {
                //qDebug()<<"process thread\n";
                if (freenect_process_events(m_ctx) < 0)
                {
                        throw std::runtime_error("Cannot process freenect events");
                }
        }
}


QKinect::~QKinect()
{
        // dtor is not called directly
        if (s_instance)
        {
                delete s_instance;
        }
}


QKinect* QKinect::instance()
{
        // this is the main singleton code first check to see if we exist
        if (s_instance==0 )
        {
                // we do so create an instance (this will validate the pointer so other
                // methods called in the init function will have a valid pointer to use)
                s_instance = new QKinect;
                // now setup the actual class (with a valid pointer)
                s_instance->init();
        }

        // otherwise return the existing pointer
        return s_instance;
}


QKinect::QKinect() : QObject(0)
{
        kinectDetected = false;

        // nothing to see here we just need a valid object pointer the init
        // method does all the hard work
        //qDebug()<<"ctor called \n";
}


void QKinect::init()
{
        // first see if we can init the kinect
        if (freenect_init(&m_ctx, NULL) < 0)
        {
                emit message("freenect_init() failed");

                kinectDetected = false;

                return;
        }

        freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG);

        int nr_devices = freenect_num_devices (m_ctx);

        emit message(QString("%1 Kinects found.").arg(nr_devices));

        m_userDeviceNumber = 0;
        m_bufferDepth.resize(FREENECT_VIDEO_RGB_SIZE);
        m_bufferVideo.resize(FREENECT_VIDEO_RGB_SIZE);
        m_bufferDepthRaw.resize(FREENECT_FRAME_PIX);
        m_bufferDepthRaw16.resize(FREENECT_FRAME_PIX);
        m_gamma.resize(2048);

        if (freenect_open_device(m_ctx, &m_dev, m_userDeviceNumber) < 0)
        {
                emit message("ERROR opening Kinect device.");

                kinectDetected = false;

                return;
        }

        for (int i=0; i<2048; ++i)
        {
                float v = i/2048.0;
                v = std::pow(v, 3)* 6;
                m_gamma[i] = v*6*256;
        }

        m_newRgbFrame=false;
        m_newDepthFrame=false;
        m_deviceActive=true;

        // set our video formats to RGB by default
        freenect_set_video_format(m_dev, FREENECT_VIDEO_RGB);
        freenect_set_depth_format(m_dev, FREENECT_DEPTH_11BIT);

        freenect_set_depth_callback(m_dev, depthCallback);
        freenect_set_video_callback(m_dev, videoCallback);

        // start the video and depth sub systems
        startVideo();
        startDepth();

        // set the thread to be active and start
        m_process = new QKinectProcessEvents(m_ctx);
        m_process->setActive();
        m_process->start();
        setGreenLed();

        kinectDetected = true;
}


void QKinect::shutDownKinect()
{
        m_process->setInActive();
        freenect_stop_depth(m_dev);
        freenect_stop_video(m_dev);
        // close down our devices
        freenect_close_device(m_dev);
        freenect_shutdown(m_ctx);

}


void QKinect::setLedOff()
{
        freenect_set_led(m_dev,LED_OFF);
}


void QKinect::setRedLed()
{
        freenect_set_led(m_dev,LED_RED);
}


void QKinect::setGreenLed()
{
        freenect_set_led(m_dev,LED_GREEN);
}


void QKinect::setYellowLed()
{
        freenect_set_led(m_dev,LED_YELLOW);
}


void QKinect::setRedLedFlash()
{
        freenect_set_led(m_dev,LED_BLINK_RED_YELLOW);
}


void QKinect::setGreenLedFlash()
{
 freenect_set_led(m_dev,LED_BLINK_GREEN);
}


void QKinect::setYellowLedFlash()
{
        freenect_set_led(m_dev,LED_BLINK_RED_YELLOW);
}


void QKinect::setVideoMode(int _mode)
{
        freenect_video_format vm=FREENECT_VIDEO_RGB;

        switch(_mode)
        {
                case 0 : { vm=FREENECT_VIDEO_RGB; break;}
                case 1 : { vm=FREENECT_VIDEO_YUV_RGB; break;}
                case 2 : { vm=FREENECT_VIDEO_IR_8BIT; break;}
                /*
          case 1 : { vm=FREENECT_VIDEO_BAYER; break;}
                case 2 : { vm=FREENECT_VIDEO_IR_8BIT; break;}
                case 3 : { vm=FREENECT_VIDEO_IR_10BIT; break;}
                case 4 : { vm=FREENECT_VIDEO_IR_10BIT_PACKED; break;}
                case 5 : { vm=FREENECT_VIDEO_YUV_RGB; break;}
                case 6 : { vm=FREENECT_VIDEO_YUV_RAW; break;}
          */
                default :
                {
                        emit message("Kinect: index out of bounds for video mode");
                        vm = FREENECT_VIDEO_RGB;
                }
                break;
        }

        freenect_stop_video(m_dev);
        freenect_set_video_format(m_dev, vm);
        freenect_start_video(m_dev);
}


void QKinect::resetAngle()
{
        freenect_set_tilt_degs(m_dev,0);
}


void QKinect::setAngle(double _angle)
{
        // constrain the angle from -30 - +30
        if(_angle > 30)
        {
                _angle = 30;
        }
        else if(_angle <-30)
        {
                _angle=-30;
        }

        freenect_set_tilt_degs(m_dev,_angle);
}


bool QKinect::getRGB(std::vector<uint8_t> &o_buffer)
{
        QMutexLocker locker( &m_mutex );

        // do we have a new frame?
        if(m_newRgbFrame)
        {
                // if so swap out and fill
                o_buffer.swap(m_bufferVideo);
                m_newRgbFrame = false;
                return true;
        }
        else
        {
                return false;
        }
}


bool QKinect::getDepth(std::vector<uint8_t> &o_buffer)
{
        // this fills the depth buffer, first lock our mutex
        QMutexLocker locker( &m_mutex );

        if(m_newDepthFrame)
        {
                // swap data
                o_buffer.swap(m_bufferDepth);
                m_newDepthFrame = false;
                return true;
        }
        else
        {
                return false;
        }
}


bool QKinect::getDepth16Bit(std::vector<uint16_t> &o_buffer)
{
        QMutexLocker locker( &m_mutex );

        if(m_newDepthFrame)
        {
                // fill our buffer if avaliable
                o_buffer.swap( m_bufferDepthRaw16 );
                m_newDepthFrame = false;
                return true;
        }
        else
        {
                return false;
        }
}


bool QKinect::getDepthRaw(std::vector<uint8_t> &o_buffer)
{
        //qDebug()<< "getDepth callback\n";
        QMutexLocker locker( &m_mutex );

        if(m_newDepthFrame)
        {
                o_buffer.swap(m_bufferDepthRaw);
                m_newDepthFrame = false;
                return true;
        }
        else
        {
                return false;
        }
}


void QKinect::grabDepth(void *_depth, uint32_t _timestamp)
{
        // this method fills all the different depth buffers at once
        // modifed from the sample code glview and cppview.cpp
        QMutexLocker locker( &m_mutex );

        // cast the void pointer to the unint16_t the data is actually in
        uint16_t* depth = static_cast<uint16_t*>(_depth);

        // now loop and fill data buffers
        for (unsigned int i = 0; i < FREENECT_FRAME_PIX; ++i)
        {
                // first our two raw buffers the first will lose precision and may well
                // be removed in the next iterations
                m_bufferDepthRaw[i]=depth[i];
                m_bufferDepthRaw16[i]=depth[i];

                // now get the index into the gamma table
                int pval = m_gamma[depth[i]];

                // get the lower bit
                int lb = pval & 0xff;

                // shift right by 8 and determine which colour value to fill the
                // array with based on the position
                switch (pval>>8)
                {
                case 0:
                        m_bufferDepth[3*i+0] = 255;
                        m_bufferDepth[3*i+1] = 255-lb;
                        m_bufferDepth[3*i+2] = 255-lb;
                        break;
                case 1:
                        m_bufferDepth[3*i+0] = 255;
                        m_bufferDepth[3*i+1] = lb;
                        m_bufferDepth[3*i+2] = 0;
                        break;
                case 2:
                        m_bufferDepth[3*i+0] = 255-lb;
                        m_bufferDepth[3*i+1] = 255;
                        m_bufferDepth[3*i+2] = 0;
                        break;
                case 3:
                        m_bufferDepth[3*i+0] = 0;
                        m_bufferDepth[3*i+1] = 255;
                        m_bufferDepth[3*i+2] = lb;
                        break;
                case 4:
                        m_bufferDepth[3*i+0] = 0;
                        m_bufferDepth[3*i+1] = 255-lb;
                        m_bufferDepth[3*i+2] = 255;
                        break;
                case 5:
                        m_bufferDepth[3*i+0] = 0;
                        m_bufferDepth[3*i+1] = 0;
                        m_bufferDepth[3*i+2] = 255-lb;
                        break;
                default:
                        m_bufferDepth[3*i+0] = 0;
                        m_bufferDepth[3*i+1] = 0;
                        m_bufferDepth[3*i+2] = 0;
                        break;
                }
        }

        // flag we have a new frame
        m_newDepthFrame = true;

        Q_UNUSED(_timestamp);
}


void QKinect::grabVideo(void *_video, uint32_t _timestamp)
{
        // lock our mutex and copy the data from the video buffer
        QMutexLocker locker( &m_mutex );

        uint8_t* rgb = static_cast<uint8_t*>(_video);
        std::copy(rgb, rgb+FREENECT_VIDEO_RGB_SIZE, m_bufferVideo.begin());
        m_newRgbFrame = true;

        Q_UNUSED(_timestamp);
}


void QKinect::startVideo()
{
        if(freenect_start_video(m_dev) < 0)
        {
                throw std::runtime_error("Cannot start RGB callback");
        }
}


void QKinect::stopVideo()
{
        if(freenect_stop_video(m_dev) < 0)
        {
                throw std::runtime_error("Cannot stop RGB callback");
        }
}


void QKinect::startDepth()
{
        if(freenect_start_depth(m_dev) < 0)
        {
                throw std::runtime_error("Cannot start depth callback");
        }
}


void QKinect::stopDepth()
{
        if(freenect_stop_depth(m_dev) < 0)
        {
                throw std::runtime_error("Cannot stop depth callback");
        }
}


void QKinect::toggleVideoState(bool _mode)
{
        if(_mode ==true)
        {
                startVideo();
        }
        else
        {
                stopVideo();
        }
}


void QKinect::toggleDepthState(bool _mode)
{
        if(_mode == true)
        {
                startDepth();
        }
        else
        {
                stopDepth();
        }
}