Merge pull request #226 from floe/registration

add convenience method & sample code for registration

Author: floe

examples/protonect/Protonect.cpp

@@ -33,6 +33,7 @@
 #include <libfreenect2/libfreenect2.hpp>
 #include <libfreenect2/frame_listener_impl.h>
 #include <libfreenect2/threading.h>
+#include <libfreenect2/registration.h>
 
 bool protonect_shutdown = false;
 
@@ -76,6 +77,9 @@ int main(int argc, char *argv[])
   std::cout << "device serial: " << dev->getSerialNumber() << std::endl;
   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)
   {
     listener.waitForNewFrame(frames);
@@ -87,6 +91,10 @@ 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));
+
     int key = cv::waitKey(1);
     protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape
 
@@ -99,5 +107,8 @@ int main(int argc, char *argv[])
   dev->stop();
   dev->close();
 
+  delete[] registered;
+  delete registration;
+
   return 0;
 }

examples/protonect/include/libfreenect2/registration.h

@@ -30,17 +30,22 @@
 #include <string>
 #include <libfreenect2/config.h>
 #include <libfreenect2/libfreenect2.hpp>
+#include <libfreenect2/frame_listener.hpp>
 
 namespace libfreenect2
 {
 
 class LIBFREENECT2_API Registration
 {
 public:
-  Registration(Freenect2Device::IrCameraParams *depth_p, Freenect2Device::ColorCameraParams *rgb_p);
+  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);
 
+  // undistort/register a whole image
+  void apply(Frame* rgb, Frame* depth, unsigned char* registered);
+
 private:
   void undistort_depth(int dx, int dy, float& mx, float& my);
   void depth_to_color(float mx, float my, float& rx, float& ry);

examples/protonect/src/registration.cpp

@@ -87,22 +87,60 @@ void Registration::apply( int dx, int dy, float dz, float& cx, float &cy)
   cy = ry * color.fy + color.cy;
 }
 
-Registration::Registration(Freenect2Device::IrCameraParams *depth_p, Freenect2Device::ColorCameraParams *rgb_p):
-  depth(*depth_p), color(*rgb_p)
+void Registration::apply(Frame* rgb, Frame* depth, unsigned char* registered)
+{
+  if (!depth || !rgb || !registered)
+    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];
+    }
+  }
+
+}
+
+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;