Util.hpp

Go to the documentation of this file.

 
/*
 * 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/io/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);
        }
    };
};
 
} // namespace lvr2
 
#endif