Skip to content

add convenience method & sample code for registration #226

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
merged 10 commits into from
May 12, 2015
Merged

add convenience method & sample code for registration #226

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
a2b72c6
switch to pass-by-value for camera param blocks
floe Mar 27, 2015
e635ce8
Merge branch 'master' into registration
floe Mar 31, 2015
c333cae
Merge branch 'master' into registration
floe May 7, 2015
2c48445
remove duplicate undistort_depth call
floe May 7, 2015
e9b337c
add all-in-one registration convenience function
floe May 7, 2015
b81e081
use bytes_per_pixel instead of hardcoded value
floe May 8, 2015
5cb1092
remove noise by setting skipped pixels to zero
floe May 10, 2015
42c9eaa
allocate registered image on freestore
floe May 11, 2015
18674e3
allocate registration object on freestore
floe May 11, 2015
769fa80
switch to portable unsigned char*
floe May 11, 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
11 changes: 11 additions & 0 deletions examples/protonect/Protonect.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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);
Expand All @@ -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];
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Given that the depth frame size is already known to be 512x424 if I recall correctly, is there any reason to not to allocate outside of the loop?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Depending on the color frame decoder, rgb->bits_per_pixel is either 3 or 4, so I'd like to keep it consistent.

registration->apply(rgb,depth,registered);
cv::imshow("registered", cv::Mat(depth->height, depth->width, CV_8UC3, registered));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This line should also handle 3 or 4 bytes per pixel?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Now that I think about it, it would probably be better to switch to 4 BPP anyway and use the extra channel to add depth data. That would make filtering a lot easier, and would probably also be faster than shifting around single bytes.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This line can be generalized when JPEG decoders of 4-byte output are added.


int key = cv::waitKey(1);
protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape

Expand All @@ -99,5 +107,8 @@ int main(int argc, char *argv[])
dev->stop();
dev->close();

delete[] registered;
delete registration;

return 0;
}
7 changes: 6 additions & 1 deletion examples/protonect/include/libfreenect2/registration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
50 changes: 44 additions & 6 deletions examples/protonect/src/registration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down