diff --git a/CMakeLists.txt b/CMakeLists.txt index dabffe4e20021aa110011ac312973528a653252b..cfad1f58e61625bbdd0282891393e458f46ace8d 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/components/common/cpp/include/ftl/config.h.in b/components/common/cpp/include/ftl/config.h.in index 5dcc8e5758c577e56850323d95a3d8aadf915573..025ad576f9878019a41e0aa46269645c442780cd 100644 --- a/components/common/cpp/include/ftl/config.h.in +++ b/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 diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt index 37b7d06061c91d9f1efed851f8947a36abfeda44..9953eb32746e74caace9aa4805fb58d5fdf996ad 100644 --- a/components/rgbd-sources/CMakeLists.txt +++ b/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) diff --git a/components/rgbd-sources/src/realsense_source.cpp b/components/rgbd-sources/src/realsense_source.cpp new file mode 100644 index 0000000000000000000000000000000000000000..db97d93ebf61b76829685fc4238fa67fdce4af79 --- /dev/null +++ b/components/rgbd-sources/src/realsense_source.cpp @@ -0,0 +1,61 @@ +#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; +} diff --git a/components/rgbd-sources/src/realsense_source.hpp b/components/rgbd-sources/src/realsense_source.hpp new file mode 100644 index 0000000000000000000000000000000000000000..d2ca206a2756d6c90a2491728e1b0b9b1bfab662 --- /dev/null +++ b/components/rgbd-sources/src/realsense_source.hpp @@ -0,0 +1,34 @@ +#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_ diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp index 2f51b6fdcf32f9661fead5ac550eb9f007ca072e..4c7379574a145004c46aaf20083484e0e8a89f81 100644 --- a/components/rgbd-sources/src/source.cpp +++ b/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; } diff --git a/components/rgbd-sources/test/source_unit.cpp b/components/rgbd-sources/test/source_unit.cpp index 8e48a53a0f4b573a60893abcd2a9362f8010f757..553d2ecbe254a4335e9348f342ff931dec93b3c0 100644 --- a/components/rgbd-sources/test/source_unit.cpp +++ b/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" diff --git a/config/config.jsonc b/config/config.jsonc index cf69dbfc7d5db2f170b2937e46969c696b05b1a5..080b0969a7367be4a2eb65bf9e7efaf2e7c20582 100644 --- a/config/config.jsonc +++ b/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": {