Skip to content

First prototype of computeCoordinates of point cloud, yet to be tested #397

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
floe merged 3 commits into from
Sep 22, 2015
Merged

First prototype of computeCoordinates of point cloud, yet to be tested #397

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
cd14949
first prototype of computeCoordinates, to be tested
Tabjones Sep 19, 2015
607f2af
updated getPointXYZRGB function, to compute a single point at a time
Tabjones Sep 22, 2015
969a70f
converted rgb to float, to suit PointXYZRGB pcl structure
Tabjones Sep 22, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions examples/protonect/include/libfreenect2/registration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,9 @@ 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;

// 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, float& rgb) const;

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;
Expand Down
38 changes: 38 additions & 0 deletions examples/protonect/src/registration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#define _USE_MATH_DEFINES
#include <math.h>
#include <libfreenect2/registration.h>
#include <limits>

namespace libfreenect2
{
Expand Down Expand Up @@ -249,6 +250,43 @@ void Registration::apply(const Frame *rgb, const Frame *depth, Frame *undistorte
delete[] depth_to_c_off;
}

/**
* 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 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::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<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;
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)
{
//depth value is not valid
x = y = z = bad_point;
rgb = 0;
return;
}
else
{
x = (c + 0.5 - cx) * fx * depth_val;
y = (r + 0.5 - cy) * fy * depth_val;
z = depth_val;
rgb = *reinterpret_cast<float*>(&registered_data[512*r+c]);
return;
}
}

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)
{
Expand Down