KinectIO.cpp

Go to the documentation of this file.

 
/*
 * KinectIO.cpp
 *
 *  Created on: 20.03.2012
 *      Author: Thomas Wiemann
 */
 
#include "lvr2/io/KinectIO.hpp"
#include "lvr2/io/PointBuffer.hpp"
#include "lvr2/io/DataStruct.hpp"
 
#include <Eigen/Dense>
 
#include <vector>
#include <set>
 
namespace lvr2
{
 
KinectIO* KinectIO::m_instance = 0;
 
KinectIO* KinectIO::instance()
{
        if(KinectIO::m_instance == 0)
        {
                KinectIO::m_instance = new KinectIO;
        }
 
        return KinectIO::m_instance;
 
}
 
KinectIO::KinectIO()
{
        // Dept calibration matrix initialization
        float fx = 594.21f;
        float fy = 591.04f;
        float a = -0.0030711f;
        float b = 3.3309495f;
        float cx = 339.5f;
        float cy = 242.7f;
 
        m_depthMatrix <<
                        1/fx,     0,  0, 0,
                        0,    -1/fy,  0, 0,
                        0,       0,  0, a,
                        -cx/fx, cy/fy, -1, b;
 
        // Init freenect stuff
        m_freenect = new Freenect::Freenect;
 
        m_grabber = &m_freenect->createDevice<KinectGrabber>(0);
        m_grabber->setDepthFormat(FREENECT_DEPTH_11BIT);
        m_grabber->startVideo();
        m_grabber->startDepth();
}
 
KinectIO::~KinectIO()
{
        //delete m_freenect;
}
 
PointBufferPtr KinectIO::getBuffer()
{
        // Get depth image from sensor
        std::vector<short> depthImage(480 * 680, 0);
        m_grabber->getDepthImage(depthImage);
 
        std::vector<uint8_t> colorImage(480 * 680 * 3, 0);
        m_grabber->getColorImage(colorImage);
 
        std::set<int> nans;
        for(size_t i = 0; i < depthImage.size(); i++)
        {
                if(isnan(depthImage[i])) nans.insert(i);
        }
 
        // Return null pointer if no image was grabbed
        if(depthImage.size() == 0) return PointBufferPtr();
 
        size_t numPoints = depthImage.size() - nans.size();
 
        // Convert depth image into point cloud
        PointBufferPtr buffer(new PointBuffer);
        floatArr points(new float[numPoints * 3]);
        ucharArr colors(new unsigned char[numPoints * 3]);
 
        int i,j;
        int index = 0;
        int c = 0;
        for (i = 0; i < 480; i++) {
                for (j = 0; j < 640; j++) {
 
                        if(nans.find(c) == nans.end())
                        {
                                Eigen::Vector4f v;
                                v << j, i, (float)(depthImage[i * 640 + j]), 1.0f;
                                v = m_depthMatrix.transpose() * v;
 
                                points[3 * index    ] = v(0) / v(3);
                                points[3 * index + 1] = v(1) / v(3);
                                points[3 * index + 2] = v(2) / v(3);
 
                                colors[3 * index    ] = colorImage[3 * c    ];
                                colors[3 * index + 1] = colorImage[3 * c + 1];
                                colors[3 * index + 2] = colorImage[3 * c + 2];
                                index++;
                        }
                        c++;
                }
        }
 
        buffer->setPointArray(points, numPoints);
        buffer->setColorArray(colors, numPoints);
        return buffer;
}
 
} // namespace lvr2