x

include/utils.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <sl/Camera.hpp>
+
+/**
+* @brief Compute the start frame of each SVO for playback to be synced
+*
+* @param svo_files Map camera index to SVO file path
+* @return Map camera index to starting SVO frame for synced playback
+*/
+std::map<int, int> syncDATA(std::map<int, std::string> svo_files) {
+    std::map<int, int> output; // map of camera index and frame index of the starting point for each
+
+    // Open all SVO
+    std::map<int, std::shared_ptr<sl::Camera>> p_zeds;
+
+    for (auto &it : svo_files) {
+        auto p_zed = std::make_shared<sl::Camera>();
+
+        sl::InitParameters init_param;
+        init_param.depth_mode = sl::DEPTH_MODE::NONE;
+        init_param.camera_disable_self_calib = true;
+        init_param.input.setFromSVOFile(it.second.c_str());
+
+        auto error = p_zed->open(init_param);
+        if (error == sl::ERROR_CODE::SUCCESS)
+            p_zeds.insert(std::make_pair(it.first, p_zed));
+        else {
+            std::cerr << "Could not open file " << it.second.c_str() << ": " << sl::toString(error) << ". Skipping" << std::endl;
+        }
+    }
+
+    // Compute the starting point, we have to take the latest one
+    sl::Timestamp start_ts = 0;
+    for (auto &it : p_zeds) {
+        it.second->grab();
+        auto ts = it.second->getTimestamp(sl::TIME_REFERENCE::IMAGE);
+
+        if (ts > start_ts)
+            start_ts = ts;
+    }
+
+    std::cout << "Found SVOs common starting time: " << start_ts << std::endl;
+
+    // The starting point is now known, let's find the frame idx for all corresponding
+    for (auto &it : p_zeds) {
+        auto frame_position_at_ts = it.second->getSVOPositionAtTimestamp(start_ts);
+
+        if (frame_position_at_ts != -1)
+            output.insert(std::make_pair(it.first, frame_position_at_ts));
+    }
+
+    for (auto &it : p_zeds) it.second->close();
+
+    return output;
+}