-
Notifications
You must be signed in to change notification settings - Fork 2
/
kinecttest2.cpp
158 lines (143 loc) · 3.87 KB
/
kinecttest2.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#include "libfreenect.hpp"
#include <iostream>
#include <vector>
#include <cmath>
#include <pthread.h>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
using namespace cv;
using namespace std;
//taken from:
//http://openkinect.org/wiki/C%2B%2BOpenCvExample
class PMutex {
public:
PMutex() {
pthread_mutex_init( &m_mutex, NULL );
}
void lock() {
pthread_mutex_lock( &m_mutex );
}
void unlock() {
pthread_mutex_unlock( &m_mutex );
}
private:
pthread_mutex_t m_mutex;
};
class MyFreenectDevice : public Freenect::FreenectDevice {
public:
MyFreenectDevice(freenect_context *_ctx, int _index)
: Freenect::FreenectDevice(_ctx, _index), m_buffer_depth(FREENECT_DEPTH_11BIT),m_buffer_rgb(FREENECT_VIDEO_RGB), m_gamma(2048), m_new_rgb_frame(false), m_new_depth_frame(false),
depthMat(Size(640,480),CV_16UC1), rgbMat(Size(640,480),CV_8UC1,Scalar(0))
{
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;
}
}
// Do not call directly even in child
void VideoCallback(void* _rgb, uint32_t timestamp) {
//std::cout << "RGB callback" << std::endl;
m_rgb_mutex.lock();
uint8_t* rgb = static_cast<uint8_t*>(_rgb);
rgbMat.data = rgb;
m_new_rgb_frame = true;
m_rgb_mutex.unlock();
};
// Do not call directly even in child
void DepthCallback(void* _depth, uint32_t timestamp) {
/*//std::cout << "Depth callback" << std::endl;
m_depth_mutex.lock();
uint16_t* depth = static_cast<uint16_t*>(_depth);
depthMat.data = (uchar*) depth;
m_new_depth_frame = true;
m_depth_mutex.unlock();*/
}
bool getVideo(Mat& output) {
m_rgb_mutex.lock();
if(m_new_rgb_frame) {
//cv::cvtColor(rgbMat, output, CV_RGB2BGR);
rgbMat.copyTo(output);
m_new_rgb_frame = false;
m_rgb_mutex.unlock();
return true;
} else {
m_rgb_mutex.unlock();
return false;
}
}
bool getDepth(Mat& output) {
m_depth_mutex.lock();
if(m_new_depth_frame) {
depthMat.copyTo(output);
m_new_depth_frame = false;
m_depth_mutex.unlock();
return true;
} else {
m_depth_mutex.unlock();
return false;
}
}
private:
std::vector<uint8_t> m_buffer_depth;
std::vector<uint8_t> m_buffer_rgb;
std::vector<uint16_t> m_gamma;
Mat depthMat;
Mat rgbMat;
PMutex m_rgb_mutex;
PMutex m_depth_mutex;
bool m_new_rgb_frame;
bool m_new_depth_frame;
};
int main(int argc, char **argv) {
bool die(false);
string filename("snapshot");
string suffix(".png");
int i_snap(0),iter(0);
Mat depthMat(Size(640,480),CV_16UC1);
Mat depthf (Size(640,480),CV_8UC1);
Mat rgbMat(Size(640,480),CV_8UC1,Scalar(0));
//The next two lines must be changed as Freenect::Freenect isn't a template but the method createDevice:
//Freenect::Freenect<MyFreenectDevice> freenect;
//MyFreenectDevice& device = freenect.createDevice(0);
//by these two lines:
Freenect::Freenect freenect;
MyFreenectDevice& device = freenect.createDevice<MyFreenectDevice>(0);
namedWindow("rgb",CV_WINDOW_AUTOSIZE);
//namedWindow("depth",CV_WINDOW_AUTOSIZE);
device.setVideoFormat(FREENECT_VIDEO_IR_8BIT, FREENECT_RESOLUTION_MEDIUM);
try {
device.startVideo();
} catch (const runtime_error e) {
std::cout << e.what() << "\n";
}
//device.startDepth();
while (!die) {
device.getVideo(rgbMat);
//device.getDepth(depthMat);
cv::imshow("rgb", rgbMat);
//depthMat.convertTo(depthf, CV_8UC1, 255.0/2048.0);
//cv::imshow("depth",depthf);
char k = cvWaitKey(5);
if( k == 27 ){
cvDestroyWindow("rgb");
device.stopVideo();
exit(0);
//cvDestroyWindow("depth");
break;
}
if( k == 8 ) {
std::ostringstream file;
file << filename << i_snap << suffix;
cv::imwrite(file.str(),rgbMat);
i_snap++;
}
//if(iter >= 1000) break;
iter++;
//if (iter % 100 == 0) printf("%d\n", iter);
}
device.stopVideo();
//device.stopDepth();
return 0;
}