From c0c560cdb1a7f158d2c3daff56e94e3d2abc3389 Mon Sep 17 00:00:00 2001 From: Lior Ramati Date: Wed, 7 Nov 2018 19:46:52 +0200 Subject: [PATCH 01/18] Start moving export_to_ply function to processing block --- include/librealsense2/hpp/rs_export.hpp | 138 ++++++++++++++++++++++++ 1 file changed, 138 insertions(+) create mode 100644 include/librealsense2/hpp/rs_export.hpp diff --git a/include/librealsense2/hpp/rs_export.hpp b/include/librealsense2/hpp/rs_export.hpp new file mode 100644 index 0000000000..26269bdb31 --- /dev/null +++ b/include/librealsense2/hpp/rs_export.hpp @@ -0,0 +1,138 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +#ifndef LIBREALSENSE_RS2_EXPORT_HPP +#define LIBREALSENSE_RS2_EXPORT_HPP + +#include +#include +#include "rs_processing.hpp" + +namespace rs2 +{ + class save_to_ply : public processing_block + { + public: + save_to_ply(std::string filename = "RealSense Pointcloud ", pointcloud pc = pointcloud()) + : processing_block([this](rs2::frame f, rs2::frame_source& s) { func(f, s); }), + _pc(std::move(pc)), fname(filename) {} + + private: + void func(rs2::frame data, rs2::frame_source& source) + { + frame depth, color; + if (auto fs = data.as()) { + depth = fs.first(RS2_STREAM_DEPTH); + color = fs.first(RS2_STREAM_COLOR); + } else { + depth = data.as(); + } + + if (!depth) throw std::runtime_error("Need depth data to save PLY"); + if (!depth.is()) { + if (color) _pc.map_to(color); + depth = _pc.calculate(depth); + } + + export_to_ply(depth, color); + } + + void export_to_ply(points p, video_frame color) { + auto profile = p.get_profile().as(); + const auto verts = p.get_vertices(); + const auto texcoords = p.get_texture_coordinates(); + std::vector new_verts; + std::vector> new_tex; + std::map idx_map; + + new_verts.reserve(p.size()); + if (color) new_tex.reserve(p.size()); + + static const auto min_distance = 1e-6; + + for (size_t i = 0; i < p.size(); ++i) { + if (fabs(verts[i].x) >= min_distance || fabs(verts[i].y) >= min_distance || + fabs(verts[i].z) >= min_distance) + { + idx_map[i] = new_verts.size(); + new_verts.push_back(verts[i]); + if (color) + { + auto rgb = get_texcolor(color, texcoords[i].u, texcoords[i].v); + new_tex.push_back(rgb); + } + } + } + + static const auto threshold = 0.05f; + auto width = profile.width(), height = profile.height(); + std::vector> faces; + for (int x = 0; x < width - 1; ++x) { + for (int y = 0; y < height - 1; ++y) { + auto a = y * width + x, b = y * width + x + 1, c = (y + 1)*width + x, d = (y + 1)*width + x + 1; + if (verts[a].z && verts[b].z && verts[c].z && verts[d].z + && abs(verts[a].z - verts[b].z) < threshold && abs(verts[a].z - verts[c].z) < threshold + && abs(verts[b].z - verts[d].z) < threshold && abs(verts[c].z - verts[d].z) < threshold) + { + if (idx_map.count(a) == 0 || idx_map.count(b) == 0 || idx_map.count(c) == 0 || + idx_map.count(d) == 0) + continue; + faces.emplace_back(idx_map[a], idx_map[b], idx_map[d]); + faces.emplace_back(idx_map[d], idx_map[c], idx_map[a]); + } + } + } + + std::ofstream out(fname + std::to_string(p.get_frame_number()) + ".ply"); + out << "ply\n"; + out << "format binary_little_endian 1.0\n" /*"format ascii 1.0\n"*/; + out << "comment pointcloud saved from Realsense Viewer\n"; + out << "element vertex " << new_verts.size() << "\n"; + out << "property float" << sizeof(float) * 8 << " x\n"; + out << "property float" << sizeof(float) * 8 << " y\n"; + out << "property float" << sizeof(float) * 8 << " z\n"; + if (color) + { + out << "property uchar red\n"; + out << "property uchar green\n"; + out << "property uchar blue\n"; + } + out << "element face " << faces.size() << "\n"; + out << "property list uchar int vertex_indices\n"; + out << "end_header\n"; + out.close(); + + out.open(fname, std::ios_base::app | std::ios_base::binary); + for (int i = 0; i < new_verts.size(); ++i) + { + // we assume little endian architecture on your device + out.write(reinterpret_cast(&(new_verts[i].x)), sizeof(float)); + out.write(reinterpret_cast(&(new_verts[i].y)), sizeof(float)); + out.write(reinterpret_cast(&(new_verts[i].z)), sizeof(float)); + + if (color) + { + uint8_t x = [0], y = new_tex[i][1], z = new_tex[i][2]; + out.write(reinterpret_cast(&x), sizeof(uint8_t)); + out.write(reinterpret_cast(&y), sizeof(uint8_t)); + out.write(reinterpret_cast(&z), sizeof(uint8_t)); + } + } + auto size = faces.size(); + for (int i = 0; i < size; ++i) { + int three = 3; + out.write(reinterpret_cast(&three), sizeof(uint8_t)); + out.write(reinterpret_cast(&(std::get<0>(faces[i]))), sizeof(int)); + out.write(reinterpret_cast(&(std::get<1>(faces[i]))), sizeof(int)); + out.write(reinterpret_cast(&(std::get<2>(faces[i]))), sizeof(int)); + } + } + + // TODO: get_texcolor, options API + + std::string fname; + pointcloud _pc; + }; +} + +#endif \ No newline at end of file From 5fd1883ef6923704982a93675e35832c366ee05c Mon Sep 17 00:00:00 2001 From: Lior Ramati Date: Mon, 12 Nov 2018 20:53:15 +0200 Subject: [PATCH 02/18] Add API for adding options to custom processing blocks --- CMakeLists.txt | 2 + include/librealsense2/h/rs_processing.h | 14 ++++++ include/librealsense2/hpp/rs_export.hpp | 48 +++++++++++++-------- include/librealsense2/hpp/rs_processing.hpp | 14 ++++++ include/librealsense2/rs.hpp | 1 + src/option.cpp | 12 +++++- src/option.h | 47 ++++++++++++++++---- src/realsense.def | 1 + src/rs.cpp | 16 +++++++ 9 files changed, 129 insertions(+), 26 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 631e1b5a7b..7f7eadc55e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,8 @@ project(librealsense2 LANGUAGES CXX C) # include librealsense general configuration include(CMake/global_config.cmake) + include/librealsense2/hpp/rs_export.hpp + include/librealsense2/rs_export.hpp config_cxx_flags() diff --git a/include/librealsense2/h/rs_processing.h b/include/librealsense2/h/rs_processing.h index 351f003c34..d328c8a003 100644 --- a/include/librealsense2/h/rs_processing.h +++ b/include/librealsense2/h/rs_processing.h @@ -60,6 +60,20 @@ rs2_processing_block* rs2_create_processing_block(rs2_frame_processor_callback* */ rs2_processing_block* rs2_create_processing_block_fptr(rs2_frame_processor_callback_ptr proc, void * context, rs2_error** error); +/** +* This method adds a custom option to a custom processing block. This is a simple float that can be accessed via rs2_set_option and rs2_get_option +* This is an infrastructure function aimed at middleware developers, and also used by provided blocks such as save_to_ply, etc.. +* \param[in] block Processing block +* \param[in] option_id an int ID for referencing the option +* \param[in] min the minimum value which will be accepted for this option +* \param[in] max the maximum value which will be accepted for this option +* \param[in] step the granularity of options which accept discrete values, or zero if the option accepts continuous values +* \param[in] def the default value of the option. This will be the initial value. +* \param[out] error if non-null, receives any error that occurs during this call, otherwise, errors are ignored +* \return true if adding the option succeeds. false if it fails e.g. an option with this id is already registered +*/ +int rs2_processing_block_register_simple_option(rs2_processing_block* block, int option_id, float min, float max, float step, float def, rs2_error** error); + /** * This method is used to direct the output from the processing block to some callback or sink object * \param[in] block Processing block diff --git a/include/librealsense2/hpp/rs_export.hpp b/include/librealsense2/hpp/rs_export.hpp index 26269bdb31..dd25fd7e39 100644 --- a/include/librealsense2/hpp/rs_export.hpp +++ b/include/librealsense2/hpp/rs_export.hpp @@ -15,8 +15,13 @@ namespace rs2 public: save_to_ply(std::string filename = "RealSense Pointcloud ", pointcloud pc = pointcloud()) : processing_block([this](rs2::frame f, rs2::frame_source& s) { func(f, s); }), - _pc(std::move(pc)), fname(filename) {} + _pc(std::move(pc)), fname(filename) + { + register_simple_option(OPTION_IGNORE_COLOR, option_range{ 0, 1, 0, 1 }); + } + + DECLARE_PB_OPTION(OPTION_IGNORE_COLOR, 1); private: void func(rs2::frame data, rs2::frame_source& source) { @@ -38,7 +43,7 @@ namespace rs2 } void export_to_ply(points p, video_frame color) { - auto profile = p.get_profile().as(); + const bool use_texcoords = color && get_option(OPTION_IGNORE_COLOR); const auto verts = p.get_vertices(); const auto texcoords = p.get_texture_coordinates(); std::vector new_verts; @@ -46,7 +51,7 @@ namespace rs2 std::map idx_map; new_verts.reserve(p.size()); - if (color) new_tex.reserve(p.size()); + if (use_texcoords) new_tex.reserve(p.size()); static const auto min_distance = 1e-6; @@ -56,7 +61,7 @@ namespace rs2 { idx_map[i] = new_verts.size(); new_verts.push_back(verts[i]); - if (color) + if (use_texcoords) { auto rgb = get_texcolor(color, texcoords[i].u, texcoords[i].v); new_tex.push_back(rgb); @@ -64,8 +69,9 @@ namespace rs2 } } - static const auto threshold = 0.05f; + auto profile = p.get_profile().as(); auto width = profile.width(), height = profile.height(); + static const auto threshold = 0.05f; std::vector> faces; for (int x = 0; x < width - 1; ++x) { for (int y = 0; y < height - 1; ++y) { @@ -77,8 +83,8 @@ namespace rs2 if (idx_map.count(a) == 0 || idx_map.count(b) == 0 || idx_map.count(c) == 0 || idx_map.count(d) == 0) continue; - faces.emplace_back(idx_map[a], idx_map[b], idx_map[d]); - faces.emplace_back(idx_map[d], idx_map[c], idx_map[a]); + faces.push_back({ idx_map[a], idx_map[b], idx_map[d] }); + faces.push_back({ idx_map[d], idx_map[c], idx_map[a] }); } } } @@ -91,7 +97,7 @@ namespace rs2 out << "property float" << sizeof(float) * 8 << " x\n"; out << "property float" << sizeof(float) * 8 << " y\n"; out << "property float" << sizeof(float) * 8 << " z\n"; - if (color) + if (use_texcoords) { out << "property uchar red\n"; out << "property uchar green\n"; @@ -110,25 +116,33 @@ namespace rs2 out.write(reinterpret_cast(&(new_verts[i].y)), sizeof(float)); out.write(reinterpret_cast(&(new_verts[i].z)), sizeof(float)); - if (color) + if (use_texcoords) { - uint8_t x = [0], y = new_tex[i][1], z = new_tex[i][2]; - out.write(reinterpret_cast(&x), sizeof(uint8_t)); - out.write(reinterpret_cast(&y), sizeof(uint8_t)); - out.write(reinterpret_cast(&z), sizeof(uint8_t)); + out.write(reinterpret_cast(&(new_tex[i][0])), sizeof(uint8_t)); + out.write(reinterpret_cast(&(new_tex[i][1])), sizeof(uint8_t)); + out.write(reinterpret_cast(&(new_tex[i][2])), sizeof(uint8_t)); } } auto size = faces.size(); for (int i = 0; i < size; ++i) { - int three = 3; + static const int three = 3; out.write(reinterpret_cast(&three), sizeof(uint8_t)); - out.write(reinterpret_cast(&(std::get<0>(faces[i]))), sizeof(int)); - out.write(reinterpret_cast(&(std::get<1>(faces[i]))), sizeof(int)); - out.write(reinterpret_cast(&(std::get<2>(faces[i]))), sizeof(int)); + out.write(reinterpret_cast(&(faces[i][0])), sizeof(int)); + out.write(reinterpret_cast(&(faces[i][1])), sizeof(int)); + out.write(reinterpret_cast(&(faces[i][2])), sizeof(int)); } } // TODO: get_texcolor, options API + std::array get_texcolor(const video_frame& texture, float u, float v) + { + const int w = texture.get_width(), h = texture.get_height(); + int x = std::min(std::max(int(u*w + .5f), 0), w - 1); + int y = std::min(std::max(int(v*h + .5f), 0), h - 1); + int idx = x * texture.get_bytes_per_pixel() + y * texture.get_stride_in_bytes(); + const auto texture_data = reinterpret_cast(texture.get_data()); + return { texture_data[idx], texture_data[idx + 1], texture_data[idx + 2] }; + } std::string fname; pointcloud _pc; diff --git a/include/librealsense2/hpp/rs_processing.hpp b/include/librealsense2/hpp/rs_processing.hpp index 127d6fb35f..8c4819e070 100644 --- a/include/librealsense2/hpp/rs_processing.hpp +++ b/include/librealsense2/hpp/rs_processing.hpp @@ -270,7 +270,14 @@ namespace rs2 operator rs2_options*() const { return (rs2_options*)get(); } rs2_processing_block* get() const { return _block.get(); } + protected: + void register_simple_option(int option_id, option_range range) { + rs2_error * e = nullptr; + rs2_processing_block_register_simple_option(_block.get(), option_id, + range.min, range.max, range.step, range.def, &e); + error::handle(e); + } std::shared_ptr _block; }; @@ -327,6 +334,13 @@ namespace rs2 frame_queue _queue; }; +/** +* This macro can be used in the public portion of a custom processing block's declaration +* to name a custom option which will be exposed as a static member of the custom type. +* the IDs are added after RS2_OPTION_COUNT to avoid colisions with library options. +*/ +#define DECLARE_PB_OPTION(name, id) static const auto name = rs2_option(RS2_OPTION_COUNT + id) + /** * Generating the 3D point cloud base on depth frame also create the mapped texture. */ diff --git a/include/librealsense2/rs.hpp b/include/librealsense2/rs.hpp index 4b58f1a198..ccedf47b1c 100644 --- a/include/librealsense2/rs.hpp +++ b/include/librealsense2/rs.hpp @@ -13,6 +13,7 @@ #include "hpp/rs_record_playback.hpp" #include "hpp/rs_sensor.hpp" #include "hpp/rs_pipeline.hpp" +#include "hpp/rs_export.hpp" namespace rs2 { diff --git a/src/option.cpp b/src/option.cpp index 57e2c6b3a2..c124c4396e 100644 --- a/src/option.cpp +++ b/src/option.cpp @@ -7,12 +7,15 @@ bool librealsense::option_base::is_valid(float value) const { - if (!std::isnormal(_opt_range.step)) + if (!std::isnormal(_opt_range.step) && _opt_range.step != 0) throw invalid_value_exception(to_string() << "is_valid(...) failed! step is not properly defined. (" << _opt_range.step << ")"); if ((value < _opt_range.min) || (value > _opt_range.max)) return false; + if (_opt_range.step == 0) + return true; + auto n = (value - _opt_range.min) / _opt_range.step; return (fabs(fmod(n, 1)) < std::numeric_limits::min()); } @@ -31,6 +34,13 @@ void librealsense::option::create_snapshot(std::shared_ptr