fixes
This commit is contained in:
ZY4N
444
source/assets/data_loaders/generic/generic_3dtk_loader.ipp
Normal file
444
source/assets/data_loaders/generic/generic_3dtk_loader.ipp
Normal file
@@ -0,0 +1,444 @@
|
||||
#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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user