From ab9425dc7fc560d1caa48ffcaf01fc25c2815508 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Mon, 9 Feb 2015 10:37:39 +0100 Subject: [PATCH 1/8] add registration class --- examples/protonect/CMakeLists.txt | 1 + .../include/libfreenect2/registration.h | 50 ++++++++++ examples/protonect/src/registration.cpp | 98 +++++++++++++++++++ 3 files changed, 149 insertions(+) create mode 100644 examples/protonect/include/libfreenect2/registration.h create mode 100644 examples/protonect/src/registration.cpp diff --git a/examples/protonect/CMakeLists.txt b/examples/protonect/CMakeLists.txt index 1c29e413d..d9f0323e9 100644 --- a/examples/protonect/CMakeLists.txt +++ b/examples/protonect/CMakeLists.txt @@ -87,6 +87,7 @@ SET(SOURCES src/usb_control.cpp src/command_transaction.cpp + src/registration.cpp src/libfreenect2.cpp ${LIBFREENECT2_THREADING_SOURCE} ${RESOURCES_INC_FILE} diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h new file mode 100644 index 000000000..b40118fe1 --- /dev/null +++ b/examples/protonect/include/libfreenect2/registration.h @@ -0,0 +1,50 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 individual OpenKinect contributors. See the CONTRIB file + * for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + +#ifndef REGISTRATION_H_ +#define REGISTRATION_H_ + +#include +#include +#include + +namespace libfreenect2 +{ + +class LIBFREENECT2_API Registration +{ +public: + Registration(protocol::DepthCameraParamsResponse *depth_p, protocol::RgbCameraParamsResponse *rgb_p); + +private: + void undistort_depth(float dx, float dy, float& mx, float& my); + + float depth_k1, depth_k2, depth_k3; + float undistort_map[512][424][2]; +}; + +} /* namespace libfreenect2 */ +#endif /* REGISTRATION_H_ */ diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp new file mode 100644 index 000000000..9fed665db --- /dev/null +++ b/examples/protonect/src/registration.cpp @@ -0,0 +1,98 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2014 individual OpenKinect contributors. See the CONTRIB file + * for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + */ + +#include +#include + +namespace libfreenect2 +{ + +/* + * most information, including the table layout in command_response.h, was + * provided by @sh0 in https://github.com/OpenKinect/libfreenect2/issues/41 + */ + +// these seem to be hardcoded in the original SDK +static const float depth_q = 0.01; +static const float color_q = 0.002199; + +void Registration::undistort_depth(float dx, float dy, float& mx, float& my) +{ + float ps = (dx * dx) + (dy * dy); + float qs = ((ps * depth_k3 + depth_k2) * ps + depth_k1) * ps + 1.0; + for (int i = 0; i < 9; i++) { + float qd = ps / (qs * qs); + qs = ((qd * depth_k3 + depth_k2) * qd + depth_k1) * qd + 1.0; + } + mx = dx / qs; + my = dy / qs; +} + +/*void kinect2_depth_to_color(float mx, float my, float z, float& rx, float& ry) +{ + mx *= depth_f * depth_q; + my *= depth_f * depth_q; + + float wx = + (mx * mx * mx * mx_x3y0) + (my * my * my * mx_x0y3) + + (mx * mx * my * mx_x2y1) + (my * my * mx * mx_x1y2) + + (mx * mx * mx_x2y0) + (my * my * mx_x0y2) + (mx * my * mx_x1y1) + + (mx * mx_x1y0) + (my * mx_x0y1) + (mx_x0y0); + float wy = + (mx * mx * mx * my_x3y0) + (my * my * my * my_x0y3) + + (mx * mx * my * my_x2y1) + (my * my * mx * my_x1y2) + + (mx * mx * my_x2y0) + (my * my * my_x0y2) + (mx * my * my_x1y1) + + (mx * my_x1y0) + (my * my_x0y1) + (my_x0y0); + + rx = wx / (color_f * color_q); + ry = wy / (color_f * color_q); + + rx += (shift_m / z) - (shift_m / shift_d); + + rx = rx * color_f + color_cx; + ry = ry * color_f + color_cy; +}*/ + +Registration::Registration(protocol::DepthCameraParamsResponse *depth_p, protocol::RgbCameraParamsResponse *rgb_p) +{ + float mx, my; + int rx, ry; + + depth_k1 = depth_p->k1; + depth_k2 = depth_p->k2; + depth_k3 = depth_p->k3; + + for (int x = 0; x < 512; x++) + for (int y = 0; y < 424; y++) { + undistort_depth(x,y,mx,my); + rx = round(mx); + ry = round(my); + undistort_map[rx][ry][0] = x; + undistort_map[rx][ry][1] = y; + } +} + +} /* namespace libfreenect2 */ From b89583f94d11624943ecbba1a8e036d3ffc5deda Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Wed, 11 Feb 2015 13:12:29 +0100 Subject: [PATCH 2/8] add first part of actual mapping (LUT generation) --- .../include/libfreenect2/registration.h | 6 +- examples/protonect/src/registration.cpp | 60 +++++++++++-------- 2 files changed, 39 insertions(+), 27 deletions(-) diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h index b40118fe1..1eb04e6cd 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -41,9 +41,13 @@ class LIBFREENECT2_API Registration private: void undistort_depth(float dx, float dy, float& mx, float& my); + void depth_to_color(float mx, float my, float& rx, float& ry); + + protocol::DepthCameraParamsResponse *depth; + protocol::RgbCameraParamsResponse *color; - float depth_k1, depth_k2, depth_k3; float undistort_map[512][424][2]; + float depth_to_color_map[512][424][2]; }; } /* namespace libfreenect2 */ diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index 9fed665db..392cbfaa0 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -30,7 +30,7 @@ namespace libfreenect2 { -/* +/* * most information, including the table layout in command_response.h, was * provided by @sh0 in https://github.com/OpenKinect/libfreenect2/issues/41 */ @@ -42,49 +42,48 @@ static const float color_q = 0.002199; void Registration::undistort_depth(float dx, float dy, float& mx, float& my) { float ps = (dx * dx) + (dy * dy); - float qs = ((ps * depth_k3 + depth_k2) * ps + depth_k1) * ps + 1.0; + float qs = ((ps * depth->k3 + depth->k2) * ps + depth->k1) * ps + 1.0; for (int i = 0; i < 9; i++) { float qd = ps / (qs * qs); - qs = ((qd * depth_k3 + depth_k2) * qd + depth_k1) * qd + 1.0; + qs = ((qd * depth->k3 + depth->k2) * qd + depth->k1) * qd + 1.0; } mx = dx / qs; my = dy / qs; } -/*void kinect2_depth_to_color(float mx, float my, float z, float& rx, float& ry) +void Registration::depth_to_color(float mx, float my, float& rx, float& ry) { - mx *= depth_f * depth_q; - my *= depth_f * depth_q; + mx *= depth->fx * depth_q; + my *= depth->fy * depth_q; - float wx = - (mx * mx * mx * mx_x3y0) + (my * my * my * mx_x0y3) + - (mx * mx * my * mx_x2y1) + (my * my * mx * mx_x1y2) + - (mx * mx * mx_x2y0) + (my * my * mx_x0y2) + (mx * my * mx_x1y1) + - (mx * mx_x1y0) + (my * mx_x0y1) + (mx_x0y0); - float wy = - (mx * mx * mx * my_x3y0) + (my * my * my * my_x0y3) + - (mx * mx * my * my_x2y1) + (my * my * mx * my_x1y2) + - (mx * mx * my_x2y0) + (my * my * my_x0y2) + (mx * my * my_x1y1) + - (mx * my_x1y0) + (my * my_x0y1) + (my_x0y0); + float wx = + (mx * mx * mx * color->mx_x3y0) + (my * my * my * color->mx_x0y3) + + (mx * mx * my * color->mx_x2y1) + (my * my * mx * color->mx_x1y2) + + (mx * mx * color->mx_x2y0) + (my * my * color->mx_x0y2) + (mx * my * color->mx_x1y1) + + (mx * color->mx_x1y0) + (my * color->mx_x0y1) + (color->mx_x0y0); - rx = wx / (color_f * color_q); - ry = wy / (color_f * color_q); + float wy = + (mx * mx * mx * color->my_x3y0) + (my * my * my * color->my_x0y3) + + (mx * mx * my * color->my_x2y1) + (my * my * mx * color->my_x1y2) + + (mx * mx * color->my_x2y0) + (my * my * color->my_x0y2) + (mx * my * color->my_x1y1) + + (mx * color->my_x1y0) + (my * color->my_x0y1) + (color->my_x0y0); - rx += (shift_m / z) - (shift_m / shift_d); + rx = wx / (color->color_f * color_q); + ry = wy / (color->color_f * color_q); +} +/* + rx += (depth->shift_m / z) - (depth->shift_m / depth->shift_d); - rx = rx * color_f + color_cx; - ry = ry * color_f + color_cy; + rx = rx * color->fx + color_cx; + ry = ry * color->fy + color_cy; }*/ -Registration::Registration(protocol::DepthCameraParamsResponse *depth_p, protocol::RgbCameraParamsResponse *rgb_p) +Registration::Registration(protocol::DepthCameraParamsResponse *depth_p, protocol::RgbCameraParamsResponse *rgb_p): + depth(depth_p), color(rgb_p) { float mx, my; int rx, ry; - depth_k1 = depth_p->k1; - depth_k2 = depth_p->k2; - depth_k3 = depth_p->k3; - for (int x = 0; x < 512; x++) for (int y = 0; y < 424; y++) { undistort_depth(x,y,mx,my); @@ -93,6 +92,15 @@ Registration::Registration(protocol::DepthCameraParamsResponse *depth_p, protoco undistort_map[rx][ry][0] = x; undistort_map[rx][ry][1] = y; } + + for (int x = 0; x < 512; x++) + for (int y = 0; y < 424; y++) { + depth_to_color(x,y,mx,my); + rx = round(mx); + ry = round(my); + depth_to_color_map[rx][ry][0] = x; + depth_to_color_map[rx][ry][1] = y; + } } } /* namespace libfreenect2 */ From d3b9d8fd5c02ffd568866d2b657c5d147d0b2021 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Thu, 12 Feb 2015 15:03:08 +0100 Subject: [PATCH 3/8] add apply method --- .../protonect/include/libfreenect2/registration.h | 2 ++ examples/protonect/src/registration.cpp | 15 ++++++++++----- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h index 1eb04e6cd..873fb9aa2 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -39,6 +39,8 @@ class LIBFREENECT2_API Registration public: Registration(protocol::DepthCameraParamsResponse *depth_p, protocol::RgbCameraParamsResponse *rgb_p); + void apply( int dx, int dy, float dz, float& cx, float &cy); + private: void undistort_depth(float dx, float dy, float& mx, float& my); void depth_to_color(float mx, float my, float& rx, float& ry); diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index 392cbfaa0..cd0bea52c 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -71,12 +71,17 @@ void Registration::depth_to_color(float mx, float my, float& rx, float& ry) rx = wx / (color->color_f * color_q); ry = wy / (color->color_f * color_q); } -/* - rx += (depth->shift_m / z) - (depth->shift_m / depth->shift_d); - rx = rx * color->fx + color_cx; - ry = ry * color->fy + color_cy; -}*/ +void Registration::apply( int dx, int dy, float dz, float& cx, float &cy) +{ + float rx = depth_to_color_map[dx][dy][0]; + float ry = depth_to_color_map[dx][dy][1]; + + rx += (color->shift_m / dz) - (color->shift_m / color->shift_d); + + cx = rx * color->color_f + color->color_cx; + cy = ry * color->color_f + color->color_cy; +} Registration::Registration(protocol::DepthCameraParamsResponse *depth_p, protocol::RgbCameraParamsResponse *rgb_p): depth(depth_p), color(rgb_p) From 1cdb32371f63ab900aa4a4a002eb2b1a1d19a8c3 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Wed, 25 Mar 2015 22:25:52 +0100 Subject: [PATCH 4/8] add missing transformation to depth camera coordinates --- .../include/libfreenect2/registration.h | 2 +- examples/protonect/src/registration.cpp | 23 ++++++++++--------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h index 873fb9aa2..65605b308 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -42,7 +42,7 @@ class LIBFREENECT2_API Registration void apply( int dx, int dy, float dz, float& cx, float &cy); private: - void undistort_depth(float dx, float dy, float& mx, float& my); + void undistort_depth(int dx, int dy, float& mx, float& my); void depth_to_color(float mx, float my, float& rx, float& ry); protocol::DepthCameraParamsResponse *depth; diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index cd0bea52c..73c2589a6 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -39,14 +39,18 @@ namespace libfreenect2 static const float depth_q = 0.01; static const float color_q = 0.002199; -void Registration::undistort_depth(float dx, float dy, float& mx, float& my) +void Registration::undistort_depth(int x, int y, float& mx, float& my) { + float dx = ((float)x - depth->cx) / depth->fx; + float dy = ((float)y - depth->cy) / depth->fy; + float ps = (dx * dx) + (dy * dy); float qs = ((ps * depth->k3 + depth->k2) * ps + depth->k1) * ps + 1.0; for (int i = 0; i < 9; i++) { float qd = ps / (qs * qs); qs = ((qd * depth->k3 + depth->k2) * qd + depth->k1) * qd + 1.0; } + mx = dx / qs; my = dy / qs; } @@ -87,24 +91,21 @@ Registration::Registration(protocol::DepthCameraParamsResponse *depth_p, protoco depth(depth_p), color(rgb_p) { float mx, my; - int rx, ry; + float rx, ry; for (int x = 0; x < 512; x++) for (int y = 0; y < 424; y++) { undistort_depth(x,y,mx,my); - rx = round(mx); - ry = round(my); - undistort_map[rx][ry][0] = x; - undistort_map[rx][ry][1] = y; + undistort_map[x][y][0] = mx; + undistort_map[x][y][1] = my; } for (int x = 0; x < 512; x++) for (int y = 0; y < 424; y++) { - depth_to_color(x,y,mx,my); - rx = round(mx); - ry = round(my); - depth_to_color_map[rx][ry][0] = x; - depth_to_color_map[rx][ry][1] = y; + undistort_depth(x,y,mx,my); + depth_to_color(mx,my,rx,ry); + depth_to_color_map[x][y][0] = rx; + depth_to_color_map[x][y][1] = ry; } } From 8ae30fda33aa807ba1e7db0461f16c29903538ed Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Wed, 25 Mar 2015 22:29:05 +0100 Subject: [PATCH 5/8] store local copy of camera params --- .../include/libfreenect2/registration.h | 4 +- examples/protonect/src/registration.cpp | 40 +++++++++---------- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h index 65605b308..b9c53649c 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -45,8 +45,8 @@ class LIBFREENECT2_API Registration void undistort_depth(int dx, int dy, float& mx, float& my); void depth_to_color(float mx, float my, float& rx, float& ry); - protocol::DepthCameraParamsResponse *depth; - protocol::RgbCameraParamsResponse *color; + protocol::DepthCameraParamsResponse depth; + protocol::RgbCameraParamsResponse color; float undistort_map[512][424][2]; float depth_to_color_map[512][424][2]; diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index 73c2589a6..29dc7e186 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -41,14 +41,14 @@ static const float color_q = 0.002199; void Registration::undistort_depth(int x, int y, float& mx, float& my) { - float dx = ((float)x - depth->cx) / depth->fx; - float dy = ((float)y - depth->cy) / depth->fy; + float dx = ((float)x - depth.cx) / depth.fx; + float dy = ((float)y - depth.cy) / depth.fy; float ps = (dx * dx) + (dy * dy); - float qs = ((ps * depth->k3 + depth->k2) * ps + depth->k1) * ps + 1.0; + float qs = ((ps * depth.k3 + depth.k2) * ps + depth.k1) * ps + 1.0; for (int i = 0; i < 9; i++) { float qd = ps / (qs * qs); - qs = ((qd * depth->k3 + depth->k2) * qd + depth->k1) * qd + 1.0; + qs = ((qd * depth.k3 + depth.k2) * qd + depth.k1) * qd + 1.0; } mx = dx / qs; @@ -57,23 +57,23 @@ void Registration::undistort_depth(int x, int y, float& mx, float& my) void Registration::depth_to_color(float mx, float my, float& rx, float& ry) { - mx *= depth->fx * depth_q; - my *= depth->fy * depth_q; + mx *= depth.fx * depth_q; + my *= depth.fy * depth_q; float wx = - (mx * mx * mx * color->mx_x3y0) + (my * my * my * color->mx_x0y3) + - (mx * mx * my * color->mx_x2y1) + (my * my * mx * color->mx_x1y2) + - (mx * mx * color->mx_x2y0) + (my * my * color->mx_x0y2) + (mx * my * color->mx_x1y1) + - (mx * color->mx_x1y0) + (my * color->mx_x0y1) + (color->mx_x0y0); + (mx * mx * mx * color.mx_x3y0) + (my * my * my * color.mx_x0y3) + + (mx * mx * my * color.mx_x2y1) + (my * my * mx * color.mx_x1y2) + + (mx * mx * color.mx_x2y0) + (my * my * color.mx_x0y2) + (mx * my * color.mx_x1y1) + + (mx * color.mx_x1y0) + (my * color.mx_x0y1) + (color.mx_x0y0); float wy = - (mx * mx * mx * color->my_x3y0) + (my * my * my * color->my_x0y3) + - (mx * mx * my * color->my_x2y1) + (my * my * mx * color->my_x1y2) + - (mx * mx * color->my_x2y0) + (my * my * color->my_x0y2) + (mx * my * color->my_x1y1) + - (mx * color->my_x1y0) + (my * color->my_x0y1) + (color->my_x0y0); + (mx * mx * mx * color.my_x3y0) + (my * my * my * color.my_x0y3) + + (mx * mx * my * color.my_x2y1) + (my * my * mx * color.my_x1y2) + + (mx * mx * color.my_x2y0) + (my * my * color.my_x0y2) + (mx * my * color.my_x1y1) + + (mx * color.my_x1y0) + (my * color.my_x0y1) + (color.my_x0y0); - rx = wx / (color->color_f * color_q); - ry = wy / (color->color_f * color_q); + rx = wx / (color.color_f * color_q); + ry = wy / (color.color_f * color_q); } void Registration::apply( int dx, int dy, float dz, float& cx, float &cy) @@ -81,14 +81,14 @@ void Registration::apply( int dx, int dy, float dz, float& cx, float &cy) float rx = depth_to_color_map[dx][dy][0]; float ry = depth_to_color_map[dx][dy][1]; - rx += (color->shift_m / dz) - (color->shift_m / color->shift_d); + rx += (color.shift_m / dz) - (color.shift_m / color.shift_d); - cx = rx * color->color_f + color->color_cx; - cy = ry * color->color_f + color->color_cy; + cx = rx * color.color_f + color.color_cx; + cy = ry * color.color_f + color.color_cy; } Registration::Registration(protocol::DepthCameraParamsResponse *depth_p, protocol::RgbCameraParamsResponse *rgb_p): - depth(depth_p), color(rgb_p) + depth(*depth_p), color(*rgb_p) { float mx, my; float rx, ry; From 129c28094649b03bc3a8ddecf125e490de25e682 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Wed, 25 Mar 2015 22:33:52 +0100 Subject: [PATCH 6/8] add missing color camera parameters --- .../include/libfreenect2/libfreenect2.hpp | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/examples/protonect/include/libfreenect2/libfreenect2.hpp b/examples/protonect/include/libfreenect2/libfreenect2.hpp index ae52e68b6..d4c842f28 100644 --- a/examples/protonect/include/libfreenect2/libfreenect2.hpp +++ b/examples/protonect/include/libfreenect2/libfreenect2.hpp @@ -45,6 +45,30 @@ class LIBFREENECT2_API Freenect2Device struct ColorCameraParams { float fx, fy, cx, cy; + + float shift_d, shift_m; + + float mx_x3y0; // xxx + float mx_x0y3; // yyy + float mx_x2y1; // xxy + float mx_x1y2; // yyx + float mx_x2y0; // xx + float mx_x0y2; // yy + float mx_x1y1; // xy + float mx_x1y0; // x + float mx_x0y1; // y + float mx_x0y0; // 1 + + float my_x3y0; // xxx + float my_x0y3; // yyy + float my_x2y1; // xxy + float my_x1y2; // yyx + float my_x2y0; // xx + float my_x0y2; // yy + float my_x1y1; // xy + float my_x1y0; // x + float my_x0y1; // y + float my_x0y0; // 1 }; struct IrCameraParams From 886bddfb61ad49dd054b446ab6441dae30cd7788 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Wed, 25 Mar 2015 22:40:22 +0100 Subject: [PATCH 7/8] switch to external structures --- examples/protonect/include/libfreenect2/registration.h | 10 +++++----- examples/protonect/src/registration.cpp | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h index b9c53649c..e7d96e563 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -27,9 +27,9 @@ #ifndef REGISTRATION_H_ #define REGISTRATION_H_ -#include +#include #include -#include +#include namespace libfreenect2 { @@ -37,7 +37,7 @@ namespace libfreenect2 class LIBFREENECT2_API Registration { public: - Registration(protocol::DepthCameraParamsResponse *depth_p, protocol::RgbCameraParamsResponse *rgb_p); + Registration(Freenect2Device::IrCameraParams *depth_p, Freenect2Device::ColorCameraParams *rgb_p); void apply( int dx, int dy, float dz, float& cx, float &cy); @@ -45,8 +45,8 @@ class LIBFREENECT2_API Registration void undistort_depth(int dx, int dy, float& mx, float& my); void depth_to_color(float mx, float my, float& rx, float& ry); - protocol::DepthCameraParamsResponse depth; - protocol::RgbCameraParamsResponse color; + Freenect2Device::IrCameraParams depth; + Freenect2Device::ColorCameraParams color; float undistort_map[512][424][2]; float depth_to_color_map[512][424][2]; diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index 29dc7e186..e5aec6d74 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -72,8 +72,8 @@ void Registration::depth_to_color(float mx, float my, float& rx, float& ry) (mx * mx * color.my_x2y0) + (my * my * color.my_x0y2) + (mx * my * color.my_x1y1) + (mx * color.my_x1y0) + (my * color.my_x0y1) + (color.my_x0y0); - rx = wx / (color.color_f * color_q); - ry = wy / (color.color_f * color_q); + rx = wx / (color.fx * color_q); + ry = wy / (color.fx * color_q); } void Registration::apply( int dx, int dy, float dz, float& cx, float &cy) @@ -83,11 +83,11 @@ void Registration::apply( int dx, int dy, float dz, float& cx, float &cy) rx += (color.shift_m / dz) - (color.shift_m / color.shift_d); - cx = rx * color.color_f + color.color_cx; - cy = ry * color.color_f + color.color_cy; + cx = rx * color.fx + color.cx; + cy = ry * color.fy + color.cy; } -Registration::Registration(protocol::DepthCameraParamsResponse *depth_p, protocol::RgbCameraParamsResponse *rgb_p): +Registration::Registration(Freenect2Device::IrCameraParams *depth_p, Freenect2Device::ColorCameraParams *rgb_p): depth(*depth_p), color(*rgb_p) { float mx, my; From cc6fc49d1714c68feb5e57b055d6c8d423457184 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Wed, 25 Mar 2015 22:50:48 +0100 Subject: [PATCH 8/8] add missing transfer of fields from raw command response --- examples/protonect/src/libfreenect2.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/examples/protonect/src/libfreenect2.cpp b/examples/protonect/src/libfreenect2.cpp index ba57cf9a0..28ae3b3e4 100644 --- a/examples/protonect/src/libfreenect2.cpp +++ b/examples/protonect/src/libfreenect2.cpp @@ -480,6 +480,31 @@ void Freenect2DeviceImpl::start() rgb_camera_params_.cx = rgb_p->color_cx; rgb_camera_params_.cy = rgb_p->color_cy; + rgb_camera_params_.shift_d = rgb_p->shift_d; + rgb_camera_params_.shift_m = rgb_p->shift_m; + + rgb_camera_params_.mx_x3y0 = rgb_p->mx_x3y0; // xxx + rgb_camera_params_.mx_x0y3 = rgb_p->mx_x0y3; // yyy + rgb_camera_params_.mx_x2y1 = rgb_p->mx_x2y1; // xxy + rgb_camera_params_.mx_x1y2 = rgb_p->mx_x1y2; // yyx + rgb_camera_params_.mx_x2y0 = rgb_p->mx_x2y0; // xx + rgb_camera_params_.mx_x0y2 = rgb_p->mx_x0y2; // yy + rgb_camera_params_.mx_x1y1 = rgb_p->mx_x1y1; // xy + rgb_camera_params_.mx_x1y0 = rgb_p->mx_x1y0; // x + rgb_camera_params_.mx_x0y1 = rgb_p->mx_x0y1; // y + rgb_camera_params_.mx_x0y0 = rgb_p->mx_x0y0; // 1 + + rgb_camera_params_.my_x3y0 = rgb_p->my_x3y0; // xxx + rgb_camera_params_.my_x0y3 = rgb_p->my_x0y3; // yyy + rgb_camera_params_.my_x2y1 = rgb_p->my_x2y1; // xxy + rgb_camera_params_.my_x1y2 = rgb_p->my_x1y2; // yyx + rgb_camera_params_.my_x2y0 = rgb_p->my_x2y0; // xx + rgb_camera_params_.my_x0y2 = rgb_p->my_x0y2; // yy + rgb_camera_params_.my_x1y1 = rgb_p->my_x1y1; // xy + rgb_camera_params_.my_x1y0 = rgb_p->my_x1y0; // x + rgb_camera_params_.my_x0y1 = rgb_p->my_x0y1; // y + rgb_camera_params_.my_x0y0 = rgb_p->my_x0y0; // 1 + command_tx_.execute(ReadStatus0x090000Command(nextCommandSeq()), result); std::cout << "[Freenect2DeviceImpl] ReadStatus0x090000 response" << std::endl; std::cout << GenericResponse(result.data, result.length).toString() << std::endl;