126 lines
2.4 KiB
C++
126 lines
2.4 KiB
C++
#include "assets/file_parsers/kitti_pose_parser.hpp"
|
|
|
|
#include <fstream>
|
|
#include <execution>
|
|
#include <glm/ext/matrix_transform.hpp>
|
|
#include "util/logger.hpp"
|
|
|
|
|
|
assets::kitti_pose_parser::parser_context::parser_context(
|
|
store_type& m_store,
|
|
std::mutex& m_store_mutex
|
|
) : m_store{ &m_store }, m_store_mutex{ &m_store_mutex }
|
|
{
|
|
m_buffer.reserve(128);
|
|
}
|
|
|
|
void assets::kitti_pose_parser::parser_context::reset()
|
|
{
|
|
m_buffer.clear();
|
|
}
|
|
|
|
void assets::kitti_pose_parser::parser_context::operator()(lookup_type::const_pointer entry) noexcept
|
|
{
|
|
const auto& [ filename, id ] = *entry;
|
|
|
|
if (const auto e = parse_file(filename, m_buffer))
|
|
{
|
|
ztu::logger::warn("Could not load pose_list %: %.", filename, e.message());
|
|
return;
|
|
}
|
|
|
|
{
|
|
auto lock = std::lock_guard{ *m_store_mutex };
|
|
m_store->add(id, m_buffer);
|
|
}
|
|
}
|
|
|
|
|
|
std::error_code assets::kitti_pose_parser::prefetch(
|
|
path_id_lookups& lookups
|
|
) {
|
|
return {};
|
|
}
|
|
|
|
std::error_code assets::kitti_pose_parser::parse(
|
|
const path_id_lookups& lookups,
|
|
store_type& store,
|
|
bool
|
|
) {
|
|
namespace fs = std::filesystem;
|
|
|
|
auto path_buffer = fs::path{};
|
|
auto in = std::ifstream{}; // TODO disable exceptions (for other loaders as well)
|
|
|
|
m_path_buffer.clear();
|
|
lookups.pose_lists.by_extension(".glsl", m_path_buffer);
|
|
|
|
auto store_mutex = std::mutex{};
|
|
|
|
std::for_each(
|
|
std::execution::parallel_unsequenced_policy{},
|
|
m_path_buffer.begin(),
|
|
m_path_buffer.end(),
|
|
parser_context{ store, store_mutex }
|
|
);
|
|
|
|
return {};
|
|
}
|
|
|
|
|
|
std::error_code assets::kitti_pose_parser::parse_file(
|
|
const std::filesystem::path& filename,
|
|
data_type &poses
|
|
) {
|
|
|
|
auto in = std::ifstream{};
|
|
|
|
in.open(filename);
|
|
if (not in.is_open())
|
|
{
|
|
return std::make_error_code(std::errc::no_such_file_or_directory);
|
|
}
|
|
|
|
in >> std::skipws;
|
|
|
|
while (in.peek() != std::ifstream::traits_type::eof())
|
|
{
|
|
auto pose = glm::identity<pose_data>();
|
|
|
|
if (const auto error = parse_pose(in, pose))
|
|
{
|
|
ztu::logger::error(
|
|
"Error occurred while parsing kitti pose file %: [%] %",
|
|
filename,
|
|
error.category().name(),
|
|
error.message()
|
|
);
|
|
continue;
|
|
}
|
|
|
|
poses.emplace_back(pose);
|
|
}
|
|
|
|
in.close();
|
|
|
|
return {};
|
|
}
|
|
|
|
std::error_code assets::kitti_pose_parser::parse_pose(
|
|
std::ifstream& in,
|
|
pose_data& pose
|
|
) {
|
|
for (pose_data::length_type row{}; row != 3; ++row)
|
|
{
|
|
for (pose_data::length_type col{}; col != 4; ++col)
|
|
{
|
|
if (not (in >> pose[row][col]))
|
|
{
|
|
return std::make_error_code(std::errc::result_out_of_range);
|
|
}
|
|
}
|
|
}
|
|
|
|
return {};
|
|
}
|