Merge pull request IntelRealSense#8084 from intel-isl/open3d/sample

Open3D wrapper examples and tutorial

CMake/lrs_options.cmake

@@ -23,6 +23,7 @@ option(BUILD_CV_EXAMPLES "Build OpenCV examples" OFF)
 option(BUILD_DLIB_EXAMPLES "Build DLIB examples - requires DLIB_DIR" OFF)
 option(BUILD_OPENVINO_EXAMPLES "Build Intel OpenVINO Toolkit examples - requires INTEL_OPENVINO_DIR" OFF)
 option(BUILD_PCL_EXAMPLES "Build PCL examples" OFF)
+option(BUILD_OPEN3D_EXAMPLES "Build Open3D examples" OFF)
 option(BUILD_NODEJS_BINDINGS "Build Node.js bindings" OFF)
 option(BUILD_OPENNI2_BINDINGS "Build OpenNI bindings" OFF)
 option(IMPORT_DEPTH_CAM_FW "Download the latest firmware for the depth cameras" ON)

wrappers/CMakeLists.txt

@@ -25,6 +25,10 @@ if (BUILD_OPENVINO_EXAMPLES)
   add_subdirectory(openvino)
 endif()
 
+if (BUILD_OPEN3D_EXAMPLES)
+  add_subdirectory(open3d)
+endif()
+
 if(BUILD_MATLAB_BINDINGS)
     add_subdirectory(matlab)
 endif()

wrappers/open3d/CMakeLists.txt

@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.12.0)
+set (CMAKE_CXX_STANDARD 14)
+
+if(POLICY CMP0091)
+    # https://stackoverflow.com/a/56490614
+    cmake_policy(SET CMP0091 NEW)
+endif()
+
+# The options need to be the same as Open3D's default
+# If Open3D is configured and built with custom options, you'll also need to
+# specify the same custom options.
+option(STATIC_WINDOWS_RUNTIME "Use static (MT/MTd) Windows runtime" ON)
+# This needs cmake_policy(SET CMP0091 NEW)
+if (STATIC_WINDOWS_RUNTIME)
+    set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$<CONFIG:Debug>:Debug>")
+else()
+    set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$<CONFIG:Debug>:Debug>DLL")
+endif()
+
+project(Open3D_RS_examples LANGUAGES CXX)
+
+# Find installed Open3D, which exports Open3D::Open3D
+if(WIN32)
+    find_package(Open3D HINTS ${CMAKE_INSTALL_PREFIX}/CMake)
+else()
+    find_package(Open3D HINTS ${CMAKE_INSTALL_PREFIX}/lib/cmake)
+endif()
+if(NOT Open3D_FOUND OR Open3D_VERSION VERSION_LESS 0.12)
+    message(FATAL_ERROR "Open3D v0.12+ not found, please use -DCMAKE_INSTALL_PREFIX=open3d_install_dir")
+endif()
+
+foreach(EXAMPLE RealSenseBagReader RealSenseRecorder)
+    add_executable(${EXAMPLE} cpp/${EXAMPLE}.cpp)
+    target_link_libraries(${EXAMPLE}  Open3D::Open3D)
+
+    # On Windows, when BUILD_SHARED_LIBS, copy .dll to the executable directory
+    if(WIN32)
+        get_target_property(open3d_type Open3D::Open3D TYPE)
+        if(open3d_type STREQUAL "SHARED_LIBRARY")
+            message(STATUS "Will copy Open3D.dll to ${CMAKE_CURRENT_BINARY_DIR}/$<CONFIG>")
+            add_custom_command(TARGET ${EXAMPLE} POST_BUILD
+                COMMAND ${CMAKE_COMMAND} -E copy
+                ${CMAKE_INSTALL_PREFIX}/bin/Open3D.dll
+                ${CMAKE_CURRENT_BINARY_DIR}/$<CONFIG>)
+        endif()
+    endif()
+endforeach()

wrappers/open3d/cpp/RealSenseBagReader.cpp

@@ -0,0 +1,198 @@
+// ----------------------------------------------------------------------------
+// -                        Open3D: www.open3d.org                            -
+// ----------------------------------------------------------------------------
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 www.open3d.org
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+// ----------------------------------------------------------------------------
+
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <thread>
+
+#include "open3d/Open3D.h"
+
+using namespace open3d;
+namespace sc = std::chrono;
+
+void PrintUsage() {
+  PrintOpen3DVersion();
+  utility::LogInfo("Usage:");
+  // clang-format off
+    utility::LogInfo("RealSenseBagReader [-V] --input input.bag [--output path]");
+  // clang-format on
+}
+
+int main(int argc, char **argv) {
+  if (!utility::ProgramOptionExists(argc, argv, "--input")) {
+    PrintUsage();
+    return 1;
+  }
+  if (utility::ProgramOptionExists(argc, argv, "-V")) {
+    utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);
+  } else {
+    utility::SetVerbosityLevel(utility::VerbosityLevel::Info);
+  }
+  std::string bag_filename =
+      utility::GetProgramOptionAsString(argc, argv, "--input");
+
+  bool write_image = false;
+  std::string output_path;
+  if (!utility::ProgramOptionExists(argc, argv, "--output")) {
+    utility::LogInfo("No output image path, only play bag.");
+  } else {
+    output_path = utility::GetProgramOptionAsString(argc, argv, "--output");
+    if (output_path.empty()) {
+      utility::LogError("Output path {} is empty, only play bag.", output_path);
+      return 1;
+    }
+    if (utility::filesystem::DirectoryExists(output_path)) {
+      utility::LogWarning("Output path {} already existing, only play bag.",
+                          output_path);
+      return 1;
+    } else if (!utility::filesystem::MakeDirectory(output_path)) {
+      utility::LogWarning("Unable to create path {}, only play bag.",
+                          output_path);
+      return 1;
+    } else {
+      utility::LogInfo("Decompress images to {}", output_path);
+      utility::filesystem::MakeDirectoryHierarchy(output_path + "/color");
+      utility::filesystem::MakeDirectoryHierarchy(output_path + "/depth");
+      write_image = true;
+    }
+  }
+
+  t::io::RSBagReader bag_reader;
+  bag_reader.Open(bag_filename);
+  if (!bag_reader.IsOpened()) {
+    utility::LogError("Unable to open {}", bag_filename);
+    return 1;
+  }
+
+  bool flag_exit = false;
+  bool flag_play = true;
+  visualization::VisualizerWithKeyCallback vis;
+  visualization::SetGlobalColorMap(
+      visualization::ColorMap::ColorMapOption::Gray);
+  vis.RegisterKeyCallback(GLFW_KEY_ESCAPE, [&](visualization::Visualizer *vis) {
+    flag_exit = true;
+    return true;
+  });
+  vis.RegisterKeyCallback(GLFW_KEY_SPACE, [&](visualization::Visualizer *vis) {
+    if (flag_play) {
+      utility::LogInfo("Playback paused, press [SPACE] to continue");
+    } else {
+      utility::LogInfo("Playback resumed, press [SPACE] to pause");
+    }
+    flag_play = !flag_play;
+    return true;
+  });
+  vis.RegisterKeyCallback(GLFW_KEY_LEFT, [&](visualization::Visualizer *vis) {
+    uint64_t now = bag_reader.GetTimestamp();
+    if (bag_reader.SeekTimestamp(now < 1'000'000 ? 0 : now - 1'000'000))
+      utility::LogInfo("Seek back 1s");
+    else
+      utility::LogWarning("Seek back 1s failed");
+    return true;
+  });
+  vis.RegisterKeyCallback(GLFW_KEY_RIGHT, [&](visualization::Visualizer *vis) {
+    uint64_t now = bag_reader.GetTimestamp();
+    if (bag_reader.SeekTimestamp(now + 1'000'000))
+      utility::LogInfo("Seek forward 1s");
+    else
+      utility::LogWarning("Seek forward 1s failed");
+    return true;
+  });
+
+  vis.CreateVisualizerWindow("Open3D Intel RealSense bag player", 1920, 540);
+  utility::LogInfo("Starting to play. Press [SPACE] to pause. Press [ESC] to "
+                   "exit.");
+
+  bool is_geometry_added = false;
+  int idx = 0;
+  const auto bag_metadata = bag_reader.GetMetadata();
+  utility::LogInfo("Recorded with device {}", bag_metadata.device_name_);
+  utility::LogInfo("    Serial number: {}", bag_metadata.serial_number_);
+  utility::LogInfo("Video resolution: {}x{}", bag_metadata.width_,
+                   bag_metadata.height_);
+  utility::LogInfo("      frame rate: {}", bag_metadata.fps_);
+  utility::LogInfo("      duration: {:.6f}s",
+                   static_cast<double>(bag_metadata.stream_length_usec_) *
+                       1e-6);
+  utility::LogInfo("      color pixel format: {}", bag_metadata.color_format_);
+  utility::LogInfo("      depth pixel format: {}", bag_metadata.depth_format_);
+
+  if (write_image) {
+    io::WriteIJsonConvertibleToJSON(
+        fmt::format("{}/intrinsic.json", output_path), bag_metadata);
+  }
+  const auto frame_interval = sc::duration<double>(1. / bag_metadata.fps_);
+
+  auto last_frame_time = std::chrono::steady_clock::now() - frame_interval;
+  using legacyRGBDImage = open3d::geometry::RGBDImage;
+  legacyRGBDImage im_rgbd;
+  while (!bag_reader.IsEOF() && !flag_exit) {
+    if (flag_play) {
+      std::this_thread::sleep_until(last_frame_time + frame_interval);
+      last_frame_time = std::chrono::steady_clock::now();
+      im_rgbd = bag_reader.NextFrame().ToLegacyRGBDImage();
+      // create shared_ptr with no-op deleter for stack RGBDImage
+      auto ptr_im_rgbd =
+          std::shared_ptr<legacyRGBDImage>(&im_rgbd, [](legacyRGBDImage *) {});
+      // Improve depth visualization by scaling
+      /* im_rgbd.depth_.LinearTransform(0.25); */
+      if (ptr_im_rgbd->IsEmpty())
+        continue;
+
+      if (!is_geometry_added) {
+        vis.AddGeometry(ptr_im_rgbd);
+        is_geometry_added = true;
+      }
+
+      ++idx;
+      if (write_image)
+#pragma omp parallel sections
+      {
+#pragma omp section
+        {
+          auto color_file =
+              fmt::format("{0}/color/{1:05d}.jpg", output_path, idx);
+          utility::LogInfo("Writing to {}", color_file);
+          io::WriteImage(color_file, im_rgbd.color_);
+        }
+#pragma omp section
+        {
+          auto depth_file =
+              fmt::format("{0}/depth/{1:05d}.png", output_path, idx);
+          utility::LogInfo("Writing to {}", depth_file);
+          io::WriteImage(depth_file, im_rgbd.depth_);
+        }
+      }
+      vis.UpdateGeometry();
+      vis.UpdateRender();
+    }
+    vis.PollEvents();
+  }
+  bag_reader.Close();
+}