first prototype of computeCoordinates, to be tested

Author: Tabjones

examples/protonect/include/libfreenect2/registration.h

@@ -29,6 +29,10 @@
 #ifndef REGISTRATION_H_
 #define REGISTRATION_H_
 
+#ifdef LIBFREENECT2_WITH_CX11_SUPPORT
+#include <array>
+#endif
+
 #include <string>
 #include <libfreenect2/config.h>
 #include <libfreenect2/libfreenect2.hpp>
@@ -37,6 +41,10 @@
 namespace libfreenect2
 {
 
+#ifdef LIBFREENECT2_WITH_CX11_SUPPORT
+typedef std::array<float, 512*424> Array;
+#endif
+
 /** Combine frames of depth and color camera. */
 class LIBFREENECT2_API Registration
 {
@@ -49,6 +57,14 @@ class LIBFREENECT2_API Registration
   // undistort/register a whole image
   void apply(const Frame* rgb, const Frame* depth, Frame* undistorted, Frame* registered, const bool enable_filter = true, Frame* bigdepth = 0) const;
 
+#ifdef LIBFREENECT2_WITH_CX11_SUPPORT
+  // compute point coordinates and color from undistored and registered frames
+  void computeCoordinatesAndColor (const Frame* undistorted, const Frame* registered, Array& X, Array& Y, Array& Z, Array& RGB) const;
+#else
+  // compute point coordinates and color from undistored and registered frames
+  void computeCoordinatesAndColor (const Frame* undistorted, const Frame* registered, float* X, float* Y, float* Z, float* RGB) const;
+#endif
+
 private:
   void distort(int mx, int my, float& dx, float& dy) const;
   void depth_to_color(float mx, float my, float& rx, float& ry) const;

examples/protonect/src/registration.cpp

@@ -29,6 +29,7 @@
 #define _USE_MATH_DEFINES
 #include <math.h>
 #include <libfreenect2/registration.h>
+#include <limits>
 
 namespace libfreenect2
 {
@@ -249,6 +250,99 @@ void Registration::apply(const Frame *rgb, const Frame *depth, Frame *undistorte
   delete[] depth_to_c_off;
 }
 
+#ifdef LIBFREENECT2_WITH_CX11_SUPPORT
+/**
+ * Computes Euclidean coordinates of pixels and their color from already registered
+ * depth and color frames. I.e. constructs a point cloud.
+ * @param undistorted Undistorted depth frame from Registration::apply.
+ * @param registered Registered color frame from Registration::apply.
+ * @param[out] X x coordinates of points.
+ * @param[out] Y y coordinates of points.
+ * @param[out] Z z coordinates of points.
+ * @param[out] RGB associated rgb color of points.
+ * @note Available only when C++11 support is enabled.
+ */
+void Registration::computeCoordinatesAndColor (const Frame* undistorted, const Frame* registered, Array& X, Array& Y, Array& Z, Array& RGB) const
+{
+  const float bad_point = std::numeric_limits<float>::quiet_NaN();
+  const float cx(depth.cx), cy(depth.cy);
+  const float fx(1/depth.fx), fy(1/depth.fy);
+  float* undistorted_data = (float *)undistorted->data;
+  float* registered_data = (float *)registered->data;
+  size_t idx(0);
+  for (size_t r=0; r<undistorted->height; ++r, ++idx)
+  {
+    for (size_t c=0; c<undistorted->width; ++c, ++idx)
+    {
+      const float depth_val = undistorted_data[idx]/1000.0f; //scaling factor, so that value of 1 is one meter.
+      if (isnan(depth_val) || depth_val <= 0.001)
+      {
+        //depth value is not valid
+        X.at(idx) = Y.at(idx) = Z.at(idx) = bad_point;
+        RGB.at(idx) = 0;
+        continue;
+      }
+      const float xu = (c + 0.5 - cx)*fx;
+      const float yu = (r + 0.5 - cy)*fy;
+      X.at(idx) = xu * depth_val;
+      Y.at(idx) = yu * depth_val;
+      Z.at(idx) = depth_val;
+      RGB.at(idx) = registered_data[idx];
+    }
+  }
+}
+#else
+/**
+ * Computes Euclidean coordinates of pixels and their color from already registered
+ * depth and color frames. I.e. constructs a point cloud.
+ * @param undistorted Undistorted depth frame from Registration::apply.
+ * @param registered Registered color frame from Registration::apply.
+ * @param[out] X x coordinates of points.
+ * @param[out] Y y coordinates of points.
+ * @param[out] Z z coordinates of points.
+ * @param[out] RGB associated rgb color of points.
+ */
+void Registration::computeCoordinatesAndColor (const Frame* undistorted, const Frame* registered, float* X, float* Y, float* Z, float* RGB) const
+{
+  const size_t wid = undistorted->width;
+  const size_t hei = undistorted->height;
+  if (X == 0)
+    X = new float[wid*hei];
+  if (Y == 0)
+    Y = new float[wid*hei];
+  if (Z == 0)
+    Z = new float[wid*hei];
+  if (RGB == 0)
+    RGB = new float[wid*hei];
+  const float bad_point = std::numeric_limits<float>::quiet_NaN();
+  const float cx(depth.cx), cy(depth.cy);
+  const float fx(1/depth.fx), fy(1/depth.fy);
+  float* undistorted_data = (float *)undistorted->data;
+  float* registered_data = (float *)registered->data;
+  size_t idx(0);
+  for (size_t r=0; r<hei; ++r, ++idx)
+  {
+    for (size_t c=0; c<wid; ++c, ++idx)
+    {
+      const float depth_val = undistorted_data[idx]/1000.0f; //scaling factor, so that value of 1 is one meter.
+      if (isnan(depth_val) || depth_val <= 0.001)
+      {
+        //depth value is not valid
+        X[idx] = Y[idx] = Z[idx] = bad_point;
+        RGB[idx] = 0;
+        continue;
+      }
+      const float xu = (c + 0.5 - cx)*fx;
+      const float yu = (r + 0.5 - cy)*fy;
+      X[idx] = xu * depth_val;
+      Y[idx] = yu * depth_val;
+      Z[idx] = depth_val;
+      RGB[idx] = registered_data[idx];
+    }
+  }
+}
+#endif
+
 Registration::Registration(Freenect2Device::IrCameraParams depth_p, Freenect2Device::ColorCameraParams rgb_p):
   depth(depth_p), color(rgb_p), filter_width_half(2), filter_height_half(1), filter_tolerance(0.01f)
 {