DisparityMessage.h

Go to the documentation of this file.

#ifndef LibMultiSense_DisparityMessage
#define LibMultiSense_DisparityMessage

#include <typeinfo>
#include <cmath>

#include "details/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 = 12;
    static CRL_CONSTEXPR uint8_t  WIRE_BYTE_ALIGNMENT = 3;
    static CRL_CONSTEXPR uint8_t  API_BITS_PER_PIXEL  = 16; // 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)
    {
        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);
       
        //
        // This conversion is for (WIRE == 12bits), (API == 16bits, 1/16th pixel, unsigned integer)

        if (12 == WIRE_BITS_PER_PIXEL && 16 == API_BITS_PER_PIXEL) {

            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)        );
            }

        //
        // This conversion is for (WIRE == 12bits), (API == 32bits, floating point)

        } else if (12 == WIRE_BITS_PER_PIXEL && 32 == API_BITS_PER_PIXEL) {

            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 
            CRL_EXCEPTION("(wire==%dbits, api=%dbits) not supported",
                          WIRE_BITS_PER_PIXEL, API_BITS_PER_PIXEL);
    }
};

#endif // !SENSORPOD_FIRMWARE

}}}}; // namespaces

#endif