Skip to content

Add FREENECT_DEPTH_REGISTERED support to fakenect #536

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
piedar merged 1 commit into from
Jan 11, 2018
Merged

Add FREENECT_DEPTH_REGISTERED support to fakenect #536

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 14 additions & 2 deletions fakenect/fakenect.c
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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);