Hi, I'm trying to learn OpenCV using a kinect v1 sensor.
OpenCV 4 and libfreenect are installed on ubuntu and I can run the sample programs.
My aim is to store the incoming images from the kinect in a cv::Mat structure and then display them.
I'm trying to adapt the cppview sample program in this libfreenect library.
All the Opengl/GLUT display code was removed and I added the function convertFrames() in an attempt to initialise the Mat data structures with the depth and RGB buffers. I then tried to display the two Mat structures using imshow()
The output is listed below:
./OKCV_test
Vector depth size = 1228800 Depth =[20496, 43475, 32556, 0, 4112, 43494, 32556, 0, 4112, 43494, 32556, 0, 0, 0, 0, 0, 4112, 43544, 32556, 0, 8208, 43558, 32556, 0, 8208, 43558, 32556, 0, 0, 0, 0, 0, 12304, 51692, 32556, 0, 36880, 51701, 32556, 0, 36880, 51701, 32556, 0, 0, 0, 0, 0, 24592, 47682, 32556, 0, 8208, 47701, 32556, 0, 8208, 47701, 32556, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
Segmentation fault (core dumped)
A blank 'Image' frame is displayed but then closes.
I'd appreciate any help on how to initialisation/populate the Mat structures correctly.
Here's the code that attempts to populate the Mat data structures and then display them:
//convert the frame buffer to opencv Mat
void convertFrames()
{
static std::vector<uint8_t> depth(640*480*4);
static std::vector<uint8_t> rgb(640*480*4);
printf("Vector depth size = %d ",depth.size());
device->updateState();
// printf("\r demanded tilt angle: %+4.2f device tilt angle: %+4.2f", freenect_angle, device->getState().getTiltDegs());
fflush(stdout);
device->getDepth(depth);
device->getRGB(rgb);
Mat mRGB_tmp (640, 480, CV_8UC3,&rgb);
Mat mDepth_tmp (640, 480, CV_16UC1,&depth);
cout<<"Depth ="<< mDepth_tmp << endl;
imshow("Image",mRGB_tmp);
imshow("Depth",mDepth_tmp);
got_frames = 0;
}
Here's the entire .cpp 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.
*/
#include <libfreenect/libfreenect.hpp>
#include <libfreenect/libfreenect_registration.h>
#include <pthread.h>
#include <stdio.h>
#include <iostream>
#include <string.h>
#include <cmath>
#include <vector>
#include <boost/thread/thread.hpp>
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
using namespace cv;
using namespace std;
#if defined(__APPLE__)
#include <GLUT/glut.h>
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#else
#include <GL/glut.h>
#include <GL/gl.h>
#include <GL/glu.h>
#endif
class Mtx {
public:
Mtx() {
pthread_mutex_init( &m_mutex, NULL );
}
void lock() {
pthread_mutex_lock( &m_mutex );
}
void unlock() {
pthread_mutex_unlock( &m_mutex );
}
class ScopedLock
{
Mtx & _mutex;
public:
ScopedLock(Mtx & mutex)
: _mutex(mutex)
{
_mutex.lock();
}
~ScopedLock()
{
_mutex.unlock();
}
};
private:
pthread_mutex_t m_mutex;
};
/* thanks to Yoda---- from IRC */
class MyFreenectDevice : public Freenect::FreenectDevice {
public:
MyFreenectDevice(freenect_context *_ctx, int _index)
: Freenect::FreenectDevice(_ctx, _index), m_buffer_depth(freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB).bytes),m_buffer_video(freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB).bytes), m_gamma(2048), m_new_rgb_frame(false), m_new_depth_frame(false)
{
for( unsigned int i = 0 ; i < 2048 ; i++) {
float v = i/2048.0;
v = std::pow(v, 3)* 6;
m_gamma[i] = v*6*256;
}
}
//~MyFreenectDevice(){}
// Do not call directly even in child
void VideoCallback(void* _rgb, uint32_t timestamp) {
Mtx::ScopedLock lock(m_rgb_mutex);
uint8_t* rgb = static_cast<uint8_t*>(_rgb);
std::copy(rgb, rgb+getVideoBufferSize(), m_buffer_video.begin());
m_new_rgb_frame = true;
};
// Do not call directly even in child
void DepthCallback(void* _depth, uint32_t timestamp) {
Mtx::ScopedLock lock(m_depth_mutex);
uint16_t* depth = static_cast<uint16_t*>(_depth);
for( unsigned int i = 0 ; i < 640*480 ; i++) {
int pval = m_gamma[depth[i]];
int lb = pval & 0xff;
switch (pval>>8) {
case 0:
m_buffer_depth[3*i+0] = 255;
m_buffer_depth[3*i+1] = 255-lb;
m_buffer_depth[3*i+2] = 255-lb;
break;
case 1:
m_buffer_depth[3*i+0] = 255;
m_buffer_depth[3*i+1] = lb;
m_buffer_depth[3*i+2] = 0;
break;
case 2:
m_buffer_depth[3*i+0] = 255-lb;
m_buffer_depth[3*i+1] = 255;
m_buffer_depth[3*i+2] = 0;
break;
case 3:
m_buffer_depth[3*i+0] = 0;
m_buffer_depth[3*i+1] = 255;
m_buffer_depth[3*i+2] = lb;
break;
case 4:
m_buffer_depth[3*i+0] = 0;
m_buffer_depth[3*i+1] = 255-lb;
m_buffer_depth[3*i+2] = 255;
break;
case 5:
m_buffer_depth[3*i+0] = 0;
m_buffer_depth[3*i+1] = 0;
m_buffer_depth[3*i+2] = 255-lb;
break;
default:
m_buffer_depth[3*i+0] = 0;
m_buffer_depth[3*i+1] = 0;
m_buffer_depth[3*i+2] = 0;
break;
}
}
m_new_depth_frame = true;
}
bool getRGB(std::vector<uint8_t> &buffer) {
Mtx::ScopedLock lock(m_rgb_mutex);
if (!m_new_rgb_frame)
return false;
buffer.swap(m_buffer_video);
m_new_rgb_frame = false;
return true;
}
bool getDepth(std::vector<uint8_t> &buffer) {
Mtx::ScopedLock lock(m_depth_mutex);
if (!m_new_depth_frame)
return false;
buffer.swap(m_buffer_depth);
m_new_depth_frame = false;
return true;
}
private:
std::vector<uint8_t> m_buffer_depth;
std::vector<uint8_t> m_buffer_video;
std::vector<uint16_t> m_gamma;
Mtx m_rgb_mutex;
Mtx m_depth_mutex;
bool m_new_rgb_frame;
bool m_new_depth_frame;
};
Freenect::Freenect freenect;
MyFreenectDevice* device;
freenect_video_format requested_format(FREENECT_VIDEO_RGB);
GLuint gl_depth_tex;
GLuint gl_rgb_tex;
double freenect_angle(0);
int got_frames(0),window(0);
int g_argc;
char **g_argv;
cv::Size imageSize;
//convert the frame buffer to opencv Mat
void convertFrames()
{
static std::vector<uint8_t> depth(640*480*4);
static std::vector<uint8_t> rgb(640*480*4);
// using getTiltDegs() in a closed loop is unstable
/*if(device->getState().m_code == TILT_STATUS_STOPPED){
freenect_angle = device->getState().getTiltDegs();
}*/
printf("Vector depth size = %d ",depth.size());
device->updateState();
// printf("\r demanded tilt angle: %+4.2f device tilt angle: %+4.2f", freenect_angle, device->getState().getTiltDegs());
fflush(stdout);
device->getDepth(depth);
device->getRGB(rgb);
Mat mRGB_tmp (640, 480, CV_8UC3,&rgb);
Mat mDepth_tmp (640, 480, CV_16UC1,&depth);
cout<<"Depth ="<< mDepth_tmp << endl;
imshow("Image",mRGB_tmp);
imshow("Depth",mDepth_tmp);
got_frames = 0;
}
void keyPressed(unsigned char key, int x, int y)
{
if (key == 27) {
// glutDestroyWindow(window);
}
if (key == '1') {
device->setLed(LED_GREEN);
}
if (key == '2') {
device->setLed(LED_RED);
}
if (key == '3') {
device->setLed(LED_YELLOW);
}
if (key == '4') {
device->setLed(LED_BLINK_GREEN);
}
if (key == '5') {
// 5 is the same as 4
device->setLed(LED_BLINK_GREEN);
}
if (key == '6') {
device->setLed(LED_BLINK_RED_YELLOW);
}
if (key == '0') {
device->setLed(LED_OFF);
}
if (key == 'f') {
if (requested_format == FREENECT_VIDEO_IR_8BIT)
requested_format = FREENECT_VIDEO_RGB;
else if (requested_format == FREENECT_VIDEO_RGB)
requested_format = FREENECT_VIDEO_YUV_RGB;
else
requested_format = FREENECT_VIDEO_IR_8BIT;
device->setVideoFormat(requested_format);
}
if (key == 'w') {
freenect_angle++;
if (freenect_angle > 30) {
freenect_angle = 30;
}
}
if (key == 's' || key == 'd') {
freenect_angle = 0;
}
if (key == 'x') {
freenect_angle--;
if (freenect_angle < -30) {
freenect_angle = -30;
}
}
if (key == 'e') {
freenect_angle = 10;
}
if (key == 'c') {
freenect_angle = -10;
}
device->setTiltDegrees(freenect_angle);
}
int main(int argc, char **argv) {
//More Kinect Setup
static std::vector<uint16_t> kdepth(640*480);
static std::vector<uint8_t> krgb(640*480*4);
// Create and setup OpenCV
Mat mRGB (640, 480, CV_8UC3);
Mat mDepth (640, 480, CV_16UC1);
imageSize = mRGB.size();
cv::namedWindow("Image", WINDOW_AUTOSIZE);
cv::namedWindow("Depth", WINDOW_AUTOSIZE);
device = &freenect.createDevice<MyFreenectDevice>(0);
device->startVideo();
device->startDepth();
convertFrames();
device->stopVideo();
device->stopDepth();
return 0;
}