57 lines
1.8 KiB
C++
Executable File
57 lines
1.8 KiB
C++
Executable File
#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;
|
|
}
|