Improved speed of registration by factor 5.

Changed type for registered image to libfreenect2::Frame, so that it is possible to check for correct size.
Changed layout of maps to be similar to the image layout.
Added a map for precomputed y color indices.

Author: Thiemo Wiedemeyer

examples/protonect/Protonect.cpp

@@ -124,6 +124,7 @@ int main(int argc, char *argv[])
 
   libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
   libfreenect2::FrameMap frames;
+  libfreenect2::Frame registered(512, 424, 3);
 
   dev->setColorFrameListener(&listener);
   dev->setIrAndDepthFrameListener(&listener);
@@ -133,7 +134,6 @@ int main(int argc, char *argv[])
   std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl;
 
   libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams());
-  unsigned char* registered = NULL;
 
   while(!protonect_shutdown)
   {
@@ -146,9 +146,8 @@ int main(int argc, char *argv[])
     cv::imshow("ir", cv::Mat(ir->height, ir->width, CV_32FC1, ir->data) / 20000.0f);
     cv::imshow("depth", cv::Mat(depth->height, depth->width, CV_32FC1, depth->data) / 4500.0f);
 
-    if (!registered) registered = new unsigned char[depth->height*depth->width*rgb->bytes_per_pixel];
-    registration->apply(rgb,depth,registered);
-    cv::imshow("registered", cv::Mat(depth->height, depth->width, CV_8UC3, registered));
+    registration->apply(rgb,depth,&registered);
+    cv::imshow("registered", cv::Mat(registered.height, registered.width, CV_8UC3, registered.data));
 
     int key = cv::waitKey(1);
     protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape
@@ -162,7 +161,6 @@ int main(int argc, char *argv[])
   dev->stop();
   dev->close();
 
-  delete[] registered;
   delete registration;
 
   return 0;

examples/protonect/include/libfreenect2/registration.h

@@ -41,10 +41,10 @@ class LIBFREENECT2_API Registration
   Registration(Freenect2Device::IrCameraParams depth_p, Freenect2Device::ColorCameraParams rgb_p);
 
   // undistort/register a single depth data point
-  void apply( int dx, int dy, float dz, float& cx, float &cy);
+  void apply(int dx, int dy, float dz, float& cx, float &cy) const;
 
   // undistort/register a whole image
-  void apply(Frame* rgb, Frame* depth, unsigned char* registered);
+  void apply(const Frame* rgb, const Frame* depth, Frame* registered) const;
 
 private:
   void undistort_depth(int dx, int dy, float& mx, float& my);
@@ -53,8 +53,10 @@ class LIBFREENECT2_API Registration
   Freenect2Device::IrCameraParams depth;
   Freenect2Device::ColorCameraParams color;
 
-  float undistort_map[512][424][2];
-  float depth_to_color_map[512][424][2];
+  float undistort_map[512 * 424 * 2];
+  float depth_to_color_map_x[512 * 424];
+  float depth_to_color_map_y[512 * 424];
+  int depth_to_color_map_i[512 * 424];
 };
 
 } /* namespace libfreenect2 */

examples/protonect/src/registration.cpp

@@ -72,78 +72,86 @@ 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.fx * color_q);
-  ry = wy / (color.fx * color_q);
+  rx = (wx / (color.fx * color_q)) - (color.shift_m / color.shift_d);
+  ry = (wy / (color.fx * color_q)) * color.fy + color.cy;
 }
 
-void Registration::apply( int dx, int dy, float dz, float& cx, float &cy)
+void Registration::apply( int dx, int dy, float dz, float& cx, float &cy) const
 {
-  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);
+  const int index = dx + dy * 512;
+  float rx = depth_to_color_map_x[index];
+  cy = depth_to_color_map_y[index];
 
+  rx += (color.shift_m / dz);
   cx = rx * color.fx + color.cx;
-  cy = ry * color.fy + color.cy;
 }
 
-void Registration::apply(Frame* rgb, Frame* depth, unsigned char* registered)
+void Registration::apply(const Frame* rgb, const Frame* depth, Frame* registered) const
 {
-  if (!depth || !rgb || !registered)
+  if (!depth || !rgb || !registered ||
+      depth->width != 512 || depth->height != 424 || depth->bytes_per_pixel != 4 ||
+      rgb->width != 1920 || rgb->height != 1080 || rgb->bytes_per_pixel != 3 ||
+      registered->width != 512 || registered->height != 424 || registered->bytes_per_pixel != 3)
     return;
 
-  float* depth_raw = (float*)depth->data;
-  float cx, cy;
-  int c_off, d_off, r_off;
-
-  for (int x = 0; x < depth->width; x++) {
-    for (int y = 0; y < depth->height; y++) {
-
-      d_off = y*depth->width + x;
-      r_off = d_off*rgb->bytes_per_pixel;
-
-      float z_raw = depth_raw[d_off];
-      if (z_raw == 0.0) {
-        registered[r_off+0] = 0;
-        registered[r_off+1] = 0;
-        registered[r_off+2] = 0;
-        continue;
-      }
-
-      apply(x,y,z_raw,cx,cy);
-
-      c_off = (round(cx) + round(cy) * rgb->width) * rgb->bytes_per_pixel;
-      if ((c_off < 0) || (c_off > rgb->width*rgb->height*rgb->bytes_per_pixel)) {
-        registered[r_off+0] = 0;
-        registered[r_off+1] = 0;
-        registered[r_off+2] = 0;
-        continue;
-      }
-
-      registered[r_off+0] = rgb->data[c_off+0];
-      registered[r_off+1] = rgb->data[c_off+1];
-      registered[r_off+2] = rgb->data[c_off+2];
+  const float *depth_raw = (float*)depth->data;
+  const float *map_x = depth_to_color_map_x;
+  const int *map_i = depth_to_color_map_i;
+  unsigned char *registered_raw = registered->data;
+  const int size_depth = 512 * 424;
+  const int size_color = 1920 * 1080 * 3;
+
+  for (int i = 0; i < size_depth; ++i, ++registered_raw, ++map_x, ++map_i, ++depth_raw) {
+    const float z_raw = *depth_raw;
+
+    if (z_raw == 0.0) {
+      *registered_raw = 0;
+      *++registered_raw = 0;
+      *++registered_raw = 0;
+      continue;
     }
-  }
 
+    const float rx = (*map_x + (color.shift_m / z_raw)) * color.fx + color.cx;
+    const int cx = rx + 0.5f; // same as round for positive numbers
+    const int cy = *map_i;
+    const int c_off = cx * 3 + cy;
+
+    if (c_off < 0 || c_off > size_color || rx < -0.5f) {
+      *registered_raw = 0;
+      *++registered_raw = 0;
+      *++registered_raw = 0;
+      continue;
+    }
+
+    const unsigned char *rgb_data = rgb->data + c_off;
+    *registered_raw = *rgb_data;
+    *++registered_raw = *++rgb_data;
+    *++registered_raw = *++rgb_data;
+  }
 }
 
 Registration::Registration(Freenect2Device::IrCameraParams depth_p, Freenect2Device::ColorCameraParams rgb_p):
   depth(depth_p), color(rgb_p)
 {
   float mx, my;
   float rx, ry;
+  float *it_undist = undistort_map;
+  float *map_x = depth_to_color_map_x;
+  float *map_y = depth_to_color_map_y;
+  int *map_i = depth_to_color_map_i;
 
-  for (int x = 0; x < 512; x++)
-    for (int y = 0; y < 424; y++) {
+  for (int y = 0; y < 424; y++) {
+    for (int x = 0; x < 512; x++) {
 
       undistort_depth(x,y,mx,my);
-      undistort_map[x][y][0] = mx;
-      undistort_map[x][y][1] = my;
+      *it_undist++ = mx;
+      *it_undist++ = my;
 
       depth_to_color(mx,my,rx,ry);
-      depth_to_color_map[x][y][0] = rx;
-      depth_to_color_map[x][y][1] = ry;
+      *map_x++ = rx;
+      *map_y++ = ry;
+      *map_i++ = round(ry) * 1920 * 3;
+    }
   }
 }