GroundSurfaceModel.hh

Go to the documentation of this file.

 
#ifndef LibMultiSense_GroundSurfaceModelHeader
#define LibMultiSense_GroundSurfaceModelHeader
 
#include "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