types.h
Go to the documentation of this file.
00001 // License: Apache 2.0. See LICENSE file in root directory.
00002 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
00003 
00004 // This header defines vocabulary types and utility mechanisms used ubiquitously by the
00005 // rest of the library. As clearer module boundaries form, declarations might be moved
00006 // out of this file and into more appropriate locations.
00007 
00008 #pragma once
00009 #ifndef LIBREALSENSE_TYPES_H
00010 #define LIBREALSENSE_TYPES_H
00011 
00012 #include "../include/librealsense/rs.h"     // Inherit all type definitions in the public API
00013 #include "../include/librealsense/rscore.hpp" // Inherit public interfaces
00014 
00015 #include <cassert>                          // For assert
00016 #include <cstring>                          // For memcmp
00017 #include <vector>                           // For vector
00018 #include <sstream>                          // For ostringstream
00019 #include <mutex>                            // For mutex, unique_lock
00020 #include <condition_variable>               // For condition_variable
00021 #include <memory>                           // For unique_ptr
00022 #include <atomic>
00023 #include <map>          
00024 #include <algorithm>
00025 #include <functional>
00026 
00027 const uint8_t RS_STREAM_NATIVE_COUNT    = 5;
00028 const int RS_USER_QUEUE_SIZE = 20;
00029 
00030 // Timestamp syncronization settings:
00031 const int RS_MAX_EVENT_QUEUE_SIZE = 500;  // Max number of timestamp events to keep for all streams
00032 const int RS_MAX_EVENT_TIME_OUT = 20;     // Max timeout in milliseconds that a frame can wait for its corresponding timestamp event
00033 // Usually timestamp events arrive much faster then frames, but due to USB arbitration the QoS isn't guaranteed.
00034 // RS_MAX_EVENT_TIME_OUT controls how much time the user is willing to wait before "giving-up" on a particular frame
00035 
00036 namespace rsimpl
00037 {
00039     // Utility types for general use //
00041 
00042     typedef uint8_t byte;
00043 
00044     struct to_string
00045     {
00046         std::ostringstream ss;
00047         template<class T> to_string & operator << (const T & val) { ss << val; return *this; }
00048         operator std::string() const { return ss.str(); }
00049     };
00050 
00051 #pragma pack(push, 1)
00052     template<class T> class big_endian
00053     {
00054         T be_value;
00055     public:
00056         operator T () const
00057         {
00058             T le_value = 0;
00059             for (unsigned int i = 0; i < sizeof(T); ++i) reinterpret_cast<char *>(&le_value)[i] = reinterpret_cast<const char *>(&be_value)[sizeof(T) - i - 1];
00060             return le_value;
00061         }
00062     };
00063 #pragma pack(pop)
00064 
00066     // Logging mechanism //
00068 
00069     void log(rs_log_severity severity, const std::string & message);
00070     void log_to_console(rs_log_severity min_severity);
00071     void log_to_file(rs_log_severity min_severity, const char * file_path);
00072     void log_to_callback(rs_log_severity min_severity, rs_log_callback * callback);
00073     void log_to_callback(rs_log_severity min_severity, void(*on_log)(rs_log_severity min_severity, const char * message, void * user), void * user);
00074     rs_log_severity get_minimum_severity();
00075 
00076 #define LOG(SEVERITY, ...) do { if(static_cast<int>(SEVERITY) >= rsimpl::get_minimum_severity()) { std::ostringstream ss; ss << __VA_ARGS__; rsimpl::log(SEVERITY, ss.str()); } } while(false)
00077 #define LOG_DEBUG(...)   LOG(RS_LOG_SEVERITY_DEBUG, __VA_ARGS__)
00078 #define LOG_INFO(...)    LOG(RS_LOG_SEVERITY_INFO,  __VA_ARGS__)
00079 #define LOG_WARNING(...) LOG(RS_LOG_SEVERITY_WARN,  __VA_ARGS__)
00080 #define LOG_ERROR(...)   LOG(RS_LOG_SEVERITY_ERROR, __VA_ARGS__)
00081 #define LOG_FATAL(...)   LOG(RS_LOG_SEVERITY_FATAL, __VA_ARGS__)
00082 
00084     // Enumerated type support //
00086 
00087 #define RS_ENUM_HELPERS(TYPE, PREFIX) const char * get_string(TYPE value); \
00088         inline bool is_valid(TYPE value) { return value >= 0 && value < RS_##PREFIX##_COUNT; } \
00089         inline std::ostream & operator << (std::ostream & out, TYPE value) { if(is_valid(value)) return out << get_string(value); else return out << (int)value; }
00090     RS_ENUM_HELPERS(rs_stream, STREAM)
00091     RS_ENUM_HELPERS(rs_format, FORMAT)
00092     RS_ENUM_HELPERS(rs_preset, PRESET)
00093     RS_ENUM_HELPERS(rs_distortion, DISTORTION)
00094     RS_ENUM_HELPERS(rs_option, OPTION)
00095     RS_ENUM_HELPERS(rs_capabilities, CAPABILITIES)
00096     RS_ENUM_HELPERS(rs_source, SOURCE)
00097     RS_ENUM_HELPERS(rs_output_buffer_format, OUTPUT_BUFFER_FORMAT)
00098     RS_ENUM_HELPERS(rs_event_source, EVENT_SOURCE)
00099     RS_ENUM_HELPERS(rs_blob_type, BLOB_TYPE)
00100     RS_ENUM_HELPERS(rs_camera_info, CAMERA_INFO)
00101     RS_ENUM_HELPERS(rs_timestamp_domain, TIMESTAMP_DOMAIN)
00102     RS_ENUM_HELPERS(rs_frame_metadata, FRAME_METADATA)
00103     #undef RS_ENUM_HELPERS
00104 
00106     // World's tiniest linear algebra library //
00108 
00109     struct int2 { int x,y; };
00110     struct float3 { float x,y,z; float & operator [] (int i) { return (&x)[i]; } };
00111     struct float3x3 { float3 x,y,z; float & operator () (int i, int j) { return (&x)[j][i]; } }; // column-major
00112     struct pose { float3x3 orientation; float3 position; };
00113     inline bool operator == (const float3 & a, const float3 & b) { return a.x==b.x && a.y==b.y && a.z==b.z; }
00114     inline float3 operator + (const float3 & a, const float3 & b) { return {a.x+b.x, a.y+b.y, a.z+b.z}; }
00115     inline float3 operator * (const float3 & a, float b) { return {a.x*b, a.y*b, a.z*b}; }
00116     inline bool operator == (const float3x3 & a, const float3x3 & b) { return a.x==b.x && a.y==b.y && a.z==b.z; }
00117     inline float3 operator * (const float3x3 & a, const float3 & b) { return a.x*b.x + a.y*b.y + a.z*b.z; }
00118     inline float3x3 operator * (const float3x3 & a, const float3x3 & b) { return {a*b.x, a*b.y, a*b.z}; }
00119     inline float3x3 transpose(const float3x3 & a) { return {{a.x.x,a.y.x,a.z.x}, {a.x.y,a.y.y,a.z.y}, {a.x.z,a.y.z,a.z.z}}; }
00120     inline bool operator == (const pose & a, const pose & b) { return a.orientation==b.orientation && a.position==b.position; }
00121     inline float3 operator * (const pose & a, const float3 & b) { return a.orientation * b + a.position; }
00122     inline pose operator * (const pose & a, const pose & b) { return {a.orientation * b.orientation, a * b.position}; }
00123     inline pose inverse(const pose & a) { auto inv = transpose(a.orientation); return {inv, inv * a.position * -1}; }
00124 
00126     // Pixel formats //
00128 
00129     struct pixel_format_unpacker
00130     {
00131         bool requires_processing;
00132         void(*unpack)(byte * const dest[], const byte * source, int count);
00133         std::vector<std::pair<rs_stream, rs_format>> outputs;
00134 
00135         bool provides_stream(rs_stream stream) const { for (auto & o : outputs) if (o.first == stream) return true; return false; }
00136         rs_format get_format(rs_stream stream) const { for (auto & o : outputs) if (o.first == stream) return o.second; throw std::logic_error("missing output"); }
00137     };
00138 
00139     struct native_pixel_format
00140     {
00141         uint32_t fourcc;
00142         int plane_count;
00143         size_t bytes_per_pixel;
00144         std::vector<pixel_format_unpacker> unpackers;
00145 
00146         size_t get_image_size(int width, int height) const { return width * height * plane_count * bytes_per_pixel; }
00147 
00148     };
00149 
00151     // Static camera info //
00153 
00154     struct subdevice_mode
00155     {
00156         int subdevice;                          // 0, 1, 2, etc...
00157         int2 native_dims;                       // Resolution advertised over UVC
00158         native_pixel_format pf;                 // Pixel format advertised over UVC
00159         int fps;                                // Framerate advertised over UVC
00160         rs_intrinsics native_intrinsics;        // Intrinsics structure corresponding to the content of image (Note: width,height may be subset of native_dims)
00161         std::vector<rs_intrinsics> rect_modes;  // Potential intrinsics of image after being rectified in software by librealsense
00162         std::vector<int> pad_crop_options;      // Acceptable padding/cropping values
00163     };
00164 
00165     struct stream_request
00166     {
00167         bool enabled;
00168         int width, height;
00169         rs_format format;
00170         int fps;
00171         rs_output_buffer_format output_format;
00172 
00173         bool contradict(stream_request req) const;
00174         bool is_filled() const;
00175     };
00176 
00177     struct interstream_rule // Requires a.*field + delta == b.*field OR a.*field + delta2 == b.*field
00178     {
00179         rs_stream a, b;
00180         int stream_request::* field;
00181         int delta, delta2;
00182         rs_stream bigger;       // if this equals to a or b, this stream must have field value bigger then the other stream
00183         bool divides, divides2; // divides = a must divide b; divides2 = b must divide a
00184         bool same_format;
00185     };
00186 
00187     struct supported_option
00188     {
00189         rs_option option;
00190         double min, max, step, def;
00191     };
00192 
00193     struct data_polling_request
00194     {
00195         bool        enabled;
00196         
00197         data_polling_request(): enabled(false) {};
00198     };
00199 
00200     class firmware_version
00201     {
00202         int                 m_major, m_minor, m_patch, m_build;
00203         bool                is_any;
00204         std::string         string_representation;
00205 
00206         std::string to_string() const;
00207         static std::vector<std::string> split(const std::string& str);
00208         static int parse_part(const std::string& name, int part);
00209 
00210     public:
00211         firmware_version() : m_major(0), m_minor(0), m_patch(0), m_build(0), is_any(true), string_representation(to_string()) {}
00212 
00213         firmware_version(int major, int minor, int patch, int build, bool is_any = false)
00214             : m_major(major), m_minor(minor), m_patch(patch), m_build(build), is_any(is_any), string_representation(to_string()) {}
00215 
00216         static firmware_version any()
00217         {
00218             return{};
00219         }
00220 
00221         explicit firmware_version(const std::string& name)
00222             : m_major(parse_part(name, 0)), m_minor(parse_part(name, 1)), m_patch(parse_part(name, 2)), m_build(parse_part(name, 3)), is_any(false), string_representation(to_string()) {}
00223 
00224         bool operator<=(const firmware_version& other) const
00225         {
00226             if (is_any || other.is_any) return true;
00227             if (m_major > other.m_major) return false;
00228             if ((m_major == other.m_major) && (m_minor > other.m_minor)) return false;
00229             if ((m_major == other.m_major) && (m_minor == other.m_minor) && (m_patch > other.m_patch)) return false;
00230             if ((m_major == other.m_major) && (m_minor == other.m_minor) && (m_patch == other.m_patch) && (m_build > other.m_build)) return false;
00231             return true;
00232         }
00233         bool operator==(const firmware_version& other) const
00234         {
00235             return is_any || (other.m_major == m_major && other.m_minor == m_minor && other.m_patch == m_patch && other.m_build == m_build);
00236         }
00237 
00238         bool operator> (const firmware_version& other) const { return !(*this < other) || is_any; }
00239         bool operator!=(const firmware_version& other) const { return !(*this == other); }
00240         bool operator<(const firmware_version& other) const { return !(*this == other) && (*this <= other); }
00241         bool operator>=(const firmware_version& other) const { return (*this == other) || (*this > other); }
00242 
00243         bool is_between(const firmware_version& from, const firmware_version& until)
00244         {
00245             return (from <= *this) && (*this <= until);
00246         }
00247 
00248         operator const char*() const
00249         {
00250             return string_representation.c_str();
00251         }
00252     };
00253 
00254     struct supported_capability
00255     {
00256         rs_capabilities     capability;
00257         firmware_version    from;
00258         firmware_version    until;
00259         rs_camera_info      firmware_type;
00260 
00261         supported_capability(rs_capabilities capability, firmware_version from, firmware_version until, rs_camera_info firmware_type = RS_CAMERA_INFO_CAMERA_FIRMWARE_VERSION)
00262             : capability(capability), from(from), until(until), firmware_type(firmware_type) {}
00263         
00264         supported_capability(rs_capabilities capability) 
00265             : capability(capability), from(), until(), firmware_type(RS_CAMERA_INFO_CAMERA_FIRMWARE_VERSION) {}
00266     };
00267 
00268     struct static_device_info
00269     {
00270         std::string name;                                                   // Model name of the camera
00271         int stream_subdevices[RS_STREAM_NATIVE_COUNT];                      // Which subdevice is used to support each stream, or -1 if stream is unavailable
00272         int data_subdevices[RS_STREAM_NATIVE_COUNT];                        // Specify whether the subdevice supports events pipe in addition to streaming, -1 if data channels are unavailable
00273         std::vector<subdevice_mode> subdevice_modes;                        // A list of available modes each subdevice can be put into
00274         std::vector<interstream_rule> interstream_rules;                    // Rules which constrain the set of available modes
00275         stream_request presets[RS_STREAM_NATIVE_COUNT][RS_PRESET_COUNT];    // Presets available for each stream
00276         std::vector<supported_option> options;
00277         pose stream_poses[RS_STREAM_NATIVE_COUNT];                          // Static pose of each camera on the device
00278         int num_libuvc_transfer_buffers;                                    // Number of transfer buffers to use in LibUVC backend
00279         std::string firmware_version;                                       // Firmware version string
00280         std::string serial;                                                 // Serial number of the camera (from USB or from SPI memory)
00281         float nominal_depth_scale;                                          // Default scale
00282         std::vector<supported_capability> capabilities_vector;
00283         std::vector<rs_frame_metadata> supported_metadata_vector;
00284         std::map<rs_camera_info, std::string> camera_info;
00285 
00286         static_device_info();
00287     };
00288 
00290     // Runtime device configuration //
00292 
00293     struct subdevice_mode_selection
00294     {
00295         subdevice_mode mode;                    // The streaming mode in which to place the hardware
00296         int pad_crop;                           // The number of pixels of padding (positive values) or cropping (negative values) to apply to all four edges of the image
00297         size_t unpacker_index;                  // The specific unpacker used to unpack the encoded format into the desired output formats
00298         rs_output_buffer_format output_format = RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS; // The output buffer format. 
00299 
00300         subdevice_mode_selection() : mode({}), pad_crop(), unpacker_index(), output_format(RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS){}
00301         subdevice_mode_selection(const subdevice_mode & mode, int pad_crop, int unpacker_index) : mode(mode), pad_crop(pad_crop), unpacker_index(unpacker_index){}
00302 
00303         const pixel_format_unpacker & get_unpacker() const {
00304             if ((size_t)unpacker_index < mode.pf.unpackers.size())
00305                 return mode.pf.unpackers[unpacker_index];
00306             throw std::runtime_error("failed to fetch an unpakcer, most likely because enable_stream was not called!");
00307         }
00308         const std::vector<std::pair<rs_stream, rs_format>> & get_outputs() const { return get_unpacker().outputs; }
00309         int get_width() const { return mode.native_intrinsics.width + pad_crop * 2; }
00310         int get_height() const { return mode.native_intrinsics.height + pad_crop * 2; }
00311         int get_framerate() const { return mode.fps; }
00312         int get_stride_x() const { return requires_processing() ? get_width() : mode.native_dims.x; }
00313         int get_stride_y() const { return requires_processing() ? get_height() : mode.native_dims.y; }
00314         size_t get_image_size(rs_stream stream) const;
00315         bool provides_stream(rs_stream stream) const { return get_unpacker().provides_stream(stream); }
00316         rs_format get_format(rs_stream stream) const { return get_unpacker().get_format(stream); }
00317         void set_output_buffer_format(const rs_output_buffer_format in_output_format);
00318 
00319         void unpack(byte * const dest[], const byte * source) const;
00320         int get_unpacked_width() const;
00321         int get_unpacked_height() const;
00322 
00323         bool requires_processing() const { return (output_format == RS_OUTPUT_BUFFER_FORMAT_CONTINUOUS) || (mode.pf.unpackers[unpacker_index].requires_processing); }
00324 
00325     };
00326 
00327     typedef void(*frame_callback_function_ptr)(rs_device * dev, rs_frame_ref * frame, void * user);
00328     typedef void(*motion_callback_function_ptr)(rs_device * dev, rs_motion_data data, void * user);
00329     typedef void(*timestamp_callback_function_ptr)(rs_device * dev, rs_timestamp_data data, void * user);
00330     typedef void(*log_callback_function_ptr)(rs_log_severity severity, const char * message, void * user);
00331 
00332     class frame_callback : public rs_frame_callback
00333     {
00334         frame_callback_function_ptr fptr;
00335         void * user;
00336         rs_device * device;
00337     public:
00338         frame_callback() : frame_callback(nullptr, nullptr, nullptr) {}
00339         frame_callback(rs_device * dev, frame_callback_function_ptr on_frame, void * user) : fptr(on_frame), user(user), device(dev) {}
00340 
00341         operator bool() { return fptr != nullptr; }
00342         void on_frame (rs_device * dev, rs_frame_ref * frame) override { 
00343             if (fptr)
00344             {
00345                 try { fptr(dev, frame, user); } catch (...) 
00346                 {
00347                     LOG_ERROR("Received an execption from frame callback!");
00348                 }
00349             }
00350         }
00351         void release() override { delete this; }
00352     };
00353 
00354     class motion_events_callback : public rs_motion_callback
00355     {
00356         motion_callback_function_ptr fptr;
00357         void        * user;
00358         rs_device   * device;
00359     public:
00360         motion_events_callback() : motion_events_callback(nullptr, nullptr, nullptr) {}
00361         motion_events_callback(rs_device * dev, motion_callback_function_ptr fptr, void * user) : fptr(fptr), user(user), device(dev) {}
00362 
00363         operator bool() { return fptr != nullptr; }
00364 
00365         void on_event(rs_motion_data data) override
00366         {
00367             if (fptr)
00368             {
00369                 try { fptr(device, data, user); } catch (...)
00370                 {
00371                     LOG_ERROR("Received an execption from motion events callback!");
00372                 }
00373             }
00374         }
00375 
00376         void release() override { }
00377     };
00378 
00379     class timestamp_events_callback : public rs_timestamp_callback
00380     {
00381         timestamp_callback_function_ptr fptr;
00382         void        * user;
00383         rs_device   * device;
00384     public:
00385         timestamp_events_callback() : timestamp_events_callback(nullptr, nullptr, nullptr) {}
00386         timestamp_events_callback(rs_device * dev, timestamp_callback_function_ptr fptr, void * user) : fptr(fptr), user(user), device(dev) {}
00387 
00388         operator bool() { return fptr != nullptr; }
00389         void on_event(rs_timestamp_data data) override {
00390             if (fptr)
00391             {
00392                 try { fptr(device, data, user); } catch (...) 
00393                 {
00394                     LOG_ERROR("Received an execption from timestamp events callback!");
00395                 }
00396             }
00397         }
00398         void release() override { }
00399     };
00400 
00401     class log_callback : public rs_log_callback
00402     {
00403         log_callback_function_ptr fptr;
00404         void        * user;
00405     public:
00406         log_callback() : log_callback(nullptr, nullptr) {}
00407         log_callback(log_callback_function_ptr fptr, void * user) : fptr(fptr), user(user) {}
00408 
00409         operator bool() { return fptr != nullptr; }
00410 
00411         void on_event(rs_log_severity severity, const char * message) override
00412         {
00413             if (fptr)
00414             {
00415                 try { fptr(severity, message, user); }
00416                 catch (...)
00417                 {
00418                     LOG_ERROR("Received an execption from log callback!");
00419                 }
00420             }
00421         }
00422 
00423         void release() override { }
00424     };
00425 
00426     typedef std::unique_ptr<rs_log_callback, void(*)(rs_log_callback*)> log_callback_ptr;
00427     typedef std::unique_ptr<rs_motion_callback, void(*)(rs_motion_callback*)> motion_callback_ptr;
00428     typedef std::unique_ptr<rs_timestamp_callback, void(*)(rs_timestamp_callback*)> timestamp_callback_ptr;
00429     class frame_callback_ptr
00430     {
00431         rs_frame_callback * callback;
00432     public:
00433         frame_callback_ptr() : callback(nullptr) {}
00434         explicit frame_callback_ptr(rs_frame_callback * callback) : callback(callback) {}
00435         frame_callback_ptr(const frame_callback_ptr&) = delete;
00436         frame_callback_ptr& operator =(frame_callback_ptr&& other)
00437         {
00438             if (callback) callback->release();
00439             callback = other.callback;
00440             other.callback = nullptr;
00441             return *this;
00442         }
00443         ~frame_callback_ptr() { if (callback) callback->release(); }
00444         operator rs_frame_callback *() { return callback; }
00445         rs_frame_callback * operator*() { return callback; }
00446     };
00447 
00448     struct device_config
00449     {
00450         const static_device_info            info;
00451         stream_request                      requests[RS_STREAM_NATIVE_COUNT];                       // Modified by enable/disable_stream calls
00452         frame_callback_ptr                  callbacks[RS_STREAM_NATIVE_COUNT];                      // Modified by set_frame_callback calls
00453         data_polling_request                data_request;                                           // Modified by enable/disable_events calls
00454         motion_callback_ptr                 motion_callback{ nullptr, [](rs_motion_callback*){} };  // Modified by set_events_callback calls
00455         timestamp_callback_ptr              timestamp_callback{ nullptr, [](rs_timestamp_callback*){} };
00456         float depth_scale;                                              // Scale of depth values
00457 
00458         explicit device_config(const rsimpl::static_device_info & info) : info(info), depth_scale(info.nominal_depth_scale)
00459         {
00460             for (auto & req : requests) req = rsimpl::stream_request();
00461         }
00462 
00463         subdevice_mode_selection select_mode(const stream_request(&requests)[RS_STREAM_NATIVE_COUNT], int subdevice_index) const;
00464         bool all_requests_filled(const stream_request(&original_requests)[RS_STREAM_NATIVE_COUNT]) const;
00465         bool find_good_requests_combination(stream_request(&output_requests)[RS_STREAM_NATIVE_COUNT], std::vector<stream_request> stream_requests[RS_STREAM_NATIVE_COUNT]) const;
00466         bool fill_requests(stream_request(&requests)[RS_STREAM_NATIVE_COUNT]) const;
00467         void get_all_possible_requestes(std::vector<stream_request> (&stream_requests)[RS_STREAM_NATIVE_COUNT]) const;
00468         std::vector<subdevice_mode_selection> select_modes(const stream_request(&requests)[RS_STREAM_NATIVE_COUNT]) const;
00469         std::vector<subdevice_mode_selection> select_modes() const { return select_modes(requests); }
00470         bool validate_requests(stream_request(&requests)[RS_STREAM_NATIVE_COUNT], bool throw_exception = false) const;
00471     };
00472 
00474     // Helper functions for library types //
00476 
00477     inline rs_intrinsics pad_crop_intrinsics(const rs_intrinsics & i, int pad_crop)
00478     {
00479         return{ i.width + pad_crop * 2, i.height + pad_crop * 2, i.ppx + pad_crop, i.ppy + pad_crop, i.fx, i.fy, i.model, {i.coeffs[0], i.coeffs[1], i.coeffs[2], i.coeffs[3], i.coeffs[4]} };
00480     }
00481 
00482     inline rs_intrinsics scale_intrinsics(const rs_intrinsics & i, int width, int height)
00483     {
00484         const float sx = (float)width / i.width, sy = (float)height / i.height;
00485         return{ width, height, i.ppx*sx, i.ppy*sy, i.fx*sx, i.fy*sy, i.model, {i.coeffs[0], i.coeffs[1], i.coeffs[2], i.coeffs[3], i.coeffs[4]} };
00486     }
00487 
00488     inline bool operator == (const rs_intrinsics & a, const rs_intrinsics & b) { return std::memcmp(&a, &b, sizeof(a)) == 0; }
00489 
00490     inline uint32_t pack(uint8_t c0, uint8_t c1, uint8_t c2, uint8_t c3)
00491     {
00492         return (c0 << 24) | (c1 << 16) | (c2 << 8) | c3;
00493     }
00494 
00495     template<class T, int C>
00496     class small_heap
00497     {
00498         T buffer[C];
00499         bool is_free[C];
00500         std::mutex mutex;
00501         bool keep_allocating = true;
00502         std::condition_variable cv;
00503         int size = 0;
00504 
00505     public:
00506         small_heap()
00507         {
00508             for (auto i = 0; i < C; i++)
00509             {
00510                 is_free[i] = true;
00511                 buffer[i] = std::move(T());
00512             }
00513         }
00514 
00515         T * allocate()
00516         {
00517             std::unique_lock<std::mutex> lock(mutex);
00518             if (!keep_allocating) return nullptr;
00519 
00520             for (auto i = 0; i < C; i++)
00521             {
00522                 if (is_free[i])
00523                 {
00524                     is_free[i] = false;
00525                     size++;
00526                     return &buffer[i];
00527                 }
00528             }
00529             return nullptr;
00530         }
00531 
00532         void deallocate(T * item)
00533         {
00534              if (item < buffer || item >= buffer + C)
00535             {
00536                 throw std::runtime_error("Trying to return item to a heap that didn't allocate it!");
00537             }
00538             auto i = item - buffer;
00539             buffer[i] = std::move(T());
00540             
00541             {
00542                 std::unique_lock<std::mutex> lock(mutex);
00543 
00544                 is_free[i] = true;
00545                 size--;
00546 
00547                 if (size == 0)
00548                 {
00549                     lock.unlock();
00550                     cv.notify_one();
00551                 }
00552             }
00553         }
00554 
00555         void stop_allocation()
00556         {
00557             std::unique_lock<std::mutex> lock(mutex);
00558             keep_allocating = false;
00559         }
00560 
00561         void wait_until_empty()
00562         {
00563             std::unique_lock<std::mutex> lock(mutex);
00564 
00565             const auto ready = [this]()
00566             {
00567                 return size == 0;
00568             };
00569             if (!ready() && !cv.wait_for(lock, std::chrono::hours(1000), ready)) // for some reason passing std::chrono::duration::max makes it return instantly
00570             {
00571                 throw std::runtime_error("Could not flush one of the user controlled objects!");
00572             }
00573         }
00574     };
00575 
00576     class frame_continuation
00577     {
00578         std::function<void()> continuation;
00579         const void* protected_data = nullptr;
00580 
00581         frame_continuation(const frame_continuation &) = delete;
00582         frame_continuation & operator=(const frame_continuation &) = delete;
00583     public:
00584         frame_continuation() : continuation([]() {}) {}
00585 
00586         explicit frame_continuation(std::function<void()> continuation, const void* protected_data) : continuation(continuation), protected_data(protected_data) {}
00587         
00588 
00589         frame_continuation(frame_continuation && other) : continuation(std::move(other.continuation)), protected_data(other.protected_data)
00590         {
00591             other.continuation = []() {};
00592             other.protected_data = nullptr;
00593         }
00594 
00595         void operator()()
00596         {
00597             continuation();
00598             continuation = []() {};
00599             protected_data = nullptr;
00600         }
00601 
00602         void reset()
00603         {
00604             protected_data = nullptr;
00605             continuation = [](){};
00606         }
00607 
00608         const void* get_data() const { return protected_data; }
00609 
00610         frame_continuation & operator=(frame_continuation && other)
00611         {
00612             continuation();
00613             protected_data = other.protected_data;
00614             continuation = other.continuation;
00615             other.continuation = []() {};
00616             other.protected_data = nullptr;
00617             return *this;
00618         }
00619 
00620         ~frame_continuation()
00621         {
00622             continuation();
00623         }
00624 
00625     };
00626 
00627     // this class is a convenience wrapper for intrinsics / extrinsics validation methods
00628     class calibration_validator 
00629     {
00630     public:
00631         calibration_validator(std::function<bool(rs_stream, rs_stream)> extrinsic_validator,
00632                               std::function<bool(rs_stream)>            intrinsic_validator);
00633         calibration_validator();
00634 
00635         bool validate_extrinsics(rs_stream from_stream, rs_stream to_stream) const;
00636         bool validate_intrinsics(rs_stream stream) const;
00637 
00638     private:
00639         std::function<bool(rs_stream from_stream, rs_stream to_stream)> extrinsic_validator;
00640         std::function<bool(rs_stream stream)> intrinsic_validator;
00641     };
00642 
00643     inline bool check_not_all_zeros(std::vector<byte> data)
00644     {
00645         return std::find_if(data.begin(), data.end(), [](byte b){ return b!=0; }) != data.end();
00646     }
00647 }
00648 
00649 #endif


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Tue Jun 25 2019 19:54:39