From a2b72c61f0235cbcc81dae56def3192f70b1f40f Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Fri, 27 Mar 2015 17:47:30 +0100 Subject: [PATCH 1/8] switch to pass-by-value for camera param blocks --- 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 e7d96e563..3be24316d 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -37,7 +37,7 @@ 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); void apply( int dx, int dy, float dz, float& cx, float &cy); diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index e5aec6d74..da98c79c5 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -87,8 +87,8 @@ 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) +Registration::Registration(Freenect2Device::IrCameraParams depth_p, Freenect2Device::ColorCameraParams rgb_p): + depth(depth_p), color(rgb_p) { float mx, my; float rx, ry; From 2c484453f3bcd5c6cc9434ff7345b53651792192 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Thu, 7 May 2015 13:38:52 +0200 Subject: [PATCH 2/8] remove duplicate undistort_depth call --- examples/protonect/src/registration.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index da98c79c5..1b52fa7ea 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -95,14 +95,11 @@ Registration::Registration(Freenect2Device::IrCameraParams depth_p, Freenect2Dev 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; From e9b337c44aaa827d27215ef867bcdf6c6d2dd11c Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Thu, 7 May 2015 15:01:45 +0200 Subject: [PATCH 3/8] add all-in-one registration convenience function --- examples/protonect/Protonect.cpp | 7 ++++ .../include/libfreenect2/registration.h | 5 +++ examples/protonect/src/registration.cpp | 37 +++++++++++++++++++ 3 files changed, 49 insertions(+) diff --git a/examples/protonect/Protonect.cpp b/examples/protonect/Protonect.cpp index baac87cd5..8971172b6 100644 --- a/examples/protonect/Protonect.cpp +++ b/examples/protonect/Protonect.cpp @@ -33,6 +33,7 @@ #include #include #include +#include bool protonect_shutdown = false; @@ -76,6 +77,8 @@ 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(dev->getIrCameraParams(), dev->getColorCameraParams()); + while(!protonect_shutdown) { listener.waitForNewFrame(frames); @@ -87,6 +90,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); + uint8_t registered[depth->height*depth->width*3]; + registration.apply(rgb,depth,registered); + cv::imshow("registered", cv::Mat(depth->height, depth->width, CV_8UC3, registered)); + int key = cv::waitKey(1); protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape diff --git a/examples/protonect/include/libfreenect2/registration.h b/examples/protonect/include/libfreenect2/registration.h index 3be24316d..a519d15f1 100644 --- a/examples/protonect/include/libfreenect2/registration.h +++ b/examples/protonect/include/libfreenect2/registration.h @@ -30,6 +30,7 @@ #include #include #include +#include namespace libfreenect2 { @@ -39,8 +40,12 @@ class LIBFREENECT2_API Registration public: 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); diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index 1b52fa7ea..93736fce6 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -87,6 +87,43 @@ void Registration::apply( int dx, int dy, float dz, float& cx, float &cy) cy = ry * color.fy + color.cy; } +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*3; + + 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); + + // is rounding the right way to go here - no subpixel precision? + c_off = (round(cx) + round(cy) * rgb->width) * 3; + if ((c_off < 0) || (c_off > rgb->width*rgb->height*3)) 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) { From b81e081db0d8024ec2a36a61052ad44a7ea3217f Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Fri, 8 May 2015 12:37:31 +0200 Subject: [PATCH 4/8] use bytes_per_pixel instead of hardcoded value --- examples/protonect/Protonect.cpp | 2 +- examples/protonect/src/registration.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/protonect/Protonect.cpp b/examples/protonect/Protonect.cpp index 8971172b6..5f1121feb 100644 --- a/examples/protonect/Protonect.cpp +++ b/examples/protonect/Protonect.cpp @@ -90,7 +90,7 @@ 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); - uint8_t registered[depth->height*depth->width*3]; + uint8_t registered[depth->height*depth->width*rgb->bytes_per_pixel]; registration.apply(rgb,depth,registered); cv::imshow("registered", cv::Mat(depth->height, depth->width, CV_8UC3, registered)); diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index 93736fce6..6a577811c 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -100,7 +100,7 @@ void Registration::apply(Frame* rgb, Frame* depth, unsigned char* registered) for (int y = 0; y < depth->height; y++) { d_off = y*depth->width + x; - r_off = d_off*3; + r_off = d_off*rgb->bytes_per_pixel; float z_raw = depth_raw[d_off]; if (z_raw == 0.0) { @@ -113,8 +113,8 @@ void Registration::apply(Frame* rgb, Frame* depth, unsigned char* registered) apply(x,y,z_raw,cx,cy); // is rounding the right way to go here - no subpixel precision? - c_off = (round(cx) + round(cy) * rgb->width) * 3; - if ((c_off < 0) || (c_off > rgb->width*rgb->height*3)) continue; + 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)) continue; registered[r_off+0] = rgb->data[c_off+0]; registered[r_off+1] = rgb->data[c_off+1]; From 5cb109201357711f7b5911a3920e294d231bd7aa Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Sun, 10 May 2015 21:40:59 +0200 Subject: [PATCH 5/8] remove noise by setting skipped pixels to zero --- examples/protonect/src/registration.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/protonect/src/registration.cpp b/examples/protonect/src/registration.cpp index 6a577811c..c8b0bcae1 100644 --- a/examples/protonect/src/registration.cpp +++ b/examples/protonect/src/registration.cpp @@ -112,9 +112,13 @@ void Registration::apply(Frame* rgb, Frame* depth, unsigned char* registered) apply(x,y,z_raw,cx,cy); - // is rounding the right way to go here - no subpixel precision? 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)) continue; + 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]; From 42c9eaa76ed0284d652ff7c4276f474a7c126808 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Mon, 11 May 2015 08:16:35 +0200 Subject: [PATCH 6/8] allocate registered image on freestore --- examples/protonect/Protonect.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/protonect/Protonect.cpp b/examples/protonect/Protonect.cpp index 5f1121feb..9eb7b30ed 100644 --- a/examples/protonect/Protonect.cpp +++ b/examples/protonect/Protonect.cpp @@ -78,6 +78,7 @@ int main(int argc, char *argv[]) std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl; libfreenect2::Registration registration(dev->getIrCameraParams(), dev->getColorCameraParams()); + uint8_t* registered = NULL; while(!protonect_shutdown) { @@ -90,7 +91,7 @@ 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); - uint8_t registered[depth->height*depth->width*rgb->bytes_per_pixel]; + if (!registered) registered = new uint8_t[depth->height*depth->width*rgb->bytes_per_pixel]; registration.apply(rgb,depth,registered); cv::imshow("registered", cv::Mat(depth->height, depth->width, CV_8UC3, registered)); @@ -106,5 +107,7 @@ int main(int argc, char *argv[]) dev->stop(); dev->close(); + delete[] registered; + return 0; } From 18674e35539956666d9f4c764d31004e761eeabf Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Mon, 11 May 2015 09:39:20 +0200 Subject: [PATCH 7/8] allocate registration object on freestore --- examples/protonect/Protonect.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/protonect/Protonect.cpp b/examples/protonect/Protonect.cpp index 9eb7b30ed..c1bad62d9 100644 --- a/examples/protonect/Protonect.cpp +++ b/examples/protonect/Protonect.cpp @@ -77,7 +77,7 @@ 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(dev->getIrCameraParams(), dev->getColorCameraParams()); + libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams()); uint8_t* registered = NULL; while(!protonect_shutdown) @@ -92,7 +92,7 @@ int main(int argc, char *argv[]) cv::imshow("depth", cv::Mat(depth->height, depth->width, CV_32FC1, depth->data) / 4500.0f); if (!registered) registered = new uint8_t[depth->height*depth->width*rgb->bytes_per_pixel]; - registration.apply(rgb,depth,registered); + registration->apply(rgb,depth,registered); cv::imshow("registered", cv::Mat(depth->height, depth->width, CV_8UC3, registered)); int key = cv::waitKey(1); @@ -108,6 +108,7 @@ int main(int argc, char *argv[]) dev->close(); delete[] registered; + delete registration; return 0; } From 769fa80a9b67461d9df1efed2960301c546ec18e Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Mon, 11 May 2015 11:26:04 +0200 Subject: [PATCH 8/8] switch to portable unsigned char* --- examples/protonect/Protonect.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/protonect/Protonect.cpp b/examples/protonect/Protonect.cpp index c1bad62d9..2f3304118 100644 --- a/examples/protonect/Protonect.cpp +++ b/examples/protonect/Protonect.cpp @@ -78,7 +78,7 @@ int main(int argc, char *argv[]) std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl; libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams()); - uint8_t* registered = NULL; + unsigned char* registered = NULL; while(!protonect_shutdown) { @@ -91,7 +91,7 @@ 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 uint8_t[depth->height*depth->width*rgb->bytes_per_pixel]; + if (!registered) registered = new unsigned char[depth->height*depth->width*rgb->bytes_per_pixel]; registration->apply(rgb,depth,registered); cv::imshow("registered", cv::Mat(depth->height, depth->width, CV_8UC3, registered));