Merge branch 'feature/57/realsense' into 'master'

Implements #57 realsense rgbd source

Closes #57

See merge request nicolas.pope/ftl!36

CMakeLists.txt

@@ -66,6 +66,18 @@ else()
 endif()
 endif()
 
+find_library( REALSENSE_LIBRARY NAMES realsense2 librealsense2)
+if (REALSENSE_LIBRARY)
+	set(HAVE_REALSENSE TRUE)
+	add_library(realsense UNKNOWN IMPORTED)
+	#set_property(TARGET nanogui PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${NANOGUI_EXTRA_INCS})
+	set_property(TARGET realsense PROPERTY IMPORTED_LOCATION ${REALSENSE_LIBRARY})
+    message(STATUS "Found Realsense SDK: ${REALSENSE_LIBRARY}")
+else()
+	set(REALSENSE_LIBRARY "")
+endif()
+
+
 find_library( NANOGUI_LIBRARY NAMES nanogui libnanogui)
 if (NANOGUI_LIBRARY)
 	set(HAVE_NANOGUI TRUE)

components/common/cpp/include/ftl/config.h.in

@@ -19,6 +19,7 @@
 #cmakedefine HAVE_PCL
 #cmakedefine HAVE_RENDER
 #cmakedefine HAVE_LIBSGM
+#cmakedefine HAVE_REALSENSE
 #cmakedefine HAVE_NANOGUI
 #cmakedefine HAVE_LIBARCHIVE
 

components/rgbd-sources/CMakeLists.txt

@@ -12,6 +12,10 @@ set(RGBDSRC
 #	src/algorithms/opencv_bm.cpp
 )
 
+if (HAVE_REALSENSE)
+	list(APPEND RGBDSRC "src/realsense_source.cpp")
+endif()
+
 if (LibArchive_FOUND)
 	list(APPEND RGBDSRC
 		src/snapshot.cpp
@@ -52,7 +56,7 @@ set_property(TARGET ftlrgbd PROPERTY CUDA_SEPARABLE_COMPILATION OFF)
 endif()
 
 #target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
-target_link_libraries(ftlrgbd ftlcommon ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} Eigen3::Eigen ftlnet ${LibArchive_LIBRARIES})
+target_link_libraries(ftlrgbd ftlcommon ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} Eigen3::Eigen ${REALSENSE_LIBRARY} ftlnet ${LibArchive_LIBRARIES})
 
 add_subdirectory(test)
 

components/rgbd-sources/src/realsense_source.cpp

+#include "realsense_source.hpp"
+#include <loguru.hpp>
+
+using ftl::rgbd::detail::RealsenseSource;
+using std::string;
+
+RealsenseSource::RealsenseSource(ftl::rgbd::Source *host)
+        : ftl::rgbd::detail::Source(host), align_to_depth_(RS2_STREAM_DEPTH) {
+    rs2::config cfg;
+    cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 30);
+    cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_BGRA8, 30);
+    //cfg.enable_stream(RS2_STREAM_DEPTH);
+    //cfg.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_BGRA8);
+    //pipe_.start(cfg);
+    rs2::pipeline_profile profile = pipe_.start(cfg);
+    rs2::device dev = profile.get_device();
+    rs2_intrinsics intrin = profile.get_stream(rs2_stream::RS2_STREAM_DEPTH).as<rs2::video_stream_profile>().get_intrinsics();
+
+    rs2::depth_sensor ds = dev.query_sensors().front().as<rs2::depth_sensor>();
+    scale_ = ds.get_depth_scale();
+    LOG(INFO) << "RS Scale = " << scale_;
+
+    params_.width = intrin.width;
+    params_.height = intrin.height;
+    params_.cx = -intrin.ppx;
+    params_.cy = -intrin.ppy;
+    params_.fx = intrin.fx;
+    params_.fy = intrin.fy;
+    params_.maxDepth = 11.0;
+    params_.minDepth = 0.1;
+
+    //LOG(INFO) << "Realsense Intrinsics: " << params_.fx << "," << params_.fy << " - " << params_.cx << "," << params_.cy << " - " << params_.width;
+}
+
+RealsenseSource::~RealsenseSource() {
+
+}
+
+bool RealsenseSource::grab() {
+    rs2::frameset frames = pipe_.wait_for_frames();
+    //rs2::align align(RS2_STREAM_DEPTH);
+    frames = align_to_depth_.process(frames); //align_to_depth_.process(frames);
+
+    rs2::depth_frame depth = frames.get_depth_frame();
+    float w = depth.get_width();
+    float h = depth.get_height();
+    rs2::frame colour = frames.first(RS2_STREAM_COLOR); //.get_color_frame();
+
+    //LOG(INFO) << " RS Frame size = " << w << "x" << h;
+
+    //std::unique_lock<std::mutex> lk(mutex_);
+    cv::Mat tmp(cv::Size((int)w, (int)h), CV_16UC1, (void*)depth.get_data(), depth.get_stride_in_bytes());
+    tmp.convertTo(depth_, CV_32FC1, scale_);
+    rgb_ = cv::Mat(cv::Size(w, h), CV_8UC4, (void*)colour.get_data(), cv::Mat::AUTO_STEP);
+    //LOG(INFO) << "RS FRAME GRABBED: " << rgb_.cols << "x" << rgb_.rows;
+    return true;
+}
+
+bool RealsenseSource::isReady() {
+    return true;
+}

components/rgbd-sources/src/realsense_source.hpp

+#pragma once
+#ifndef _FTL_RGBD_REALSENSE_HPP_
+#define _FTL_RGBD_REALSENSE_HPP_
+
+#include <ftl/rgbd/detail/source.hpp>
+#include <librealsense2/rs.hpp>
+#include <string>
+
+namespace ftl {
+
+namespace rgbd {
+
+namespace detail {
+
+class RealsenseSource : public ftl::rgbd::detail::Source {
+	public:
+	RealsenseSource(ftl::rgbd::Source *host);
+	~RealsenseSource();
+
+	bool grab();
+	bool isReady();
+
+	private:
+	bool ready_;
+    float scale_;
+    rs2::pipeline pipe_;
+    rs2::align align_to_depth_;
+};
+
+}
+}
+}
+
+#endif  // _FTL_RGBD_REALSENSE_HPP_

components/rgbd-sources/src/source.cpp

@@ -9,6 +9,11 @@
 #include "snapshot_source.hpp"
 #endif
 
+#ifdef HAVE_REALSENSE
+#include "realsense_source.hpp"
+using ftl::rgbd::detail::RealsenseSource;
+#endif
+
 using ftl::rgbd::Source;
 using ftl::Configurable;
 using std::string;
@@ -20,7 +25,7 @@ using ftl::rgbd::detail::NetSource;
 using ftl::rgbd::detail::ImageSource;
 using ftl::rgbd::capability_t;
 
-Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), net_(nullptr) {
+Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4f::Identity()), net_(nullptr) {
 	impl_ = nullptr;
 	params_ = {0};
 	reset();
@@ -31,7 +36,7 @@ Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), net_(nullptr) {
 	});
 }
 
-Source::Source(ftl::config::json_t &cfg, ftl::net::Universe *net) : Configurable(cfg), net_(net) {
+Source::Source(ftl::config::json_t &cfg, ftl::net::Universe *net) : Configurable(cfg), pose_(Eigen::Matrix4f::Identity()), net_(net) {
 	impl_ = nullptr;
 	params_ = {0};
 	reset();
@@ -116,6 +121,12 @@ ftl::rgbd::detail::Source *Source::_createNetImpl(const ftl::URI &uri) {
 ftl::rgbd::detail::Source *Source::_createDeviceImpl(const ftl::URI &uri) {
 	if (uri.getPathSegment(0) == "video") {
 		return new StereoVideoSource(this);
+	} else if (uri.getPathSegment(0) == "realsense") {
+#ifdef HAVE_REALSENSE
+		return new RealsenseSource(this);
+#else
+		LOG(ERROR) << "You do not have 'librealsense2' installed";
+#endif
 	}
 	return nullptr;
 }

components/rgbd-sources/test/source_unit.cpp

@@ -47,6 +47,16 @@ class NetSource : public ftl::rgbd::detail::Source {
 	bool isReady() { return true; };
 };
 
+class RealsenseSource : public ftl::rgbd::detail::Source {
+	public:
+	RealsenseSource(ftl::rgbd::Source *host) : ftl::rgbd::detail::Source(host) {
+		last_type = "realsense";
+	}
+
+	bool grab() { return true; };
+	bool isReady() { return true; };
+};
+
 }	
 }	
 }
@@ -58,6 +68,7 @@ class NetSource : public ftl::rgbd::detail::Source {
 #define _FTL_RGBD_NET_HPP_
 #define _FTL_RGBD_SNAPSHOT_SOURCE_HPP_
 #define _FTL_RGBD_IMAGE_HPP_
+#define _FTL_RGBD_REALSENSE_HPP_
 
 #include "../src/source.cpp"
 

config/config.jsonc

@@ -53,7 +53,11 @@
 			"disparity": { "$ref": "#disparity/libsgm" }
 		},
 		"stereovid": {},
-		"localhost": {}
+		"localhost": {},
+		"realsense": {
+			"$id": "ftl://utu.fi#vision_default/source",
+			"type": "realsense"
+		}
 		
 	},
 	// Listen to localhost
@@ -129,7 +133,7 @@
 			"SDFTruncationScale": 0.05,
 			"SDFIntegrationWeightSample": 10,
 			"SDFIntegrationWeightMax": 255,
-			"hash_renderer": false
+			"hash_renderer": true
 		}
 	},
 
@@ -141,6 +145,14 @@
 		"stream": {}
 	},
 
+	"vision_rs": {
+		"source": { "$ref": "#sources/realsense" },
+		"middlebury": { "$ref": "#middlebury/none" },
+		"display": { "$ref": "#displays/none" },
+		"net": { "$ref": "#net/default_vision" },
+		"stream": {}
+	},
+
 	"reconstruction_default": {
 		"net": {
 			"peers": ["tcp://localhost:9001"],
@@ -185,6 +197,29 @@
 		"stream": {}
 	},
 
+	"reconstruction_node4": {
+		"net": {
+			"peers": ["tcp://ftl-node-4:9001"]
+		},
+		"sources": [
+			{"type": "net", "uri":"ftl://utu.fi/node4#vision_default/source"}
+		],
+		"display": { "$ref": "#displays/left" },
+		"virtual": { "$ref": "#virtual_cams/default" },
+		"voxelhash": { "$ref": "#hash_conf/default" },
+		"registration": {
+			"reference-source" : "ftl://utu.fi/node4#vision_default/source",
+			"calibration" : {
+				"max_error": 25,
+				"run": false,
+				"iterations" : 10,
+				"delay" : 500,
+				"patternsize" : [9, 6]
+				}
+		},
+		"stream": {}
+	},
+
 
 	"gui_node5": {
 		"net": {