From f202780ca3009aeb56ff5618db09c1bb105172b9 Mon Sep 17 00:00:00 2001
From: Florian Echtler <floe@butterbrot.org>
Date: Wed, 23 May 2018 16:07:55 +0200
Subject: [PATCH] all ransac code moved

---
 kinect.cpp    | 30 ++++++------------------------
 libfreenect2  |  2 +-
 realsense.cpp | 36 +++++++++---------------------------
 3 files changed, 16 insertions(+), 52 deletions(-)

diff --git a/kinect.cpp b/kinect.cpp
index 6f99f83..1aaaf6d 100644
--- a/kinect.cpp
+++ b/kinect.cpp
@@ -1,27 +1,10 @@
-int dw = 512, dh = 424;
-int cw = 1920, ch = 1080;
+libfreenect2::Registration* registration;
+libfreenect2::Frame* undistorted;
 
-// use RANSAC to compute a plane out of sparse point cloud
-PlaneModel<float> ransac_plane(libfreenect2::Registration* registration, libfreenect2::Frame* undistorted, float distance) {
-
-  std::vector<Eigen::Vector3f> points;
-
-  for (int y = 0; y < dh; y++) {
-    for (int x = 0; x < dw; x++) {
-      float px,py,pz;
-      registration->getPointXYZ(undistorted,y,x,px,py,pz);
-      if (std::isnan(pz) || std::isinf(pz) || pz <= 0) continue;
-      Eigen::Vector3f point = { px, py, pz };
-      points.push_back( point );
-    }
-  }
-
-  std::cout << "3D point count: " << points.size() << std::endl;
-  PlaneModel<float> plane = ransac<PlaneModel<float>>( points, distance*0.01, 200 );
-  if (plane.d < 0.0) { plane.d = -plane.d; plane.n = -plane.n; }
-  std::cout << "Ransac computed plane: n=" << plane.n.transpose() << " d=" << plane.d << std::endl;
-
-  return plane;
+void get_3d_point(int x, int y, float* out) {
+    float px,py,pz;
+    registration->getPointXYZ(undistorted,y,x,px,py,pz);
+    out[0] = px; out[1] = py; out[2] = pz;
 }
 
 void denoise_depth(libfreenect2::Registration* registration, libfreenect2::Frame* undistorted, float distance, PlaneModel<float>* plane) {
@@ -74,4 +57,3 @@ void apply_to_color(libfreenect2::Registration* registration, libfreenect2::Fram
     }
   }
 }
-
diff --git a/libfreenect2 b/libfreenect2
index 1dc025d..6833990 160000
--- a/libfreenect2
+++ b/libfreenect2
@@ -1 +1 @@
-Subproject commit 1dc025df381a264406c0973fccd3a850387f0f9e
+Subproject commit 68339901b3cdc0593d0231089390a9ab58c572bc
diff --git a/realsense.cpp b/realsense.cpp
index 6446ef3..b1fb2c5 100644
--- a/realsense.cpp
+++ b/realsense.cpp
@@ -10,33 +10,14 @@
 #include <iostream>
 #include "common.h"
 
-int dw = 1280, dh = 720;
-int cw = 1280, ch = 720;
+rs2::depth_frame* depthp;
+rs2_intrinsics intrinsics;
 
-// use RANSAC to compute a plane out of sparse point cloud
-PlaneModel<float> ransac_plane(rs2::depth_frame* depth, rs2_intrinsics* intr, float distance) {
-
-  std::vector<Eigen::Vector3f> points;
-
-  for (int y = 0; y < dh; y++) {
-    for (int x = 0; x < dw; x++) {
-      float pt[3]; float px[2] = { x, y };
-      rs2_deproject_pixel_to_point( pt, intr, px, depth->get_distance(x,y) );
-      if (std::isnan(pt[2]) || std::isinf(pt[2]) || pt[2] <= 0) continue;
-      Eigen::Vector3f point = { pt[0], pt[1], pt[2] };
-      points.push_back( point );
-    }
-  }
-
-  std::cout << "3D point count: " << points.size() << std::endl;
-  PlaneModel<float> plane = ransac<PlaneModel<float>>( points, distance*0.01, 200 );
-  if (plane.d < 0.0) { plane.d = -plane.d; plane.n = -plane.n; }
-  std::cout << "Ransac computed plane: n=" << plane.n.transpose() << " d=" << plane.d << std::endl;
-
-  return plane;
+void get_3d_point(int x, int y, float* out) {
+    float px[2] = { (float)x, (float)y };
+    rs2_deproject_pixel_to_point( out, &intrinsics, px, depthp->get_distance(x,y) );
 }
 
-
 void apply_to_color(rs2::depth_frame* depth, rs2_intrinsics* intr, float distance, PlaneModel<float>* plane, uint8_t* p_other_frame) {
 
   // blank out all remaining color pixels _below_ the plane
@@ -61,7 +42,7 @@ int main(int argc, char* argv[]) {
   bool _quit = false; quit = &_quit;
   if (argc > 2) gstpipe = argv[2];
 
-  opencv_init();
+  opencv_init(1280,720,1280,720);
   gstreamer_init(argc,argv,"RGBx");
 
   // Create a Pipeline - this serves as a top-level API for streaming and processing frames
@@ -95,7 +76,7 @@ int main(int argc, char* argv[]) {
   rs2::align align(RS2_STREAM_COLOR);
 
   auto stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
-  auto intrinsics = stream.get_intrinsics(); // Calibration data
+  intrinsics = stream.get_intrinsics(); // Calibration data
 
   while (!_quit) {
 
@@ -109,11 +90,12 @@ int main(int argc, char* argv[]) {
 
     // Try to get a frame of a depth image
     rs2::depth_frame depth = processed.get_depth_frame(); 
+    depthp = &depth;
 
     //rs2::video_frame color_frame = color_map(depth);
 
     if (find_plane) {
-      plane = ransac_plane(&depth,&intrinsics,distance);
+      plane = ransac_plane(get_3d_point);
       find_plane = false;
     }