xyzi.hpp

 
#ifndef UFO_IO_XYZI_HPP
#define UFO_IO_XYZI_HPP
 
// UFO
#include <ufo/cloud/point_cloud.hpp>
#include <ufo/core/intensity.hpp>
#include <ufo/io/cloud_properties.hpp>
#include <ufo/io/file_handler.hpp>
#include <ufo/numeric/vec.hpp>
 
// STL
#include <cstdio>
#include <filesystem>
#include <print>
 
namespace ufo
{
[[nodiscard]] CloudProperties cloudPropertiesXYZI(std::filesystem::path const& file);
 
template <std::size_t Dim, class T, class... Ts>
bool readXYZI(std::filesystem::path const& file, PointCloud<Dim, T, Ts...>& pc)
{
    FileHandler fp(file.c_str(), "rb");
 
    if (!fp) {
        std::println(stderr, "[UFO | Read XYZI] Failed to open file: {}", file.string());
        return false;
    }
 
    pc.clear();
 
    for (char* line = fp.readline(); nullptr != line; line = fp.readline()) {
        Vec3d  p;
        double i;
        if (4 == std::sscanf(line, "%lf %lf %lf %lf", &p.x, &p.y, &p.z, &i)) {
            if constexpr ((is_intensity_v<Ts> || ...)) {
                pc.push_back(convert<Vec<Dim, T>>(p), Intensity(i));
            } else {
                pc.push_back(convert<Vec<Dim, T>>(p));
            }
        }
    }
 
    return true;
}
 
template <std::size_t Dim, class T, class... Ts>
bool writeXYZI(std::filesystem::path const& file, PointCloud<Dim, T, Ts...> const& pc)
{
    FileHandler fp(file.c_str(), "wb");
 
    if (!fp) {
        std::println(stderr, "[UFO | Write XYZI] Failed to create file: {}", file.string());
        return false;
    }
 
    std::size_t const size        = pc.size();
    auto              points      = view<Vec<Dim, T>>(pc);
    auto              intensities = view<Intensity>(pc);
    for (std::size_t i{}; size > i; ++i) {
        auto p         = convert<Vec<3, T>>(points[i]);
        auto intensity = intensities[i];
        if constexpr (std::is_same_v<T, float>) {
            std::print(fp.get(), "{:.6} {:.6} {:.6}", p.x, p.y, p.z);
        } else if constexpr (std::is_floating_point_v<T>) {
            std::print(fp.get(), "{:.10} {:.10} {:.10}", p.x, p.y, p.z);
        } else {
            std::print(fp.get(), "{} {} {}", p.x, p.y, p.z);
        }
        if constexpr ((is_intensity_v<Ts> || ...)) {
            std::println(fp.get(), " {:.6}", intensity.intensity);
        } else {
            std::println(fp.get(), " 0");
        }
    }
 
    return true;
}
}  // namespace ufo
 
#endif  // UFO_IO_XYZI_HPP