DisparityMessage.h
Go to the documentation of this file.
00001 
00038 #ifndef LibMultiSense_DisparityMessage
00039 #define LibMultiSense_DisparityMessage
00040 
00041 #include <typeinfo>
00042 #include <cmath>
00043 
00044 #include "details/utility/Portability.hh"
00045 
00046 namespace crl {
00047 namespace multisense {
00048 namespace details {
00049 namespace wire {
00050 
00051 class WIRE_HEADER_ATTRIBS_ DisparityHeader {
00052 public:
00053 
00054     static CRL_CONSTEXPR IdType      ID      = ID_DATA_DISPARITY;
00055     static CRL_CONSTEXPR VersionType VERSION = 1;
00056 
00057     static CRL_CONSTEXPR uint8_t  WIRE_BITS_PER_PIXEL = 12;
00058     static CRL_CONSTEXPR uint8_t  WIRE_BYTE_ALIGNMENT = 3;
00059     static CRL_CONSTEXPR uint8_t  API_BITS_PER_PIXEL  = 16; // after custom assemble()
00060     static CRL_CONSTEXPR uint32_t META_LENGTH         = 16; // packed, includes type/version
00061 
00062 #ifdef SENSORPOD_FIRMWARE
00063     IdType      id;
00064     VersionType version;
00065 #endif // SENSORPOD_FIRMWARE
00066 
00067     int64_t  frameId;
00068     uint16_t width;
00069     uint16_t height;
00070 
00071     DisparityHeader() 
00072         : 
00073 #ifdef SENSORPOD_FIRMWARE
00074         id(ID),
00075         version(VERSION),
00076 #endif // SENSORPOD_FIRMWARE
00077         frameId(0),
00078         width(0),
00079         height(0) {};
00080 };
00081 
00082 #ifndef SENSORPOD_FIRMWARE
00083 
00084 class Disparity : public DisparityHeader {
00085 public:
00086 
00087     void *dataP;
00088 
00089     //
00090     // Constructors
00091 
00092     Disparity(utility::BufferStreamReader&r, VersionType v) {serialize(r,v);};
00093     Disparity() : dataP(NULL) {};
00094   
00095     //
00096     // Serialization routine
00097 
00098     template<class Archive>
00099         void serialize(Archive&          message,
00100                        const VersionType version)
00101     {
00102         message & frameId;
00103         message & width;
00104         message & height;
00105 
00106         const uint32_t imageSize = static_cast<uint32_t> (std::ceil(((double) API_BITS_PER_PIXEL / 8.0) * width * height));
00107 
00108         if (typeid(Archive) == typeid(utility::BufferStreamWriter)) {
00109           
00110             message.write(dataP, imageSize);
00111 
00112         } else {
00113 
00114             dataP = message.peek();
00115             message.seek(message.tell() + imageSize);
00116         }
00117     }
00118 
00119     //
00120     // UDP assembler 
00121 
00122     static void assembler(utility::BufferStreamWriter& stream,
00123                           const uint8_t               *dataP,
00124                           uint32_t                     offset,
00125                           uint32_t                     length)
00126     {   
00127         //
00128         // Special case, 1st packet contains header only. Firmware
00129         // does not have to worry about the header length being
00130         // byte-aligned on a WIRE_BITS_PER_PIXEL boundary
00131 
00132         if (0 == offset) {
00133             stream.seek(0);
00134             stream.write(dataP, META_LENGTH);
00135             return;
00136         }
00137     
00138         //
00139         // The data section of each incoming packet is byte-aligned 
00140         // on a WIRE_BITS_PER_PIXEL boundary
00141 
00142         const uint8_t *sP           = dataP;
00143         const uint32_t sourceOffset = offset - META_LENGTH;
00144         const uint32_t count        = (8 * length) / WIRE_BITS_PER_PIXEL;
00145         const uint32_t destOffset   = META_LENGTH + (((8 * sourceOffset) / WIRE_BITS_PER_PIXEL) * 
00146                                                      (API_BITS_PER_PIXEL / 8));
00147         //
00148         // Seek to the proper location
00149 
00150         stream.seek(destOffset);
00151        
00152         //
00153         // This conversion is for (WIRE == 12bits), (API == 16bits, 1/16th pixel, unsigned integer)
00154 
00155         if (12 == WIRE_BITS_PER_PIXEL && 16 == API_BITS_PER_PIXEL) {
00156 
00157             uint16_t *dP = reinterpret_cast<uint16_t*>(stream.peek());
00158 
00159             for(uint32_t i=0; i<count; i+=2, sP+=3) {
00160                 dP[i]   = ((sP[0]     ) | ((sP[1] & 0x0F) << 8));
00161                 dP[i+1] = ((sP[1] >> 4) |  (sP[2] << 4)        );
00162             }
00163 
00164         //
00165         // This conversion is for (WIRE == 12bits), (API == 32bits, floating point)
00166 
00167         } else if (12 == WIRE_BITS_PER_PIXEL && 32 == API_BITS_PER_PIXEL) {
00168 
00169             float *dP = reinterpret_cast<float*>(stream.peek());
00170         
00171             for(uint32_t i=0; i<count; i+=2, sP+=3) {
00172                 
00173                 dP[i]   = static_cast<float>((sP[0]     ) | ((sP[1] & 0x0F) << 8)) / 16.0f;
00174                 dP[i+1] = static_cast<float>((sP[1] >> 4) |  (sP[2] << 4)        ) / 16.0f;
00175             }
00176 
00177         } else 
00178             CRL_EXCEPTION("(wire==%dbits, api=%dbits) not supported",
00179                           WIRE_BITS_PER_PIXEL, API_BITS_PER_PIXEL);
00180     }
00181 };
00182 
00183 #endif // !SENSORPOD_FIRMWARE
00184 
00185 }}}}; // namespaces
00186 
00187 #endif


multisense_lib
Author(s):
autogenerated on Thu Aug 27 2015 14:01:11