Merge pull request #190 from floe/registration

add basic Registration class based on information by @sh0

Author: floe

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}

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

examples/protonect/include/libfreenect2/registration.h

@@ -0,0 +1,56 @@
+/*
+ * 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 <string>
+#include <libfreenect2/config.h>
+#include <libfreenect2/libfreenect2.hpp>
+
+namespace libfreenect2
+{
+
+class LIBFREENECT2_API Registration
+{
+public:
+  Registration(Freenect2Device::IrCameraParams *depth_p, Freenect2Device::ColorCameraParams *rgb_p);
+
+  void apply( int dx, int dy, float dz, float& cx, float &cy);
+
+private:
+  void undistort_depth(int dx, int dy, float& mx, float& my);
+  void depth_to_color(float mx, float my, float& rx, float& ry);
+
+  Freenect2Device::IrCameraParams depth;
+  Freenect2Device::ColorCameraParams color;
+
+  float undistort_map[512][424][2];
+  float depth_to_color_map[512][424][2];
+};
+
+} /* namespace libfreenect2 */
+#endif /* REGISTRATION_H_ */

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;

examples/protonect/src/registration.cpp

@@ -0,0 +1,112 @@
+/*
+ * 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 <math.h>
+#include <libfreenect2/registration.h>
+
+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(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;
+}
+
+void Registration::depth_to_color(float mx, float my, float& rx, float& ry)
+{
+  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);
+
+  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 = wx / (color.fx * color_q);
+  ry = wy / (color.fx * color_q);
+}
+
+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.fx + color.cx;
+  cy = ry * color.fy + color.cy;
+}
+
+Registration::Registration(Freenect2Device::IrCameraParams *depth_p, Freenect2Device::ColorCameraParams *rgb_p):
+  depth(*depth_p), color(*rgb_p)
+{
+  float mx, my;
+  float rx, ry;
+
+  for (int x = 0; x < 512; x++)
+    for (int y = 0; y < 424; y++) {
+      undistort_depth(x,y,mx,my);
+      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++) {
+      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;
+  }
+}
+
+} /* namespace libfreenect2 */