GroundSurfaceModel.hh

Go to the documentation of this file.

#ifndef LibMultiSense_GroundSurfaceModelHeader
#define LibMultiSense_GroundSurfaceModelHeader

#include "MultiSense/details/utility/Portability.hh"

namespace crl {
namespace multisense {
namespace details {
namespace wire {

class WIRE_HEADER_ATTRIBS_ GroundSurfaceModelHeader {
public:
    static CRL_CONSTEXPR IdType      ID      = ID_DATA_GROUND_SURFACE_SPLINE_DATA_MESSAGE;
    static CRL_CONSTEXPR VersionType VERSION = 1;

#ifdef SENSORPOD_FIRMWARE
    IdType      id;
    VersionType version;
#endif // SENSORPOD_FIRMWARE

    //
    // Frame ID timestamp and success flag of images that the algorithm was processed on

    int64_t     frameId;
    int64_t     timestamp;
    uint8_t     success;

    //
    // Control points dynamic array (treated as image) members

    uint32_t    controlPointsBitsPerPixel;
    uint32_t    controlPointsWidth;
    uint32_t    controlPointsHeight;

    //
    // Spline origin and size details

    float xzCellOrigin[2];
    float xzCellSize[2];
    float xzLimit[2];
    float minMaxAzimuthAngle[2];

    //
    // Extrinsic calibration that was used during the computation of the spline

    float extrinsics[6];

    //
    // Ground Base Model (ax^2 + by^2 + cxy + dx + ey + f)

    float quadraticParams[6];

    //
    // Constructors
    GroundSurfaceModelHeader() :
#ifdef SENSORPOD_FIRMWARE
        id(ID),
        version(VERSION),
#endif // SENSORPOD_FIRMWARE
        frameId(0),
        timestamp(0),
        success(0),
        controlPointsBitsPerPixel(0),
        controlPointsWidth(0),
        controlPointsHeight(0)
    {};
};

#ifndef SENSORPOD_FIRMWARE

class GroundSurfaceModel : public GroundSurfaceModelHeader {
public:

    void *controlPointsDataP;

    //
    // Constructors

    GroundSurfaceModel(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);};
    GroundSurfaceModel() : controlPointsDataP(NULL) {};

    //
    // Serialization routine

    template<class Archive>
        void serialize(Archive&          message,
                       const VersionType version)
    {
        (void) version;

        message & frameId;
        message & timestamp;
        message & success;

        message & controlPointsBitsPerPixel;
        message & controlPointsWidth;
        message & controlPointsHeight;

        for (unsigned int i = 0; i < 2; i++)
            message & xzCellOrigin[i];

        for (unsigned int i = 0; i < 2; i++)
            message & xzCellSize[i];

        for (unsigned int i = 0; i < 2; i++)
            message & xzLimit[i];

        for (unsigned int i = 0; i < 2; i++)
            message & minMaxAzimuthAngle[i];

        for (unsigned int i = 0; i < 6; i++)
            message & extrinsics[i];

        for (unsigned int i = 0; i < 6; i++)
            message & quadraticParams[i];

        const uint32_t imageSize = static_cast<uint32_t> (std::ceil(((double) controlPointsBitsPerPixel / 8.0) * controlPointsWidth * controlPointsHeight));

        if (typeid(Archive) == typeid(utility::BufferStreamWriter)) {

            message.write(controlPointsDataP, imageSize);

        } else {

            controlPointsDataP = message.peek();
            message.seek(message.tell() + imageSize);
        }
    }
};

#endif // !SENSORPOD_FIRMWARE

}}}} // namespaces

#endif