diff --git a/.travis.yml b/.travis.yml index a260b0816c..65289c0bd4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -47,7 +47,7 @@ before_install: - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then sudo add-apt-repository --yes ppa:ubuntu-toolchain-r/test; sudo apt-get update; - sudo apt-get install -qq build-essential xorg-dev libglu1-mesa-dev libglew-dev libglm-dev; + sudo apt-get install -qq build-essential xorg-dev libgl1-mesa-dev libglu1-mesa-dev libglew-dev libglm-dev; sudo apt-get install -qq libusb-1.0-0-dev; sudo apt-get install -qq libgtk-3-dev; sudo apt-get install -qq python python-dev; @@ -93,6 +93,7 @@ script: # Exit immediately if a command exits with a non-zero status - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then set -e; + pyenv install 3.6.1; pyenv local `pyenv versions|sed 's/ //g'|grep "^3"|tail -1`; mkdir build && cd build; cmake .. -DBUILD_EXAMPLES:BOOL=true -DBUILD_PYTHON_BINDINGS:BOOL=true -DBUILD_NODEJS_BINDINGS:BOOL=true; diff --git a/common/model-views.cpp b/common/model-views.cpp index 155e969106..d35b9dcf9d 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -196,83 +196,95 @@ namespace rs2 texture_data[idx], texture_data[idx + 1], texture_data[idx + 2]); } - void export_to_ply(const std::string& fname, notifications_model& ns, points points, video_frame texture) + void export_to_ply(const std::string& fname, notifications_model& ns, frameset frames, video_frame texture) { - - std::thread([&ns, points, texture, fname]() { + std::thread([&ns, frames, texture, fname]() mutable { std::string texfname(fname); texfname += ".png"; - const auto vertices = points.get_vertices(); - const auto texcoords = points.get_texture_coordinates(); - const auto tex = reinterpret_cast(texture.get_data()); - std::vector new_vertices; - //std::vector new_texcoords; - std::vector> new_tex; - new_vertices.reserve(points.size()); - //new_texcoords.reserve(points.size()); - new_tex.reserve(points.size()); - - for (int i = 0; i < points.size(); ++i) - if (std::abs(vertices[i].x) >= 1e-6 || std::abs(vertices[i].y) >= 1e-6 || std::abs(vertices[i].z) >= 1e-6) - { - new_vertices.push_back(vertices[i]); - if (texture) - { - //new_texcoords.push_back(texcoords[i]); - auto color = get_texcolor(texture, texcoords[i]); - new_tex.push_back(color); - } + points p; - } - - std::ofstream out(fname); - out << "ply\n"; - out << "format binary_little_endian 1.0\n" /*"format ascii 1.0\n"*/; - out << "comment pointcloud saved from Realsense Viewer\n"; - //if (texture) out << "comment TextureFile " << get_file_name(texfname) << "\n"; - out << "element vertex " << new_vertices.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 (texture) + for (auto&& f : frames) { - //out << "property float" << sizeof(float) * 8 << " u\n"; - //out << "property float" << sizeof(float) * 8 << " v\n"; - out << "property uchar red\n"; - out << "property uchar green\n"; - out << "property uchar blue\n"; + if (p = f.as()) + { + break; + } } - out << "end_header\n"; - out.close(); - out.open(fname, std::ios_base::app | std::ios_base::binary); - for (int i = 0; i < new_vertices.size(); ++i) + if (p) { - // we assume little endian architecture on your device - out.write(reinterpret_cast(&(new_vertices[i].x)), sizeof(float)); - out.write(reinterpret_cast(&(new_vertices[i].y)), sizeof(float)); - out.write(reinterpret_cast(&(new_vertices[i].z)), sizeof(float)); - // out << new_vertices[i].x << ' ' << new_vertices[i].y << ' ' << new_vertices[i].z; + const auto vertices = p.get_vertices(); + const auto texcoords = p.get_texture_coordinates(); + const auto tex = reinterpret_cast(texture.get_data()); + std::vector new_vertices; + //std::vector new_texcoords; + std::vector> new_tex; + new_vertices.reserve(p.size()); + //new_texcoords.reserve(points.size()); + new_tex.reserve(p.size()); + assert(p.size()); + for (size_t i = 0; i < p.size(); ++i) + if (std::abs(vertices[i].x) >= 1e-6 || std::abs(vertices[i].y) >= 1e-6 || std::abs(vertices[i].z) >= 1e-6) + { + new_vertices.push_back(vertices[i]); + if (texture) + { + //new_texcoords.push_back(texcoords[i]); + auto color = get_texcolor(texture, texcoords[i]); + new_tex.push_back(color); + } + + } + + std::ofstream out(fname); + out << "ply\n"; + out << "format binary_little_endian 1.0\n" /*"format ascii 1.0\n"*/; + out << "comment pointcloud saved from Realsense Viewer\n"; + //if (texture) out << "comment TextureFile " << get_file_name(texfname) << "\n"; + out << "element vertex " << new_vertices.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 (texture) { - //out.write(reinterpret_cast(&(new_texcoords[i].u)), sizeof(float)); - //out.write(reinterpret_cast(&(new_texcoords[i].v)), sizeof(float)); - out.write(reinterpret_cast(&(std::get<0>(new_tex[i]))), sizeof(uint8_t)); - out.write(reinterpret_cast(&(std::get<1>(new_tex[i]))), sizeof(uint8_t)); - out.write(reinterpret_cast(&(std::get<2>(new_tex[i]))), sizeof(uint8_t)); - // out << std::hex << ' ' << std::get<0>(new_tex[i]) << ' ' << std::get<1>(new_tex[i]) << ' ' << std::get<2>(new_tex[i]); + //out << "property float" << sizeof(float) * 8 << " u\n"; + //out << "property float" << sizeof(float) * 8 << " v\n"; + out << "property uchar red\n"; + out << "property uchar green\n"; + out << "property uchar blue\n"; } - // out << '\n'; - } + out << "end_header\n"; + out.close(); - /* save texture to texfname */ - //if (texture) stbi_write_png(texfname.data(), texture.get_width(), texture.get_height(), texture.get_bytes_per_pixel(), texture.get_data(), texture.get_width() * texture.get_bytes_per_pixel()); + out.open(fname, std::ios_base::app | std::ios_base::binary); + for (int i = 0; i < new_vertices.size(); ++i) + { + // we assume little endian architecture on your device + out.write(reinterpret_cast(&(new_vertices[i].x)), sizeof(float)); + out.write(reinterpret_cast(&(new_vertices[i].y)), sizeof(float)); + out.write(reinterpret_cast(&(new_vertices[i].z)), sizeof(float)); + // out << new_vertices[i].x << ' ' << new_vertices[i].y << ' ' << new_vertices[i].z; + if (texture) + { + //out.write(reinterpret_cast(&(new_texcoords[i].u)), sizeof(float)); + //out.write(reinterpret_cast(&(new_texcoords[i].v)), sizeof(float)); + out.write(reinterpret_cast(&(std::get<0>(new_tex[i]))), sizeof(uint8_t)); + out.write(reinterpret_cast(&(std::get<1>(new_tex[i]))), sizeof(uint8_t)); + out.write(reinterpret_cast(&(std::get<2>(new_tex[i]))), sizeof(uint8_t)); + // out << std::hex << ' ' << std::get<0>(new_tex[i]) << ' ' << std::get<1>(new_tex[i]) << ' ' << std::get<2>(new_tex[i]); + } + // out << '\n'; + } - ns.add_notification({ to_string() << "Finished saving 3D view " << (texture ? "to " : "without texture to ") << fname, - std::chrono::duration_cast>(std::chrono::high_resolution_clock::now().time_since_epoch()).count(), - RS2_LOG_SEVERITY_INFO, - RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR }); + /* save texture to texfname */ + //if (texture) stbi_write_png(texfname.data(), texture.get_width(), texture.get_height(), texture.get_bytes_per_pixel(), texture.get_data(), texture.get_width() * texture.get_bytes_per_pixel()); + + ns.add_notification({ to_string() << "Finished saving 3D view " << (texture ? "to " : "without texture to ") << fname, + std::chrono::duration_cast>(std::chrono::high_resolution_clock::now().time_since_epoch()).count(), + RS2_LOG_SEVERITY_INFO, + RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR }); + } }).detach(); } @@ -285,7 +297,7 @@ namespace rs2 size_t pixel_width, size_t pixels_height, size_t bytes_per_pixel, const void* raster_data, size_t stride_bytes) { - return stbi_write_png(filename, pixel_width, pixels_height, bytes_per_pixel, raster_data, stride_bytes); + return stbi_write_png(filename, (int)pixel_width, (int)pixels_height, bytes_per_pixel, raster_data, stride_bytes); } std::vector get_string_pointers(const std::vector& vec) @@ -615,9 +627,9 @@ namespace rs2 owner->dev, *owner->s, &owner->options_invalidated, owner, block, error_message); } - subdevice_model::subdevice_model(device& dev, + subdevice_model::subdevice_model(device& dev, std::shared_ptr s, std::string& error_message) - : s(s), dev(dev), ui(), last_valid_ui(), + : s(s), dev(dev), ui(), last_valid_ui(), streaming(false), _pause(false), depth_colorizer(std::make_shared()) { try @@ -1113,15 +1125,22 @@ namespace rs2 _pause = false; } - void subdevice_model::play(const std::vector& profiles) + void subdevice_model::play(const std::vector& profiles, viewer_model& viewer) { s->open(profiles); try { s->start([&](frame f) { auto index = f.get_profile().unique_id(); - queues.at(index).enqueue(std::move(f)); + if (viewer.synchronization_enable) + { + if (index == viewer.selected_depth_source_uid || index == viewer.selected_tex_source_uid) + viewer.s(f); + } + queues.at(index).enqueue(f); }); + } + catch (...) { s->close(); @@ -1493,7 +1512,6 @@ namespace rs2 // } //} - auto model = pc.get_points(); const auto top_bar_height = 32.f; const auto num_of_buttons = 4; @@ -1596,8 +1614,9 @@ namespace rs2 tex_sources.push_back(s.second.profile.unique_id()); auto dev_name = s.second.dev ? s.second.dev->dev.get_info(RS2_CAMERA_INFO_NAME) : "Unknown"; - auto stream_name = rs2_stream_to_string(s.second.profile.stream_type()); - + std::string stream_name = rs2_stream_to_string(s.second.profile.stream_type()); + if (s.second.profile.stream_index()) + stream_name += "_" + std::to_string(s.second.profile.stream_index()); tex_sources_str.push_back(to_string() << dev_name << " " << stream_name); i++; @@ -1610,9 +1629,8 @@ namespace rs2 ImGui::SetCursorPosY(7); ImGui::Text("Texture Source:"); ImGui::SameLine(); - ImGui::SetCursorPosY(7); - ImGui::PushItemWidth(190); + ImGui::PushItemWidth(200); draw_combo_box("##Tex Source", tex_sources_str, selected_tex_source); i = 0; @@ -1657,17 +1675,6 @@ namespace rs2 //} //ImGui::PopItemWidth(); - if (selected_depth_source_uid >= 0) { - auto depth = streams[selected_depth_source_uid].texture->get_last_frame(); - if (depth) pc.push_frame(depth); - } - - frame tex; - if (selected_tex_source_uid >= 0) - { - tex = streams[selected_tex_source_uid].texture->get_last_frame(); - if (tex) pc.update_texture(tex); - } ImGui::SetCursorPos({ stream_rect.w - 32 * num_of_buttons - 5, 0 }); @@ -1713,6 +1720,15 @@ namespace rs2 { if (auto ret = file_dialog_open(save_file, "Polygon File Format (PLY)\0*.ply\0", NULL, NULL)) { + auto model = pc.get_points(); + + frame tex; + if (selected_tex_source_uid >= 0) + { + tex = streams[selected_tex_source_uid].texture->get_last_frame(); + if (tex) pc.update_texture(tex); + } + std::string fname(ret); if (!ends_with(to_lower(fname), ".ply")) fname += ".ply"; export_to_ply(fname.c_str(), not_model, model, tex); @@ -1732,28 +1748,32 @@ namespace rs2 ImGui::SameLine(); - if (syncronize) + if(support_non_syncronized_mode) { - ImGui::PushStyleColor(ImGuiCol_Text, light_blue); - ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, light_blue); - if (ImGui::Button(u8"\uf09c", { 24, top_bar_height })) + if (synchronization_enable) { - syncronize = false; + ImGui::PushStyleColor(ImGuiCol_Text, light_blue); + ImGui::PushStyleColor(ImGuiCol_TextSelectedBg, light_blue); + if (ImGui::Button(u8"\uf09c", { 24, top_bar_height })) + { + synchronization_enable = false; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Disable syncronization between the pointcloud and the texture"); + ImGui::PopStyleColor(2); } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Disable syncronization between the pointcloud and the texture"); - ImGui::PopStyleColor(2); - } - else - { - if (ImGui::Button(u8"\uf023", { 24, top_bar_height })) + else { - syncronize = true; + if (ImGui::Button(u8"\uf023", { 24, top_bar_height })) + { + synchronization_enable = true; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Keep the pointcloud and the texture sycronized"); } - if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Keep the pointcloud and the texture sycronized"); } + ImGui::End(); ImGui::PopStyleColor(6); ImGui::PopStyleVar(); @@ -1801,12 +1821,19 @@ namespace rs2 void viewer_model::gc_streams() { + std::lock_guard lock(streams_mutex); std::vector streams_to_remove; for (auto&& kvp : streams) { if (!kvp.second.is_stream_visible() && (!kvp.second.dev || (!kvp.second.dev->is_paused() && !kvp.second.dev->streaming))) + { + if(kvp.first == selected_depth_source_uid) + { + pc.depth_stream_active = false; + } streams_to_remove.push_back(kvp.first); + } } for (auto&& i : streams_to_remove) { streams.erase(i); @@ -2086,7 +2113,7 @@ namespace rs2 ImGui::Text("%s", label.c_str()); if (ImGui::IsItemHovered()) { - ImGui::SetTooltip("Hardware Timestamp unavailable! This is often an indication of inproperly applied Kernel patch.\nPlease refer to installation.md for mode information"); + ImGui::SetTooltip("Hardware Timestamp unavailable! This is often an indication of improperly applied Kernel patch.\nPlease refer to installation.md for mode information"); } ImGui::PopStyleColor(); } @@ -2363,7 +2390,7 @@ namespace rs2 if (profiles.empty()) continue; - sub->play(profiles); + sub->play(profiles, viewer); for (auto&& profile : profiles) { @@ -2598,28 +2625,92 @@ namespace rs2 { show_icon(font_18, "recording_icon", u8"\uf111", x, y, id, from_rgba(255, 46, 54, alpha_delta * 255)); } - void async_pointclound_mapper::render_loop() + + void async_pointclound_mapper::proccess(rs2::frame f, const rs2::frame_source& source) { - while (keep_calculating_pointcloud) + + points p; + std::vector results; + + + if(viewer.synchronization_enable) { - try + if (auto composite = f.as()) { - frame f; - if (depth_frames_to_render.poll_for_frame(&f)) + for(auto&& f: composite) { - if (f.get_frame_number() == last_frame_number && - f.get_timestamp() <= last_timestamp && - f.get_profile().unique_id() == last_stream_id) - continue; + if(f.get_profile().unique_id() == viewer.selected_depth_source_uid) + { + p = pc.calculate(f); + results.push_back(std::move(p)); + + } + if(f.get_profile().unique_id() == viewer.selected_tex_source_uid) + { + update_texture(f); + results.push_back(std::move(f)); + } + } + } + + + auto res = source.allocate_composite_frame(results); + source.frame_ready(std::move(res)); + + + } + else + { + if(f.get_profile().unique_id() == viewer.selected_depth_source_uid) + { - resulting_3d_models.enqueue(pc.calculate(f)); + if(last_tex_frame) + results.push_back(last_tex_frame); + p = pc.calculate(f); + results.push_back(std::move(p)); - last_frame_number = f.get_frame_number(); - last_timestamp = f.get_timestamp(); - last_stream_id = f.get_profile().unique_id(); + auto res = source.allocate_composite_frame(results); + source.frame_ready(std::move(res)); + } + + } + } + + void async_pointclound_mapper::render_loop() + { + while (keep_calculating_pointcloud) + { + if(depth_stream_active) + { + try + { + if(viewer.synchronization_enable) + { + frameset frames; + if (viewer.syncer_queue.poll_for_frame(&frames)) + { + processing_block.invoke(frames); + } + } + else + { + std::lock_guard lock(viewer.streams_mutex); + for (auto&& s : viewer.streams) + { + if(s.first == viewer.selected_tex_source_uid) + { + update_texture(s.second.texture->get_last_frame()); + last_tex_frame = s.second.texture->get_last_frame(); + } + if(s.first == viewer.selected_depth_source_uid) + { + processing_block.invoke(s.second.texture->get_last_frame()); + } + } + } } + catch (...) {} } - catch (...) {} // There is no practical reason to re-calculate the 3D model // at higher frequency then 100 FPS @@ -2925,40 +3016,26 @@ namespace rs2 glColor4f(1.f, 1.f, 1.f, 1.f); - if (syncronize) - { - auto tex = streams[selected_tex_source_uid].texture->get_last_frame(); - if (tex) s(tex); - } - if (auto points = pc.get_points()) + if(!paused) { - if (syncronize) - { - s(points); - rs2::frameset fs; - if (s.poll_for_frames(&fs)) - if (fs && fs.size() > 1) - { - for (auto&& f : fs) - { - if (f.is()) last_points = f; - else last_texture = f; - } - } - } - else + auto res = pc.get_points(); + + for (auto&& f : res) { - last_texture = streams[selected_tex_source_uid].texture->get_last_frame(); - last_points = points; + if (f.is()) + last_points = f; + else + last_texture = f; } - if (draw_frustrum) - { + } + if (draw_frustrum && last_points) + { glLineWidth(1.f); glBegin(GL_LINES); - auto intrin = points.get_profile().as().get_intrinsics(); + auto intrin = last_points.get_profile().as().get_intrinsics(); glColor4f(sensor_bg.x, sensor_bg.y, sensor_bg.z, 0.5f); @@ -2990,11 +3067,11 @@ namespace rs2 glColor4f(1.f, 1.f, 1.f, 1.f); } - if (last_points && last_texture) + if ( last_points && last_texture) { texture.upload(last_texture); - glPointSize((float)viewer_rect.w / points.get_profile().as().width()); + glPointSize((float)viewer_rect.w / last_points.get_profile().as().width()); if (selected_tex_source_uid >= 0) { @@ -3008,23 +3085,25 @@ namespace rs2 //glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, tex_border_color); - glBegin(GL_POINTS); - auto vertices = last_points.get_vertices(); - auto tex_coords = last_points.get_texture_coordinates(); + glBegin(GL_POINTS); - for (int i = 0; i < last_points.size(); i++) - { - if (vertices[i].z) + auto vertices = last_points.get_vertices(); + auto tex_coords = last_points.get_texture_coordinates(); + + for (int i = 0; i < last_points.size(); i++) { - glVertex3fv(vertices[i]); - glTexCoord2fv(tex_coords[i]); + if (vertices[i].z) + { + glVertex3fv(vertices[i]); + glTexCoord2fv(tex_coords[i]); + } + } + glEnd(); - } - glEnd(); } - } + glDisable(GL_DEPTH_TEST); @@ -3142,6 +3221,9 @@ namespace rs2 void viewer_model::upload_frame(frame&& f) { + if(f.get_profile().stream_type() == RS2_STREAM_DEPTH) + pc.depth_stream_active = true; + auto index = f.get_profile().unique_id(); streams[index].upload_frame(std::move(f)); } diff --git a/common/model-views.h b/common/model-views.h index f6b480bf27..54f851c0ff 100644 --- a/common/model-views.h +++ b/common/model-views.h @@ -143,10 +143,6 @@ namespace rs2 frame_queue& at(int id) { std::lock_guard lock(_lookup_mutex); - if (_queues.find(id) == _queues.end()) - { - _queues[id] = frame_queue(4); - } return _queues[id]; } @@ -172,7 +168,8 @@ namespace rs2 std::map selected_format_id; }; - class subdevice_model; + class viewer_model; + class subdevice_model; class processing_block_model { @@ -209,7 +206,7 @@ namespace rs2 bool is_selected_combination_supported(); std::vector get_selected_profiles(); void stop(); - void play(const std::vector& profiles); + void play(const std::vector& profiles, viewer_model& viewer); void update(std::string& error_message, notifications_model& model); void draw_options(const std::vector& drawing_order, bool update_read_only_options, std::string& error_message, @@ -478,14 +475,22 @@ namespace rs2 std::string get_file_name(const std::string& path); + class viewer_model; class async_pointclound_mapper { public: - async_pointclound_mapper() - : keep_calculating_pointcloud(true), - resulting_3d_models(1), depth_frames_to_render(1), - t([this]() {render_loop(); }) + async_pointclound_mapper(viewer_model& viewer) + : processing_block([&](rs2::frame f, const rs2::frame_source& source) + { + this->proccess(std::move(f),source); + }), + viewer(viewer), + keep_calculating_pointcloud(true), + depth_stream_active(false), + resulting_3d_models(1), + t([this]() {render_loop(); }) { + processing_block.start(resulting_3d_models); } ~async_pointclound_mapper() { stop(); } @@ -501,17 +506,13 @@ namespace rs2 } } - void push_frame(frame f) - { - depth_frames_to_render.enqueue(f); - } - - points get_points() + rs2::frameset get_points() { frame f; if (resulting_3d_models.poll_for_frame(&f)) { - model = f; + rs2::frameset frameset(f); + model = frameset; } return model; } @@ -521,16 +522,22 @@ namespace rs2 rs2::frame f{}; model = f; while (resulting_3d_models.poll_for_frame(&f)); - while (depth_frames_to_render.poll_for_frame(&f)); } + std::atomic depth_stream_active; + private: - void render_loop(); + viewer_model& viewer; + void render_loop(); + void proccess(rs2::frame f, const rs2::frame_source& source); + rs2::frame last_tex_frame; + rs2::processing_block processing_block; pointcloud pc; - points model; + rs2::frameset model; std::atomic keep_calculating_pointcloud; - frame_queue depth_frames_to_render; + + frame_queue resulting_3d_models; std::thread t; @@ -552,12 +559,19 @@ namespace rs2 float get_output_height() const { return (is_output_collapsed ? default_log_h : 20); } viewer_model() + :pc(*this), + synchronization_enable(true) { reset_camera(); rs2_error* e = nullptr; not_model.add_log(to_string() << "librealsense version: " << api_version_to_string(rs2_get_api_version(&e)) << "\n"); } + ~viewer_model() + { + pc.stop(); + streams.clear(); + } void upload_frame(frame&& f); std::map calc_layout(const rect& r); @@ -586,6 +600,7 @@ namespace rs2 void gc_streams(); + std::mutex streams_mutex; std::map streams; bool fullscreen = false; stream_model* selected_stream = nullptr; @@ -597,6 +612,7 @@ namespace rs2 bool is_3d_view = false; bool paused = false; + void draw_viewport(const rect& viewer_rect, ux_window& window, int devices, std::string& error_message); bool allow_3d_source_change = true; @@ -606,14 +622,19 @@ namespace rs2 bool draw_plane = false; bool draw_frustrum = true; - bool syncronize = false; + bool support_non_syncronized_mode = true; + std::atomic synchronization_enable; int selected_depth_source_uid = -1; int selected_tex_source_uid = -1; float dim_level = 1.f; + rs2::syncer s; + rs2::frame_queue syncer_queue; private: + + friend class async_pointclound_mapper; std::map get_interpolated_layout(const std::map& l); void show_icon(ImFont* font_18, const char* label_str, const char* text, int x, int y, int id, const ImVec4& color, const std::string& tooltip = ""); @@ -633,10 +654,10 @@ namespace rs2 rs2::points last_points; rs2::frame last_texture; texture_buffer texture; - rs2::syncer s; + }; - void export_to_ply(const std::string& file_name, notifications_model& ns, points points, video_frame texture); + void export_to_ply(const std::string& file_name, notifications_model& ns, frameset points, video_frame texture); // Wrapper for cross-platform dialog control enum file_dialog_mode { diff --git a/common/rendering.h b/common/rendering.h index 23b1729419..442efbdc84 100644 --- a/common/rendering.h +++ b/common/rendering.h @@ -755,7 +755,7 @@ namespace rs2 return last; } - texture_buffer() : last_queue(1), texture(), + texture_buffer() : last_queue(1), texture(), colorize(std::make_shared()) {} GLuint get_gl_handle() const { return texture; } @@ -777,6 +777,7 @@ namespace rs2 void upload(rs2::frame frame) { + last_queue.enqueue(frame); // If the frame timestamp has changed since the last time show(...) was called, re-upload the texture if (!texture) glGenTextures(1, &texture); @@ -864,7 +865,7 @@ namespace rs2 //} //break; default: - throw std::runtime_error("The requested format is not suported for rendering"); + throw std::runtime_error("The requested format is not supported for rendering"); } glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); @@ -873,7 +874,6 @@ namespace rs2 glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); glBindTexture(GL_TEXTURE_2D, 0); - last_queue.enqueue(frame); } static void draw_axis(float axis_size = 1.f, float axisWidth = 4.f) diff --git a/common/ux-window.cpp b/common/ux-window.cpp index ebc3c4aed4..5dd4b0ec12 100644 --- a/common/ux-window.cpp +++ b/common/ux-window.cpp @@ -14,7 +14,7 @@ namespace rs2 _win(nullptr), _width(0), _height(0), _output_height(0), _font_14(nullptr), _font_18(nullptr), _app_ready(false), _first_frame(true), _query_devices(true), _missing_device(false), - _hourglass_index(0), _dev_stat_message{} + _hourglass_index(0), _dev_stat_message{}, _keep_alive(true) { if (!glfwInit()) exit(1); @@ -22,12 +22,19 @@ namespace rs2 rs2_error* e = nullptr; _title_str = to_string() << title << " v" << api_version_to_string(rs2_get_api_version(&e)); + _width = 1024; + _height = 768; + + // Dynamically adjust new window size (by detecting monitor resolution) auto primary = glfwGetPrimaryMonitor(); - const auto mode = glfwGetVideoMode(primary); + if (primary) + { + const auto mode = glfwGetVideoMode(primary); + _width = int(mode->width * 0.7f); + _height = int(mode->height * 0.7f); + } // Create GUI Windows - _width = int(mode->width * 0.7f); - _height = int(mode->height * 0.7f); _win = glfwCreateWindow(_width, _height, _title_str.c_str(), nullptr, nullptr); glfwMakeContextCurrent(_win); ImGui_ImplGlfw_Init(_win, true); @@ -71,7 +78,7 @@ namespace rs2 // Prepare the splash screen and do some initialization in the background int x, y, comp; - auto r = stbi_load_from_memory(splash, splash_size, &x, &y, &comp, false); + auto r = stbi_load_from_memory(splash, (int)splash_size, &x, &y, &comp, false); _splash_tex.upload_image(x, y, r); // Apply initial UI state @@ -97,8 +104,10 @@ namespace rs2 if (_first_frame) { - std::thread first_load([&]() { - do + assert(!_first_load.joinable()); // You must call to reset() before initiate new thread + + _first_load = std::thread([&]() { + while (_keep_alive && !_app_ready) { try { @@ -108,10 +117,9 @@ namespace rs2 { std::this_thread::sleep_for(std::chrono::seconds(1)); // Wait for connect event and retry } - } while (!_app_ready); + } }); _first_frame = false; - first_load.detach(); } // If we are just getting started, render the Splash Screen instead of normal UI @@ -123,7 +131,7 @@ namespace rs2 begin_frame(); glPushMatrix(); - glViewport(0.f, 0.f, _fb_width, _fb_height); + glViewport(0, 0, _fb_width, _fb_height); glClearColor(0.036f, 0.044f, 0.051f, 1.f); glClear(GL_COLOR_BUFFER_BIT); @@ -131,8 +139,8 @@ namespace rs2 glOrtho(0, _width, _height, 0, -1, +1); // Fade-in the logo - auto opacity = smoothstep(_splash_timer.elapsed_ms(), 100.f, 2000.f); - _splash_tex.show({ 0.f,0.f,(float)_width,(float)_height }, opacity); + auto opacity = smoothstep(float(_splash_timer.elapsed_ms()), 100.f, 2000.f); + _splash_tex.show({ 0.f,0.f,float(_width),float(_height) }, opacity); std::string hourglass = u8"\uf250"; static periodic_timer every_200ms(std::chrono::milliseconds(200)); @@ -163,7 +171,7 @@ namespace rs2 ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, { 5, 5 }); ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 1); ImGui::PushStyleColor(ImGuiCol_WindowBg, transparent); - ImGui::SetNextWindowPos({ (float)_width/2 - 150, (float)_height/2 + 70 }); + ImGui::SetNextWindowPos({ (float)_width / 2 - 150, (float)_height / 2 + 70 }); ImGui::PushFont(_font_18); ImGui::SetNextWindowSize({ (float)_width, (float)_height }); ImGui::Begin("Splash Screen Banner", nullptr, flags); @@ -211,6 +219,12 @@ namespace rs2 ux_window::~ux_window() { + if (_first_load.joinable()) + { + _keep_alive = false; + _first_load.join(); + } + ImGui::GetIO().Fonts->ClearFonts(); // To be refactored into Viewer theme object ImGui_ImplGlfw_Shutdown(); glfwDestroyWindow(_win); @@ -262,6 +276,13 @@ namespace rs2 void ux_window::reset() { + if (_first_load.joinable()) + { + _keep_alive = false; + _first_load.join(); + _keep_alive = true; + } + _query_devices = true; _missing_device = false; _hourglass_index = 0; @@ -269,5 +290,10 @@ namespace rs2 _app_ready = false; _splash_timer.reset(); _dev_stat_message = u8"\uf287 Please connect Intel RealSense device!"; + + { + std::lock_guard lock(_on_load_message_mtx); + _on_load_message.clear(); + } } } diff --git a/common/ux-window.h b/common/ux-window.h index cbdbbc10e4..b86544af0e 100644 --- a/common/ux-window.h +++ b/common/ux-window.h @@ -6,6 +6,7 @@ #include "imgui.h" #include #include +#include #include "rendering.h" namespace rs2 @@ -71,8 +72,10 @@ namespace rs2 std::string _error_message; float _scale_factor; + std::thread _first_load; bool _first_frame; std::atomic _app_ready; + std::atomic _keep_alive; texture_buffer _splash_tex; timer _splash_timer; std::string _title_str; diff --git a/config/99-realsense-libusb.rules b/config/99-realsense-libusb.rules index 1f5411c9f3..4e78d7df1d 100644 --- a/config/99-realsense-libusb.rules +++ b/config/99-realsense-libusb.rules @@ -11,6 +11,7 @@ SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad2", MODE:="066 SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad3", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad4", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad5", MODE:="0666", GROUP:="plugdev" +SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad6", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af6", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0afe", MODE:="0666", GROUP:="plugdev" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0aff", MODE:="0666", GROUP:="plugdev" diff --git a/doc/RaspberryPi3.md b/doc/RaspberryPi3.md new file mode 100644 index 0000000000..37c491cc15 --- /dev/null +++ b/doc/RaspberryPi3.md @@ -0,0 +1,28 @@ +# Intel® RealSense™ D400 cameras with Raspberry Pi + +This document describes how to use the Intel RealSense D400 cameras on Raspberry PI 3 Model B (which offers only USB2 interface). + +> **Note:** Both Raspberry Pi platform and USB2 support in general are experimental features and are **not** officially supported by Intel RealSense group at this point. Camera capabilities are severely reduced when connected to USB2 port due to lack of bandwidth. + +

+ +## Installation Instructions +1. Install [Ubuntu MATE](https://ubuntu-mate.org/) on your [Raspberry PI 3](https://www.raspberrypi.org/products/raspberry-pi-3-model-b/). + +2. Clone and compile the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/latest) by following the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/development/doc/installation.md). +In section [Video4Linux backend preparation](https://github.com/IntelRealSense/librealsense/blob/development/doc/installation.md#video4linux-backend-preparation), perform only the first phase "Install udev rules located in librealsense source directory". + +> **Note**: In some cases the RAM capacity is not sufficient to compile the SDK, so if the compilation process crashes or exits with an error-code try to create a [swap file](https://www.howtoforge.com/ubuntu-swap-file) and then recompile the SDK. + +3. [Activate](https://ubuntu-mate.community/t/tutorial-activate-opengl-driver-for-ubuntu-mate-16-04/7094) the OpenGL driver. +4. Plug the D400 camera and play with RealSense SDK [examples](https://github.com/IntelRealSense/librealsense/tree/master/examples)/[tools](https://github.com/IntelRealSense/librealsense/tree/master/tools). + +## Expected Output +1. Streaming Depth data with 480x270 resolution and 30 FPS using the [Intel RealSense Viewer](https://github.com/IntelRealSense/librealsense/tree/master/tools/realsense-viewer). +

+ +2. Streaming Depth and IR with 424x240 resolution and 30 FPS: +

+ +3. Point-cloud of depth and IR: +

diff --git a/doc/readme.md b/doc/readme.md index 1ef83de43a..d9c4944f29 100644 --- a/doc/readme.md +++ b/doc/readme.md @@ -24,4 +24,5 @@ * [D400 at software.intel.com](https://software.intel.com/en-us/realsense/d400) - Camera specifications * [D400 and External Devices](rs400/external_devices.md) - Notes on integrating RS400 with external devices * [D400 Advanced Mode](rs400/rs400_advanced_mode.md) - Overview of the Advanced Mode APIs + * [D400 cameras with Raspberry Pi](./RaspberryPi3.md) - Example of low-end system without USB3 interface * [Record and Playback](../src/media/readme.md) - SDK Record and Playback functionality using ROS-bag file format diff --git a/examples/C/depth/rs-depth.c b/examples/C/depth/rs-depth.c index 48a7b5b960..3f8d24d6ad 100644 --- a/examples/C/depth/rs-depth.c +++ b/examples/C/depth/rs-depth.c @@ -56,7 +56,7 @@ float get_depth_unit_value(const rs2_device* const dev) if (1 == is_depth_sensor_found) { - depth_scale = rs2_get_option(sensor, RS2_OPTION_DEPTH_UNITS, &e); + depth_scale = rs2_get_option((const rs2_options*)sensor, RS2_OPTION_DEPTH_UNITS, &e); check_error(e); rs2_delete_sensor(sensor); break; diff --git a/examples/align/rs-align.cpp b/examples/align/rs-align.cpp index 5dc79c98d6..cf5b9548fe 100644 --- a/examples/align/rs-align.cpp +++ b/examples/align/rs-align.cpp @@ -36,11 +36,11 @@ int main(int argc, char * argv[]) try // Each depth camera might have different units for depth pixels, so we get it here // Using the pipeline's profile, we can retrieve the device that the pipeline uses float depth_scale = get_depth_scale(profile.get_device()); - + //Pipeline could choose a device that does not have a color stream //If there is no color stream, choose to align depth to another stream rs2_stream align_to = find_stream_to_align(profile.get_streams()); - + // Create a rs2::align object. // rs2::align allows us to perform alignment of depth frames to others frames //The "align_to" is the stream type to which we plan to align depth frames. diff --git a/examples/example.hpp b/examples/example.hpp index e33647fb11..68afb3d637 100644 --- a/examples/example.hpp +++ b/examples/example.hpp @@ -89,7 +89,7 @@ class texture glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, frame.get_data()); break; default: - throw std::runtime_error("The requested format is not suported by this demo!"); + throw std::runtime_error("The requested format is not supported by this demo!"); } glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); diff --git a/examples/pointcloud/rs-pointcloud.cpp b/examples/pointcloud/rs-pointcloud.cpp index 1651f0b4c7..e4305e53ba 100644 --- a/examples/pointcloud/rs-pointcloud.cpp +++ b/examples/pointcloud/rs-pointcloud.cpp @@ -42,18 +42,18 @@ int main(int argc, char * argv[]) try auto frames = pipe.wait_for_frames(); auto depth = frames.get_depth_frame(); - + // Generate the pointcloud and texture mappings points = pc.calculate(depth); - + auto color = frames.get_color_frame(); - + // Tell pointcloud object to map to this color frame pc.map_to(color); // Upload the color frame to OpenGL app_state.tex.upload(color); - + // Draw the pointcloud draw_pointcloud(app, app_state, points); } diff --git a/examples/sensor-control/api_how_to.h b/examples/sensor-control/api_how_to.h index 9d6109f85d..35d31840eb 100644 --- a/examples/sensor-control/api_how_to.h +++ b/examples/sensor-control/api_how_to.h @@ -32,7 +32,7 @@ class how_to if (devices.size() == 0) { std::cerr << "No device connected, please connect a RealSense device" << std::endl; - + //To help with the boilerplate code of waiting for a device to connect //The SDK provides the rs2::device_hub class rs2::device_hub device_hub(ctx); @@ -61,7 +61,7 @@ class how_to throw std::out_of_range("Selected device index is out of range"); } - // Update the selected device + // Update the selected device selected_device = devices[selected_device_index]; } @@ -90,7 +90,7 @@ class how_to std::cout << "N/A" << std::endl; } } - + static std::string get_device_name(const rs2::device& dev) { // Each device provides some information on itself, such as name: @@ -105,7 +105,7 @@ class how_to return name + " " + sn; } - + static std::string get_sensor_name(const rs2::sensor& sensor) { // Sensors support additional information, such as a human readable name @@ -114,7 +114,7 @@ class how_to else return "Unknown Sensor"; } - + static rs2::sensor get_a_sensor_from_a_device(const rs2::device& dev) { // A rs2::device is a container of rs2::sensors that have some correlation between them. @@ -143,10 +143,10 @@ class how_to return sensors[selected_sensor_index]; } - + static rs2_option get_sensor_option(const rs2::sensor& sensor) { - // Sensors usually have several options to control their properties + // Sensors usually have several options to control their properties // such as Exposure, Brightness etc. std::cout << "Sensor supports the following options:\n" << std::endl; @@ -189,7 +189,7 @@ class how_to } return static_cast(selected_sensor_option); } - + static float get_depth_units(const rs2::sensor& sensor) { //A Depth stream contains an image that is composed of pixels with depth information. @@ -205,11 +205,11 @@ class how_to else throw std::runtime_error("Given sensor is not a depth sensor"); } - + static void get_field_of_view(const rs2::stream_profile& stream) { // A sensor's stream (rs2::stream_profile) is in general a stream of data with no specific type. - // For video streams (streams of images), the sensor that produces the data has a lens and thus has properties such + // For video streams (streams of images), the sensor that produces the data has a lens and thus has properties such // as a focal point, distortion, and principal point. // To get these intrinsics parameters, we need to take a stream and first check if it is a video stream if (auto video_stream = stream.as()) @@ -239,7 +239,7 @@ class how_to std::cerr << "Given stream profile is not a video stream profile" << std::endl; } } - + static void get_extrinsics(const rs2::stream_profile& from_stream, const rs2::stream_profile& to_stream) { // If the device/sensor that you are using contains more than a single stream, and it was calibrated @@ -258,10 +258,10 @@ class how_to std::cerr << "Failed to get extrinsics for the given streams. " << e.what() << std::endl; } } - + static void change_sensor_option(const rs2::sensor& sensor, rs2_option option_type) { - // Sensors usually have several options to control their properties + // Sensors usually have several options to control their properties // such as Exposure, Brightness etc. // To control an option, use the following api: @@ -305,13 +305,13 @@ class how_to } catch (const rs2::error& e) { - // Some options can only be set while the camera is streaming, + // Some options can only be set while the camera is streaming, // and generally the hardware might fail so it is good practice to catch exceptions from set_option std::cerr << "Failed to set option " << option_type << ". (" << e.what() << ")" << std::endl; } } } - + static rs2::stream_profile choose_a_streaming_profile(const rs2::sensor& sensor) { // A Sensor is an object that is capable of streaming one or more types of data. @@ -404,7 +404,7 @@ class how_to return stream_profiles[selected_profile_index]; } - + static void start_streaming_a_profile(const rs2::sensor& sensor, const rs2::stream_profile& stream_profile) { // The sensor controls turning the streaming on and off @@ -420,7 +420,7 @@ class how_to std::ostringstream oss; oss << "Displaying profile " << stream_profile.stream_name(); - + // In order to begin getting data from the sensor, we need to register a callback to handle frames (data) // To register a callback, the sensor's start() method should be invoked. // The start() method takes any type of callable object that takes a frame as its parameter @@ -432,7 +432,7 @@ class how_to // This behavior requires the provided frame handler to the // start method to be re-entrant - // In this example we have created a class to handle the frames, + // In this example we have created a class to handle the frames, // and we capture it by reference inside a C++11 lambda which is passed to the start() function helper::frame_viewer display(oss.str()); sensor.start([&](rs2::frame f) { display(f); }); diff --git a/examples/sensor-control/rs-sensor-control.cpp b/examples/sensor-control/rs-sensor-control.cpp index d0e8953349..f9604e26ad 100644 --- a/examples/sensor-control/rs-sensor-control.cpp +++ b/examples/sensor-control/rs-sensor-control.cpp @@ -23,7 +23,7 @@ int main(int argc, char * argv[]) try // and finally run some operations on that sensor //We will use the "how_to" class to make the code clear, expressive and encapsulate common actions inside a function - + std::vector sensor_actions = create_sensor_actions(); bool choose_a_device = true; @@ -127,7 +127,7 @@ void show_extrinsics_between_streams(rs2::device device, rs2::sensor sensor) std::vector create_sensor_actions() { //This function creates several functions ("sensor_action") that takes a device and a sensor, - // and perform some specific action + // and perform some specific action return std::vector { std::make_pair(&control_sensor_options, "Control sensor's options"), std::make_pair(&display_live_stream, "Control sensor's streams"), diff --git a/include/librealsense2/h/rs_frame.h b/include/librealsense2/h/rs_frame.h index 01c8eb8456..5088191097 100644 --- a/include/librealsense2/h/rs_frame.h +++ b/include/librealsense2/h/rs_frame.h @@ -228,7 +228,7 @@ rs2_frame* rs2_allocate_composite_frame(rs2_source* source, rs2_frame** frames, * \param[in] index Index of the frame to extract within the composite frame * \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored * \return returns reference to a frame existing within the composite frame -* If you wish to keep this frame after the composite is released, you need to call aquire_ref +* If you wish to keep this frame after the composite is released, you need to call acquire_ref * Otherwise the resulting frame lifetime is bound by owning composite frame */ rs2_frame* rs2_extract_frame(rs2_frame* composite, int index, rs2_error** error); diff --git a/include/librealsense2/hpp/rs_sensor.hpp b/include/librealsense2/hpp/rs_sensor.hpp index 111c1967d3..71d5191621 100644 --- a/include/librealsense2/hpp/rs_sensor.hpp +++ b/include/librealsense2/hpp/rs_sensor.hpp @@ -107,13 +107,8 @@ namespace rs2 class options { public: - options(rs2_options* options = nullptr) - { - _options = options; - } - /** - * check if particular option is supported by a subdevice + * check if particular option is supported * \param[in] option option id to be checked * \return true if option is supported */ @@ -153,7 +148,7 @@ namespace rs2 } /** - * read option value from the device + * read option's value * \param[in] option option id to be queried * \return value of the option */ @@ -180,7 +175,7 @@ namespace rs2 } /** - * write new value to device option + * write new value to the option * \param[in] option option id to be queried * \param[in] value new value for the option */ @@ -211,6 +206,8 @@ namespace rs2 } protected: + explicit options(rs2_options* o = nullptr) : _options(o) {} + template options& operator=(const T& dev) { @@ -230,8 +227,8 @@ namespace rs2 using options::supports; /** - * open subdevice for exclusive access, by committing to a configuration - * \param[in] profile configuration committed by the device + * open sensor for exclusive access, by committing to a configuration + * \param[in] profile configuration committed by the sensor */ void open(const stream_profile& profile) const { @@ -245,7 +242,7 @@ namespace rs2 /** * check if specific camera info is supported * \param[in] info the parameter to check for support - * \return true if the parameter both exist and well-defined for the specific device + * \return true if the parameter both exist and well-defined for the specific sensor */ bool supports(rs2_camera_info info) const { @@ -258,7 +255,7 @@ namespace rs2 /** * retrieve camera specific information, like versions of various internal components * \param[in] info camera info type to retrieve - * \return the requested camera info string, in a format specific to the device model + * \return the requested camera info string, in a format specific to the sensor model */ const char* get_info(rs2_camera_info info) const { @@ -269,9 +266,9 @@ namespace rs2 } /** - * open subdevice for exclusive access, by committing to composite configuration, specifying one or more stream profiles + * open sensor for exclusive access, by committing to composite configuration, specifying one or more stream profiles * this method should be used for interdependent streams, such as depth and infrared, that have to be configured together - * \param[in] profiles vector of configurations to be commited by the device + * \param[in] profiles vector of configurations to be commited by the sensor */ void open(const std::vector& profiles) const { @@ -292,8 +289,8 @@ namespace rs2 } /** - * close subdevice for exclusive access - * this method should be used for releasing device resource + * close sensor for exclusive access + * this method should be used for releasing sensor resource */ void close() const { @@ -339,8 +336,8 @@ namespace rs2 /** - * check if physical subdevice is supported - * \return list of stream profiles that given subdevice can provide, should be released by rs2_delete_profiles_list + * check if physical sensor is supported + * \return list of stream profiles that given sensor can provide, should be released by rs2_delete_profiles_list */ std::vector get_stream_profiles() const { @@ -377,19 +374,19 @@ namespace rs2 return intrin; } - sensor& operator=(const std::shared_ptr dev) - { - options::operator=(dev); + sensor& operator=(const std::shared_ptr other) + { + options::operator=(other); _sensor.reset(); - _sensor = dev; + _sensor = other; return *this; } - sensor& operator=(const sensor& dev) + sensor& operator=(const sensor& other) { *this = nullptr; - options::operator=(dev._sensor); - _sensor = dev._sensor; + options::operator=(other._sensor); + _sensor = other._sensor; return *this; } sensor() : _sensor(nullptr) {} diff --git a/include/librealsense2/rs.h b/include/librealsense2/rs.h index f1c4d1d8ab..1447e0f2e6 100644 --- a/include/librealsense2/rs.h +++ b/include/librealsense2/rs.h @@ -24,7 +24,7 @@ extern "C" { #define RS2_API_MAJOR_VERSION 2 #define RS2_API_MINOR_VERSION 8 -#define RS2_API_PATCH_VERSION 1 +#define RS2_API_PATCH_VERSION 2 #define RS2_API_BUILD_VERSION 0 #define STRINGIFY(arg) #arg diff --git a/readme.md b/readme.md index 588665a38a..99488ec726 100644 --- a/readme.md +++ b/readme.md @@ -37,7 +37,7 @@ Information about the Intel® RealSense™ technology at [realsense.intel.com](h These simple examples demonstrate how to easily use the SDK to include code snippets that access the camera into your applications. **[Wrappers](https://github.com/IntelRealSense/librealsense/tree/development/wrappers)** - We provide a C, C++, [Python](./wrappers/python), [Node.js](./wrappers/nodejs) API and [ROS](https://github.com/intel-ros/realsense/releases). More to come , including LabView, PCL, Unity and Matlab. + We provide a C, C++, [Python](./wrappers/python), [Node.js](./wrappers/nodejs) API, [ROS](https://github.com/intel-ros/realsense/releases) and [LabVIEW](./wrappers/labview). More to come, including C# and Matlab. ## Quick Start diff --git a/src/backend.h b/src/backend.h index 0d8a0ecbb9..69a4000b10 100644 --- a/src/backend.h +++ b/src/backend.h @@ -22,12 +22,12 @@ #include -const uint16_t MAX_RETRIES = 100; -const uint16_t VID_INTEL_CAMERA = 0x8086; -const uint8_t DEFAULT_FRAME_BUFFERS = 4; -const uint16_t DELAY_FOR_RETRIES = 50; +const uint16_t MAX_RETRIES = 100; +const uint16_t VID_INTEL_CAMERA = 0x8086; +const uint8_t DEFAULT_V4L2_FRAME_BUFFERS = 4; +const uint16_t DELAY_FOR_RETRIES = 50; -const uint8_t MAX_META_DATA_SIZE = 0xff; // UVC Metadata total length +const uint8_t MAX_META_DATA_SIZE = 0xff; // UVC Metadata total length // is limited by (UVC Bulk) design to 255 bytes namespace librealsense @@ -362,7 +362,7 @@ namespace librealsense class uvc_device { public: - virtual void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers = DEFAULT_FRAME_BUFFERS) = 0; + virtual void probe_and_commit(stream_profile profile, frame_callback callback, int buffers = DEFAULT_V4L2_FRAME_BUFFERS) = 0; virtual void stream_on(std::function error_handler = [](const notification& n){}) = 0; virtual void start_callbacks() = 0; virtual void stop_callbacks() = 0; @@ -400,9 +400,9 @@ namespace librealsense explicit retry_controls_work_around(std::shared_ptr dev) : _dev(dev) {} - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override { - _dev->probe_and_commit(profile, zero_copy, callback, buffers); + _dev->probe_and_commit(profile, callback, buffers); } void stream_on(std::function error_handler = [](const notification& n){}) override @@ -661,11 +661,11 @@ namespace librealsense : _dev(dev) {} - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override { auto dev_index = get_dev_index_by_profiles(profile); _configured_indexes.insert(dev_index); - _dev[dev_index]->probe_and_commit(profile, zero_copy, callback, buffers); + _dev[dev_index]->probe_and_commit(profile, callback, buffers); } diff --git a/src/device.cpp b/src/device.cpp index 18c0df9b3f..8640dd03be 100644 --- a/src/device.cpp +++ b/src/device.cpp @@ -101,6 +101,7 @@ void device::hardware_reset() std::shared_ptr librealsense::device::create_matcher(const frame_holder& frame) const { + return std::make_shared( frame.frame->get_stream()->get_unique_id(), frame.frame->get_stream()->get_stream_type()); } diff --git a/src/ds5/advanced_mode/json_loader.hpp b/src/ds5/advanced_mode/json_loader.hpp index 7f07f8d904..fd8062abe2 100644 --- a/src/ds5/advanced_mode/json_loader.hpp +++ b/src/ds5/advanced_mode/json_loader.hpp @@ -373,7 +373,6 @@ namespace librealsense { "aux-param-depthclampmin", make_field(p.depth_table, &STDepthTableControl::depthClampMin) }, { "aux-param-depthclampmax", make_field(p.depth_table, &STDepthTableControl::depthClampMax) }, { "param-disparitymode", make_field(p.depth_table, &STDepthTableControl::disparityMode) }, - { "aux-param-disparitymultiplier", make_field(p.depth_table, &STDepthTableControl::disparityMode) }, { "param-disparityshift", make_field(p.depth_table, &STDepthTableControl::disparityShift) }, { "aux-param-disparityshift", make_field(p.depth_table, &STDepthTableControl::disparityShift) }, @@ -392,6 +391,12 @@ namespace librealsense { "param-regionspatialthresholdv", make_ignored_field() }, { "result:", make_ignored_field() }, { "result", make_ignored_field() }, + { "aux-param-disparitymultiplier", make_ignored_field() }, + { "stream-depth-format", make_ignored_field() }, + { "stream-ir-format", make_ignored_field() }, + { "stream-width", make_ignored_field() }, + { "stream-height", make_ignored_field() }, + { "stream-fps", make_ignored_field() }, }; static const std::map auto_control_values{ { "False", 0.f }, { "True", 1.f } }; diff --git a/src/ds5/ds5-active.cpp b/src/ds5/ds5-active.cpp index 76a99f01ec..eae0dda717 100644 --- a/src/ds5/ds5-active.cpp +++ b/src/ds5/ds5-active.cpp @@ -25,22 +25,26 @@ namespace librealsense { using namespace ds; - auto& depth_ep = get_depth_sensor(); - auto emitter_enabled = std::make_shared(depth_ep); - depth_ep.register_option(RS2_OPTION_EMITTER_ENABLED, emitter_enabled); + auto pid = group.uvc_devices.front().pid; + if (pid != RS_USB2_PID) + { + auto& depth_ep = get_depth_sensor(); + auto emitter_enabled = std::make_shared(depth_ep); + depth_ep.register_option(RS2_OPTION_EMITTER_ENABLED, emitter_enabled); - auto laser_power = std::make_shared>(depth_ep, - depth_xu, - DS5_LASER_POWER, - "Manual laser power in mw. applicable only when laser power mode is set to Manual"); - depth_ep.register_option(RS2_OPTION_LASER_POWER, - std::make_shared( - laser_power, - emitter_enabled, - std::vector{0.f, 2.f}, 1.f)); + auto laser_power = std::make_shared>(depth_ep, + depth_xu, + DS5_LASER_POWER, + "Manual laser power in mw. applicable only when laser power mode is set to Manual"); + depth_ep.register_option(RS2_OPTION_LASER_POWER, + std::make_shared( + laser_power, + emitter_enabled, + std::vector{0.f, 2.f}, 1.f)); - depth_ep.register_option(RS2_OPTION_PROJECTOR_TEMPERATURE, - std::make_shared(depth_ep, - RS2_OPTION_PROJECTOR_TEMPERATURE)); + depth_ep.register_option(RS2_OPTION_PROJECTOR_TEMPERATURE, + std::make_shared(depth_ep, + RS2_OPTION_PROJECTOR_TEMPERATURE)); + } } } diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp index 3fb756d229..9d5520f11b 100644 --- a/src/ds5/ds5-device.cpp +++ b/src/ds5/ds5-device.cpp @@ -192,6 +192,7 @@ namespace librealsense std::unique_ptr ds5_timestamp_reader_backup(new ds5_timestamp_reader(backend.create_time_service())); auto depth_ep = std::make_shared(this, std::make_shared(depth_devices), std::unique_ptr(new ds5_timestamp_reader_from_metadata(std::move(ds5_timestamp_reader_backup)))); + depth_ep->register_xu(depth_xu); // make sure the XU is initialized everytime we power the camera @@ -229,12 +230,13 @@ namespace librealsense get_depth_sensor())); } - // Define Left-Right extrinsics calculation (lazy) + // Define Left-to-Right extrinsics calculation (lazy) + // Reference CS - Right-handed; positive [X,Y,Z] point to [Left,Up,Forward] accordingly. _left_right_extrinsics = std::make_shared>([this]() { rs2_extrinsics ext = identity_matrix(); auto table = check_calib(*_coefficients_table_raw); - ext.translation[0] = -0.001f * table->baseline; + ext.translation[0] = 0.001f * table->baseline; // mm to meters return ext; }); @@ -413,8 +415,9 @@ namespace librealsense std::vector> depth_matchers; for (auto& s : streams) + { depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type())); - + } return std::make_shared(depth_matchers); } } diff --git a/src/ds5/ds5-factory.cpp b/src/ds5/ds5-factory.cpp index 5b0b2e798a..574e84cf4f 100644 --- a/src/ds5/ds5-factory.cpp +++ b/src/ds5/ds5-factory.cpp @@ -207,6 +207,8 @@ namespace librealsense return std::make_shared(ctx, group, register_device_notifications); case RS435_RGB_PID: return std::make_shared(ctx, group, register_device_notifications); + case RS_USB2_PID: + return std::make_shared(ctx, group, register_device_notifications); default: throw std::runtime_error("Unsupported RS400 model!"); } diff --git a/src/ds5/ds5-private.cpp b/src/ds5/ds5-private.cpp index d4958c3e08..47a1b624ad 100644 --- a/src/ds5/ds5-private.cpp +++ b/src/ds5/ds5-private.cpp @@ -166,16 +166,17 @@ namespace librealsense result = *it; switch (info.pid) { + case RS_USB2_PID: case RS400_PID: case RS410_PID: case RS460_PID: case RS430_PID: case RS420_PID: - found = result.mi == 3; + found = (result.mi == 3); break; case RS430_MM_PID: case RS420_MM_PID: - found = result.mi == 6; + found = (result.mi == 6); break; case RS415_PID: case RS435_RGB_PID: diff --git a/src/ds5/ds5-private.h b/src/ds5/ds5-private.h index 333a70da63..66bc46d769 100644 --- a/src/ds5/ds5-private.h +++ b/src/ds5/ds5-private.h @@ -27,7 +27,8 @@ namespace librealsense const uint16_t RS430_MM_PID = 0x0ad5; // AWGT const uint16_t RS430_MM_RGB_PID = 0x0b01; // AWGCT const uint16_t RS435_RGB_PID = 0x0b07; // AWGC - const uint16_t RS460_PID = 0x0b03; // DS5U + const uint16_t RS460_PID = 0x0b03; // DS5U + const uint16_t RS_USB2_PID = 0x0ad6; // USB2 // DS5 depth XU identifiers const uint8_t DS5_HWMONITOR = 1; @@ -52,7 +53,8 @@ namespace librealsense ds::RS430_MM_PID, ds::RS430_MM_RGB_PID, ds::RS435_RGB_PID, - ds::RS460_PID + ds::RS460_PID, + ds::RS_USB2_PID }; static const std::map rs400_sku_names = { @@ -68,6 +70,7 @@ namespace librealsense { RS430_MM_PID, "Intel RealSense 430 with Tracking Module and RGB Module"}, { RS435_RGB_PID,"Intel RealSense 435"}, { RS460_PID, "Intel RealSense 460" }, + { RS_USB2_PID, "Intel RealSense USB2" } }; // DS5 fisheye XU identifiers diff --git a/src/ds5/ds5-rolling-shutter.cpp b/src/ds5/ds5-rolling-shutter.cpp index 20fdbde008..8ffe673ac2 100644 --- a/src/ds5/ds5-rolling-shutter.cpp +++ b/src/ds5/ds5-rolling-shutter.cpp @@ -25,7 +25,8 @@ namespace librealsense { using namespace ds; - if (_fw_version >= firmware_version("5.5.8.0")) + auto pid = group.uvc_devices.front().pid; + if ((_fw_version >= firmware_version("5.5.8.0")) && (pid != RS_USB2_PID)) { get_depth_sensor().register_option(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, std::make_shared>(get_depth_sensor(), diff --git a/src/ivcam/sr300.cpp b/src/ivcam/sr300.cpp index 2d36ddb6a6..57fddf463a 100644 --- a/src/ivcam/sr300.cpp +++ b/src/ivcam/sr300.cpp @@ -313,8 +313,9 @@ namespace librealsense std::vector streams = { _depth_stream.get(), _ir_stream.get()}; for (auto& s : streams) + { depth_matchers.push_back(std::make_shared( s->get_unique_id(), s->get_stream_type())); - + } std::vector> matchers; matchers.push_back( std::make_shared(depth_matchers)); diff --git a/src/libuvc/libuvc.cpp b/src/libuvc/libuvc.cpp index 0f240fdf96..20698c4e16 100644 --- a/src/libuvc/libuvc.cpp +++ b/src/libuvc/libuvc.cpp @@ -294,7 +294,7 @@ namespace librealsense } /* request to set up a streaming profile and its calback */ - void probe_and_commit(stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override { uvc_error_t res; uvc_stream_ctrl_t ctrl; diff --git a/src/linux/backend-v4l2.cpp b/src/linux/backend-v4l2.cpp index 387f5c6416..2974c4ecdd 100644 --- a/src/linux/backend-v4l2.cpp +++ b/src/linux/backend-v4l2.cpp @@ -132,11 +132,11 @@ namespace librealsense } } - void named_mutex::aquire() + void named_mutex::acquire() { auto ret = lockf(_fildes, F_LOCK, 0); if (ret != 0) - throw linux_backend_exception(to_string() << "Aquire failed"); + throw linux_backend_exception(to_string() << "Acquire failed"); } void named_mutex::release() @@ -517,11 +517,8 @@ namespace librealsense if (_thread) _thread->join(); } - void v4l_uvc_device::probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) + void v4l_uvc_device::probe_and_commit(stream_profile profile, frame_callback callback, int buffers) { - if(!zero_copy) - buffers = 1; - if(!_is_capturing && !_callback) { v4l2_fmtdesc pixel_format = {}; @@ -538,15 +535,15 @@ namespace librealsense // Microsoft Depth GUIDs for R400 series are not yet recognized // by the Linux kernel, but they do not require a patch, since there // are "backup" Z16 and Y8 formats in place - std::set known_problematic_formats = { + static const std::set pending_formats = { "00000050-0000-0010-8000-00aa003", "00000032-0000-0010-8000-00aa003", }; - if (std::find(known_problematic_formats.begin(), - known_problematic_formats.end(), + if (std::find(pending_formats.begin(), + pending_formats.end(), (const char*)pixel_format.description) == - known_problematic_formats.end()) + pending_formats.end()) { const std::string s(to_string() << "!" << pixel_format.description); std::regex rgx("!([0-9a-f]+)-.*"); diff --git a/src/linux/backend-v4l2.h b/src/linux/backend-v4l2.h index db6e03d431..1c5381f5ee 100644 --- a/src/linux/backend-v4l2.h +++ b/src/linux/backend-v4l2.h @@ -54,7 +54,7 @@ namespace librealsense named_mutex(const named_mutex&) = delete; - void lock() { aquire(); } + void lock() { acquire(); } void unlock() { release(); } bool try_lock(); @@ -62,7 +62,7 @@ namespace librealsense ~named_mutex(); private: - void aquire(); + void acquire(); void release(); @@ -141,7 +141,7 @@ namespace librealsense ~v4l_uvc_device(); - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override; + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override; void stream_on(std::function error_handler) override; diff --git a/src/log.cpp b/src/log.cpp index 41c3580004..df9c8b3e5e 100644 --- a/src/log.cpp +++ b/src/log.cpp @@ -99,12 +99,13 @@ namespace librealsense if (content) { std::string content_str(content); - std::transform(content_str.begin(), content_str.end(), content_str.begin(), ::toupper); + std::transform(content_str.begin(), content_str.end(), content_str.begin(), ::tolower); for (uint32_t i = 0; i < RS2_LOG_SEVERITY_COUNT; i++) { auto current = (rs2_log_severity)i; std::string name = librealsense::get_string(current); + std::transform(name.begin(), name.end(), name.begin(), ::tolower); if (content_str == name) { severity = current; diff --git a/src/mock/recorder.cpp b/src/mock/recorder.cpp index 22ca60f117..40ef7fc41d 100644 --- a/src/mock/recorder.cpp +++ b/src/mock/recorder.cpp @@ -628,11 +628,11 @@ namespace librealsense } - void record_uvc_device::probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) + void record_uvc_device::probe_and_commit(stream_profile profile, frame_callback callback, int buffers) { - _owner->try_record([this, zero_copy, callback, profile](recording* rec, lookup_key k) + _owner->try_record([this, callback, profile](recording* rec, lookup_key k) { - _source->probe_and_commit(profile, zero_copy, [this, callback](stream_profile p, frame_object f, std::function continuation) + _source->probe_and_commit(profile, [this, callback](stream_profile p, frame_object f, std::function continuation) { _owner->try_record([this, callback, p, &f, continuation](recording* rec1, lookup_key key1) { @@ -1179,7 +1179,7 @@ namespace librealsense }); } - void playback_uvc_device::probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) + void playback_uvc_device::probe_and_commit(stream_profile profile, frame_callback callback, int buffers) { auto stored = _rec->load_stream_profiles(_entity_id, call_type::uvc_probe_commit); vector input{ profile }; diff --git a/src/mock/recorder.h b/src/mock/recorder.h index bcf8086fd8..0250741bbb 100644 --- a/src/mock/recorder.h +++ b/src/mock/recorder.h @@ -324,7 +324,7 @@ namespace librealsense class record_uvc_device : public uvc_device { public: - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override; + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override; void stream_on(std::function error_handler = [](const notification& n) {}) override; void start_callbacks() override; void stop_callbacks() override; @@ -458,7 +458,7 @@ namespace librealsense { auto&& c = _rec->add_call(k); c.had_error = true; - c.inline_string = "Unknown exception has occured!"; + c.inline_string = "Unknown exception has occurred!"; throw; } @@ -502,7 +502,7 @@ namespace librealsense class playback_uvc_device : public uvc_device { public: - void probe_and_commit( stream_profile profile, bool zero_copy, frame_callback callback, int buffers) override; + void probe_and_commit(stream_profile profile, frame_callback callback, int buffers) override; void stream_on(std::function error_handler = [](const notification& n) {}) override; void start_callbacks() override; void stop_callbacks() override; diff --git a/src/option.h b/src/option.h index 6053d1a3be..6bec308bf8 100644 --- a/src/option.h +++ b/src/option.h @@ -71,23 +71,23 @@ namespace librealsense _on_set = [](float x) {}; } - void set(float value) override + void set(float value) override { - if (_max < value || _min > value) + if (_max < value || _min > value) throw invalid_value_exception("Given value is outside valid range!"); - *_value = value; + *_value = value; _on_set(value); } - float query() const override + float query() const override { return *_value; } - option_range get_range() const override { - return{ - (float)_min, (float)_max, - (float)_step, (float)_def }; + option_range get_range() const override { + return{ + (float)_min, (float)_max, + (float)_step, (float)_def }; } bool is_enabled() const override { return true; } @@ -199,7 +199,7 @@ namespace librealsense { return dev.get_xu_range(_xu, _id, sizeof(T)); }); - + if (uvc_range.min.size() < sizeof(int32_t)) return option_range{0,0,1,0}; auto min = *(reinterpret_cast(uvc_range.min.data())); diff --git a/src/pipeline.cpp b/src/pipeline.cpp index 8bf00305b6..039e686ef9 100644 --- a/src/pipeline.cpp +++ b/src/pipeline.cpp @@ -538,7 +538,6 @@ namespace librealsense } catch(...) { - } // Stop will throw if device was disconnected. TODO - refactoring anticipated } _active_profile.reset(); diff --git a/src/sensor.cpp b/src/sensor.cpp index 2e1f791502..0cb036f2b2 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -318,7 +318,7 @@ namespace librealsense { try { - _device->probe_and_commit(mode.profile, !mode.requires_processing(), + _device->probe_and_commit(mode.profile, [this, mode, timestamp_reader, requests](platform::stream_profile p, platform::frame_object f, std::function continuation) mutable { auto system_time = environment::get_instance().get_time_service()->get_time(); @@ -419,8 +419,7 @@ namespace librealsense if (pref->get_stream().get()) _source.invoke_callback(std::move(pref)); } - }, - static_cast(_source.get_published_size_option()->query())); + }); } catch(...) { diff --git a/src/source.cpp b/src/source.cpp index 0ebb44b67b..1db33402e5 100644 --- a/src/source.cpp +++ b/src/source.cpp @@ -38,7 +38,7 @@ namespace librealsense std::shared_ptr