Merge pull request #253 from wiedemeyer/improved_registration

Added filtering of shadowed color regions to registration

Author: floe

examples/protonect/Protonect.cpp

@@ -124,7 +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 undistorted(512, 424, 4), registered(512, 424, 3);
+  libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
 
   dev->setColorFrameListener(&listener);
   dev->setIrAndDepthFrameListener(&listener);
@@ -142,14 +142,14 @@ int main(int argc, char *argv[])
     libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
     libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
 
-    cv::imshow("rgb", cv::Mat(rgb->height, rgb->width, CV_8UC3, rgb->data));
+    cv::imshow("rgb", cv::Mat(rgb->height, rgb->width, CV_8UC4, rgb->data));
     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);
 
     registration->apply(rgb,depth,&undistorted,&registered);
 
     cv::imshow("undistorted", cv::Mat(undistorted.height, undistorted.width, CV_32FC1, undistorted.data) / 4500.0f);
-    cv::imshow("registered", cv::Mat(registered.height, registered.width, CV_8UC3, registered.data));
+    cv::imshow("registered", cv::Mat(registered.height, registered.width, CV_8UC4, registered.data));
 
     int key = cv::waitKey(1);
     protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape

examples/protonect/include/libfreenect2/registration.h

@@ -44,7 +44,7 @@ class LIBFREENECT2_API Registration
   void apply(int dx, int dy, float dz, float& cx, float &cy) const;
 
   // undistort/register a whole image
-  void apply(const Frame* rgb, const Frame* depth, Frame* undistorted, Frame* registered) const;
+  void apply(const Frame* rgb, const Frame* depth, Frame* undistorted, Frame* registered, const bool enable_filter = true) const;
 
 private:
   void distort(int mx, int my, float& dx, float& dy) const;
@@ -57,6 +57,10 @@ class LIBFREENECT2_API Registration
   float depth_to_color_map_x[512 * 424];
   float depth_to_color_map_y[512 * 424];
   int depth_to_color_map_yi[512 * 424];
+
+  const int filter_width_half;
+  const int filter_height_half;
+  const float filter_tolerance;
 };
 
 } /* namespace libfreenect2 */

examples/protonect/src/registration.cpp

@@ -84,80 +84,149 @@ void Registration::apply( int dx, int dy, float dz, float& cx, float &cy) const
   cx = rx * color.fx + color.cx;
 }
 
-void Registration::apply(const Frame *rgb, const Frame *depth, Frame *undistorted, Frame *registered) const
+void Registration::apply(const Frame *rgb, const Frame *depth, Frame *undistorted, Frame *registered, const bool enable_filter) const
 {
   // Check if all frames are valid and have the correct size
   if (!undistorted || !rgb || !registered ||
-      rgb->width != 1920 || rgb->height != 1080 || rgb->bytes_per_pixel != 3 ||
+      rgb->width != 1920 || rgb->height != 1080 || rgb->bytes_per_pixel != 4 ||
       depth->width != 512 || depth->height != 424 || depth->bytes_per_pixel != 4 ||
       undistorted->width != 512 || undistorted->height != 424 || undistorted->bytes_per_pixel != 4 ||
-      registered->width != 512 || registered->height != 424 || registered->bytes_per_pixel != 3)
+      registered->width != 512 || registered->height != 424 || registered->bytes_per_pixel != 4)
     return;
 
   const float *depth_data = (float*)depth->data;
+  const unsigned int *rgb_data = (unsigned int*)rgb->data;
   float *undistorted_data = (float*)undistorted->data;
-  unsigned char *registered_data = registered->data;
+  unsigned int *registered_data = (unsigned int*)registered->data;
   const int *map_dist = distort_map;
   const float *map_x = depth_to_color_map_x;
   const int *map_yi = depth_to_color_map_yi;
+
   const int size_depth = 512 * 424;
-  const int size_color = 1920 * 1080 * 3;
+  const int size_color = 1920 * 1080;
   const float color_cx = color.cx + 0.5f; // 0.5f added for later rounding
 
+  // size of filter map with a border of filter_height_half on top and bottom so that no check for borders is needed.
+  // since the color image is wide angle no border to the sides is needed.
+  const int size_filter_map = size_color + 1920 * filter_height_half * 2;
+  // offset to the important data
+  const int offset_filter_map = 1920 * filter_height_half;
+
+  // map for storing the min z values used for each color pixel
+  float *filter_map = NULL;
+  // pointer to the beginning of the important data
+  float *p_filter_map = NULL;
+
+  // map for storing the color offest for each depth pixel
+  int *depth_to_c_off = new int[size_depth];
+  int *map_c_off = depth_to_c_off;
+
+  // initializing the depth_map with values outside of the Kinect2 range
+  if(enable_filter){
+    filter_map = new float[size_filter_map];
+    p_filter_map = filter_map + offset_filter_map;
+
+    for(float *it = filter_map, *end = filter_map + size_filter_map; it != end; ++it){
+      *it = 65536.0f;
+    }
+  }
+
   // iterating over all pixels from undistorted depth and registered color image
-  // the three maps have the same structure as the images, so their pointers are increased each iteration as well
-  for (int i = 0; i < size_depth; ++i, ++registered_data, ++undistorted_data, ++map_dist, ++map_x, ++map_yi) {
+  // the four maps have the same structure as the images, so their pointers are increased each iteration as well
+  for(int i = 0; i < size_depth; ++i, ++undistorted_data, ++map_dist, ++map_x, ++map_yi, ++map_c_off){
     // getting index of distorted depth pixel
     const int index = *map_dist;
 
     // check if distorted depth pixel is outside of the depth image
     if(index < 0){
+      *map_c_off = -1;
       *undistorted_data = 0;
-      *registered_data = 0;
-      *++registered_data = 0;
-      *++registered_data = 0;
       continue;
     }
 
     // getting depth value for current pixel
-    const float z_raw = depth_data[index];
-    *undistorted_data = z_raw;
+    const float z = depth_data[index];
+    *undistorted_data = z;
 
     // checking for invalid depth value
-    if (z_raw <= 0.0f) {
-      *registered_data = 0;
-      *++registered_data = 0;
-      *++registered_data = 0;
+    if(z <= 0.0f){
+      *map_c_off = -1;
       continue;
     }
 
     // calculating x offset for rgb image based on depth value
-    const float rx = (*map_x + (color.shift_m / z_raw)) * color.fx + color_cx;
+    const float rx = (*map_x + (color.shift_m / z)) * color.fx + color_cx;
     const int cx = rx; // same as round for positive numbers (0.5f was already added to color_cx)
     // getting y offset for depth image
     const int cy = *map_yi;
     // combining offsets
-    const int c_off = cx * 3 + cy;
+    const int c_off = cx + cy * 1920;
 
     // check if c_off is outside of rgb image
     // checking rx/cx is not needed because the color image is much wider then the depth image
-    if (c_off < 0 || c_off >= size_color) {
-      *registered_data = 0;
-      *++registered_data = 0;
-      *++registered_data = 0;
+    if(c_off < 0 || c_off >= size_color){
+      *map_c_off = -1;
       continue;
     }
 
-    // Setting RGB or registered image
-    const unsigned char *rgb_data = rgb->data + c_off;
-    *registered_data = *rgb_data;
-    *++registered_data = *++rgb_data;
-    *++registered_data = *++rgb_data;
+    // saving the offset for later
+    *map_c_off = c_off;
+
+    if(enable_filter){
+      // setting a window around the filter map pixel corresponding to the color pixel with the current z value
+      int yi = (cy - filter_height_half) * 1920 + cx - filter_width_half; // index of first pixel to set
+      for(int r = -filter_height_half; r <= filter_height_half; ++r, yi += 1920) // index increased by a full row each iteration
+      {
+        float *it = p_filter_map + yi;
+        for(int c = -filter_width_half; c <= filter_width_half; ++c, ++it)
+        {
+          // only set if the current z is smaller
+          if(z < *it)
+            *it = z;
+        }
+      }
+    }
+  }
+
+  // reseting the pointers to the beginning
+  map_c_off = depth_to_c_off;
+  undistorted_data = (float*)undistorted->data;
+
+  if(enable_filter){
+    // run through all registered color pixels and set them based on filter results
+    for(int i = 0; i < size_depth; ++i, ++map_c_off, ++undistorted_data, ++registered_data){
+      const int c_off = *map_c_off;
+
+      // check if offset is out of image
+      if(c_off < 0){
+        *registered_data = 0;
+        continue;
+      }
+
+      const float min_z = p_filter_map[c_off];
+      const float z = *undistorted_data;
+
+      // check for allowed depth noise
+      *registered_data = (z - min_z) / z > filter_tolerance ? 0 : *(rgb_data + c_off);
+    }
+
+    delete[] filter_map;
+  }
+  else
+  {
+    // run through all registered color pixels and set them based on c_off
+    for(int i = 0; i < size_depth; ++i, ++map_c_off, ++registered_data){
+      const int c_off = *map_c_off;
+
+      // check if offset is out of image
+      *registered_data = c_off < 0 ? 0 : *(rgb_data + c_off);
+    }
   }
+  delete[] depth_to_c_off;
 }
 
 Registration::Registration(Freenect2Device::IrCameraParams depth_p, Freenect2Device::ColorCameraParams rgb_p):
-  depth(depth_p), color(rgb_p)
+  depth(depth_p), color(rgb_p), filter_width_half(2), filter_height_half(1), filter_tolerance(0.01f)
 {
   float mx, my;
   int ix, iy, index;
@@ -186,7 +255,7 @@ Registration::Registration(Freenect2Device::IrCameraParams depth_p, Freenect2Dev
       *map_x++ = rx;
       *map_y++ = ry;
       // compute the y offset to minimize later computations
-      *map_yi++ = roundf(ry) * 1920 * 3;
+      *map_yi++ = roundf(ry);
     }
   }
 }

examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp

@@ -73,7 +73,7 @@ class TurboJpegRgbPacketProcessorImpl
 
   void newFrame()
   {
-    frame = new Frame(1920, 1080, tjPixelSize[TJPF_BGR]);
+    frame = new Frame(1920, 1080, tjPixelSize[TJPF_BGRX]);
   }
 
   void startTiming()
@@ -115,7 +115,7 @@ void TurboJpegRgbPacketProcessor::process(const RgbPacket &packet)
     impl_->frame->timestamp = packet.timestamp;
     impl_->frame->sequence = packet.sequence;
 
-    int r = tjDecompress2(impl_->decompressor, packet.jpeg_buffer, packet.jpeg_buffer_length, impl_->frame->data, 1920, 1920 * tjPixelSize[TJPF_BGR], 1080, TJPF_BGR, 0);
+    int r = tjDecompress2(impl_->decompressor, packet.jpeg_buffer, packet.jpeg_buffer_length, impl_->frame->data, 1920, 1920 * tjPixelSize[TJPF_BGRX], 1080, TJPF_BGRX, 0);
 
     if(r == 0)
     {