From cd1494970d6e589e570c6f152e1df93def22d681 Mon Sep 17 00:00:00 2001 From: Federico Spinelli Date: Sat, 19 Sep 2015 15:48:20 +0200 Subject: [PATCH 1/3] first prototype of computeCoordinates, to be tested --- .../include/libfreenect2/registration.h | 16 ++++ examples/protonect/src/registration.cpp | 94 +++++++++++++++++++ 2 files changed, 110 insertions(+) diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h index be3586f06..ff3c3bfd7 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -29,6 +29,10 @@ #ifndef REGISTRATION_H_ #define REGISTRATION_H_ +#ifdef LIBFREENECT2_WITH_CX11_SUPPORT +#include +#endif + #include #include #include @@ -37,6 +41,10 @@ namespace libfreenect2 { +#ifdef LIBFREENECT2_WITH_CX11_SUPPORT +typedef std::array 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; diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index 41915d337..e3c33d680 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -29,6 +29,7 @@ #define _USE_MATH_DEFINES #include #include +#include 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::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; rheight; ++r, ++idx) + { + for (size_t c=0; cwidth; ++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::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 Date: Tue, 22 Sep 2015 09:03:50 +0200 Subject: [PATCH 2/3] updated getPointXYZRGB function, to compute a single point at a time --- .../include/libfreenect2/registration.h | 17 +--- examples/protonect/src/registration.cpp | 98 ++++--------------- 2 files changed, 23 insertions(+), 92 deletions(-) diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h index ff3c3bfd7..ca8ba3180 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -29,10 +29,6 @@ #ifndef REGISTRATION_H_ #define REGISTRATION_H_ -#ifdef LIBFREENECT2_WITH_CX11_SUPPORT -#include -#endif - #include #include #include @@ -41,10 +37,6 @@ namespace libfreenect2 { -#ifdef LIBFREENECT2_WITH_CX11_SUPPORT -typedef std::array Array; -#endif - /** Combine frames of depth and color camera. */ class LIBFREENECT2_API Registration { @@ -57,13 +49,8 @@ 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 + // compute point XYZ RGB from undistored and registered frames + void getPointXYZRGB (const Frame* undistorted, const Frame* registered, int r, int c, float& x, float& y, float& z, unsigned int& rgb) const; private: void distort(int mx, int my, float& dx, float& dy) const; diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index e3c33d680..a9b2287c0 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -250,98 +250,42 @@ 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. + * Computes Euclidean coordinates of a pixel and its color from already registered + * depth and color frames. I.e. constructs a point to fill 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. + * @param r row index of depth frame this point belong to. + * @param c column index of depth frame this point belong to. + * @param[out] x x coordinate of point. + * @param[out] y y coordinate of point. + * @param[out] z z coordinate of point. + * @param[out] RGB associated rgb color of point. */ -void Registration::computeCoordinatesAndColor (const Frame* undistorted, const Frame* registered, Array& X, Array& Y, Array& Z, Array& RGB) const +void Registration::getPointXYZRGB (const Frame* undistorted, const Frame* registered, int r, int c, float& x, float& y, float& z, unsigned int& rgb) const { const float bad_point = std::numeric_limits::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; rheight; ++r, ++idx) + const float depth_val = undistorted_data[512*r+c]/1000.0f; //scaling factor, so that value of 1 is one meter. + if (isnan(depth_val) || depth_val <= 0.001) { - for (size_t c=0; cwidth; ++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]; - } + //depth value is not valid + x = y = z = bad_point; + rgb = 0; + return; } -} -#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::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 Date: Tue, 22 Sep 2015 10:45:23 +0200 Subject: [PATCH 3/3] converted rgb to float, to suit PointXYZRGB pcl structure --- examples/protonect/include/libfreenect2/registration.h | 2 +- examples/protonect/src/registration.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h index ca8ba3180..453ba8903 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -50,7 +50,7 @@ class LIBFREENECT2_API Registration void apply(const Frame* rgb, const Frame* depth, Frame* undistorted, Frame* registered, const bool enable_filter = true, Frame* bigdepth = 0) const; // compute point XYZ RGB from undistored and registered frames - void getPointXYZRGB (const Frame* undistorted, const Frame* registered, int r, int c, float& x, float& y, float& z, unsigned int& rgb) const; + void getPointXYZRGB (const Frame* undistorted, const Frame* registered, int r, int c, float& x, float& y, float& z, float& rgb) const; private: void distort(int mx, int my, float& dx, float& dy) const; diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index a9b2287c0..7e4175456 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -262,7 +262,7 @@ void Registration::apply(const Frame *rgb, const Frame *depth, Frame *undistorte * @param[out] z z coordinate of point. * @param[out] RGB associated rgb color of point. */ -void Registration::getPointXYZRGB (const Frame* undistorted, const Frame* registered, int r, int c, float& x, float& y, float& z, unsigned int& rgb) const +void Registration::getPointXYZRGB (const Frame* undistorted, const Frame* registered, int r, int c, float& x, float& y, float& z, float& rgb) const { const float bad_point = std::numeric_limits::quiet_NaN(); const float cx(depth.cx), cy(depth.cy); @@ -282,7 +282,7 @@ void Registration::getPointXYZRGB (const Frame* undistorted, const Frame* regist x = (c + 0.5 - cx) * fx * depth_val; y = (r + 0.5 - cy) * fy * depth_val; z = depth_val; - rgb = registered_data[512*r+c]; + rgb = *reinterpret_cast(®istered_data[512*r+c]); return; } }