Skip to content

Commit

Permalink
Add FREENECT_DEPTH_REGISTERED support to fakenect (#536)
Browse files Browse the repository at this point in the history 
Reviewed-by: Benn Snyder <benn.snyder@gmail.com>
  • Loading branch information
@Cwiiis @piedar
Cwiiis authored and piedar committed Jan 11, 2018
1 parent 5ac2987 commit 83e57e1
Show file tree
Hide file tree
Showing 4 changed files with 34 additions and 14 deletions.
16 changes: 14 additions & 2 deletions fakenect/fakenect.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,10 @@ static freenect_frame_mode depth_11_mode =
{MAKE_RESERVED(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT),
FREENECT_RESOLUTION_MEDIUM, {FREENECT_DEPTH_11BIT},
640*480*2, 640, 480, 11, 5, 30, 1};
static freenect_frame_mode depth_registered_mode =
{MAKE_RESERVED(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED),
FREENECT_RESOLUTION_MEDIUM, {FREENECT_DEPTH_REGISTERED},
640*480*2, 640, 480, 16, 0, 30, 1};
static freenect_frame_mode depth_mm_mode =
{MAKE_RESERVED(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_MM),
FREENECT_RESOLUTION_MEDIUM, {FREENECT_DEPTH_MM},
Expand Down Expand Up @@ -246,6 +250,9 @@ int freenect_process_events(freenect_context *ctx)
case FREENECT_DEPTH_11BIT:
memcpy(depth_buffer, cur_depth, mode.bytes);
break;
case FREENECT_DEPTH_REGISTERED:
freenect_apply_registration(fake_dev, cur_depth, depth_buffer, true);
break;
case FREENECT_DEPTH_MM:
freenect_apply_depth_unpacked_to_mm(fake_dev, cur_depth, depth_buffer);
break;
Expand Down Expand Up @@ -353,7 +360,8 @@ int freenect_set_depth_mode(freenect_device* dev, const freenect_frame_mode mode
// underlying data. Would be better to check for conflict.
depth_mode = mode;

if (mode.depth_format == FREENECT_DEPTH_MM &&
if ((mode.depth_format == FREENECT_DEPTH_MM ||
mode.depth_format == FREENECT_DEPTH_REGISTERED) &&
dev->registration.zero_plane_info.reference_distance == 0) {
printf("Warning: older fakenect recording doesn't contain "
"registration info for mapping depth to MM units\n");
Expand Down Expand Up @@ -407,6 +415,8 @@ freenect_frame_mode freenect_find_depth_mode(freenect_resolution res, freenect_d
switch (fmt) {
case FREENECT_DEPTH_11BIT:
return depth_11_mode;
case FREENECT_DEPTH_REGISTERED:
return depth_registered_mode;
case FREENECT_DEPTH_MM:
return depth_mm_mode;
default:
Expand All @@ -420,7 +430,7 @@ freenect_frame_mode freenect_find_depth_mode(freenect_resolution res, freenect_d

int freenect_get_depth_mode_count()
{
return 1;
return 3;
}

freenect_frame_mode freenect_get_depth_mode(int mode_num)
Expand All @@ -429,6 +439,8 @@ freenect_frame_mode freenect_get_depth_mode(int mode_num)
return depth_11_mode;
else if (mode_num == 1)
return depth_mm_mode;
else if (mode_num == 2)
return depth_registered_mode;
else {
freenect_frame_mode invalid = { 0 };
return invalid;
Expand Down
2 changes: 1 addition & 1 deletion src/cameras.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -397,7 +397,7 @@ static void depth_process(freenect_device *dev, uint8_t *pkt, int len)
convert_packed11_to_16bit(dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf, 640*480);
break;
case FREENECT_DEPTH_REGISTERED:
freenect_apply_registration(dev, dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf );
freenect_apply_registration(dev, dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf, false);
break;
case FREENECT_DEPTH_MM:
freenect_apply_depth_to_mm(dev, dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf );
Expand Down
27 changes: 17 additions & 10 deletions src/registration.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ static inline void unpack_8_pixels(uint8_t *raw, uint16_t *frame)
}

// apply registration data to a single packed frame
FN_INTERNAL int freenect_apply_registration(freenect_device* dev, uint8_t* input_packed, uint16_t* output_mm)
FN_INTERNAL int freenect_apply_registration(freenect_device* dev, uint8_t* input, uint16_t* output_mm, bool unpacked)
{
freenect_registration* reg = &(dev->registration);
// set output buffer to zero using pointer-sized memory access (~ 30-40% faster than memset)
Expand All @@ -116,15 +116,22 @@ FN_INTERNAL int freenect_apply_registration(freenect_device* dev, uint8_t* input
for (y = 0; y < DEPTH_Y_RES; y++) {
for (x = 0; x < DEPTH_X_RES; x++) {

// get 8 pixels from the packed frame
if (source_index == 8) {
unpack_8_pixels( input_packed, unpack );
source_index = 0;
input_packed += 11;
}

// get the value at the current depth pixel, convert to millimeters
uint16_t metric_depth = reg->raw_to_mm_shift[ unpack[source_index++] ];
uint16_t metric_depth;

if (unpacked) {
uint32_t buf_index = y * DEPTH_X_RES + x;
metric_depth = reg->raw_to_mm_shift[((uint16_t *)input)[buf_index]];
} else {
// get 8 pixels from the packed frame
if (source_index == 8) {
unpack_8_pixels( input, unpack );
source_index = 0;
input += 11;
}

// get the value at the current depth pixel, convert to millimeters
metric_depth = reg->raw_to_mm_shift[ unpack[source_index++] ];
}

// so long as the current pixel has a depth value
if (metric_depth == DEPTH_NO_MM_VALUE) continue;
Expand Down
3 changes: 2 additions & 1 deletion src/registration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,10 +26,11 @@

#pragma once

#include <stdbool.h>
#include "libfreenect.h"

// Internal function declarations relating to registration
int freenect_init_registration(freenect_device* dev);
int freenect_apply_registration(freenect_device* dev, uint8_t* input_packed, uint16_t* output_mm);
int freenect_apply_registration(freenect_device* dev, uint8_t* input, uint16_t* output_mm, bool unpacked);
int freenect_apply_depth_to_mm(freenect_device* dev, uint8_t* input_packed, uint16_t* output_mm);
int freenect_apply_depth_unpacked_to_mm(freenect_device* dev, uint16_t* input, uint16_t* output_mm);

0 comments on commit 83e57e1

Please sign in to comment.