// ----------------------------------------------------------------------------
// - 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();
}