DisparityMessage.hh

Go to the documentation of this file.

 
#ifndef LibMultiSense_DisparityMessage
#define LibMultiSense_DisparityMessage
 
#include <typeinfo>
#include <cmath>
 
#include "utility/Portability.hh"
 
namespace crl {
namespace multisense {
namespace details {
namespace wire {
 
class WIRE_HEADER_ATTRIBS_ DisparityHeader {
public:
 
    static CRL_CONSTEXPR IdType      ID      = ID_DATA_DISPARITY;
    static CRL_CONSTEXPR VersionType VERSION = 1;
 
    static CRL_CONSTEXPR uint8_t  WIRE_BITS_PER_PIXEL = MULTISENSE_WIRE_BITS_PER_PIXEL;
    static CRL_CONSTEXPR uint8_t  WIRE_BYTE_ALIGNMENT = 3;
    static CRL_CONSTEXPR uint8_t  API_BITS_PER_PIXEL  = MULTISENSE_API_BITS_PER_PIXEL; // after custom assemble()
    static CRL_CONSTEXPR uint32_t META_LENGTH         = 16; // packed, includes type/version
 
#ifdef SENSORPOD_FIRMWARE
    IdType      id;
    VersionType version;
#endif // SENSORPOD_FIRMWARE
 
    int64_t  frameId;
    uint16_t width;
    uint16_t height;
 
    DisparityHeader()
        :
#ifdef SENSORPOD_FIRMWARE
        id(ID),
        version(VERSION),
#endif // SENSORPOD_FIRMWARE
        frameId(0),
        width(0),
        height(0) {};
};
 
#ifndef SENSORPOD_FIRMWARE
 
class Disparity : public DisparityHeader {
public:
 
    void *dataP;
 
    //
    // Constructors
 
    Disparity(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);};
    Disparity() : dataP(NULL) {};
 
    //
    // Serialization routine
 
    template<class Archive>
        void serialize(Archive&          message,
                       const VersionType version)
    {
        (void) version;
        message & frameId;
        message & width;
        message & height;
 
        const uint32_t imageSize = static_cast<uint32_t> (std::ceil(((double) API_BITS_PER_PIXEL / 8.0) * width * height));
 
        if (typeid(Archive) == typeid(utility::BufferStreamWriter)) {
 
            message.write(dataP, imageSize);
 
        } else {
 
            dataP = message.peek();
            message.seek(message.tell() + imageSize);
        }
    }
 
    //
    // UDP assembler
 
    static void assembler(utility::BufferStreamWriter& stream,
                          const uint8_t               *dataP,
                          uint32_t                     offset,
                          uint32_t                     length)
    {
        //
        // Special case, 1st packet contains header only. Firmware
        // does not have to worry about the header length being
        // byte-aligned on a WIRE_BITS_PER_PIXEL boundary
 
        if (0 == offset) {
            stream.seek(0);
            stream.write(dataP, META_LENGTH);
            return;
        }
 
        //
        // The data section of each incoming packet is byte-aligned
        // on a WIRE_BITS_PER_PIXEL boundary
 
        const uint8_t *sP           = dataP;
        const uint32_t sourceOffset = offset - META_LENGTH;
        const uint32_t count        = (8 * length) / WIRE_BITS_PER_PIXEL;
        const uint32_t destOffset   = META_LENGTH + (((8 * sourceOffset) / WIRE_BITS_PER_PIXEL) *
                                                     (API_BITS_PER_PIXEL / 8));
        //
        // Seek to the proper location
 
        stream.seek(destOffset);
 
#if MULTISENSE_WIRE_BITS_PER_PIXEL == 12 && MULTISENSE_API_BITS_PER_PIXEL == 16
        //
        // This conversion is for (WIRE == 12bits), (API == 16bits, 1/16th pixel, unsigned integer)
 
            uint16_t *dP = reinterpret_cast<uint16_t*>(stream.peek());
 
            for(uint32_t i=0; i<count; i+=2, sP+=3) {
                dP[i]   = ((sP[0]     ) | ((sP[1] & 0x0F) << 8));
                dP[i+1] = ((sP[1] >> 4) |  (sP[2] << 4)        );
            }
 
#elif MULTISENSE_WIRE_BITS_PER_PIXEL == 12 && MULTISENSE_API_BITS_PER_PIXEL == 32
        //
        // This conversion is for (WIRE == 12bits), (API == 32bits, floating point)
 
            float *dP = reinterpret_cast<float*>(stream.peek());
 
            for(uint32_t i=0; i<count; i+=2, sP+=3) {
 
                dP[i]   = static_cast<float>((sP[0]     ) | ((sP[1] & 0x0F) << 8)) / 16.0f;
                dP[i+1] = static_cast<float>((sP[1] >> 4) |  (sP[2] << 4)        ) / 16.0f;
            }
 
#else
#error MULTISENSE_WIRE_BITS_PER_PIXEL and MULTISENSE_API_BITS_PER_PIXEL not supported
#endif
 
    }
};
 
#endif // !SENSORPOD_FIRMWARE
 
}}}} // namespaces
 
#endif