445 lines
9.5 KiB
C++
445 lines
9.5 KiB
C++
#ifndef INCLUDE_GENERIC_3DTK_LOADER_IMPLEMENTATION
|
|
# error Never include this file directly include 'generic_3dtk_loader.hpp'
|
|
#endif
|
|
|
|
#include <charconv>
|
|
#include <fstream>
|
|
#include "util/logger.hpp"
|
|
#include "glm/glm.hpp"
|
|
#include "glm/gtx/euler_angles.hpp"
|
|
#include <charconv>
|
|
#include <fstream>
|
|
|
|
template<bool Normal, bool Color, bool Reflectance>
|
|
ztu::result<pose_prefetch_lookup::index_type> generic_3dtk_loader<Normal, Color, Reflectance>::parse_index(
|
|
const std::string_view filename
|
|
) {
|
|
static constexpr auto prefix = std::string_view{ "scan" };
|
|
|
|
auto name_view = filename.substr(0, name_view.find('.'));
|
|
|
|
if (name_view.length() <= prefix.length()) [[unlikely]]
|
|
{
|
|
return std::make_error_code(std::errc::invalid_argument);
|
|
}
|
|
|
|
name_view = name_view.substr(prefix.length());
|
|
|
|
pose_prefetch_lookup::index_type index;
|
|
|
|
const auto res = std::from_chars(name_view.begin(), name_view.end(), index);
|
|
|
|
if (res.ec != std::errc{}) [[unlikely]]
|
|
{
|
|
return std::make_error_code(res.ec);
|
|
}
|
|
|
|
return index;
|
|
}
|
|
|
|
template<bool Normal, bool Color, bool Reflectance>
|
|
std::error_code generic_3dtk_loader<Normal, Color, Reflectance>::prefetch(
|
|
const file_dir_list& paths,
|
|
prefetch_queue& queue
|
|
) {
|
|
|
|
auto path_buffer = std::filesystem::path{};
|
|
|
|
for (const auto filename : paths.files)
|
|
{
|
|
path_buffer.assign(filename.begin(), filename.end());
|
|
path_buffer.replace_extension(".pose");
|
|
queue.uos_queue.files.push_back(path_buffer.c_str());
|
|
}
|
|
|
|
// TODO optimize
|
|
for (const auto directory : paths.directories)
|
|
{
|
|
queue.uos_queue.directories.push_back(directory);
|
|
}
|
|
}
|
|
|
|
template<bool Normal, bool Color, bool Reflectance>
|
|
std::error_code generic_3dtk_loader<Normal, Color, Reflectance>::load(
|
|
dynamic_point_cloud_buffer& buffer,
|
|
const file_dir_list& paths,
|
|
prefetch_lookup& asset_lookup,
|
|
dynamic_point_cloud_store& store,
|
|
const bool pedantic
|
|
) {
|
|
namespace fs = std::filesystem;
|
|
|
|
auto in = std::ifstream{};
|
|
auto path_buffer = fs::path{};
|
|
auto error = std::error_code{};
|
|
|
|
const auto load_file = [&](const char* filename)
|
|
{
|
|
// TODO look up pose
|
|
auto scan_index = pose_prefetch_lookup::index_type{};
|
|
|
|
if (auto res = parse_index(filename))
|
|
{
|
|
scan_index = *res;
|
|
}
|
|
else [[unlikely]]
|
|
{
|
|
error = res.error();
|
|
ztu::logger::error(
|
|
"Error occurred while parsing scan index in filename %: [%] %",
|
|
error.category().name(),
|
|
error.message()
|
|
);
|
|
}
|
|
|
|
auto pose = asset_lookup.poses.find();
|
|
|
|
|
|
const auto id_it = id_lookup.find(filename);
|
|
|
|
if (id_it != id_lookup.end()) [[unlikely]]
|
|
{
|
|
return;
|
|
}
|
|
|
|
in.open(filename);
|
|
if (in.is_open())
|
|
{
|
|
if ((error = this->read_point_file(filename, buffer))) {
|
|
return error;
|
|
}
|
|
|
|
this->transform_point_cloud(buffer.positions(), pose);
|
|
|
|
const auto id = store.add(std::move(buffer));
|
|
id_lookup.emplace_hint(id_it, filename, id);
|
|
}
|
|
else
|
|
{
|
|
ztu::logger::error("Cannot open 3dtk file %", filename);
|
|
}
|
|
|
|
in.close();
|
|
};
|
|
|
|
for (const auto filename : paths.files)
|
|
{
|
|
load_file(filename.data());
|
|
}
|
|
|
|
|
|
|
|
for (const auto directory : paths.directories)
|
|
{
|
|
directory_buffer.assign(directory.begin(), directory.end());
|
|
directory_buffer /= "frames";
|
|
|
|
const auto directory_exists = not fs::is_directory(directory_buffer, error);
|
|
|
|
if (error or not directory_exists) [[unlikely]]
|
|
{
|
|
ztu::logger::error("Could not open point cloud directory %", directory_buffer);
|
|
continue;
|
|
}
|
|
|
|
for (const auto& filename : fs::directory_iterator{ directory_buffer }) {
|
|
|
|
auto point_filename = reinterpret_cast<const fs::path&>(filename);
|
|
if (point_filename.extension() != ".3d")
|
|
{
|
|
continue;
|
|
}
|
|
|
|
load_file(filename.c_str());
|
|
}
|
|
}
|
|
|
|
return {};
|
|
}
|
|
|
|
template<bool Normal, bool Color, bool Reflectance>
|
|
std::error_code generic_3dtk_loader<Normal, Color, Reflectance>::load_directory(
|
|
dynamic_data_loader_ctx& ctx,
|
|
dynamic_point_cloud_store& store,
|
|
const std::filesystem::path& path,
|
|
const bool pedantic
|
|
) {
|
|
namespace fs = std::filesystem;
|
|
|
|
std::error_code error;
|
|
|
|
const auto directory_exists = not fs::is_directory(path, error);
|
|
if (error)
|
|
{
|
|
return error;
|
|
}
|
|
|
|
if (not directory_exists)
|
|
{
|
|
return make_error_code(std::errc::no_such_file_or_directory);
|
|
}
|
|
|
|
for (const auto& filename : fs::directory_iterator{ path / "frames" }) {
|
|
|
|
auto point_filename = reinterpret_cast<const fs::path&>(filename);
|
|
if (point_filename.extension() != ".3d") {
|
|
continue;
|
|
}
|
|
|
|
if ((error = load(ctx, store, point_filename, pedantic)))
|
|
{
|
|
ztu::logger::error(
|
|
"Error while loading point cloud '%': [%] %",
|
|
point_filename,
|
|
error.category().name(),
|
|
error.message()
|
|
);
|
|
}
|
|
}
|
|
|
|
return {};
|
|
}
|
|
|
|
template<typename T, std::size_t Count>
|
|
std::error_code read_vector(std::string_view& line, std::array<T, Count>& vec) {
|
|
for (auto& component : vec)
|
|
{
|
|
auto it = line.begin();
|
|
|
|
const auto [minus, plus] = std::pair{ *it == '-', *it == '+' };
|
|
it += plus or minus ? 3 : 2; // skip '[-+]?0x'
|
|
|
|
const auto [ ptr, ec ] = std::from_chars(
|
|
it, line.end(),
|
|
component,
|
|
std::chars_format::hex
|
|
);
|
|
|
|
if (ec != std::errc{})
|
|
{
|
|
return std::make_error_code(ec);
|
|
}
|
|
|
|
if (minus) {
|
|
component *= -1.0;
|
|
}
|
|
|
|
line = { ptr + sizeof(' '), line.end() };
|
|
}
|
|
|
|
return {};
|
|
}
|
|
|
|
template<bool Normal, bool Color, bool Reflectance>
|
|
std::error_code generic_3dtk_loader<Normal, Color, Reflectance>::read_point_file(
|
|
const std::filesystem::path& filename,
|
|
dynamic_point_cloud_data& point_cloud
|
|
) {
|
|
std::error_code error;
|
|
|
|
auto in = std::ifstream(filename);
|
|
|
|
if (not in.is_open()) {
|
|
return std::make_error_code(static_cast<std::errc>(errno));
|
|
}
|
|
|
|
std::string line;
|
|
if (not std::getline(in, line))
|
|
{
|
|
return std::make_error_code(std::errc::invalid_seek);
|
|
}
|
|
|
|
constexpr auto expected_component_count = []()
|
|
{
|
|
auto count = std::tuple_size_v<components::point_cloud_vertex::position>;
|
|
|
|
if (Normal)
|
|
{
|
|
count += std::tuple_size_v<components::point_cloud_vertex::normal>;
|
|
}
|
|
|
|
if (Color)
|
|
{
|
|
count += std::tuple_size_v<components::point_cloud_vertex::color>;
|
|
}
|
|
|
|
if (Reflectance)
|
|
{
|
|
count += std::tuple_size_v<components::point_cloud_vertex::reflectance>;
|
|
}
|
|
|
|
return count;
|
|
}();
|
|
|
|
ztu::u32 component_count;
|
|
std::chars_format float_format;
|
|
|
|
if ((error = analyze_component_format(line, component_count, float_format)))
|
|
{
|
|
return error;
|
|
}
|
|
|
|
if (component_count != expected_component_count)
|
|
{
|
|
return std::make_error_code(std::errc::invalid_argument);
|
|
}
|
|
|
|
auto& positions = point_cloud.positions();
|
|
auto& normals = point_cloud.normals();
|
|
auto& colors = point_cloud.colors();
|
|
auto& reflectances = point_cloud.reflectances();
|
|
|
|
do
|
|
{
|
|
auto line_view = std::string_view{ line };
|
|
|
|
components::point_cloud_vertex::position position;
|
|
if ((error = read_vector(line_view, position)))
|
|
{
|
|
return error;
|
|
}
|
|
positions.push_back(position);
|
|
|
|
if constexpr (Normal)
|
|
{
|
|
components::point_cloud_vertex::normal normal;
|
|
if ((error = read_vector(line_view, normal)))
|
|
{
|
|
return error;
|
|
}
|
|
normals.push_back(normal);
|
|
}
|
|
|
|
if constexpr (Color)
|
|
{
|
|
components::point_cloud_vertex::color color;
|
|
if ((error = read_vector(line_view, color)))
|
|
{
|
|
return error;
|
|
}
|
|
colors.push_back(color);
|
|
}
|
|
|
|
if constexpr (Reflectance)
|
|
{
|
|
components::point_cloud_vertex::reflectance reflectance;
|
|
if ((error = read_vector(line_view, reflectance)))
|
|
{
|
|
return error;
|
|
}
|
|
reflectances.push_back(reflectance);
|
|
}
|
|
}
|
|
while (std::getline(in, line));
|
|
|
|
return {};
|
|
}
|
|
|
|
std::error_code base_3dtk_loader::read_pose_file(
|
|
const std::filesystem::path& filename,
|
|
glm::mat4& pose
|
|
) {
|
|
auto in = std::ifstream(filename);
|
|
if (not in.is_open()) {
|
|
return std::make_error_code(static_cast<std::errc>(errno));
|
|
}
|
|
std::string line;
|
|
|
|
std::array<glm::vec3, 2> numbers{};
|
|
|
|
for (std::size_t row{}; row != 2; ++row) {
|
|
|
|
std::getline(in, line);
|
|
|
|
auto it = line.cbegin().base();
|
|
auto end = line.cend().base();
|
|
|
|
for (glm::vec3::length_type col{}; col != 3; ++col) {
|
|
|
|
const auto [ ptr, ec ] = std::from_chars(
|
|
it, end,
|
|
numbers[row][col],
|
|
std::chars_format::general
|
|
);
|
|
|
|
if (ec != std::errc{}) {
|
|
return std::make_error_code(ec);
|
|
}
|
|
|
|
it = ptr + 1; // skip space in between components
|
|
}
|
|
}
|
|
|
|
const auto& translation = numbers[0];
|
|
auto& angles = numbers[1];
|
|
angles *= static_cast<float>(M_PI / 180.0);
|
|
|
|
pose = (
|
|
glm::translate(glm::identity<glm::mat4>(), translation) *
|
|
glm::eulerAngleXYZ(angles[0], angles[1], angles[2])
|
|
);
|
|
|
|
return {};
|
|
}
|
|
|
|
std::error_code base_3dtk_loader::analyze_component_format(
|
|
const std::string& line,
|
|
ztu::u32& component_count,
|
|
std::chars_format& format
|
|
) {
|
|
auto begin = line.cbegin().base();
|
|
auto end = line.cend().base();
|
|
|
|
format = std::chars_format::general;
|
|
|
|
component_count = 0;
|
|
float buffer;
|
|
|
|
for (auto it = begin; it < end; it += sizeof(' '))
|
|
{
|
|
it += *it == '-' or *it == '+';
|
|
|
|
std::chars_format current_format;
|
|
if (*it == '0' and std::next(it) < end and *std::next(it) == 'x')
|
|
{
|
|
it += 2; // skip '0x'
|
|
current_format = std::chars_format::hex;
|
|
}
|
|
else
|
|
{
|
|
current_format = std::chars_format::general;
|
|
}
|
|
|
|
if (it == begin and current_format != format)
|
|
{
|
|
return std::make_error_code(std::errc::invalid_argument);
|
|
}
|
|
|
|
const auto [next_it, err] = std::from_chars(it, end, buffer, current_format);
|
|
if (err != std::errc())
|
|
{
|
|
return std::make_error_code(err);
|
|
}
|
|
|
|
it = next_it;
|
|
format = current_format;
|
|
++component_count;
|
|
}
|
|
|
|
return {};
|
|
}
|
|
|
|
|
|
void base_3dtk_loader::transform_point_cloud(
|
|
std::span<components::point_cloud_vertex::position::value_type> points,
|
|
const glm::mat4& pose
|
|
) {
|
|
for (auto& [ x, y, z ] : points) {
|
|
auto vec = glm::vec4{ x, y, z, 1.0f };
|
|
vec = pose * vec;
|
|
x = vec.x;
|
|
y = vec.y;
|
|
z = vec.z;
|
|
}
|
|
}
|