Program Listing for File Util.hpp

↰ Return to documentation for file (include/lvr2/util/Util.hpp)

/*
 * Util.hpp
 *
 *  @date 30.10.2018
 *  @author Alexander Loehr (aloehr@uos.de)
 */

#ifndef LVR2_UTIL_HPP
#define LVR2_UTIL_HPP

#include <vector>
#include <boost/shared_array.hpp>

#include "lvr2/types/MatrixTypes.hpp"
#include "lvr2/types/PointBuffer.hpp"
#include "lvr2/geometry/BaseVector.hpp"
#include "lvr2/geometry/Matrix4.hpp"


namespace lvr2
{

using VecUChar = BaseVector<unsigned char>;

class Util
{
public:

    static int getSpectralChannel(int wavelength, PointBufferPtr p, int fallback = -1);

    static int getSpectralWavelength(int channel, PointBufferPtr p, int fallback = -1);

    static float wavelengthPerChannel(PointBufferPtr p);

    template<typename T>
    static boost::shared_array<T> convert_vector_to_shared_array(std::vector<T> source)
    {
        boost::shared_array<T> ret = boost::shared_array<T>( new T[source.size()] );
        std::copy(source.begin(), source.end(), ret.get());

        return ret;
    }

    template <typename BaseVecT>
    static Matrix4<BaseVecT> riegl_to_slam6d_transform(const Matrix4<BaseVecT> &in)
    {
        Matrix4<BaseVecT> ret;

        ret[0] = in[5];
        ret[1] = -in[9];
        ret[2] = -in[1];
        ret[3] = -in[13];
        ret[4] = -in[6];
        ret[5] = in[10];
        ret[6] = in[2];
        ret[7] = in[14];
        ret[8] = -in[4];
        ret[9] = in[8];
        ret[10] = in[0];
        ret[11] = in[12];
        ret[12] = -100*in[7];
        ret[13] = 100*in[11];
        ret[14] = 100*in[3];
        ret[15] = in[15];

        return ret;
    }

    template <typename T>
    Transform<T> slam6d_to_riegl_transform(const Transform<T> &mat)
    {
        T* in = mat.data();
        T* ret[16];
        ret[0] = in[10];
        ret[1] = -in[2];
        ret[2] = in[6];
        ret[3] = in[14]/100.0;
        ret[4] = -in[8];
        ret[5] = in[0];
        ret[6] = -in[4];
        ret[7] = -in[12]/100.0;
        ret[8] = in[9];
        ret[9] = -in[1];
        ret[10] = in[5];
        ret[11] = in[13]/100.0;
        ret[12] = in[11];
        ret[13] = -in[3];
        ret[14] = in[7];
        ret[15] = in[15];

        return Eigen::Map<Eigen::Matrix<T, 4, 4, Eigen::RowMajor> >(ret);
    }

    template <typename ValueType>
    static BaseVector<ValueType> slam6d_to_riegl_point(const BaseVector<ValueType> &in)
    {
        return {
            in.z   / (ValueType) 100.0,
            - in.x / (ValueType) 100.0,
            in.y   / (ValueType) 100.0
        };
    }



    template <typename ValueType>
    static ValueType deg_to_rad(ValueType deg)
    {
        return M_PI / 180.0 * deg;
    }

    template <typename ValueType>
    static ValueType rad_to_deg(ValueType rad)
    {
        return rad * 180 / M_PI;
    }

    struct ColorVecCompare
    {

        bool operator() (const VecUChar& lhs, const VecUChar& rhs) const
        {
            return (lhs.x < rhs.x) ||
                   (lhs.x == rhs.x && lhs.y < rhs.y) ||
                   (lhs.x == rhs.x && lhs.y == rhs.y && lhs.z < rhs.z);
        }
    };

    template <typename BaseVecT>
    [[nodiscard]]
    static inline Vector3<typename BaseVecT::CoordType> to_eigen(const BaseVecT& vec) noexcept
    {
        return Vector3<typename BaseVecT::CoordType>(
            vec[0],
            vec[1],
            vec[2]
        );
    }
};

} // namespace lvr2

#endif