diff --git a/CMake/realsense.def b/CMake/realsense.def index 221a45f8fb..f9cc2f1595 100644 --- a/CMake/realsense.def +++ b/CMake/realsense.def @@ -97,12 +97,14 @@ EXPORTS rs2_distortion_to_string rs2_option_to_string rs2_camera_info_to_string + rs2_frame_metadata_to_string rs2_timestamp_domain_to_string rs2_visual_preset_to_string + rs2_notification_category_to_string rs2_log_to_console rs2_log_to_file - + rs2_create_syncer rs2_start_syncer rs2_wait_for_frames diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ff4097417..d139b9ed54 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,6 +137,8 @@ set(REALSENSE_HPP src/subdevice.h src/algo.h src/option.h + src/metadata.h + src/metadata-parser.h src/error-handling.h src/hw-monitor.h src/image.h @@ -226,7 +228,7 @@ if(WIN32) src/sr300.h src/ivcam-private.h ) - + foreach(flag_var CMAKE_CXX_FLAGS CMAKE_CXX_FLAGS_DEBUG CMAKE_CXX_FLAGS_RELEASE CMAKE_CXX_FLAGS_MINSIZEREL CMAKE_CXX_FLAGS_RELWITHDEBINFO @@ -274,7 +276,7 @@ elseif(UNIX) find_package(PkgConfig REQUIRED) find_package (Threads REQUIRED) list(APPEND librealsense_PKG_DEPS "Threads") - + pkg_search_module(LIBUSB1 REQUIRED libusb-1.0) if(LIBUSB1_FOUND) include_directories(SYSTEM ${LIBUSB1_INCLUDE_DIRS}) diff --git a/bindings/pybackend_extras.cpp b/bindings/pybackend_extras.cpp index 88b5ca0afa..67c65fc7dd 100644 --- a/bindings/pybackend_extras.cpp +++ b/bindings/pybackend_extras.cpp @@ -33,9 +33,9 @@ namespace pybackend2 { const uint16_t pre_header_data = 0xcdab; raw_data.resize(HW_MONITOR_BUFFER_SIZE); auto write_ptr = raw_data.data(); - auto header_size = 4; + size_t header_size = 4; - auto cur_index = 2; + size_t cur_index = 2; *reinterpret_cast(write_ptr + cur_index) = pre_header_data; cur_index += sizeof(uint16_t); *reinterpret_cast(write_ptr + cur_index) = cmd_op_code; diff --git a/bindings/python.cpp b/bindings/python.cpp index 50bbe807db..398842b5bf 100644 --- a/bindings/python.cpp +++ b/bindings/python.cpp @@ -207,7 +207,13 @@ PYBIND11_PLUGIN(NAME) { .value("count", RS2_TIMESTAMP_DOMAIN_COUNT); py::enum_ frame_metadata(m, "frame_metadata"); - frame_metadata.value("actual_exposure", RS2_FRAME_METADATA_ACTUAL_EXPOSURE) + frame_metadata.value("frame counter", RS2_FRAME_METADATA_FRAME_COUNTER) + .value("frame_timestamp_usec", RS2_FRAME_METADATA_FRAME_TIMESTAMP) + .value("sensor_timestamp_usec", RS2_FRAME_METADATA_SENSOR_TIMESTAMP) + .value("actual_exposure_usec", RS2_FRAME_METADATA_ACTUAL_EXPOSURE) + .value("gain_level", RS2_FRAME_METADATA_GAIN_LEVEL) + .value("auto_exposure", RS2_FRAME_METADATA_AUTO_EXPOSURE) + .value("white_balance_temp_k", RS2_FRAME_METADATA_WHITE_BALANCE) .value("count", RS2_FRAME_METADATA_COUNT); py::class_ frame(m, "frame"); diff --git a/examples/cpp-config-ui.cpp b/examples/cpp-config-ui.cpp index 6651d29479..bc8222a7a2 100644 --- a/examples/cpp-config-ui.cpp +++ b/examples/cpp-config-ui.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #pragma comment(lib, "opengl32.lib") @@ -807,20 +808,30 @@ class fps_calc typedef std::map streams_layout; +struct frame_metadata +{ + std::array,RS2_FRAME_METADATA_COUNT> md_attributes{}; +}; + class stream_model { public: - rect layout; - texture_buffer texture; - float2 size; - rs2_format format; + rect layout; + texture_buffer texture; + float2 size; + rs2_stream stream; + rs2_format format; std::chrono::high_resolution_clock::time_point last_frame; - double timestamp; - unsigned long long frame_number; + double timestamp; + unsigned long long frame_number; rs2_timestamp_domain timestamp_domain; - fps_calc fps; - rect roi_display_rect; - bool capturing_roi = false; + fps_calc fps; + rect roi_display_rect{}; + frame_metadata frame_md; + bool metadata_displayed = false; + bool roi_supported = false; // supported by device in its current state + bool roi_checked = false; // actively selected by user + bool capturing_roi = false; // active modification of roi std::shared_ptr dev; void upload_frame(frame&& f) @@ -832,12 +843,24 @@ class stream_model auto height = (is_motion) ? 480.f : f.get_height(); size = { static_cast(width), static_cast(height)}; + stream = f.get_stream_type(); format = f.get_format(); frame_number = f.get_frame_number(); timestamp_domain = f.get_frame_timestamp_domain(); timestamp = f.get_timestamp(); fps.add_timestamp(f.get_timestamp(), f.get_frame_number()); + // populate frame metadata attributes + for (auto i=0; i< RS2_FRAME_METADATA_COUNT; i++) + { + if (f.supports_frame_metadata((rs2_frame_metadata)i)) + frame_md.md_attributes[i] = std::make_pair(true,f.get_frame_metadata((rs2_frame_metadata)i)); + else + frame_md.md_attributes[i].first = false; + } + + roi_supported = (dev.get() && dev->auto_exposure_enabled); + texture.upload(f); } @@ -882,7 +905,7 @@ class stream_model void update_ae_roi_rect(const rect& stream_rect, const mouse_info& mouse, std::string& error_message) { - if (dev.get() && dev->auto_exposure_enabled) + if (roi_checked) { // Case 1: Starting Dragging of the ROI rect // Pre-condition: not capturing already + mouse is down + we are inside stream rect @@ -936,7 +959,6 @@ class stream_model catch (const error& e) { error_message = error_to_string(e); - dev->auto_exposure_enabled = false; } } else // If the rect is empty @@ -958,7 +980,6 @@ class stream_model catch (const error& e) { error_message = error_to_string(e); - dev->auto_exposure_enabled = false; } } } @@ -992,6 +1013,39 @@ class stream_model update_ae_roi_rect(stream_rect, g, error_message); } + void show_metadata(const mouse_info& g) + { + + auto lines = RS2_FRAME_METADATA_COUNT; + auto flags = ImGuiWindowFlags_ShowBorders; + + ImGui::PushStyleColor(ImGuiCol_WindowBg, { 0.3f, 0.3f, 0.3f, 0.5}); + ImGui::PushStyleColor(ImGuiCol_TitleBg, { 0.f, 0.25f, 0.3f, 1 }); + ImGui::PushStyleColor(ImGuiCol_TitleBgActive, { 0.f, 0.3f, 0.8f, 1 }); + ImGui::PushStyleColor(ImGuiCol_Text, { 1, 1, 1, 1 }); + + std::string label = to_string() << rs2_stream_to_string(stream) << " Stream Metadata #"; + ImGui::Begin(label.c_str(), nullptr, flags); + + // Print all available frame metadata attributes + for (size_t i=0; i < RS2_FRAME_METADATA_COUNT; i++ ) + { + if (frame_md.md_attributes[i].first) + { + label = to_string() << rs2_frame_metadata_to_string((rs2_frame_metadata)i) << " = " << frame_md.md_attributes[i].second; + ImGui::Text("%s", label.c_str()); + } + } + + ImGui::End(); + + ImGui::PopStyleColor(); + ImGui::PopStyleColor(); + ImGui::PopStyleColor(); + ImGui::PopStyleColor(); + + } + float _frame_timeout = 700.0f; float _min_timeout = 167.0f; }; @@ -1068,6 +1122,7 @@ class device_model std::vector> subdevices; std::map streams; bool fullscreen = false; + bool metadata_supported = false; rs2_stream selected_stream = RS2_STREAM_ANY; private: @@ -1351,7 +1406,6 @@ int main(int, char**) try auto list = ctx.query_devices(); // Query RealSense connected devices list - // If no device is connected... while (list.size() == 0) { @@ -1401,6 +1455,7 @@ int main(int, char**) try data.curr_window = window; data.mouse = &mouse; + glfwSetWindowUserPointer(window, &data); glfwSetCursorPosCallback(window, [](GLFWwindow * w, double cx, double cy) @@ -1648,8 +1703,7 @@ int main(int, char**) try if (update_read_only_options) { metadata.update_supported(error_message); - if (metadata.supported && - sub->streaming) + if (metadata.supported && sub->streaming) { metadata.update_read_only(error_message); if (metadata.read_only) @@ -1734,7 +1788,7 @@ int main(int, char**) try auto layout = model.calc_layout(panel_size, 0.f, w - panel_size, (float)h); - for (auto kvp : layout) + for (auto &&kvp : layout) { auto&& view_rect = kvp.second; auto stream = kvp.first; @@ -1796,6 +1850,30 @@ int main(int, char**) try } } + if (!layout.empty()) + { + if (model.streams[stream].roi_supported ) + { + ImGui::SameLine((int)ImGui::GetWindowWidth() - 160); + ImGui::Checkbox("[ROI]", &model.streams[stream].roi_checked); + + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Auto Exposure Region-of-Interest Selection"); + } + else + model.streams[stream].roi_checked = false; + } + + // Control metadata overlay widget + ImGui::SameLine((int)ImGui::GetWindowWidth() - 90); // metadata GUI hint + if (!layout.empty()) + { + ImGui::Checkbox("[MD]", &model.streams[stream].metadata_displayed); + + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Show per-frame metadata"); + } + label = to_string() << "Timestamp: " << std::fixed << std::setprecision(3) << model.streams[stream].timestamp << ", Domain:"; ImGui::Text("%s", label.c_str()); @@ -1859,6 +1937,12 @@ int main(int, char**) try } } + // Metadata overlay windows shall be drawn after textures to preserve z-buffer functionality + for (auto &&kvp : layout) + { + if (model.streams[kvp.first].metadata_displayed) + model.streams[kvp.first].show_metadata(mouse); + } ImGui::Render(); diff --git a/examples/cpp-data-collect.cpp b/examples/cpp-data-collect.cpp index 95b857e056..19ea08c44e 100644 --- a/examples/cpp-data-collect.cpp +++ b/examples/cpp-data-collect.cpp @@ -155,6 +155,8 @@ void save_data_to_file(list buffer[], string filename = ".\\frames_d csv << line.str(); } } + + csv.close(); } int main(int argc, char** argv) try diff --git a/examples/cpp-headless.cpp b/examples/cpp-headless.cpp index 70df7c3370..9c6927f856 100644 --- a/examples/cpp-headless.cpp +++ b/examples/cpp-headless.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #define STB_IMAGE_WRITE_IMPLEMENTATION #include "third_party/stb_image_write.h" @@ -25,10 +26,32 @@ using namespace rs2; using namespace std; +void metadata_to_csv(const frame& frm, const std::string& filename) +{ + ofstream csv; + + csv.open(filename); + + cout << "Writing metadata to " << filename << endl; + csv << "Stream," << rs2_stream_to_string(frm.get_stream_type()) << "\nMetadata Attribute,Value\n"; + + // Record all the available metadata attributes + for (size_t i = 0; i < RS2_FRAME_METADATA_COUNT; i++) + { + if (frm.supports_frame_metadata((rs2_frame_metadata)i)) + { + csv << rs2_frame_metadata_to_string((rs2_frame_metadata)i) << "," + << frm.get_frame_metadata((rs2_frame_metadata)i) << "\n"; + } + } + + csv.close(); +} + + int main() try { log_to_console(RS2_LOG_SEVERITY_WARN); - //log_to_file(log_severity::debug, "librealsense.log"); context ctx; auto list = ctx.query_devices(); @@ -98,6 +121,10 @@ int main() try bpp, pixels, frame.get_width() * bpp ); + + /* Record per-frame metadata for UVC streams*/ + std::string metadata_file_name = rs2::to_string() << "cpp-headless-output-" << stream_type << "-metadata.csv"; + metadata_to_csv(frame, metadata_file_name); } frames_by_stream.clear(); diff --git a/examples/cpp-pointcloud.cpp b/examples/cpp-pointcloud.cpp index c2d1487fe3..9ce367e073 100644 --- a/examples/cpp-pointcloud.cpp +++ b/examples/cpp-pointcloud.cpp @@ -172,7 +172,7 @@ int main(int argc, char * argv[]) try } if (frame.get_stream_type() == mapped) - { + { mapped_tex.upload(frame); } } @@ -248,7 +248,7 @@ int main(int argc, char * argv[]) try ImGui::Text("Texture Source:"); ImGui::PushItemWidth(-1); - if (ImGui::Combo(" texture", &selected_stream, stream_names.data(), stream_names.size())) + if (ImGui::Combo(" texture", &selected_stream, stream_names.data(), static_cast(stream_names.size()))) { stream.stop(); dev.close(); diff --git a/include/librealsense/rs2.h b/include/librealsense/rs2.h index 78465af797..0c6bd9484e 100644 --- a/include/librealsense/rs2.h +++ b/include/librealsense/rs2.h @@ -87,7 +87,13 @@ typedef enum rs2_format /** \brief Per-Frame-Metadata are set of read-only properties that might be exposed for each individual frame */ typedef enum rs2_frame_metadata { - RS2_FRAME_METADATA_ACTUAL_EXPOSURE, + RS2_FRAME_METADATA_FRAME_COUNTER , /**< frame counter */ + RS2_FRAME_METADATA_FRAME_TIMESTAMP , /**< Timestamp. Start of readout. usec*/ + RS2_FRAME_METADATA_SENSOR_TIMESTAMP , /**< Timestamp, Designates the middle of sensor's exposure. usec*/ + RS2_FRAME_METADATA_ACTUAL_EXPOSURE , /**< Frame's actual exposure length. usec*/ + RS2_FRAME_METADATA_GAIN_LEVEL , /**< Transition function raw->scale : [16-248] -> [1-15.5]*/ + RS2_FRAME_METADATA_AUTO_EXPOSURE , /**< Auto-exposure mode. Enumerated according to MS specification */ + RS2_FRAME_METADATA_WHITE_BALANCE , /**< Temperature. Kelvin degrees. Applicable for Color Sensors*/ RS2_FRAME_METADATA_COUNT } rs2_frame_metadata; @@ -157,7 +163,7 @@ typedef enum rs2_option } rs2_option; /** \brief Read-only strings that can be queried from the device. - Not all information fields are available on all camera types. + Not all information attributes are available on all camera types. This information is mainly available for camera debug and troubleshooting and should not be used in applications. */ typedef enum rs2_camera_info { RS2_CAMERA_INFO_DEVICE_NAME , /**< Device friendly name */ @@ -253,7 +259,8 @@ typedef void (*rs2_frame_callback_ptr)(rs2_frame*, void*); typedef void (*rs2_notification_callback_ptr)(rs2_notification*, void*); typedef void (*rs2_log_callback_ptr)(rs2_log_severity min_severity, const char* message, void* user); -typedef double rs2_time_t; /**< Timestamp format. units are milliseconds */ +typedef double rs2_time_t; /**< Timestamp format. units are milliseconds */ +typedef long rs2_metadata_t; /**< Metadata attribute type*/ /** * \brief Creates RealSense context that is required for the rest of the API. @@ -344,7 +351,7 @@ void rs2_get_extrinsics(const rs2_device * from_dev, rs2_stream from_stream, * \param[in] stream type of stream * \param[in] width stream width * \param[in] height stream height - * \param[in] fps stream fps (in most cases will not affect resulting intrinsics) + * \param[in] fps stream fps (in most cases will not affect resulting intrinsics) * \param[in] format stream output format * \param[out] error if non-null, receives any error that occurs during this call, otherwise, errors are ignored */ @@ -545,7 +552,7 @@ rs2_notification_category rs2_get_notification_category(rs2_notification * notif * \param[in] frame_metadata the rs2_frame_metadata whose latest frame we are interested in * \return the metadata value */ -double rs2_get_frame_metadata(const rs2_frame* frame, rs2_frame_metadata frame_metadata, rs2_error** error); +rs2_metadata_t rs2_get_frame_metadata(const rs2_frame* frame, rs2_frame_metadata frame_metadata, rs2_error** error); /** * determine device metadata @@ -934,15 +941,18 @@ void rs2_free_error (rs2_error * error); rs2_exception_type rs2_get_librealsense_exception_type(const rs2_error * error); const char * rs2_exception_type_to_string(rs2_exception_type type); -const char * rs2_stream_to_string (rs2_stream stream); -const char * rs2_format_to_string (rs2_format format); -const char * rs2_distortion_to_string (rs2_distortion distortion); -const char * rs2_option_to_string (rs2_option option); -const char * rs2_camera_info_to_string(rs2_camera_info info); -const char * rs2_camera_info_to_string(rs2_camera_info info); -const char * rs2_timestamp_domain_to_string(rs2_timestamp_domain info); -const char * rs2_log_severity_to_string(rs2_log_severity info); -const char * rs2_visual_preset_to_string (rs2_visual_preset preset); +const char * rs2_stream_to_string (rs2_stream stream); +const char * rs2_format_to_string (rs2_format format); +const char * rs2_distortion_to_string (rs2_distortion distortion); +const char * rs2_option_to_string (rs2_option option); +const char * rs2_camera_info_to_string (rs2_camera_info info); +const char * rs2_frame_metadata_to_string (rs2_frame_metadata metadata); +const char * rs2_timestamp_domain_to_string (rs2_timestamp_domain info); +const char * rs2_notification_category_to_string(rs2_notification_category category); +const char * rs2_visual_preset_to_string (rs2_visual_preset preset); +const char * rs2_log_severity_to_string (rs2_log_severity info); +const char * rs2_visual_preset_to_string (rs2_visual_preset preset); +const char * rs2_exception_type_to_string (rs2_exception_type type); void rs2_log_to_console(rs2_log_severity min_severity, rs2_error ** error); void rs2_log_to_file(rs2_log_severity min_severity, const char * file_path, rs2_error ** error); diff --git a/include/librealsense/rs2.hpp b/include/librealsense/rs2.hpp index c1a7c43ee5..7a3758d9e4 100644 --- a/include/librealsense/rs2.hpp +++ b/include/librealsense/rs2.hpp @@ -261,7 +261,7 @@ namespace rs2 * \param[in] frame_metadata the frame_metadata whose value should be retrieved * \return the value of the frame_metadata */ - double get_frame_metadata(rs2_frame_metadata frame_metadata) const + rs2_metadata_t get_frame_metadata(rs2_frame_metadata frame_metadata) const { rs2_error * e = nullptr; auto r = rs2_get_frame_metadata(frame_ref, frame_metadata, &e); @@ -678,7 +678,7 @@ namespace rs2 void set_notifications_callback(T callback) const { rs2_error * e = nullptr; - rs2_set_notifications_callback_cpp(_dev.get(), + rs2_set_notifications_callback_cpp(_dev.get(), new notifications_callback(std::move(callback)), &e); error::handle(e); } @@ -975,7 +975,6 @@ namespace rs2 { } - std::shared_ptr _context; advanced _debug; }; @@ -1286,5 +1285,11 @@ inline std::ostream & operator << (std::ostream & o, rs2_format format) { return inline std::ostream & operator << (std::ostream & o, rs2_distortion distortion) { return o << rs2_distortion_to_string(distortion); } inline std::ostream & operator << (std::ostream & o, rs2_option option) { return o << rs2_option_to_string(option); } inline std::ostream & operator << (std::ostream & o, rs2_log_severity severity) { return o << rs2_log_severity_to_string(severity); } +inline std::ostream & operator << (std::ostream & o, rs2_camera_info camera_info) { return o << rs2_camera_info_to_string(camera_info); } +inline std::ostream & operator << (std::ostream & o, rs2_frame_metadata metadata) { return o << rs2_frame_metadata_to_string(metadata); } +inline std::ostream & operator << (std::ostream & o, rs2_timestamp_domain domain) { return o << rs2_timestamp_domain_to_string(domain); } +inline std::ostream & operator << (std::ostream & o, rs2_notification_category notificaton) { return o << rs2_notification_category_to_string(notificaton); } +inline std::ostream & operator << (std::ostream & o, rs2_visual_preset preset) { return o << rs2_visual_preset_to_string(preset); } +inline std::ostream & operator << (std::ostream & o, rs2_exception_type exception_type) { return o << rs2_exception_type_to_string(exception_type); } #endif // LIBREALSENSE_RS2_HPP diff --git a/scripts/patch-realsense-ubuntu-xenial.sh b/scripts/patch-realsense-ubuntu-xenial.sh index e271fe24b8..3e9640a131 100755 --- a/scripts/patch-realsense-ubuntu-xenial.sh +++ b/scripts/patch-realsense-ubuntu-xenial.sh @@ -1,5 +1,8 @@ #!/bin/bash +#Break execution on any error received +set -e + if [ $(ls /dev/video* | wc -l) -ne 0 ]; then read -p "Remove all RealSense cameras attached. Hit any key when ready" @@ -63,16 +66,17 @@ fi sudo cp /usr/src/linux-headers-$(uname -r)/.config . sudo cp /usr/src/linux-headers-$(uname -r)/Module.symvers . -# Basic build so we can build just the uvcvideo module +# Basic build for kernel modules #yes "" | make silentoldconfig modules_prepare -make silentoldconfig modules_prepare +echo -e "\e[32mPrepare kernel modules configuration\e[0m" +sudo make silentoldconfig modules_prepare # Build the uvc, accel and gyro modules KBASE=`pwd` cd drivers/media/usb/uvc sudo cp $KBASE/Module.symvers . echo -e "\e[32mCompiling uvc module\e[0m" -make -C $KBASE M=$KBASE/drivers/media/usb/uvc/ modules +sudo make -C $KBASE M=$KBASE/drivers/media/usb/uvc/ modules echo -e "\e[32mCompiling accelerometer and gyro modules\e[0m" make -C $KBASE M=$KBASE/drivers/iio/accel modules make -C $KBASE M=$KBASE/drivers/iio/gyro modules diff --git a/src/algo.cpp b/src/algo.cpp index 0d7bbd8ed2..32e9f578ae 100644 --- a/src/algo.cpp +++ b/src/algo.cpp @@ -70,17 +70,10 @@ auto_exposure_mechanism::auto_exposure_mechanism(option& gain_option, option& ex double values[2] = {}; - if (frame->get()->supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE)) - { - auto gain = _gain_option.query(); - values[0] = frame->get()->get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE); - values[1] = gain; - } - else - { - values[0] = _exposure_option.query(); - values[1] = _gain_option.query(); - } + values[0] = frame->get()->supports_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) ? + static_cast(frame->get()->get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE)) : _exposure_option.query(); + values[1] = frame->get()->supports_frame_metadata(RS2_FRAME_METADATA_GAIN_LEVEL) ? + static_cast(frame->get()->get_frame_metadata(RS2_FRAME_METADATA_GAIN_LEVEL)) : _gain_option.query(); values[0] /= 1000.; // Fisheye exposure value by extension control- // is in units of MicroSeconds, from FW version 5.6.3.0 diff --git a/src/archive.cpp b/src/archive.cpp index e7087dc7a9..40301084db 100644 --- a/src/archive.cpp +++ b/src/archive.cpp @@ -1,14 +1,15 @@ +#include "metadata-parser.h" #include "archive.h" using namespace rsimpl2; frame_archive::frame_archive(std::atomic* in_max_frame_queue_size, std::shared_ptr ts, - std::shared_ptr dev) + std::shared_ptr dev, + std::shared_ptr parsers) : max_frame_queue_size(in_max_frame_queue_size), mutex(), recycle_frames(true), _time_service(ts), - device(dev) - + device(dev), _metadata_parsers(parsers) { published_frames_count = 0; } @@ -173,14 +174,37 @@ frame* frame::publish(std::shared_ptr new_owner) return owner->publish_frame(std::move(*this)); } -double frame::get_frame_metadata(const rs2_frame_metadata& /*frame_metadata*/) const +rs2_metadata_t frame::get_frame_metadata(const rs2_frame_metadata& frame_metadata) const { - throw not_implemented_exception("unsupported metadata type"); + auto md_parsers = owner->get_md_parsers(); + + if ((!md_parsers) || (0 == additional_data.metadata_size)) + throw invalid_value_exception(to_string() << "metadata not available for " + << get_string(get_stream_type())<<" stream"); + + auto it = md_parsers.get()->find(frame_metadata); + if (it == md_parsers.get()->end()) // Possible user error - md attribute is not supported by this frame type + throw invalid_value_exception(to_string() << get_string(frame_metadata) + << " attribute is not applicable for " + << get_string(get_stream_type()) << " stream "); + + // Proceed to parse and extract the required data attribute + return it->second->get(*this); } -bool frame::supports_frame_metadata(const rs2_frame_metadata& /*frame_metadata*/) const +bool frame::supports_frame_metadata(const rs2_frame_metadata& frame_metadata) const { - return false; + auto md_parsers = owner->get_md_parsers(); + + // verify preconditions + if ((!md_parsers) || (0 == additional_data.metadata_size)) + return false; // No parsers are available or no metadata was attached + + auto it = md_parsers.get()->find(frame_metadata); + if (it == md_parsers.get()->end()) // Possible user error - md attribute is not supported by this frame type + return false; + + return it->second->supports(*this); } const byte* frame::get_frame_data() const diff --git a/src/archive.h b/src/archive.h index 6a2426af50..60a1e1fdce 100644 --- a/src/archive.h +++ b/src/archive.h @@ -5,10 +5,13 @@ #include "types.h" #include +#include +#include namespace rsimpl2 { class frame_archive; + class md_attribute_parser_base; } struct frame_additional_data @@ -20,18 +23,22 @@ struct frame_additional_data int fps = 0; int stride = 0; int bpp = 1; - rs2_format format = RS2_FORMAT_ANY; - rs2_stream stream_type = RS2_STREAM_COUNT; + rs2_format format = RS2_FORMAT_ANY; + rs2_stream stream_type = RS2_STREAM_COUNT; rs2_timestamp_domain timestamp_domain = RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK; - rs2_time_t system_time = 0; - rs2_time_t frame_callback_started = 0; + rs2_time_t system_time = 0; + rs2_time_t frame_callback_started = 0; + uint32_t metadata_size = 0; + bool fisheye_ae_mode = false; + std::array metadata_blob; frame_additional_data() {}; frame_additional_data(double in_timestamp, unsigned long long in_frame_number, double in_system_time, int in_width, int in_height, int in_fps, int in_stride, int in_bpp, - const rs2_format in_format, rs2_stream in_stream_type) + const rs2_format in_format, rs2_stream in_stream_type, + uint8_t md_size, const uint8_t* md_buf) : timestamp(in_timestamp), frame_number(in_frame_number), system_time(in_system_time), @@ -41,7 +48,13 @@ struct frame_additional_data stride(in_stride), bpp(in_bpp), format(in_format), - stream_type(in_stream_type) {} + stream_type(in_stream_type), + metadata_size(md_size) + { + // Copy up to 255 bytes to preserve metadata as raw data + if (metadata_size) + std::copy(md_buf,md_buf+ std::min(md_size,MAX_META_DATA_SIZE),metadata_blob.begin()); + } }; // Define a movable but explicitly noncopyable buffer type to hold our frame data @@ -80,7 +93,7 @@ struct frame ~frame() { on_release.reset(); } - double get_frame_metadata(const rs2_frame_metadata& frame_metadata) const; + rs2_metadata_t get_frame_metadata(const rs2_frame_metadata& frame_metadata) const; bool supports_frame_metadata(const rs2_frame_metadata& frame_metadata) const; const byte* get_frame_data() const; rs2_time_t get_frame_timestamp() const; @@ -215,6 +228,8 @@ struct callback_invocation_holder namespace rsimpl2 { + typedef std::map> metadata_parser_map; + // Defines general frames storage model class frame_archive : public std::enable_shared_from_this { @@ -230,11 +245,13 @@ namespace rsimpl2 std::recursive_mutex mutex; std::shared_ptr _time_service; std::shared_ptr device; + std::shared_ptr _metadata_parsers = nullptr; public: explicit frame_archive(std::atomic* max_frame_queue_size, std::shared_ptr ts, - std::shared_ptr dev); + std::shared_ptr dev, + std::shared_ptr parsers); callback_invocation_holder begin_callback() { @@ -251,6 +268,7 @@ namespace rsimpl2 frame alloc_frame(const size_t size, const frame_additional_data& additional_data, bool requires_memory); rs2_frame* track_frame(frame& f); void log_frame_callback_end(frame* frame) const; + std::shared_ptr get_md_parsers(void) const { return _metadata_parsers; }; void flush(); diff --git a/src/backend-hid.cpp b/src/backend-hid.cpp index 14d4722478..972c2b3a25 100644 --- a/src/backend-hid.cpp +++ b/src/backend-hid.cpp @@ -47,7 +47,7 @@ const size_t MAX_DEV_PARENT_DIR = 10; -const size_t HID_METADATA_SIZE = 8; // bytes +const uint8_t HID_METADATA_SIZE = 8; // bytes const size_t HID_DATA_ACTUAL_SIZE = 6; // bytes const std::string IIO_DEVICE_PREFIX("iio:device"); @@ -559,7 +559,7 @@ namespace rsimpl2 auto hid_data_size = channel_size - HID_METADATA_SIZE; - sens_data.fo = {hid_data_size, metadata?HID_METADATA_SIZE:0, p_raw_data, metadata?p_raw_data + hid_data_size:nullptr}; + sens_data.fo = {hid_data_size, metadata?HID_METADATA_SIZE: uint8_t(0), p_raw_data, metadata?p_raw_data + hid_data_size:nullptr}; this->_callback(sens_data); } @@ -1081,13 +1081,17 @@ namespace rsimpl2 bool v4l_hid_device::get_hid_device_info(const char* dev_path, hid_device_info& device_info) { char device_path[PATH_MAX] = {}; - realpath(dev_path, device_path); + if (nullptr == realpath(dev_path, device_path)) + { + LOG_WARNING("Could not resolve HID path: " << dev_path); + return false; + } std::string device_path_str(device_path); device_path_str+="/"; std::string busnum, devnum, devpath, vid, pid, dev_id, dev_name; std::ifstream(device_path_str + "name") >> dev_name; - auto good = false; + auto valid = false; for(auto i=0; i < MAX_DEV_PARENT_DIR; ++i) { if(std::ifstream(device_path_str + "busnum") >> busnum) @@ -1102,7 +1106,7 @@ namespace rsimpl2 { if(std::ifstream(device_path_str + "dev") >> dev_id) { - good = true; + valid = true; break; } } @@ -1113,7 +1117,7 @@ namespace rsimpl2 device_path_str += "../"; } - if (good) + if (valid) { device_info.vid = vid; device_info.pid = pid; @@ -1122,7 +1126,7 @@ namespace rsimpl2 device_info.device_path = device_path; } - return good; + return valid; } } } diff --git a/src/backend-v4l2.cpp b/src/backend-v4l2.cpp index 9751da0907..6b6713939a 100644 --- a/src/backend-v4l2.cpp +++ b/src/backend-v4l2.cpp @@ -48,7 +48,6 @@ const size_t MAX_DEV_PARENT_DIR = 10; -const size_t META_DATA_SIZE = 256; // bytes #ifdef ANDROID @@ -192,8 +191,8 @@ namespace rsimpl2 } else { - _length += META_DATA_SIZE; - _start = static_cast(malloc( buf.length + META_DATA_SIZE)); + _length += MAX_META_DATA_SIZE; + _start = static_cast(malloc( buf.length + MAX_META_DATA_SIZE)); if (!_start) linux_backend_exception("User_p allocation failed!"); memset(_start, 0, _length); } @@ -249,8 +248,8 @@ namespace rsimpl2 { if (V4L2_MEMORY_USERPTR == _use_memory_map) { - auto metadata_offset = get_full_length() - META_DATA_SIZE; - memset((byte*)(get_frame_start()) + metadata_offset, 0, META_DATA_SIZE); + auto metadata_offset = get_full_length() - MAX_META_DATA_SIZE; + memset((byte*)(get_frame_start()) + metadata_offset, 0, MAX_META_DATA_SIZE); } if (xioctl(fd, VIDIOC_QBUF, &_buf) < 0) @@ -724,7 +723,7 @@ namespace rsimpl2 if (_is_started) { - if((buf.bytesused < buffer->get_full_length() - META_DATA_SIZE) && + if((buf.bytesused < buffer->get_full_length() - MAX_META_DATA_SIZE) && buf.bytesused > 0) { auto percentage = (100 * buf.bytesused) / buffer->get_full_length(); @@ -738,7 +737,7 @@ namespace rsimpl2 else { frame_object fo{ buffer->get_length_frame_only(), - has_metadata() ? META_DATA_SIZE : 0, + has_metadata() ? MAX_META_DATA_SIZE : uint8_t(0), buffer->get_frame_start(), has_metadata() ? buffer->get_frame_start() + buffer->get_length_frame_only() : nullptr }; diff --git a/src/backend.h b/src/backend.h index e6cf82d6ef..faa14c4286 100644 --- a/src/backend.h +++ b/src/backend.h @@ -23,6 +23,9 @@ const uint16_t VID_INTEL_CAMERA = 0x8086; const uint8_t DEFAULT_FRAME_BUFFERS = 4; const uint16_t DELAY_FOR_RETRIES = 50; +const uint8_t MAX_META_DATA_SIZE = 0xff; // UVC Metadata total length + // is limited by design to 255 bytes + namespace rsimpl2 { struct notification; @@ -116,12 +119,25 @@ namespace rsimpl2 (a.format == b.format); } +#pragma pack(push, 1) + struct uvc_header + { + uint8_t length; // UVC Metadata total length is + // limited by design to 255 bytes + uint8_t info; + uint32_t timestamp; + uint8_t source_clock[6]; + }; +#pragma pack(pop) + + constexpr uint8_t uvc_header_size = sizeof(uvc_header); + struct frame_object { - size_t frame_size; - size_t metadata_size; - const void * pixels; - const void * metadata; + size_t frame_size; + uint8_t metadata_size; + const void * pixels; + const void * metadata; }; typedef std::function)> frame_callback; @@ -423,7 +439,7 @@ namespace rsimpl2 for (auto&& dev : _dev) dev->open(); } - void close() override + void close() override { for (auto&& dev : _dev) dev->close(); } diff --git a/src/ds5-options.cpp b/src/ds5-options.cpp index f825865018..bd3afa5f6d 100644 --- a/src/ds5-options.cpp +++ b/src/ds5-options.cpp @@ -61,27 +61,27 @@ namespace rsimpl2 return temp; })); - int8_t temperature::* feild; - uint8_t temperature::* is_valid_feild; + int8_t temperature::* field; + uint8_t temperature::* is_valid_field; switch (_option) { case RS2_OPTION_ASIC_TEMPERATURE: - feild = &temperature::asic_temperature; - is_valid_feild = &temperature::is_asic_valid; + field = &temperature::asic_temperature; + is_valid_field = &temperature::is_asic_valid; break; case RS2_OPTION_PROJECTOR_TEMPERATURE: - feild = &temperature::projector_temperature; - is_valid_feild = &temperature::is_projector_valid; + field = &temperature::projector_temperature; + is_valid_field = &temperature::is_projector_valid; break; default: throw invalid_value_exception(to_string() << rs2_option_to_string(_option) << " is not temperature option!"); } - if (!static_cast(temperature_data.*is_valid_feild)) + if (0 == temperature_data.*is_valid_field) throw invalid_value_exception(to_string() << rs2_option_to_string(_option) << " value is not valid!"); - return temperature_data.*feild; + return temperature_data.*field; } option_range asic_and_projector_temperature_options::get_range() const @@ -249,6 +249,8 @@ namespace rsimpl2 if (!_to_add_frames || stream != RS2_STREAM_FISHEYE) return; + f.get()->additional_data.fisheye_ae_mode = true; + _auto_exposure->add_frame(f.get()->get_owner()->clone_frame(&f), std::move(callback)); }); } @@ -356,7 +358,7 @@ namespace rsimpl2 cmd.param1 = ds::etDepthTableControl; auto depth_table = get_depth_table(ds::GET_VAL); - depth_table.depth_units = 1000000 * value; + depth_table.depth_units = static_cast(1000000 * value); auto ptr = (uint8_t*)(&depth_table); cmd.data = std::vector(ptr, ptr + sizeof(ds::depth_table_control)); @@ -366,7 +368,7 @@ namespace rsimpl2 float depth_scale_option::query() const { auto table = get_depth_table(ds::GET_VAL); - return (float)(0.000001 * table.depth_units); + return (float)(0.000001 * (float)table.depth_units); } option_range depth_scale_option::get_range() const diff --git a/src/ds5-private.cpp b/src/ds5-private.cpp index dbe35fb9be..03d5fb1ea8 100644 --- a/src/ds5-private.cpp +++ b/src/ds5-private.cpp @@ -53,7 +53,7 @@ namespace rsimpl2 } rs2_intrinsics get_intrinsic_fisheye_table(const std::vector& raw_data, uint32_t width, uint32_t height) - { + { auto table = check_calib(raw_data); rs2_intrinsics intrinsics; diff --git a/src/ds5-private.h b/src/ds5-private.h index 2fc1f3078e..32d987dbce 100644 --- a/src/ds5-private.h +++ b/src/ds5-private.h @@ -177,14 +177,6 @@ namespace rsimpl2 int32_t disparity_shift; }; - struct uvc_header - { - byte length; - byte info; - uint32_t timestamp; - byte source_clock[6]; - }; - struct metadata_header { uint32_t metaDataID; @@ -207,7 +199,7 @@ namespace rsimpl2 struct metadata { - uvc_header header; + uvc::uvc_header header; metadata_capture_timing md_capture_timing; }; diff --git a/src/ds5-timestamp.cpp b/src/ds5-timestamp.cpp index 3ca00fe8af..aaf6958125 100644 --- a/src/ds5-timestamp.cpp +++ b/src/ds5-timestamp.cpp @@ -190,6 +190,7 @@ namespace rsimpl2 unsigned long long ds5_iio_hid_timestamp_reader::get_frame_counter(const request_mapping & mode, const uvc::frame_object& fo) const { std::lock_guard lock(_mtx); + if (nullptr == mode.pf) return 0; // Windows support is limited int index = 0; if (mode.pf->fourcc == 'GYRO') index = 1; diff --git a/src/ds5.cpp b/src/ds5.cpp index 8e00a30008..c65802f368 100644 --- a/src/ds5.cpp +++ b/src/ds5.cpp @@ -5,11 +5,13 @@ #include #include #include +#include #include "device.h" #include "context.h" #include "image.h" +#include "metadata-parser.h" #include "ds5.h" #include "ds5-private.h" #include "ds5-options.h" @@ -317,7 +319,7 @@ namespace rsimpl2 depth_ep->register_pixel_format(pf_y8); // Left Only - Luminance depth_ep->register_pixel_format(pf_yuyv); // Left Only depth_ep->register_pixel_format(pf_uyvyl); // Color from Depth - depth_ep->register_pixel_format(pf_rgb888); + depth_ep->register_pixel_format(pf_rgb888); // TODO: These if conditions will be implemented as inheritance classes @@ -428,7 +430,7 @@ namespace rsimpl2 std::string motion_module_fw_version{""}; auto pid = dev_info.front().pid; - auto pid_hex_str = hexify(pid>>8) + hexify(pid); + auto pid_hex_str = hexify(pid>>8) + hexify(static_cast(pid)); std::string is_camera_locked{""}; if (camera_fw_version >= firmware_version("5.6.3.0")) @@ -516,9 +518,42 @@ namespace rsimpl2 depth_ep.register_option(RS2_OPTION_DEPTH_UNITS, std::make_shared(*_hw_monitor)); else depth_ep.register_option(RS2_OPTION_DEPTH_UNITS, std::make_shared("Number of meters represented by a single depth unit", - 0.001)); + 0.001f)); + // Metadata registration + depth_ep.register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_uvc_header_parser(&uvc::uvc_header::timestamp)); - // TODO: These if conditions will be implemented as inheritance classes + // attributes of md_capture_timing + auto md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_depth_mode, depth_y_mode) + + offsetof(md_depth_y_normal_mode, intel_capture_timing); + + depth_ep.register_metadata(RS2_FRAME_METADATA_FRAME_COUNTER, make_attribute_parser(&md_capture_timing::frame_counter, md_capture_timing_attributes::frame_counter_attribute,md_prop_offset)); + depth_ep.register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, make_attribute_parser(&md_capture_timing::sensor_timestamp, md_capture_timing_attributes::sensor_timestamp_attribute, md_prop_offset)); + + // attributes of md_capture_stats + md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_depth_mode, depth_y_mode) + + offsetof(md_depth_y_normal_mode, intel_capture_stats); + + depth_ep.register_metadata(RS2_FRAME_METADATA_WHITE_BALANCE, make_attribute_parser(&md_capture_stats::white_balance, md_capture_stat_attributes::white_balance_attribute, md_prop_offset)); + + // attributes of md_depth_control + md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_depth_mode, depth_y_mode) + + offsetof(md_depth_y_normal_mode, intel_depth_control); + + depth_ep.register_metadata(RS2_FRAME_METADATA_GAIN_LEVEL, make_attribute_parser(&md_depth_control::manual_gain, md_depth_control_attributes::gain_attribute, md_prop_offset)); + depth_ep.register_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE, make_attribute_parser(&md_depth_control::manual_exposure, md_depth_control_attributes::exposure_attribute, md_prop_offset)); + depth_ep.register_metadata(RS2_FRAME_METADATA_AUTO_EXPOSURE, make_attribute_parser(&md_depth_control::auto_exposure_mode, md_depth_control_attributes::ae_mode_attribute, md_prop_offset)); + + // md_configuration - will be used for internal validation only + md_prop_offset = offsetof(metadata_raw, mode) + offsetof(md_depth_mode, depth_y_mode) + offsetof(md_depth_y_normal_mode, intel_configuration); + + depth_ep.register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_HW_TYPE, make_attribute_parser(&md_configuration::hw_type, md_configuration_attributes::hw_type_attribute, md_prop_offset)); + depth_ep.register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_SKU_ID, make_attribute_parser(&md_configuration::sku_id, md_configuration_attributes::sku_id_attribute, md_prop_offset)); + depth_ep.register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_FORMAT, make_attribute_parser(&md_configuration::format, md_configuration_attributes::format_attribute, md_prop_offset)); + depth_ep.register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_WIDTH, make_attribute_parser(&md_configuration::width, md_configuration_attributes::width_attribute, md_prop_offset)); + depth_ep.register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_HEIGHT, make_attribute_parser(&md_configuration::height, md_configuration_attributes::height_attribute, md_prop_offset)); std::shared_ptr fisheye_ep; @@ -555,6 +590,42 @@ namespace rsimpl2 "Exposure time of Fisheye camera")); } + // Metadata registration + fisheye_ep->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_uvc_header_parser(&uvc::uvc_header::timestamp)); + fisheye_ep->register_metadata(RS2_FRAME_METADATA_AUTO_EXPOSURE, make_additional_data_parser(&frame_additional_data::fisheye_ae_mode)); + + // attributes of md_capture_timing + md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_fisheye_mode, fisheye_mode) + + offsetof(md_fisheye_normal_mode, intel_capture_timing); + + fisheye_ep->register_metadata(RS2_FRAME_METADATA_FRAME_COUNTER, make_attribute_parser(&md_capture_timing::frame_counter, md_capture_timing_attributes::frame_counter_attribute,md_prop_offset)); + fisheye_ep->register_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP, make_attribute_parser(&md_capture_timing::sensor_timestamp, md_capture_timing_attributes::sensor_timestamp_attribute, md_prop_offset)); + + // attributes of md_capture_stats + md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_fisheye_mode, fisheye_mode) + + offsetof(md_fisheye_normal_mode, intel_capture_stats); + + // attributes of md_capture_stats + md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_fisheye_mode, fisheye_mode) + + offsetof(md_fisheye_normal_mode, intel_configuration); + + fisheye_ep->register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_HW_TYPE, make_attribute_parser(&md_configuration::hw_type, md_configuration_attributes::hw_type_attribute, md_prop_offset)); + fisheye_ep->register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_SKU_ID, make_attribute_parser(&md_configuration::sku_id, md_configuration_attributes::sku_id_attribute, md_prop_offset)); + fisheye_ep->register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_FORMAT, make_attribute_parser(&md_configuration::format, md_configuration_attributes::format_attribute, md_prop_offset)); + fisheye_ep->register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_WIDTH, make_attribute_parser(&md_configuration::width, md_configuration_attributes::width_attribute, md_prop_offset)); + fisheye_ep->register_metadata((rs2_frame_metadata)RS2_FRAME_METADATA_HEIGHT, make_attribute_parser(&md_configuration::height, md_configuration_attributes::height_attribute, md_prop_offset)); + + // attributes of md_fisheye_control + md_prop_offset = offsetof(metadata_raw, mode) + + offsetof(md_fisheye_mode, fisheye_mode) + + offsetof(md_fisheye_normal_mode, intel_fisheye_control); + + fisheye_ep->register_metadata(RS2_FRAME_METADATA_GAIN_LEVEL, make_attribute_parser(&md_fisheye_control::manual_gain, md_depth_control_attributes::gain_attribute, md_prop_offset)); + fisheye_ep->register_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE, make_attribute_parser(&md_fisheye_control::manual_exposure, md_depth_control_attributes::exposure_attribute, md_prop_offset)); + // Add fisheye endpoint _fisheye_device_idx = add_endpoint(fisheye_ep); @@ -655,7 +726,7 @@ namespace rsimpl2 camera_info[RS2_CAMERA_INFO_IS_CAMERA_LOCKED] = is_camera_locked; register_endpoint_info(_fisheye_device_idx, camera_info); - } + } else if (color_ep && element.pid == RS415_PID && element.mi == 3) // mi 3 is related to Color device { std::map camera_info = { { RS2_CAMERA_INFO_DEVICE_NAME, device_name }, @@ -694,13 +765,13 @@ namespace rsimpl2 if (is_left(to_stream) && from_stream == RS2_STREAM_INFRARED2) { auto table = ds::check_calib(*_coefficients_table_raw); - ext.translation[0] = -0.001 * table->baseline; + ext.translation[0] = -0.001f * table->baseline; return ext; } else if (to_stream == RS2_STREAM_INFRARED2 && is_left(from_stream)) { auto table = ds::check_calib(*_coefficients_table_raw); - ext.translation[0] = 0.001 * table->baseline; + ext.translation[0] = 0.001f * table->baseline; return ext; } } diff --git a/src/ds5.h b/src/ds5.h index 75c723e811..8e8af6d1fc 100644 --- a/src/ds5.h +++ b/src/ds5.h @@ -139,7 +139,6 @@ namespace rsimpl2 std::unique_ptr _polling_error_handler; - }; class ds5_notification_decoder :public notification_decoder diff --git a/src/error-handling.cpp b/src/error-handling.cpp index e165c7392b..658510c097 100644 --- a/src/error-handling.cpp +++ b/src/error-handling.cpp @@ -11,7 +11,7 @@ namespace rsimpl2 _active_object([this](dispatcher::cancellable_timer cancellable_timer) { polling(cancellable_timer); - }), + }), _option(std::move(option)), _notifications_proccessor(proccessor), _decoder(std::move(decoder)) @@ -47,7 +47,6 @@ namespace rsimpl2 auto strong = _notifications_proccessor.lock(); if (strong)strong->raise_notification(notification); } - } catch (const std::exception& ex) { diff --git a/src/metadata-parser.h b/src/metadata-parser.h new file mode 100644 index 0000000000..fb58761cdf --- /dev/null +++ b/src/metadata-parser.h @@ -0,0 +1,172 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. +// Metadata attributes provided by RS4xx Depth Cameras + +#pragma once + +#include "types.h" +#include "archive.h" +#include "metadata.h" + +using namespace rsimpl2; + +namespace rsimpl2 +{ +/** \brief Metadata fields that are utilized internally by librealsense + Provides extention to the r2_frame_metadata list of attributes*/ + enum frame_metadata_internal + { + RS2_FRAME_METADATA_HW_TYPE = RS2_FRAME_METADATA_COUNT +1 , /**< 8-bit Module type: RS4xx, IVCAM*/ + RS2_FRAME_METADATA_SKU_ID , /**< 8-bit SKU Id*/ + RS2_FRAME_METADATA_FORMAT , /**< 16-bit Frame format*/ + RS2_FRAME_METADATA_WIDTH , /**< 16-bit Frame width. pixels*/ + RS2_FRAME_METADATA_HEIGHT , /**< 16-bit Frame height. pixels*/ + RS2_FRAME_METADATA_COUNT + }; + + /**\brief Base class that establishes the interface for retrieving metadata attributes*/ + class md_attribute_parser_base + { + public: + virtual rs2_metadata_t get(const frame & frm) const = 0; + virtual bool supports(const frame & frm) const = 0; + + virtual ~md_attribute_parser_base() = default; + }; + + /**\brief The metadata parser class directly access the metadata attribute in the blob recieved from HW. + * Given the metadata-nested construct, and the c++ lack of pointers + * to the inner struct, we pre-calculate and store the attribute offset internally + * http://stackoverflow.com/questions/1929887/is-pointer-to-inner-struct-member-forbidden*/ + template + class md_attribute_parser : public md_attribute_parser_base + { + public: + md_attribute_parser(Attribute S::* attribute_name, Flag flag, unsigned long long offset) : + _md_attribute(attribute_name), _md_flag(flag), _offset(offset) {}; + + rs2_metadata_t get(const frame & frm) const override + { + auto s = reinterpret_cast(((const uint8_t*)frm.additional_data.metadata_blob.data()) + _offset); + + if (!is_attribute_valid(s)) + throw invalid_value_exception("metadata not available"); + + return static_cast((*s).*_md_attribute); + } + + // Verifies that the parameter is both supported and available + bool supports(const frame & frm) const override + { + auto s = reinterpret_cast(((const uint8_t*)frm.additional_data.metadata_blob.data()) + _offset); + + return is_attribute_valid(s); + } + + protected: + + bool is_attribute_valid(const S* s) const + { + // verify that the struct is of the correct type + // Check that the header id and the struct size corresponds. + // Note that this heurisic is not deterministic and may validate false frames! TODO - requires review + md_type expected_type = md_type_trait::type; + + if ((s->header.md_type_id != expected_type) || (s->header.md_size !=sizeof(*s))) + { + std::string type = (md_type_desc.count(s->header.md_type_id) > 0) ? + md_type_desc.at(s->header.md_type_id) : (to_string() << static_cast(s->header.md_type_id)); + LOG_DEBUG("metadata mismatch - received: " << type << ", expected: " << md_type_desc.at(expected_type)); + return false; + } + + // Check if the attribute's flag is set + auto attribute_enabled = (0 !=(s->flags & static_cast(_md_flag))); + if (!attribute_enabled) + LOG_DEBUG("metadata attribute No: "<< (*s.*_md_attribute) << "is not active"); + + return attribute_enabled; + } + + private: + md_attribute_parser() = delete; + md_attribute_parser(const md_attribute_parser&) = delete; + + Attribute S::* _md_attribute; // Pointer to the attribute within struct that holds the relevant data + Flag _md_flag; // Bit that indicates whether the particluar attribute is actiive + unsigned long long _offset; // Inner struct offset with regard to the most outer one + }; + + /**\brief A helper function to create a specialized attribute parser. + * Return it as a pointer to a base-class*/ + template + std::shared_ptr make_attribute_parser(Attribute S::* attribute, Flag flag, unsigned long long offset) + { + std::shared_ptr> parser(new md_attribute_parser(attribute,flag, offset)); + return parser; + } + + /**\brief A UVC-Header parser class*/ + template + class md_uvc_header_parser : public md_attribute_parser_base + { + public: + md_uvc_header_parser(Attribute St::* attribute_name) : + _md_attribute(attribute_name) {}; + + rs2_metadata_t get(const frame & frm) const override + { + auto val = static_cast((*reinterpret_cast((const uint8_t*)frm.additional_data.metadata_blob.data())).*_md_attribute); + + return val; + } + + bool supports(const frame & frm) const override + { return true; } + + private: + md_uvc_header_parser() = delete; + md_uvc_header_parser(const md_uvc_header_parser&) = delete; + + Attribute St::* _md_attribute; // Pointer to the attribute within uvc header that provides the relevant data + }; + + /**\brief A utility function to create uvh header parser*/ + template + std::shared_ptr make_uvc_header_parser(Attribute St::* attribute) + { + std::shared_ptr> parser(new md_uvc_header_parser(attribute)); + return parser; + } + + /**\brief provide attributes generated and stored internally by the library*/ + template + class md_additional_parser : public md_attribute_parser_base + { + public: + md_additional_parser(Attribute St::* attribute_name) : + _md_attribute(attribute_name) {}; + + rs2_metadata_t get(const frame & frm) const override + { + return static_cast(frm.additional_data.*_md_attribute); + } + + bool supports(const frame & frm) const override + { return true; } + + private: + md_additional_parser() = delete; + md_additional_parser(const md_additional_parser&) = delete; + + Attribute St::* _md_attribute; // Pointer to the attribute within uvc header that provides the relevant data + }; + + /**\brief A utility function to create additional_data parser*/ + template + std::shared_ptr make_additional_data_parser(Attribute St::* attribute) + { + std::shared_ptr> parser(new md_additional_parser(attribute)); + return parser; + } +} diff --git a/src/metadata.h b/src/metadata.h new file mode 100644 index 0000000000..52e77e2c41 --- /dev/null +++ b/src/metadata.h @@ -0,0 +1,411 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. +// Metadata attributes provided by RS4xx Depth Cameras + +#pragma once + +#include "types.h" + +#define REGISTER_MD_TYPE(A,B)\ + template<>\ + struct md_type_trait\ + {\ + static const md_type type = B;\ + }; + + +namespace rsimpl2 +{ + template + struct md_type_trait; + + // Metadata tables version + const int META_DATA_INTEL_DEPTH_CONTROL_VERSION = 0x1; + const int META_DATA_INTEL_CONFIGURATION_VERSION = 0x1; + const int META_DATA_INTEL_STAT_VERSION = 0x1; + const int META_DATA_INTEL_CAPTURE_TIMING_VERSION = 0x1; + + /**\brief md_mode - enumerates the types of metadata modes(structs) supported */ + enum class md_type: uint32_t + { + META_DATA_INTEL_DEPTH_CONTROL_ID = 0x80000000, + META_DATA_INTEL_CAPTURE_TIMING_ID = 0x80000001, + META_DATA_INTEL_CONFIGURATION_ID = 0x80000002, + META_DATA_INTEL_STAT_ID = 0x80000003, + META_DATA_INTEL_FISH_EYE_CONTROL_ID = 0x80000004, + META_DATA_INTEL_RGB_CONTROL_ID = 0x80000005, + META_DATA_INTEl_FE_FOV_MODEL_ID = 0x80000006, + META_DATA_CAPTURE_STATS_ID = 0x00000003, + META_DATA_CAMERA_EXTRINSICS_ID = 0x00000004, + META_DATA_CAMERA_INTRINSICS_ID = 0x00000005, + META_DATA_CAMERA_DEBUG_ID = 0x800000FF, + }; + + static const std::map md_type_desc = + { + { md_type::META_DATA_INTEL_DEPTH_CONTROL_ID, "Intel Depth Control"}, + { md_type::META_DATA_INTEL_CAPTURE_TIMING_ID, "Intel Capture timing"}, + { md_type::META_DATA_INTEL_CONFIGURATION_ID, "Intel Configuration"}, + { md_type::META_DATA_INTEL_STAT_ID, "Intel Statistics"}, + { md_type::META_DATA_INTEL_FISH_EYE_CONTROL_ID, "Intel Fisheye Control"}, + { md_type::META_DATA_INTEL_RGB_CONTROL_ID, "Intel RGB Control"}, + { md_type::META_DATA_INTEl_FE_FOV_MODEL_ID, "Intel Fisheye FOV Model"}, + { md_type::META_DATA_CAPTURE_STATS_ID, "Capture Statistics"}, + { md_type::META_DATA_CAMERA_EXTRINSICS_ID, "Camera Extrinsic"}, + { md_type::META_DATA_CAMERA_INTRINSICS_ID, "Camera Intrinsic"}, + { md_type::META_DATA_CAMERA_DEBUG_ID, "Camera Debug"}, + }; + + /**\brief md_capture_timing_attributes - enumerate the bit offset to check + * a specific attribute of md_capture_timing struct for validity. + * The enumeration includes up to 32 attributes, according to the size + * of flags parameter in all the defined structs */ + enum class md_capture_timing_attributes : uint32_t + { + frame_counter_attribute = (1u << 0), + sensor_timestamp_attribute = (1u << 1), + readout_time_attribute = (1u << 2), + exposure_attribute = (1u << 3), + frame_interval_attribute = (1u << 4), + pipe_latency_attribute = (1u << 5), + }; + + /**\brief md_capture_stat_attributes - bit mask to find enabled attributes + * in md_capture_stats */ + enum class md_capture_stat_attributes : uint32_t + { + exposure_time_attribute = (1u << 0), + exposure_compensation_attribute = (1u << 1), + iso_speed_attribute = (1u << 2), + focus_state_attribute = (1u << 3), + lens_posiiton_attribute = (1u << 4), + white_balance_attribute = (1u << 5), + flash_attribute = (1u << 6), + flash_power_attribute = (1u << 7), + zoom_factor_attribute = (1u << 8), + scene_mode_attribute = (1u << 9), + sensor_framerate_attribute = (1u << 10), + }; + + /**\brief md_depth_control_attributes - bit mask to find active attributes, + * md_depth_control struct */ + enum class md_depth_control_attributes : uint32_t + { + gain_attribute = (1u << 0), + exposure_attribute = (1u << 1), + laser_pwr_attribute = (1u << 2), + ae_mode_attribute = (1u << 3), + exposure_priority_attribute = (1u << 4), + roi_attribute = (1u << 5), + preset_attribute = (1u << 6), + }; + + /**\brief md_fisheye_control_attributes - bit mask to find active attributes, + * md_fisheye_control struct */ + enum class md_fisheye_control_attributes : uint32_t + { + gain_attribute = (1u << 0), + exposure_attribute = (1u << 1), + }; + + /**\brief md_configuration_attributes - bit mask to find active attributes, + * md_configuration struct */ + enum class md_configuration_attributes : uint32_t + { + hw_type_attribute = (1u << 0), + sku_id_attribute = (1u << 1), + cookie_attribute = (1u << 2), + format_attribute = (1u << 3), + width_attribute = (1u << 4), + height_attribute = (1u << 5), + fps_attribute = (1u << 6), + trigger_attribute = (1u << 7), + calibration_count_attribute = (1u << 8), + }; + + /**\brief md_stat_attributes - bit mask to find active attributes, + * md_stat struct */ + enum class md_stat_attributes : uint32_t + { + left_sum_attribute = (1u << 0), + left_dark_count_attribute = (1u << 1), + left_bright_count_attribute = (1u << 2), + right_sum_attribute = (1u << 3), + right_dark_count_attribute = (1u << 4), + right_bright_count_attribute = (1u << 5), + red_frame_count_attribute = (1u << 6), + left_red_sum_attribute = (1u << 7), + left_greeen1_attribute = (1u << 8), + left_greeen2_attribute = (1u << 9), + left_blue_sum_attribute = (1u << 10), + right_red_sum_attribute = (1u << 11), + right_greeen1_attribute = (1u << 12), + right_greeen2_attribute = (1u << 13), + right_blue_sum_attribute = (1u << 14), + }; + +#pragma pack(push, 1) + + /**\brief md_header - metadata header is a integral part of all metadata objects */ + struct md_header + { + md_type md_type_id; // The type of the metadata struct + uint32_t md_size; // Actual size of metadata struct without header + }; + + /**\brief md_capture_timing - properties associated with sensor configuration + * during video streaming. Corresponds to FW STMetaDataIntelCaptureTiming object*/ + struct md_capture_timing + { + md_header header; + uint32_t version; + uint32_t flags; // Bit array to specify attributes that are valid + uint32_t frame_counter; + uint32_t sensor_timestamp; //In millisecond unit + uint32_t readout_time; //The readout time in millisecond second unit + uint32_t exposure_time; //The exposure time in microsecond second unit + uint32_t frame_interval; //The frame interval in microsecond second unit + uint32_t pipe_latency; //The latency between start of frame to frame ready in USB buffer + }; + + REGISTER_MD_TYPE(md_capture_timing, md_type::META_DATA_INTEL_CAPTURE_TIMING_ID) + + /**\brief md_capture_stats - properties associated with optical sensor + * during video streaming. Corresponds to FW STMetaDataCaptureStats object*/ + struct md_capture_stats + { + md_header header; + uint32_t flags; + uint32_t reserved; + uint64_t exposure_time; + uint64_t exposure_compensation_flags; + int32_t exposure_compensation_value; + uint32_t iso_speed; + uint32_t focus_state; + uint32_t lens_position; // a.k.a Focus + uint32_t white_balance; + uint32_t flash; + uint32_t flash_power; + uint32_t zoom_factor; + uint64_t scene_mode; + uint64_t sensor_framerate; + }; + + REGISTER_MD_TYPE(md_capture_stats, md_type::META_DATA_CAPTURE_STATS_ID) + + /**\brief md_depth_control - depth data-related parameters. + * Corresponds to FW's STMetaDataIntelDepthControl object*/ + struct md_depth_control + { + md_header header; + uint32_t version; + uint32_t flags; + uint32_t manual_gain; // Manual gain value + uint32_t manual_exposure; // Manual exposure + uint32_t laser_power ; // Laser power value + uint32_t auto_exposure_mode; // AE mode. When active handles both Exposure and Gain + uint32_t exposure_priority ; + uint32_t exposure_roi_left; + uint32_t exposure_roi_right; + uint32_t exposure_roi_top; + uint32_t exposure_roi_bottom; + uint32_t preset; + }; + + REGISTER_MD_TYPE(md_depth_control, md_type::META_DATA_INTEL_DEPTH_CONTROL_ID) + + + /**\brief md_fisheye_control - fisheye-related parameters. + * Corresponds to FW's STMetaDataIntelFishEyeControl object*/ + struct md_fisheye_control + { + md_header header; + uint32_t version; + uint32_t flags; + uint32_t manual_gain; // Manual gain value + uint32_t manual_exposure; // Manual exposure + }; + + REGISTER_MD_TYPE(md_fisheye_control, md_type::META_DATA_INTEL_FISH_EYE_CONTROL_ID) + + /**\brief md_configuration - device/stream configuration. + * Corresponds to FW's STMetaDataIntelConfiguration object*/ + struct md_configuration + { + md_header header; + uint32_t version; + uint32_t flags; + uint8_t hw_type; // IVCAM2 , RS4xx, etc' + uint8_t sku_id; + uint32_t cookie; /* Place Holder enable FW to bundle cookie + with control state and configuration.*/ + uint16_t format; + uint16_t width; // Requested resolution + uint16_t height; + uint16_t fps; // Requested FPS + uint16_t trigger; /* Byte <0> 0 free-running + 1 in sync + 2 external trigger (depth only) + Byte <1> configured delay (depth only)*/ + uint16_t calibration_count; + uint8_t reserved[6]; + }; + + REGISTER_MD_TYPE(md_configuration, md_type::META_DATA_INTEL_CONFIGURATION_ID) + + /**\brief md_intel_stat + * Corresponds to FW's STMetaDataIntelStat object*/ + struct md_intel_stat + { + md_header header; + uint32_t version; + uint32_t flag; + uint32_t exposure_left_sum; + uint32_t exposure_left_dark_count; + uint32_t exposure_right_sum; + uint32_t exposure_right_dark_count; + uint32_t exposure_right_bright_count; + uint32_t rec_frame_count; + uint32_t left_red_sum; + uint32_t left_green1_sum; + uint32_t left_green2_sum; + uint32_t left_blue_sum; + uint32_t right_red_sum; + uint32_t right_green1_sum; + uint32_t right_green2_sum; + uint32_t right_blue_sum; + uint32_t reserved; + }; + + REGISTER_MD_TYPE(md_intel_stat, md_type::META_DATA_INTEL_STAT_ID) + + struct md_intrinsic_pinhole_cam_model + { + float2 focal_length; + float2 principal_point; + }; + + /**\brief md_intrinsic_distortion_model - Distortion coefficients + * of sensor instrinsic */ + struct md_intrinsic_distortion_model + { + float radial_k1; + float radial_k2; + float radial_k3; + float tangential_p1; + float tangential_p2; + }; + + /**\brief md_pinhole_cam_intrinsic_model - Pinhole sensor's characteristics*/ + struct md_pinhole_cam_intrinsic_model + { + uint32_t width; + uint32_t height; + md_intrinsic_pinhole_cam_model radial_k1; + md_intrinsic_distortion_model radial_k2; + }; + + const uint8_t INTRINSICS_MODEL_COUNT = 1; + + struct md_pinhole_camera_intrinsics + { + uint32_t intrinsic_model_count; + md_pinhole_cam_intrinsic_model intrinsic_models[INTRINSICS_MODEL_COUNT]; + }; + + struct md_camera_intrinsic + { + md_header header; + md_pinhole_camera_intrinsics pinhole_cam_intrinsics; + }; + + REGISTER_MD_TYPE(md_camera_intrinsic, md_type::META_DATA_CAMERA_INTRINSICS_ID) + + const uint8_t UVC_GUID_SIZE = 16; + struct md_extrinsic_calibrated_transform + { + uint8_t calibration_id[UVC_GUID_SIZE]; + float3 position; + float4 orientation; // quaternion representation + }; + + const uint8_t TRANSFORM_COUNT = 1; // TODO requires explanation + struct mf_camera_extrinsic + { + uint32_t transform_count; + md_extrinsic_calibrated_transform calibrated_transform[TRANSFORM_COUNT]; + }; + + struct md_camera_extrinsic + { + md_header header; + mf_camera_extrinsic camera_extrinsic; + }; + + REGISTER_MD_TYPE(md_camera_extrinsic, md_type::META_DATA_CAMERA_EXTRINSICS_ID) + + struct md_depth_y_normal_mode + { + md_capture_timing intel_capture_timing; + md_capture_stats intel_capture_stats; + md_depth_control intel_depth_control; + md_configuration intel_configuration; + }; + + struct md_fisheye_normal_mode + { + md_capture_timing intel_capture_timing; + md_capture_stats intel_capture_stats; + md_fisheye_control intel_fisheye_control; + md_configuration intel_configuration; + }; + + struct md_calibration_mode + { + md_capture_timing intel_capture_timing; + md_camera_extrinsic intel_camera_extrinsic; + md_camera_intrinsic intel_camera_intrinsic; + }; + + struct md_stat_mode + { + md_capture_timing intel_capture_timing; + md_capture_stats intel_capture_stats; + md_intel_stat metadata_intel_stat; + md_configuration intel_configuration; + }; + + union md_depth_mode + { + md_depth_y_normal_mode depth_y_mode; + md_calibration_mode calib_mode; + md_stat_mode stat_mode; + }; + + union md_fisheye_mode + { + md_fisheye_normal_mode fisheye_mode; + md_calibration_mode calib_mode; + }; + + /**\brief metadata_raw - aggrevative structure that represents all the possible + * metadata struct types to be handled */ + union md_modes + { + md_depth_mode depth_mode; + md_fisheye_mode fisheye_mode; + }; + + /**\brief metadata_raw - metadata structure + * layout as transmitted and recieved by backend */ + struct metadata_raw + { + uvc::uvc_header header; + md_modes mode; + }; + + constexpr uint8_t metadata_raw_size = sizeof(metadata_raw); + +#pragma pack(pop) + +} diff --git a/src/recorder.cpp b/src/recorder.cpp index 60fd6cb4cf..c62832bd5c 100644 --- a/src/recorder.cpp +++ b/src/recorder.cpp @@ -1111,9 +1111,9 @@ namespace rsimpl2 bool playback_uvc_device::set_xu(const extension_unit& xu, uint8_t ctrl, const uint8_t* data, int len) { - auto&& c = _rec->find_call(call_type::uvc_set_xu, _entity_id, [&](const call& call_found) + auto&& c = _rec->find_call(call_type::uvc_set_xu, _entity_id, [&](const call& call_found) { - return call_found.param1 == ctrl; + return call_found.param1 == ctrl; }); auto stored_data = _rec->load_blob(c.param2); @@ -1140,7 +1140,7 @@ namespace rsimpl2 control_range playback_uvc_device::get_xu_range(const extension_unit& xu, uint8_t ctrl, int len) const { control_range res; - auto&& c = _rec->find_call(call_type::uvc_get_xu_range, _entity_id, [&](const call& call_found) + auto&& c = _rec->find_call(call_type::uvc_get_xu_range, _entity_id, [&](const call& call_found) { return call_found.param1 == ctrl; }); @@ -1179,7 +1179,6 @@ namespace rsimpl2 { return call_found.param1 == opt; }); - auto range = _rec->load_blob(c.param2); rsimpl2::copy(&res, range.data(), range.size()); return res; @@ -1284,7 +1283,7 @@ namespace rsimpl2 sd.fo.pixels = (void*)sd_data.data(); sd.fo.frame_size = sd_data.size(); - auto metadata = _rec->load_blob(c_ptr->param3); + auto metadata = _rec->load_blob(c_ptr->param2); sd.fo.metadata = (void*)metadata.data(); sd.fo.metadata_size = metadata.size(); @@ -1321,7 +1320,7 @@ namespace rsimpl2 vector playback_usb_device::send_receive(const vector& data, int timeout_ms, bool require_response) { - auto&& c = _rec->find_call(call_type::send_command, _entity_id, [&](const call& call_found) + auto&& c = _rec->find_call(call_type::send_command, _entity_id, [&](const call& call_found) { return call_found.param3 == timeout_ms && (call_found.param4 > 0) == require_response && _rec->load_blob(call_found.param1) == data; }); @@ -1364,7 +1363,8 @@ namespace rsimpl2 frame_blob = _compression.decode(_rec->load_blob(c_ptr->param2)); } metadata_blob =_rec->load_blob(c_ptr->param5); - frame_object fo{ frame_blob.size(), metadata_blob.size(), + frame_object fo{ frame_blob.size(), + static_cast(metadata_blob.size()), // Metadata is limited to 0xff bytes by design frame_blob.data(),metadata_blob.data() }; pair.second(p, fo, []() {}); break; diff --git a/src/rs.cpp b/src/rs.cpp index a8c4ebe899..a0f3e3689a 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -500,7 +500,6 @@ int rs2_supports_camera_info(const rs2_device* device, rs2_camera_info info, rs2 } HANDLE_EXCEPTIONS_AND_RETURN(false, device, info) - void rs2_start(const rs2_device* device, rs2_frame_callback_ptr on_frame, void * user, rs2_error ** error) try { VALIDATE_NOT_NULL(device); @@ -581,12 +580,21 @@ void rs2_stop_stream(const rs2_device* device, rs2_stream stream, rs2_error ** e } HANDLE_EXCEPTIONS_AND_RETURN(, stream, device) -double rs2_get_frame_metadata(const rs2_frame * frame, rs2_frame_metadata frame_metadata, rs2_error ** error) try +int rs2_supports_frame_metadata(const rs2_frame * frame, rs2_frame_metadata frame_metadata, rs2_error ** error) try +{ + VALIDATE_NOT_NULL(frame); + VALIDATE_ENUM(frame_metadata); + return frame->get()->supports_frame_metadata(frame_metadata); +} +HANDLE_EXCEPTIONS_AND_RETURN(0, frame, frame_metadata) + +rs2_metadata_t rs2_get_frame_metadata(const rs2_frame * frame, rs2_frame_metadata frame_metadata, rs2_error ** error) try { VALIDATE_NOT_NULL(frame); + VALIDATE_ENUM(frame_metadata); return frame->get()->get_frame_metadata(frame_metadata); } -HANDLE_EXCEPTIONS_AND_RETURN(0, frame) +HANDLE_EXCEPTIONS_AND_RETURN(0, frame, frame_metadata) const char * rs2_get_notification_description(rs2_notification * notification, rs2_error** error)try { @@ -616,13 +624,6 @@ rs2_notification_category rs2_get_notification_category(rs2_notification * notif } HANDLE_EXCEPTIONS_AND_RETURN(RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR, notification) -int rs2_supports_frame_metadata(const rs2_frame * frame, rs2_frame_metadata frame_metadata, rs2_error ** error) try -{ - VALIDATE_NOT_NULL(frame); - return frame->get()->supports_frame_metadata(frame_metadata); -} -HANDLE_EXCEPTIONS_AND_RETURN(0, frame) - rs2_time_t rs2_get_frame_timestamp(const rs2_frame * frame_ref, rs2_error ** error) try { VALIDATE_NOT_NULL(frame_ref); @@ -831,7 +832,7 @@ HANDLE_EXCEPTIONS_AND_RETURN(, device, intrinsics) void rs2_hardware_reset(const rs2_device * device, rs2_error ** error) try { VALIDATE_NOT_NULL(device); - device->device->hardware_reset(); + device->device->hardware_reset(); } HANDLE_EXCEPTIONS_AND_RETURN(, device) @@ -979,6 +980,7 @@ const char * rs2_format_to_string(rs2_format format) { return rsimpl2::get_strin const char * rs2_distortion_to_string(rs2_distortion distortion) { return rsimpl2::get_string(distortion); } const char * rs2_option_to_string(rs2_option option) { return rsimpl2::get_string(option); } const char * rs2_camera_info_to_string(rs2_camera_info info) { return rsimpl2::get_string(info); } +const char * rs2_frame_metadata_to_string(rs2_frame_metadata metadata) { return rsimpl2::get_string(metadata); } const char * rs2_timestamp_domain_to_string(rs2_timestamp_domain info){ return rsimpl2::get_string(info); } const char * rs2_notification_category_to_string(rs2_notification_category category){ return rsimpl2::get_string(category); } const char * rs2_visual_preset_to_string(rs2_visual_preset preset) { return rsimpl2::get_string(preset); } diff --git a/src/subdevice.cpp b/src/subdevice.cpp index 22406d929b..e8f37bef97 100644 --- a/src/subdevice.cpp +++ b/src/subdevice.cpp @@ -26,7 +26,7 @@ namespace rsimpl2 *_ptr = value; } - float query() const override { return _ptr->load(); } + float query() const override { return static_cast(_ptr->load()); } bool is_enabled() const override { return true; } @@ -47,6 +47,7 @@ namespace rsimpl2 _stream_profiles([this]() { return this->init_stream_profiles(); }), _notifications_proccessor(std::shared_ptr(new notifications_proccessor())), _start_adaptor(this), + _metadata_parsers(std::make_shared()), _on_before_frame_callback(nullptr) { _options[RS2_OPTION_FRAMES_QUEUE_SIZE] = std::make_shared(&_max_publish_list_size, @@ -285,7 +286,8 @@ namespace rsimpl2 throw wrong_api_call_sequence_exception("open(...) failed. UVC device is already opened!"); auto on = std::unique_ptr(new power(shared_from_this())); - _archive = std::make_shared(&_max_publish_list_size,_ts, _device); + _archive = std::make_shared(&_max_publish_list_size,_ts, _device,_metadata_parsers); + auto mapping = resolve_requests(requests); auto timestamp_reader = _timestamp_reader.get(); @@ -334,6 +336,7 @@ namespace rsimpl2 frames_drops_counter.fetch_add(int(frame_counter - frame_drops_status->prev_frame_counter - 1)); frame_drops_status->prev_frame_counter = frame_counter; }*/ + auto&& unpacker = *mode.unpacker; for (auto&& output : unpacker.outputs) { @@ -351,8 +354,9 @@ namespace rsimpl2 width, bpp, output.second, - output.first); - + output.first, + static_cast(f.metadata_size), + (const uint8_t*)f.metadata); frame_holder frame = this->alloc_frame(width * height * bpp / 8, additional_data, requires_processing); if (frame.frame) @@ -577,6 +581,15 @@ namespace rsimpl2 register_option(id, std::make_shared(*this, id)); } + void endpoint::register_metadata(rs2_frame_metadata metadata, std::shared_ptr metadata_parser) + { + if (_metadata_parsers.get()->end() != _metadata_parsers.get()->find(metadata)) + throw invalid_value_exception( to_string() << "Metadata attribute parser for " << rs2_frame_metadata_to_string(metadata) + << " is already defined"); + + _metadata_parsers.get()->insert(std::pair>(metadata, metadata_parser)); + } + hid_endpoint::~hid_endpoint() { try @@ -697,7 +710,7 @@ namespace rsimpl2 else if(!_is_opened) throw wrong_api_call_sequence_exception("start_streaming(...) failed. Hid device was not opened!"); - _archive = std::make_shared(&_max_publish_list_size, _ts, nullptr); + _archive = std::make_shared(&_max_publish_list_size, _ts, nullptr, _metadata_parsers); _callback = std::move(callback); std::vector configured_hid_profiles; for (auto& elem : _configured_profiles) @@ -749,7 +762,7 @@ namespace rsimpl2 auto timestamp = timestamp_reader->get_frame_timestamp(mode, sensor_data.fo); auto frame_counter = timestamp_reader->get_frame_counter(mode, sensor_data.fo); - frame_additional_data additional_data; + frame_additional_data additional_data{}; additional_data.format = _configured_profiles[sensor_name].format; // TODO: Add frame_additional_data reader? @@ -766,7 +779,7 @@ namespace rsimpl2 LOG_DEBUG("FrameAccepted," << get_string(additional_data.stream_type) << "," << frame_counter << ",Arrived," << std::fixed << system_time - << ",TS," << timestamp + << ",TS," << std::fixed << timestamp << ",TS_Domain," << rs2_timestamp_domain_to_string(additional_data.timestamp_domain)); auto frame = this->alloc_frame(data_size, additional_data, true); diff --git a/src/subdevice.h b/src/subdevice.h index 9d9ce9861f..5d077f8043 100644 --- a/src/subdevice.h +++ b/src/subdevice.h @@ -55,7 +55,7 @@ namespace rsimpl2 class frame_callback : public rs2_frame_callback { public: - explicit frame_callback(start_adaptor* owner) + explicit frame_callback(start_adaptor* owner) : _owner(owner) {} void on_frame(rs2_frame* f) override; @@ -121,6 +121,8 @@ namespace rsimpl2 bool supports_info(rs2_camera_info info) const; void register_info(rs2_camera_info info, const std::string& val); + void register_metadata(rs2_frame_metadata metadata, std::shared_ptr metadata_parser); + void set_pose(lazy p) { _pose = std::move(p); } pose get_pose() const { return *_pose; } @@ -158,6 +160,7 @@ namespace rsimpl2 std::shared_ptr _ts; std::shared_ptr _notifications_proccessor; on_before_frame_callback _on_before_frame_callback; + std::shared_ptr _metadata_parsers = nullptr; private: std::map> _options; @@ -166,6 +169,7 @@ namespace rsimpl2 lazy _pose; std::map _camera_info; std::shared_ptr _roi_method = nullptr; + start_adaptor _start_adaptor; }; diff --git a/src/types.cpp b/src/types.cpp index f6ce6ac35f..71b4fe4213 100644 --- a/src/types.cpp +++ b/src/types.cpp @@ -225,6 +225,23 @@ namespace rsimpl2 #undef CASE } + const char * get_string(rs2_frame_metadata value) + { + #define CASE(X) case RS2_FRAME_METADATA_##X: return #X; + switch (value) + { + CASE(FRAME_COUNTER) + CASE(FRAME_TIMESTAMP) + CASE(SENSOR_TIMESTAMP) + CASE(ACTUAL_EXPOSURE) + CASE(GAIN_LEVEL) + CASE(AUTO_EXPOSURE) + CASE(WHITE_BALANCE) + default: assert(!is_valid(value)); return unknown; + } + #undef CASE + } + const char * get_string(rs2_timestamp_domain value) { #define CASE(X) case RS2_TIMESTAMP_DOMAIN_##X: return #X; diff --git a/src/types.h b/src/types.h index 75e38e71b7..851139f07b 100644 --- a/src/types.h +++ b/src/types.h @@ -210,7 +210,7 @@ namespace rsimpl2 class lazy { public: - lazy() : _init([]() { T t; return t; }) {} + lazy() : _init([]() { T t{}; return t; }) {} lazy(std::function initializer) : _init(std::move(initializer)) {} T& operator*() @@ -310,6 +310,7 @@ namespace rsimpl2 RS2_ENUM_HELPERS(rs2_distortion, DISTORTION) RS2_ENUM_HELPERS(rs2_option, OPTION) RS2_ENUM_HELPERS(rs2_camera_info, CAMERA_INFO) + RS2_ENUM_HELPERS(rs2_frame_metadata, FRAME_METADATA) RS2_ENUM_HELPERS(rs2_timestamp_domain, TIMESTAMP_DOMAIN) RS2_ENUM_HELPERS(rs2_visual_preset, VISUAL_PRESET) RS2_ENUM_HELPERS(rs2_exception_type, EXCEPTION_TYPE) @@ -322,6 +323,7 @@ namespace rsimpl2 //////////////////////////////////////////// #pragma pack(push, 1) struct int2 { int x, y; }; + struct float2 { float x, y; float & operator [] (int i) { return (&x)[i]; } }; struct float3 { float x, y, z; float & operator [] (int i) { return (&x)[i]; } }; struct float4 { float x, y, z, w; float & operator [] (int i) { return (&x)[i]; } }; struct float3x3 { float3 x, y, z; float & operator () (int i, int j) { return (&x)[j][i]; } }; // column-major diff --git a/src/win/win-helpers.cpp b/src/win/win-helpers.cpp index 13b319f2d5..3cba5e43e3 100644 --- a/src/win/win-helpers.cpp +++ b/src/win/win-helpers.cpp @@ -40,7 +40,7 @@ namespace rsimpl2 std::string hr_to_string(HRESULT hr) { _com_error err(hr); - std::wstring errorMessage = err.ErrorMessage(); + std::wstring errorMessage = (err.ErrorMessage()) ? err.ErrorMessage() : L""; std::stringstream ss; ss << "HResult 0x" << std::hex << hr << ": \"" << std::string(errorMessage.begin(), errorMessage.end()) << "\""; return ss.str(); diff --git a/src/win/win-hid.cpp b/src/win/win-hid.cpp index 7be12bf848..6f6a1bbf91 100644 --- a/src/win/win-hid.cpp +++ b/src/win/win-hid.cpp @@ -198,7 +198,6 @@ namespace rsimpl2 void wmf_hid_device::stop_capture() { - _sensor->SetEventSink(NULL); _cb = nullptr; @@ -272,7 +271,7 @@ namespace rsimpl2 DWORD cVals = 0; // Count of returned properties. if (SUCCEEDED(hr)) { - // Get the number of values returned. + // Get the number of values returned. hr = pValues->GetCount(&cVals); if (SUCCEEDED(hr)) { diff --git a/unit-tests/unit-tests-common.h b/unit-tests/unit-tests-common.h index 082fb33dd7..dce45e3b2f 100644 --- a/unit-tests/unit-tests-common.h +++ b/unit-tests/unit-tests-common.h @@ -17,6 +17,7 @@ #include #include #include +#include // noexcept is not accepted by Visual Studio 2013 yet, but noexcept(false) is require on throwing destructors on gcc and clang // It is normally advisable not to throw in a destructor, however, this usage is safe for require_error/require_no_error because @@ -252,11 +253,27 @@ struct test_duration { uint32_t frames_to_capture; }; +struct frame_metadata +{ + std::array,RS2_FRAME_METADATA_COUNT> md_attributes{}; +}; + + struct frame_additional_data { - double timestamp; - unsigned long long frame_number; - rs2_timestamp_domain timestamp_domain; + double timestamp; + unsigned long long frame_number; + rs2_timestamp_domain timestamp_domain; + rs2_stream stream; + rs2_format format; + frame_metadata frame_md; // Metadata attributes + + frame_additional_data(const double &ts, const unsigned long long frame_num, const rs2_timestamp_domain& ts_domain, const rs2_stream& strm, const rs2_format& fmt) : + timestamp(ts), + frame_number(frame_num), + timestamp_domain(ts_domain), + stream(strm), + format(fmt){} }; inline void check_fps(double actual_fps, double configured_fps) diff --git a/unit-tests/unit-tests-live.cpp b/unit-tests/unit-tests-live.cpp index df2b4b09ae..ee6b38de7b 100644 --- a/unit-tests/unit-tests-live.cpp +++ b/unit-tests/unit-tests-live.cpp @@ -5,6 +5,7 @@ // This set of tests is valid for any number and combination of RealSense cameras, including R200 and F200 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////// +#include #include "unit-tests-common.h" using namespace rs2; @@ -49,7 +50,6 @@ TEST_CASE("Device metadata enumerates correctly", "[live]") } } } - } @@ -824,7 +824,56 @@ TEST_CASE("All suggested profiles can be opened", "[live]") { } } -TEST_CASE("Metadata sanity check", "[live]") { +/* Apply heuristic test to check metadata attributes for sanity*/ +void metadata_verification(const std::vector& data) +{ + // Heuristics that we use to verify metadata + // Metadata sanity + // Frame numbers and timestamps increase monotonically + // Sensor timestamp should be less or equal to frame timestamp + // Exposure time and gain values are greater than zero + // Sensor framerate is bounded to >0 and < 200 fps for uvc streams + int64_t last_val[3] = { -1, -1, -1 }; + + for (size_t i=0; i < data.size(); i++) + { + + // Check that Frame/Sensor timetamps, frame number rise monotonically + for (int j=RS2_FRAME_METADATA_FRAME_COUNTER; j <=RS2_FRAME_METADATA_SENSOR_TIMESTAMP; j++) + { + if (data[i].frame_md.md_attributes[j].first) + { + int64_t value = data[i].frame_md.md_attributes[j].second; + CAPTURE(value); + CAPTURE(last_val[j]); + + REQUIRE_NOTHROW( ( value > last_val[0] ) ); + if (RS2_FRAME_METADATA_FRAME_COUNTER==j) // In addition, there shall be no frame number gaps + { + REQUIRE_NOTHROW( ( 1 == ( value - last_val[j] ) ) ); + } + + last_val[j] = data[i].frame_md.md_attributes[j].second; + } + } + + // Sensor timestamp should be less or equal to frame timestamp + if ( data[i].frame_md.md_attributes[RS2_FRAME_METADATA_SENSOR_TIMESTAMP].first && + data[i].frame_md.md_attributes[RS2_FRAME_METADATA_FRAME_TIMESTAMP].first) + { + REQUIRE(data[i].frame_md.md_attributes[RS2_FRAME_METADATA_FRAME_TIMESTAMP].second > + data[i].frame_md.md_attributes[RS2_FRAME_METADATA_SENSOR_TIMESTAMP].second); + } + + // Exposure time and gain values are greater than zero + if (data[i].frame_md.md_attributes[RS2_FRAME_METADATA_ACTUAL_EXPOSURE].first) + REQUIRE(data[i].frame_md.md_attributes[RS2_FRAME_METADATA_ACTUAL_EXPOSURE].second > 0); + if (data[i].frame_md.md_attributes[RS2_FRAME_METADATA_GAIN_LEVEL].first) + REQUIRE(data[i].frame_md.md_attributes[RS2_FRAME_METADATA_GAIN_LEVEL].second > 0); + } +} + +TEST_CASE("Per-frame metadata sanity check", "[live]") { //Require at least one device to be plugged in rs2::context ctx; if(make_context(space_to_underscore(Catch::getCurrentContext().getResultCapture()->getCurrentTestName()).c_str(), &ctx)) @@ -847,10 +896,11 @@ TEST_CASE("Metadata sanity check", "[live]") { WARN(subdevice.get_camera_info(RS2_CAMERA_INFO_MODULE_NAME)); //the test will be done only on sub set of profile for each sub device - for (int i = 0; i < modes.size(); i += std::ceil((float)modes.size() / (float)num_of_profiles_for_each_subdevice)) + for (int i = 0; i < modes.size(); i += static_cast(std::ceil((float)modes.size() / (float)num_of_profiles_for_each_subdevice))) { - if (modes[i].width == 1920) continue; // Full-HD is often times too heavy for the build machine to handle - // Disabling for now + if ((modes[i].width == 1920) || // Full-HD is often times too heavy for the build machine to handle + ((RS2_STREAM_GPIO1 <= modes[i].stream) && (RS2_STREAM_GPIO4 >= modes[i].stream))) // GPIO Requires external triggers to produce events + continue; // Disabling for now CAPTURE(modes[i].format); CAPTURE(modes[i].fps); @@ -870,19 +920,40 @@ TEST_CASE("Metadata sanity check", "[live]") { REQUIRE_NOTHROW(subdevice.start([&](frame f) { - if (frames >= frames_before_start_measure && frames_additional_data.size() < frames_for_fps_measure) + if ((frames >= frames_before_start_measure) && (frames_additional_data.size() < frames_for_fps_measure)) { if (first) { start = ctx.get_time(); } first = false; - auto frame_number = f.get_frame_number(); - auto ts = f.get_timestamp(); - auto ts_domain = f.get_frame_timestamp_domain(); + + frame_additional_data data { f.get_timestamp(), + f.get_frame_number(), + f.get_frame_timestamp_domain(), + f.get_stream_type(), + f.get_format()}; + + // Store frame metadata attributes, verify API behavior correctness + for (auto i=0; i< RS2_FRAME_METADATA_COUNT; i++) + { + bool supported = false; + REQUIRE_NOTHROW(supported = f.supports_frame_metadata((rs2_frame_metadata)i)); + if (supported) + { + rs2_metadata_t val{}; + REQUIRE_NOTHROW(val =f.get_frame_metadata((rs2_frame_metadata)i)); + data.frame_md.md_attributes[i] = std::make_pair(true,val); + } + else + { + REQUIRE_THROWS(f.get_frame_metadata((rs2_frame_metadata)i)); + data.frame_md.md_attributes[i].first = false; + } + } std::unique_lock lock(m); - frames_additional_data.push_back({ ts,frame_number, ts_domain }); + frames_additional_data.push_back(data); } frames++; if (frames_additional_data.size() >= frames_for_fps_measure) @@ -945,10 +1016,13 @@ TEST_CASE("Metadata sanity check", "[live]") { CAPTURE(actual_fps); CAPTURE(metadata_fps); - //it the diff in percentege between metadata fps and actual fps is bigger than max_diff_between_real_and_metadata_fps + //it the diff in percentage between metadata fps and actual fps is bigger than max_diff_between_real_and_metadata_fps //the test will fail REQUIRE(std::abs(metadata_fps / actual_fps - 1) < max_diff_between_real_and_metadata_fps); + // Verify per-frame metadata attributes + metadata_verification(frames_additional_data); + std::cout << modes[i].format << "MODE: " << modes[i].fps << " " << modes[i].height << " " << modes[i].width << " " << modes[i].stream << " succeed\n"; } } @@ -1052,7 +1126,7 @@ TEST_CASE("Error handling sanity", "[live]") { } } - } + } } } @@ -1087,9 +1161,9 @@ TEST_CASE("Auto exposure behavior", "[live]") { if(curr_pid != sr300_pid && subdevice.supports(RS2_OPTION_ENABLE_AUTO_EXPOSURE)) { - option_range range; + option_range range{}; - float val; + float val{}; auto info = subdevice.get_camera_info(RS2_CAMERA_INFO_MODULE_NAME); CAPTURE(info); @@ -1100,7 +1174,7 @@ TEST_CASE("Auto exposure behavior", "[live]") { { REQUIRE_NOTHROW(range = subdevice.get_option_range(RS2_OPTION_EXPOSURE)); - REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_EXPOSURE, range.max)); + REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_EXPOSURE,range.max)); CAPTURE(range.max); REQUIRE_NOTHROW(val = subdevice.get_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE)); REQUIRE(val == 0); @@ -1112,7 +1186,7 @@ TEST_CASE("Auto exposure behavior", "[live]") { { REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE,1)); REQUIRE_NOTHROW(range = subdevice.get_option_range(RS2_OPTION_WHITE_BALANCE)); - REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_WHITE_BALANCE, range.max)); + REQUIRE_NOTHROW(subdevice.set_option(RS2_OPTION_WHITE_BALANCE,range.max)); CAPTURE(range.max); REQUIRE_NOTHROW(val = subdevice.get_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE)); REQUIRE(val == 0);