points.hpp

#ifndef VELODYNEUTILS_POINTS_HPP
#define VELODYNEUTILS_POINTS_HPP

#include <sys/time.h> // Necessary for velodyneLib.h
#include <sys/types.h> // Necessary for velodyneLib.h and libregionMap.h

#include <algorithm>
#include <iostream>

#include <libregionMap.h>
#include <velodyne/velodyneLib.h>

#include <pcl/point_types.h>

struct PointXYZIL {
    PCL_ADD_POINT4D;
    union {
        struct {
            uint32_t label;
            uint8_t intensity;
        };
        float data1[4];
    };

    inline PointXYZIL() {
        x = y = z = 0.0f;
        data[3] = 1.0f;
        intensity = 0;
        label = 0;
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIL,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (uint8_t, intensity, intensity)
                                  (uint32_t, label, label))

inline std::ostream& operator<<(std::ostream& s, const PointXYZIL& point) {
    s << "(" << point.x << "," << point.y << "," << point.z << "), "
      << static_cast<uint32_t>(point.intensity) << ".";
    return s;
}

struct NormalPure {
    PCL_ADD_NORMAL4D;

    inline NormalPure() {
        normal_x = normal_y = normal_z = 0.0f;
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(NormalPure,
                                  (float, normal_x, normal_x)
                                  (float, normal_y, normal_y)
                                  (float, normal_z, normal_z))

inline std::ostream& operator<<(std::ostream& s, const NormalPure& point) {
    s << "(" << point.normal_x << "," << point.normal_y << "," << point.normal_z
      << ").";
    return s;
}

struct PointNormalL {
    PCL_ADD_POINT4D;
    PCL_ADD_NORMAL4D;
    union {
        struct {
            uint32_t label;
        };
        float data1[4];
    };

    inline PointNormalL() {
        x = y = z = 0.0f;
        data[3] = 1.0f;
        normal_x = normal_y = normal_z = 0.0f;
        label = 0;
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointNormalL,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (float, normal_x, normal_x)
                                  (float, normal_y, normal_y)
                                  (float, normal_z, normal_z)
                                  (uint32_t, label, label))

inline std::ostream& operator<<(std::ostream& s, const PointNormalL& point) {
    s << "(" << point.x << "," << point.y << "," << point.z << "), ("
      << point.normal_x << "," << point.normal_y << "," << point.normal_z
      << ").";
    return s;
}

struct PointNormalIL {
    PCL_ADD_POINT4D;
    PCL_ADD_NORMAL4D;
    union {
        struct {
            uint32_t label;
            uint8_t intensity;
        };
        float data1[4];
    };

    inline PointNormalIL() {
        x = y = z = 0.0f;
        data[3] = 1.0f;
        normal_x = normal_y = normal_z = 0.0f;
        intensity = 0;
        label = 0;
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointNormalIL,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (float, normal_x, normal_x)
                                  (float, normal_y, normal_y)
                                  (float, normal_z, normal_z)
                                  (uint8_t, intensity, intensity)
                                  (uint32_t, label, label))

inline std::ostream& operator<<(std::ostream& s, const PointNormalIL& point) {
    s << "(" << point.x << "," << point.y << "," << point.z << "), ("
      << point.normal_x << "," << point.normal_y << "," << point.normal_z
      << "), " << point.intensity << ".";
    return s;
}

struct PointHDL {
    PCL_ADD_POINT4D;
    union {
        struct {
            float label;
            float intensity;
        };
        float data1[4];
    };
    union {
        struct {
            float line;
            float column;
            float packet;
            float block; // Corresponds to enum VELODYNE_BLOCK.
        };
        float data2[4];
    };

    inline PointHDL() {
        x = y = z = 0.0f;
        data[3] = 1.0f;
        intensity = 0.0;
        line = column = 0.0;
        packet = 0.0;
        block = static_cast<float>(VELODYNE_UPPER);
        label = 0.0;
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointHDL,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (float, intensity, intensity)
                                  (float, line, line)
                                  (float, column, column)
                                  (float, packet, packet)
                                  (float, block, block)
                                  (float, label, label))

inline std::ostream& operator<<(std::ostream& s, const PointHDL& point) {
    int intensity = static_cast<int>(point.intensity);
    int line = static_cast<int>(point.line);
    int column = static_cast<int>(point.column);
    int packet = static_cast<int>(point.packet);
    s << "(" << point.x << "," << point.y << "," << point.z << "), "
      << intensity << ", (" << line << "," << column << "), " << packet << ", ";
    if (point.block == static_cast<float>(VELODYNE_UPPER))
        s << "upper block.";
    else
        s << "lower block.";
    return s;
}

struct PointRegionMap {
    PCL_ADD_POINT4D;
    union {
        struct {
            // Features
            float nbPts;
            float meanHDistance;
            float minZ;
            float maxZ;
            float deltaZ;
            float meanZ;
            float varZ;
            float meanDeltaRhoLine;
            float varDeltaRhoLine;
            float meanDeltaRhoLineSmooth;
            float varDeltaRhoLineSmooth;
            float nX;
            float nY;
            float nZ;
            float varNX;
            float varNY;
            float varNZ;
            float normVarN;

            // Features status
            float hasFeatures; // bool

            // Class label
            float label;
        };
        float features[NB_3D_ATTRIBUTES];
        float data1[20];
    };

    inline PointRegionMap() {
        x = y = z = 0.0f;
        data[3] = 1.0f;
        std::fill(features, features + NB_3D_ATTRIBUTES, 0.0f);
        hasFeatures = 0.0f;
        label = 0.0f;
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT(PointRegionMap,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (float, nbPts, nbPts)
                                  (float, meanHDistance, meanHDistance)
                                  (float, minZ, minZ)
                                  (float, maxZ, maxZ)
                                  (float, deltaZ, deltaZ)
                                  (float, meanZ, meanZ)
                                  (float, varZ, varZ)
                                  (float, nX, nX)
                                  (float, nY, nY)
                                  (float, nZ, nZ)
                                  (float, varNX, varNX)
                                  (float, varNY, varNY)
                                  (float, varNZ, varNZ)
                                  (float, normVarN, normVarN)
                                  (float, hasFeatures, hasFeatures)
                                  (float, label, label))

inline std::ostream& operator<<(std::ostream& s, const PointRegionMap& point) {
    s << "(" << point.x << "," << point.y << "," << point.z << "), ";
    if (static_cast<bool>(point.hasFeatures))
        for (int i = 0; i < NB_3D_ATTRIBUTES; ++i)
            s << (i == 0 ? "(" : "") << point.features[i]
              << (i < (NB_3D_ATTRIBUTES - 1) ? ", " : "), ");
    else
        s << "-, ";
    s << point.label;
    return s;
}

#endif // VELODYNEUTILS_POINTS_HPP