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
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
|
#include "frame-queue.hpp"
#include "log.hpp"
#include "compat/macros1.h"
using namespace std::chrono_literals;
FrameQueue::FrameQueue(uint32_t frame_size) :
frame_buffer(std::make_unique<uint8_t[]>(frame_size * num_frames)),
frame_size(frame_size)
{
}
uint8_t* FrameQueue::Enqueue()
{
std::lock_guard<std::mutex> lock(mutex);
uint8_t* new_frame;
// Unlike traditional producer/consumer, we don't block the producer if the buffer is full (ie. the consumer is not reading data fast enough).
// Instead, if the buffer is full, we simply return the current frame pointer, causing the producer to overwrite the previous frame.
// This allows performance to degrade gracefully: if the consumer is not fast enough (< Camera FPS), it will miss frames, but if it is fast enough (>= Camera FPS), it will see everything.
//
// Note that because the the producer is writing directly to the ring buffer, we can only ever be a maximum of num_frames-1 ahead of the consumer,
// otherwise the producer could overwrite the frame the consumer is currently reading (in case of a slow consumer)
if (available >= num_frames - 1)
return ptr() + head * frame_size;
// Note: we don't need to copy any data to the buffer since the USB packets are directly written to the frame buffer.
// We just need to update head and available count to signal to the consumer that a new frame is available
head = (head + 1) % num_frames;
available++;
// Determine the next frame pointer that the producer should write to
new_frame = ptr() + head * frame_size;
// Signal consumer that data became available
queue_cvar.notify_one();
return new_frame;
}
bool FrameQueue::Dequeue(uint8_t* dest, int width, int height, ps3eye_camera::format fmt, bool flip_v)
{
std::unique_lock<std::mutex> lock(mutex);
if (!queue_cvar.wait_for(lock, 3000ms, [this] { return available != 0; }))
{
debug2("frame timeout\n");
return false;
}
// Copy from internal buffer
uint8_t* src = ptr() + frame_size * tail;
using f = typename ps3eye_camera::format;
switch (fmt)
{
case f::format_Bayer:
memcpy(dest, src, frame_size);
break;
case f::format_BGR:
case f::format_RGB:
debayer_RGB<3>(width, height, src, dest, fmt == f::format_BGR, flip_v);
break;
case f::format_BGRA:
case f::format_RGBA:
debayer_RGB<4>(width, height, src, dest, fmt == ps3eye_camera::format_BGRA, flip_v);
break;
case f::format_Gray:
DebayerGray(width, height, src, dest);
break;
default:
unreachable();
}
// Update tail and available count
tail = (tail + 1) % num_frames;
available--;
return true;
}
void FrameQueue::DebayerGray(int frame_width, int frame_height, const uint8_t* inBayer, uint8_t* outBuffer)
{
// PSMove output is in the following Bayer format (GRBG):
//
// G R G R G R
// B G B G B G
// G R G R G R
// B G B G B G
//
// This is the normal Bayer pattern shifted left one place.
int source_stride = frame_width;
const uint8_t* source_row = inBayer; // Start at first bayer pixel
int dest_stride = frame_width;
uint8_t* dest_row = outBuffer + dest_stride + 1; // We start outputting at the second pixel of the second row's G component
uint32_t R,G,B;
// Fill rows 1 to height-2 of the destination buffer. First and last row are filled separately (they are copied from the second row and second-to-last rows respectively)
for (int y = 0; y < frame_height-2; source_row += source_stride, dest_row += dest_stride, ++y)
{
const uint8_t* source = source_row;
const uint8_t* source_end = source + (source_stride-2); // -2 to deal with the fact that we're starting at the second pixel of the row and should end at the second-to-last pixel of the row (first and last are filled separately)
uint8_t* dest = dest_row;
// Row starting with Green
if (y % 2 == 0)
{
// Fill first pixel (green)
B = (uint32_t) ((source[source_stride] + source[source_stride + 2] + 1) >> 1);
G = source[source_stride + 1];
R = (uint32_t) ((source[1] + source[source_stride * 2 + 1] + 1) >> 1);
*dest = (uint8_t)((R*77 + G*151 + B*28)>>8);
source++;
dest++;
// Fill remaining pixel
for (; source <= source_end - 2; source += 2, dest += 2)
{
// Blue pixel
B = source[source_stride + 1];
G = (uint32_t) ((source[1] + source[source_stride] + source[source_stride + 2] + source[source_stride * 2 + 1] + 2) >> 2);
R = (uint32_t) ((source[0] + source[2] + source[source_stride * 2] + source[source_stride * 2 + 2] + 2) >> 2);
dest[0] = (uint8_t)((R*77 + G*151 + B*28)>>8);
// Green pixel
B = (uint32_t) ((source[source_stride + 1] + source[source_stride + 3] + 1) >> 1);
G = source[source_stride + 2];
R = (uint32_t) ((source[2] + source[source_stride * 2 + 2] + 1) >> 1);
dest[1] = (uint8_t)((R*77 + G*151 + B*28)>>8);
}
}
else
{
for (; source <= source_end - 2; source += 2, dest += 2)
{
// Red pixel
B = (uint32_t) ((source[0] + source[2] + source[source_stride * 2] + source[source_stride * 2 + 2] + 2) >> 2);;
G = (uint32_t) ((source[1] + source[source_stride] + source[source_stride + 2] + source[source_stride * 2 + 1] + 2) >> 2);;
R = source[source_stride + 1];
dest[0] = (uint8_t)((R*77 + G*151 + B*28)>>8);
// Green pixel
B = (uint32_t) ((source[2] + source[source_stride * 2 + 2] + 1) >> 1);
G = source[source_stride + 2];
R = (uint32_t) ((source[source_stride + 1] + source[source_stride + 3] + 1) >> 1);
dest[1] = (uint8_t)((R*77 + G*151 + B*28)>>8);
}
}
if (source < source_end)
{
B = source[source_stride + 1];
G = (uint32_t) ((source[1] + source[source_stride] + source[source_stride + 2] + source[source_stride * 2 + 1] + 2) >> 2);
R = (uint32_t) ((source[0] + source[2] + source[source_stride * 2] + source[source_stride * 2 + 2] + 2) >> 2);;
dest[0] = (uint8_t)((R*77 + G*151 + B*28)>>8);
source++;
dest++;
}
// Fill first pixel of row (copy second pixel)
uint8_t* first_pixel = dest_row-1;
first_pixel[0] = dest_row[0];
// Fill last pixel of row (copy second-to-last pixel). Note: dest row starts at the *second* pixel of the row, so dest_row + (width-2) * num_output_channels puts us at the last pixel of the row
uint8_t* last_pixel = dest_row + (frame_width - 2);
uint8_t* second_to_last_pixel = last_pixel - 1;
last_pixel[0] = second_to_last_pixel[0];
}
// Fill first & last row
for (int i = 0; i < dest_stride; i++)
{
outBuffer[i] = outBuffer[i + dest_stride];
outBuffer[i + (frame_height - 1)*dest_stride] = outBuffer[i + (frame_height - 2)*dest_stride];
}
}
template<int nchannels>
void FrameQueue::debayer_RGB(int frame_width, int frame_height, const uint8_t* inBayer, uint8_t* outBuffer, bool inBGR, bool flip_v)
{
// PSMove output is in the following Bayer format (GRBG):
//
// G R G R G R
// B G B G B G
// G R G R G R
// B G B G B G
//
// This is the normal Bayer pattern shifted left one place.
int source_stride = frame_width;
int dest_stride = frame_width * nchannels;
// Start at first bayer pixel
const uint8_t* source_row = inBayer;
// We start outputting at the second pixel of the second row's G component
uint8_t* dest_row = outBuffer + dest_stride + nchannels + 1;
int swap_br = inBGR ? 1 : -1;
// Fill rows 1 to height-2 of the destination buffer. First and last row are filled separately (they are copied from the second row and second-to-last rows respectively)
for (int y = 0; y < frame_height-2; source_row += source_stride, dest_row += dest_stride, ++y)
{
const uint8_t* source = source_row;
// -2 to deal with the fact that we're starting at the second pixel of the row and should end at the second-to-last pixel of the row (first and last are filled separately)
const uint8_t* source_end = source + (source_stride-2);
uint8_t* dest = dest_row;
// Row starting with Green
if (y % 2 == (int)flip_v)
{
// Fill first pixel (green)
dest[-1*swap_br] = (uint8_t) ((source[source_stride] + source[source_stride + 2] + 1) >> 1);
dest[0] = source[source_stride + 1];
dest[1*swap_br] = (uint8_t) ((source[1] + source[source_stride * 2 + 1] + 1) >> 1);
set_alpha<nchannels>(dest);
source++;
dest += nchannels;
// Fill remaining pixel
for (; source <= source_end - 2; source += 2, dest += nchannels * 2)
{
// Blue pixel
uint8_t* cur_pixel = dest;
cur_pixel[-1*swap_br] = source[source_stride + 1];
cur_pixel[0] = (uint8_t) ((source[1] +
source[source_stride] +
source[source_stride + 2] +
source[source_stride * 2 + 1] +
2) >> 2);
cur_pixel[1*swap_br] = (uint8_t) ((source[0] +
source[2] +
source[source_stride * 2] +
source[source_stride * 2 + 2] +
2) >> 2);
set_alpha<nchannels>(cur_pixel);
// Green pixel
uint8_t* next_pixel = cur_pixel+nchannels;
next_pixel[-1*swap_br] = (uint8_t) ((source[source_stride + 1] + source[source_stride + 3] + 1) >> 1);
next_pixel[0] = source[source_stride + 2];
next_pixel[1*swap_br] = (uint8_t) ((source[2] + source[source_stride * 2 + 2] + 1) >> 1);
set_alpha<nchannels>(next_pixel);
}
}
else
{
for (; source <= source_end - 2; source += 2, dest += nchannels * 2)
{
// Red pixel
uint8_t* cur_pixel = dest;
cur_pixel[-1*swap_br] = (uint8_t) ((source[0] +
source[2] +
source[source_stride * 2] +
source[source_stride * 2 + 2] +
2) >> 2);;
cur_pixel[0] = (uint8_t) ((source[1] +
source[source_stride] +
source[source_stride + 2] +
source[source_stride * 2 + 1] +
2) >> 2);;
cur_pixel[1*swap_br] = source[source_stride + 1];
set_alpha<nchannels>(cur_pixel);
// Green pixel
uint8_t* next_pixel = cur_pixel+nchannels;
next_pixel[-1*swap_br] = (uint8_t) ((source[2] + source[source_stride * 2 + 2] + 1) >> 1);
next_pixel[0] = source[source_stride + 2];
next_pixel[1*swap_br] = (uint8_t) ((source[source_stride + 1] + source[source_stride + 3] + 1) >> 1);
set_alpha<nchannels>(next_pixel);
}
}
if (source < source_end)
{
dest[-1*swap_br] = source[source_stride + 1];
dest[0] = (uint8_t) ((source[1] +
source[source_stride] +
source[source_stride + 2] +
source[source_stride * 2 + 1] +
2) >> 2);
dest[1*swap_br] = (uint8_t) ((source[0] +
source[2] +
source[source_stride * 2] +
source[source_stride * 2 + 2] +
2) >> 2);
set_alpha<nchannels>(dest);
source++;
dest += nchannels;
}
// Fill first pixel of row (copy second pixel)
uint8_t* first_pixel = dest_row-nchannels;
first_pixel[-1*swap_br] = dest_row[-1*swap_br];
first_pixel[0] = dest_row[0];
first_pixel[1*swap_br] = dest_row[1*swap_br];
set_alpha<nchannels>(first_pixel);
// Fill last pixel of row (copy second-to-last pixel). Note: dest row starts at the *second* pixel of the row, so dest_row + (width-2) * nchannels puts us at the last pixel of the row
uint8_t* last_pixel = dest_row + (frame_width - 2)*nchannels;
uint8_t* second_to_last_pixel = last_pixel - nchannels;
last_pixel[-1*swap_br] = second_to_last_pixel[-1*swap_br];
last_pixel[0] = second_to_last_pixel[0];
last_pixel[1*swap_br] = second_to_last_pixel[1*swap_br];
set_alpha<nchannels>(last_pixel);
}
// Fill first & last row
for (int i = 0; i < dest_stride; i++)
{
outBuffer[i] = outBuffer[i + dest_stride];
outBuffer[i + (frame_height - 1)*dest_stride] = outBuffer[i + (frame_height - 2)*dest_stride];
}
}
template<> void FrameQueue::set_alpha<3>(uint8_t*) {}
template<> void FrameQueue::set_alpha<4>(uint8_t* destGreen) { destGreen[2] = 255; }
template void FrameQueue::debayer_RGB<3>(int, int, const uint8_t*, uint8_t*, bool, bool);
template void FrameQueue::debayer_RGB<4>(int, int, const uint8_t*, uint8_t*, bool, bool);
|
