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
|
#include <error.h>
#include <libfreenect/libfreenect.h>
#include <libusb-1.0/libusb.h>
#include <pthread.h>
#include <stdio.h>
#include <unistd.h>
#include "main.h"
FILE *depth_stream = NULL;
FILE *video_stream = NULL;
FILE *accel_stream = NULL;
static
void dump_ffmpeg_24(FILE *stream, uint32_t timestamp UNUSED, void *data,
size_t data_size)
{
fwrite(data, data_size, 1, stream);
}
static
void dump_ffmpeg_pad16(FILE *stream, uint32_t timestamp UNUSED, void *data_anon,
size_t data_size)
{
uint16_t* data = data_anon;
uint16_t* end = (void*)&((char*)data_anon)[data_size];
while (data < end) {
uint32_t z = *data;
fwrite(((char*)(&z)), 3, 1, stream);
data = &data[1];
}
}
static
void handle_accel(freenect_device *dev UNUSED, freenect_raw_tilt_state* data)
{
double x, y, z;
freenect_get_mks_accel(data, &x, &y, &z);
//fprintf(accel_stream, "x=%f\ty=%f\tz=%f\n", x, y, z);
}
static
void handle_depth(freenect_device *dev UNUSED, void *depth, uint32_t timestamp)
{
printf("handle depth\n");
dump_ffmpeg_pad16(depth_stream, timestamp, depth,
freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM,
FREENECT_DEPTH_11BIT).bytes);
}
static
void handle_video(freenect_device *dev, void *rgb, uint32_t timestamp)
{
printf("handle depth\n");
dump_ffmpeg_24(video_stream, timestamp, rgb,
freenect_get_current_video_mode(dev).bytes);
}
static
void print_mode(const char *name, freenect_frame_mode mode) {
/* This is just a courtesy function to let the user know the mode
if it becomes a bother for maintainability just comment out the
code in its body. It will only break if struct entries go missing.
*/
printf("%s Mode: {%d, %d, {%d}, %d, %d, %d, %d, %d, %d, %d}\n", name,
mode.reserved, (int)mode.resolution, (int)mode.video_format, mode.bytes, mode.width,
mode.height, mode.data_bits_per_pixel, mode.padding_bits_per_pixel,
mode.framerate, mode.is_valid);
}
void thread_kinect(int video_fd, int depth_fd, int accel_fd) {
int res = 0;
freenect_context *ctx;
freenect_device *dev;
pthread_setname_np(pthread_self(), "libusb");
res = freenect_init(&ctx, 0);
pthread_setname_np(pthread_self(), "kinect");
if (res) {
error(0, 0, "freenect_init: %s", libusb_strerror(res));
goto end;
}
freenect_select_subdevices(ctx, (freenect_device_flags)(FREENECT_DEVICE_CAMERA | FREENECT_DEVICE_MOTOR));
if ((res = freenect_open_device(ctx, &dev, 0))) {
error(0, 0, "freenect_open_device: %s", libusb_strerror(res));
goto end;
}
print_mode("Depth", freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_depth_mode(dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_start_depth(dev);
print_mode("Video", freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_video_mode(dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_start_video(dev);
depth_stream = fdopen(depth_fd, "w");
video_stream = fdopen(video_fd, "w");
accel_stream = fdopen(accel_fd, "w");
freenect_set_depth_callback(dev, handle_depth);
freenect_set_video_callback(dev, handle_video);
while (running && freenect_process_events(ctx) >= 0) {
freenect_raw_tilt_state* state;
freenect_update_tilt_state(dev);
state = freenect_get_tilt_state(dev);
handle_accel(dev, state);
}
freenect_stop_depth(dev);
freenect_stop_video(dev);
freenect_close_device(dev);
end:
freenect_shutdown(ctx);
close(video_fd);
close(depth_fd);
close(accel_fd);
}
|
