diff --git a/CMakeLists.txt b/CMakeLists.txt index 631e1b5a7b..5618667c0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,7 @@ project(librealsense2 LANGUAGES CXX C) # include librealsense general configuration include(CMake/global_config.cmake) + config_cxx_flags() # include os specific macros diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 5c92d262d8..a5355638f4 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -18,6 +18,7 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/librealsense2/hpp/rs_types.hpp" "${CMAKE_CURRENT_LIST_DIR}/librealsense2/hpp/rs_context.hpp" "${CMAKE_CURRENT_LIST_DIR}/librealsense2/hpp/rs_device.hpp" + "${CMAKE_CURRENT_LIST_DIR}/librealsense2/hpp/rs_export.hpp" "${CMAKE_CURRENT_LIST_DIR}/librealsense2/hpp/rs_frame.hpp" "${CMAKE_CURRENT_LIST_DIR}/librealsense2/hpp/rs_processing.hpp" "${CMAKE_CURRENT_LIST_DIR}/librealsense2/hpp/rs_record_playback.hpp" diff --git a/include/librealsense2/h/rs_processing.h b/include/librealsense2/h/rs_processing.h index 351f003c34..62edd0aa3c 100644 --- a/include/librealsense2/h/rs_processing.h +++ b/include/librealsense2/h/rs_processing.h @@ -16,6 +16,7 @@ extern "C" { #include "rs_types.h" #include "rs_sensor.h" +#include "rs_option.h" /** * Creates Depth-Colorizer processing block that can be used to quickly visualize the depth data @@ -60,6 +61,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, rs2_option 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 new file mode 100644 index 0000000000..83b4f65203 --- /dev/null +++ b/include/librealsense2/hpp/rs_export.hpp @@ -0,0 +1,237 @@ +// 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 +#include +#include +#include "rs_processing.hpp" +#include "rs_internal.hpp" + +namespace rs2 +{ + class save_to_ply : public filter + { + public: + save_to_ply(std::string filename = "RealSense Pointcloud ", pointcloud pc = pointcloud()) + : filter([this](frame f, frame_source& s) { func(f, s); }), + _pc(std::move(pc)), fname(filename) + { + register_simple_option(OPTION_IGNORE_COLOR, option_range{ 0, 1, 0, 1 }); + } + + + static const auto OPTION_IGNORE_COLOR = rs2_option(RS2_OPTION_COUNT + 1); + private: + void func(frame data, frame_source& source) + { + frame depth, color; + if (auto fs = data.as()) { + for (auto f : fs) { + if (f.is()) depth = f; + else if (!depth && f.is()) depth = f; + else if (!color && f.is()) color = f; + } + } else if (data.is() || data.is()) { + depth = data; + } + + 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); + source.frame_ready(data); // passthrough filter because processing_block::process doesn't support sinks + } + + void export_to_ply(points p, video_frame color) { + 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; + std::vector> new_tex; + std::map idx_map; + + new_verts.reserve(p.size()); + if (use_texcoords) 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 (use_texcoords) + { + auto rgb = get_texcolor(color, texcoords[i].u, texcoords[i].v); + new_tex.push_back(rgb); + } + } + } + + 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) { + 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 + && fabs(verts[a].z - verts[b].z) < threshold && fabs(verts[a].z - verts[c].z) < threshold + && fabs(verts[b].z - verts[d].z) < threshold && fabs(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.push_back({ idx_map[a], idx_map[b], idx_map[d] }); + faces.push_back({ idx_map[d], idx_map[c], idx_map[a] }); + } + } + } + + std::stringstream name; + name << fname << p.get_frame_number() << ".ply"; + std::ofstream out(name.str()); + out << "ply\n"; + out << "format binary_little_endian 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 (use_texcoords) + { + 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(name.str(), 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 (use_texcoords) + { + 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) { + static const int three = 3; + out.write(reinterpret_cast(&three), sizeof(uint8_t)); + 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; + }; + + class save_single_frameset : public filter { + public: + save_single_frameset(std::string filename = "RealSense Frameset ") + : filter([this](frame f, frame_source& s) { save(f, s); }), fname(filename) + {} + + private: + void save(frame data, frame_source& source, bool do_signal=true) + { + software_device dev; + + std::vector> sensors; + if (auto fs = data.as()) { + int uid = 0; + for (int i = 0; i < fs.size(); ++i) { + frame f = fs[i]; + auto profile = f.get_profile(); + std::stringstream sname; + sname << "Sensor (" << uid << ")"; + auto s = dev.add_sensor(sname.str()); + stream_profile software_profile; + + if (auto vf = f.as()) { + auto vp = profile.as(); + rs2_video_stream stream{ vp.stream_type(), vp.stream_index(), uid++, vp.width(), vp.height(), vp.fps(), vf.get_bytes_per_pixel(), vp.format(), vp.get_intrinsics() }; + software_profile = s.add_video_stream(stream); + } else if (f.is()) { + auto mp = profile.as(); + rs2_motion_stream stream{ mp.stream_type(), mp.stream_index(), uid++, mp.fps(), mp.format(), mp.get_motion_intrinsics() }; + software_profile = s.add_motion_stream(stream); + } else if (f.is()) { + rs2_pose_stream stream{ profile.stream_type(), profile.stream_index(), uid++, profile.fps(), profile.format() }; + software_profile = s.add_pose_stream(stream); + } else { + // TODO: How to handle other frame types? (e.g. points) + assert(false); + } + sensors.emplace_back(s, software_profile, i); + } + + // Recorder needs sensors to already exist when its created + std::stringstream name; + name << fname << data.get_frame_number() << ".bag"; + recorder rec(name.str(), dev); + + for (auto group : sensors) { + auto s = std::get<0>(group); + auto profile = std::get<1>(group); + s.open(profile); + s.start([](frame) {}); + frame f = fs[std::get<2>(group)]; + if (auto vf = f.as()) { + s.on_video_frame({ const_cast(vf.get_data()), [](void*) {}, vf.get_stride_in_bytes(), vf.get_bytes_per_pixel(), + vf.get_timestamp(), vf.get_frame_timestamp_domain(), static_cast(vf.get_frame_number()), profile }); + } else if (f.is()) { + s.on_motion_frame({ const_cast(f.get_data()), [](void*) {}, f.get_timestamp(), + f.get_frame_timestamp_domain(), static_cast(f.get_frame_number()), profile }); + } else if (f.is()) { + s.on_pose_frame({ const_cast(f.get_data()), [](void*) {}, f.get_timestamp(), + f.get_frame_timestamp_domain(), static_cast(f.get_frame_number()), profile }); + } + } + } else { + // single frame + auto set = source.allocate_composite_frame({ data }); + save(set, source, false); + } + + if (do_signal) + source.frame_ready(data); + } + + std::string fname; + }; +} + +#endif \ No newline at end of file diff --git a/include/librealsense2/hpp/rs_processing.hpp b/include/librealsense2/hpp/rs_processing.hpp index 127d6fb35f..527ef8ba3a 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(rs2_option 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; }; diff --git a/src/api.h b/src/api.h index f311c52ed4..50d7d31328 100644 --- a/src/api.h +++ b/src/api.h @@ -406,6 +406,7 @@ return __p.invoke(func);\ #define VALIDATE_NOT_NULL(ARG) if(!(ARG)) throw std::runtime_error("null pointer passed for argument \"" #ARG "\""); #define VALIDATE_ENUM(ARG) if(!librealsense::is_valid(ARG)) { std::ostringstream ss; ss << "invalid enum value for argument \"" #ARG "\""; throw librealsense::invalid_value_exception(ss.str()); } + #define VALIDATE_OPTION(OBJ, OPT_ID) if(!OBJ->options->supports_option(OPT_ID)) { std::ostringstream ss; ss << "object doesn't support option #" << std::to_string(OPT_ID); throw librealsense::invalid_value_exception(ss.str()); } #define VALIDATE_RANGE(ARG, MIN, MAX) if((ARG) < (MIN) || (ARG) > (MAX)) { std::ostringstream ss; ss << "out of range value for argument \"" #ARG "\""; throw librealsense::invalid_value_exception(ss.str()); } #define VALIDATE_LE(ARG, MAX) if((ARG) > (MAX)) { std::ostringstream ss; ss << "out of range value for argument \"" #ARG "\""; throw std::runtime_error(ss.str()); } #define VALIDATE_INTERFACE_NO_THROW(X, T) \ 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