BinaryIO.h

Go to the documentation of this file.

#pragma once
 
#include <fstream>
#include <istream>
#include <ostream>
 
#include "../VoxelGrid.hpp"
#include "exceptions.h"
 
namespace visionx::voxelgrid::io
{
    class BinaryIO
    {
    public:
        // VOXEL READ/WRITE
 
        /**
         * @brief Read a voxel from the out stream.
         *
         * This is a free template method used by voxel_grid_io::read(),
         * so you can specialize it if necessary.
         */
        template <typename VoxelT>
        static void readVoxel(std::istream& is, VoxelT& voxel);
 
 
        /**
         * @brief Write a voxel to the out stream.
         *
         * This is a free template method used by voxel_grid_io::write(),
         * so you can specialize it if necessary.
         */
        template <typename VoxelT>
        static void writeVoxel(std::ostream& os, const VoxelT& voxel);
 
 
        // VOXEL GRID READ
 
        /**
         * @brief Read a voxel grid from the file.
         *
         * Sets `voxelGrid`'s layout and fills the voxel data.
         */
        template <typename VoxelT>
        static void read(VoxelGrid<VoxelT>& voxelGrid, const std::string& filename);
 
        /**
         * @brief Read a voxel grid from the in-stream.
         *
         * Sets `voxelGrid`'s layout and fills the voxel data.
         * `is` must be open in binary mode with a valid voxel grid file.
         */
        template <typename VoxelT>
        static void read(VoxelGrid<VoxelT>& voxelGrid, std::istream& is);
 
 
        /**
         * @brief Read a voxel grid from the filename.
         *
         * The specified voxel grid type `VoxelGridT` must provide a default constructor.
         * Sets `voxelGrid`'s layout and fills the voxel data.
         * `is` must be open in binary mode with a valid voxel grid file.
         */
        template <typename VoxelGridT>
        static VoxelGridT read(const std::string& filename);
 
        /**
         * @brief Read a voxel grid from the in-stream.
         *
         * The specified voxel grid type `VoxelGridT` must provide a default constructor.
         * Sets `voxelGrid`'s layout and fills the voxel data.
         * `is` must be open in binary mode with a valid voxel grid file.
         */
        template <typename VoxelGridT>
        static VoxelGridT read(std::istream& is);
 
 
        // VOXEL GRID WRITE
 
        /**
         * @brief Write a voxel grid to a file.
         */
        template <typename VoxelT>
        static void write(const VoxelGrid<VoxelT>& voxelGrid, const std::string& filename);
 
        /**
         * @brief Write a voxel grid to an out-stream.
         *
         * The out-stream `os` must be open in binary mode.
         */
        template <typename VoxelT>
        static void write(const VoxelGrid<VoxelT>& voxelGrid, std::ostream& os);
 
 
    private:
        /// Throws an exception if value is not positive 0.
        static void checkPositive(int value);
        static void checkPositive(float value);
    };
} // namespace visionx::voxelgrid::io
 
// IMPLEMENTATIONS
 
namespace visionx::voxelgrid::io::detail
{
 
    template <typename DataT>
    void
    readData(std::istream& is, DataT& data)
    {
        is.read(reinterpret_cast<char*>(&data), sizeof(DataT));
    }
 
    template <typename DataT>
    void
    writeData(std::ostream& os, const DataT& data)
    {
        os.write(reinterpret_cast<const char*>(&data), sizeof(DataT));
    }
 
    template <typename ReadT, typename Derived>
    void
    readEigen(std::istream& is, Eigen::MatrixBase<Derived>& matrix)
    {
        using Scalar = typename Eigen::MatrixBase<Derived>::Scalar;
 
        auto cast = matrix.template cast<ReadT>().eval();
        is.read(reinterpret_cast<char*>(cast.data()), cast.size() * sizeof(Scalar));
 
        matrix = cast.template cast<Scalar>();
    }
 
    template <typename WriteT, typename Derived>
    void
    writeEigen(std::ostream& os, const Eigen::MatrixBase<Derived>& matrix)
    {
        auto eval = matrix.template cast<WriteT>().eval();
        os.write(reinterpret_cast<const char*>(eval.data()), eval.size() * sizeof(WriteT));
    }
 
} // namespace visionx::voxelgrid::io::detail
 
namespace visionx::voxelgrid::io
{
    template <typename VoxelT>
    void
    BinaryIO::readVoxel(std::istream& is, VoxelT& voxel)
    {
        detail::readData<VoxelT>(is, voxel);
    }
 
    template <typename VoxelT>
    void
    BinaryIO::writeVoxel(std::ostream& os, const VoxelT& voxel)
    {
        detail::writeData<VoxelT>(os, voxel);
    }
 
    template <typename VoxelGridT>
    VoxelGridT
    BinaryIO::read(const std::string& filename)
    {
        VoxelGridT grid;
        read(grid, filename);
        return grid;
    }
 
    template <typename VoxelGridT>
    VoxelGridT
    BinaryIO::read(std::istream& is)
    {
        VoxelGridT grid;
        read(grid, is);
        return grid;
    }
 
    template <typename VoxelT>
    void
    BinaryIO::read(VoxelGrid<VoxelT>& voxelGrid, const std::string& filename)
    {
        std::ifstream ifs;
        ifs.exceptions(std::ifstream::failbit | std::ifstream::badbit);
 
        try
        {
            ifs.open(filename, std::ios_base::binary | std::ios_base::in);
        }
        catch (const std::ios_base::failure& e)
        {
            throw error::io::BinaryIOError("read", filename, "Failed to open file.", e);
        }
 
        try
        {
            read(voxelGrid, ifs);
        }
        catch (const std::ios_base::failure& e)
        {
            throw error::io::BinaryIOError("read", filename, "Error while reading file.", e);
        }
    }
 
    template <typename VoxelT>
    void
    BinaryIO::read(VoxelGrid<VoxelT>& voxelGrid, std::istream& is)
    {
        Eigen::Vector3f voxelSizes = Eigen::Vector3f::Zero();
        Eigen::Vector3f gridSizes = Eigen::Vector3f::Zero();
        Eigen::Vector3f origin = Eigen::Vector3f::Zero();
        Eigen::Vector4f orientationCoeffs = Eigen::Vector4f::Zero();
 
        try
        {
            // read voxel sizes
            detail::readEigen<float>(is, voxelSizes);
            // read grid sizes (as float to be uniform)
            detail::readEigen<float>(is, gridSizes);
            // read grid origin
            detail::readEigen<float>(is, origin);
            // read orientation coeffs in [w x y z] order
            detail::readEigen<float>(is, orientationCoeffs);
        }
        catch (const std::ios_base::failure& e)
        {
            throw error::io::BinaryReadError("Failed to read voxel grid structure.", e);
        }
 
        checkPositive(voxelSizes.minCoeff());
        checkPositive(gridSizes.minCoeff());
 
        Eigen::Quaternionf orientation;
        orientation.w() = orientationCoeffs(0);
        orientation.coeffs().head<3>() = orientationCoeffs.tail<3>();
 
        voxelGrid.setVoxelSizes(voxelSizes);
        voxelGrid.setGridSizes(gridSizes.cast<int>());
        voxelGrid.setOrigin(origin);
        voxelGrid.setOrientation(orientation);
 
        // read voxel data
        for (std::size_t i = 0; i < voxelGrid.numVoxels(); ++i)
        {
            try
            {
                readVoxel<VoxelT>(is, voxelGrid.getVoxel(i));
            }
            catch (const std::ios_base::failure& e)
            {
                std::stringstream ss;
                ss << "Failed to read voxel " << i << " of " << voxelGrid.numVoxels() << ".";
                throw error::io::BinaryReadError(ss.str(), e);
            }
        }
    }
 
    template <typename VoxelT>
    void
    BinaryIO::write(const VoxelGrid<VoxelT>& voxelGrid, const std::string& filename)
    {
        std::ofstream ofs;
        ofs.exceptions(std::ofstream::failbit | std::ofstream::badbit);
 
        try
        {
            ofs.open(filename, std::ios_base::binary | std::ios_base::out);
        }
        catch (const std::ios_base::failure& e)
        {
            throw error::io::BinaryIOError("write", filename, "Failed to open file.", e);
        }
 
        try
        {
            write(voxelGrid, ofs);
        }
        catch (const std::ios_base::failure& e)
        {
            throw error::io::BinaryIOError("write", filename, "Error while reading file.", e);
        }
    }
 
    template <typename VoxelT>
    void
    BinaryIO::write(const VoxelGrid<VoxelT>& voxelGrid, std::ostream& os)
    {
        const Eigen::Vector3f voxelSizes = voxelGrid.getVoxelSizes();
        const Eigen::Vector3i gridSizes = voxelGrid.getGridSizes();
        const Eigen::Vector3f origin = voxelGrid.getOrigin();
        const Eigen::Quaternionf orientation = voxelGrid.getOrientation();
 
        // orientation in [w x y z] order
        Eigen::Vector4f orientationCoeffs;
        orientationCoeffs(0) = orientation.w();
        orientationCoeffs.tail<3>() = orientation.coeffs().head<3>();
 
        // write voxel sizes
        detail::writeEigen<float>(os, voxelSizes);
        // write grid sizes (as float to be uniform)
        detail::writeEigen<float>(os, gridSizes.cast<float>());
        // write grid origin
        detail::writeEigen<float>(os, origin);
        // write grid orientation
        detail::writeEigen<float>(os, orientationCoeffs);
 
        // write voxel data
        for (std::size_t i = 0; i < voxelGrid.numVoxels(); ++i)
        {
            writeVoxel<VoxelT>(os, voxelGrid.getVoxel(i));
        }
    }
 
} // namespace visionx::voxelgrid::io