MultiSenseTypes.hh

Go to the documentation of this file.

 
#pragma once
 
#include <array>
#include <chrono>
#include <cstdint>
#include <map>
#include <memory>
#include <optional>
#include <stdexcept>
#include <string>
#include <tuple>
#include <vector>
 
#ifdef HAVE_OPENCV
#include <opencv2/core/mat.hpp>
#endif
 
#if !defined(MULTISENSE_API)
#if defined (_MSC_VER)
#define MULTISENSE_API __declspec(dllexport)
#else
#define MULTISENSE_API
#endif
#endif
 
 
namespace multisense
{
 
using TimeT = std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds>;
 
enum class Status : uint8_t
{
    UNKNOWN,
    OK,
    TIMEOUT,
    INTERNAL_ERROR,
    FAILED,
    UNSUPPORTED,
    EXCEPTION,
    UNINITIALIZED,
    INCOMPLETE_APPLICATION
};
 
enum class DataSource : uint16_t
{
    UNKNOWN,
    ALL,
    LEFT_MONO_RAW,
    RIGHT_MONO_RAW,
    LEFT_MONO_COMPRESSED,
    RIGHT_MONO_COMPRESSED,
    LEFT_RECTIFIED_RAW,
    RIGHT_RECTIFIED_RAW,
    LEFT_RECTIFIED_COMPRESSED,
    RIGHT_RECTIFIED_COMPRESSED,
    LEFT_DISPARITY_RAW,
    LEFT_DISPARITY_COMPRESSED,
    AUX_COMPRESSED,
    AUX_RECTIFIED_COMPRESSED,
    AUX_LUMA_RAW,
    AUX_LUMA_RECTIFIED_RAW,
    AUX_CHROMA_RAW,
    AUX_CHROMA_RECTIFIED_RAW,
    AUX_RAW,
    AUX_RECTIFIED_RAW,
    COST_RAW,
    IMU
};
 
enum class ColorImageEncoding : uint16_t
{
    NONE,
    YCBCR420
};
 
struct CameraCalibration
{
    enum class DistortionType : uint8_t
    {
        NONE,
        PLUMBBOB,
        RATIONAL_POLYNOMIAL
    };
 
    std::array<std::array<float, 3>, 3> K{{{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}};
 
    std::array<std::array<float, 3>, 3> R{{{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}};
 
    std::array<std::array<float, 4>, 3> P{{{0.0f, 0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f, 0.0f}}};
 
    DistortionType distortion_type = DistortionType::NONE;
 
    std::vector<float> D = {};
 
    std::array<float, 3> rectified_translation() const
    {
        return std::array<float, 3>{(P[0][0] == 0.0f ? 0.0f : P[0][3] / P[0][0]),
                                    (P[1][1] == 0.0f ? 0.0f : P[1][3] / P[1][1]),
                                    P[2][3]};
    }
};
 
struct StereoCalibration
{
    CameraCalibration left;
 
    CameraCalibration right;
 
    std::optional<CameraCalibration> aux = std::nullopt;
};
 
struct Image
{
    enum class PixelFormat : uint8_t
    {
        UNKNOWN,
        MONO8,
        BGR8,
        MONO16,
        FLOAT32,
        JPEG,
        H264
    };
 
    std::shared_ptr<const std::vector<uint8_t>> raw_data = nullptr;
 
    int64_t image_data_offset = 0;
 
    size_t image_data_length = 0;
 
    PixelFormat format = PixelFormat::UNKNOWN;
 
    int width = -1;
 
    int height = -1;
 
    TimeT camera_timestamp{};
 
    TimeT ptp_timestamp{};
 
    DataSource source = DataSource::UNKNOWN;
 
    CameraCalibration calibration{};
 
    template <typename T>
    std::optional<T> at(int w, int h) const
    {
        if (w < 0 || h < 0 || w >= width || h >= height ||
            raw_data == nullptr ||
            format == PixelFormat::UNKNOWN ||
            format == PixelFormat::JPEG ||
            format == PixelFormat::H264)
        {
            return std::nullopt;
        }
 
        if ((sizeof(T) == 1 && format == PixelFormat::MONO8) ||
            (sizeof(T) == 2 && format == PixelFormat::MONO16) ||
            (sizeof(T) == 3 && format == PixelFormat::BGR8) ||
            (sizeof(T) == 4 && format == PixelFormat::FLOAT32))
        {
            const size_t offset = sizeof(T) * ((width * h) + w);
 
            return *reinterpret_cast<const T*>(raw_data->data() + image_data_offset + offset);
        }
 
        return std::nullopt;
    }
 
#ifdef HAVE_OPENCV
    cv::Mat cv_mat() const;
#endif
};
 
struct ImageHistogram
{
    uint32_t channels = 0;
 
    uint32_t bins = 0;
 
    std::vector<uint32_t> data{};
};
 
struct ImageFrame
{
    void add_image(const Image& image)
    {
        images[image.source] = image;
    }
 
    const Image& get_image(const DataSource &source) const
    {
        auto it = images.find(source);
        if (it == images.end())
        {
            throw std::runtime_error("No image found for requested DataSource");
        }
        return it->second;
    }
 
    bool has_image(const DataSource &source) const
    {
        return (images.find(source) != images.end());
    }
 
    int64_t frame_id = 0;
 
    std::map<DataSource, Image> images{};
 
    StereoCalibration calibration{};
 
    TimeT frame_time{};
 
    TimeT ptp_frame_time{};
 
    ColorImageEncoding aux_color_encoding = ColorImageEncoding::NONE;
 
    std::optional<ImageHistogram> stereo_histogram = std::nullopt;
 
    std::chrono::microseconds capture_exposure_time{0};
 
    float capture_gain = 0.0;
};
 
struct ImuSample
{
    struct Measurement
    {
        float x = 0.0f;
 
        float y = 0.0f;
 
        float z = 0.0f;
    };
 
    std::optional<Measurement> accelerometer = std::nullopt;
    std::optional<Measurement> gyroscope = std::nullopt;
    std::optional<Measurement> magnetometer = std::nullopt;
 
    TimeT sample_time{};
 
    TimeT ptp_sample_time{};
};
 
struct ImuFrame
{
    std::vector<ImuSample> samples;
};
 
struct ImuRate
{
    float sample_rate = 0.0f;
 
    float bandwith_cutoff = 0.0f;
 
    bool operator==(const ImuRate &rhs) const
    {
        return sample_rate == rhs.sample_rate && bandwith_cutoff == rhs.bandwith_cutoff;
    }
};
 
struct ImuRange
{
    float range = 0.0f;
 
    float resolution = 0.0f;
 
    bool operator==(const ImuRange &rhs) const
    {
        return range == rhs.range && resolution == rhs.resolution;
    }
};
 
 
struct MultiSenseConfig
{
    struct StereoConfig
    {
        float postfilter_strength = 0.85f;
 
        bool operator==(const StereoConfig &rhs) const
        {
            return postfilter_strength == rhs.postfilter_strength;
        }
    };
 
    struct ManualExposureConfig
    {
        float gain = 1.68f;
 
        std::chrono::microseconds exposure_time{10000};
 
        bool operator==(const ManualExposureConfig &rhs) const
        {
           return gain == rhs.gain && exposure_time == rhs.exposure_time;
        }
    };
 
    struct AutoExposureRoiConfig
    {
        uint16_t top_left_x_position = 0;
        uint16_t top_left_y_position = 0;
        uint16_t width = 0;
        uint16_t height = 0;
 
        bool operator==(const AutoExposureRoiConfig &rhs) const
        {
            return top_left_x_position == rhs.top_left_x_position &&
                   top_left_y_position == rhs.top_left_y_position &&
                   width == rhs.width &&
                   height == rhs.height;
        }
    };
 
    struct AutoExposureConfig
    {
        std::chrono::microseconds max_exposure_time{10000};
 
        uint32_t decay = 3;
 
        float target_intensity = 0.5f;
 
        float target_threshold = 0.85f;
 
        float max_gain = 2.0f;
 
        AutoExposureRoiConfig roi{};
 
        bool operator==(const AutoExposureConfig &rhs) const
        {
            return max_exposure_time == rhs.max_exposure_time &&
                   decay == rhs.decay &&
                   target_intensity == rhs.target_intensity &&
                   target_threshold == rhs.target_threshold &&
                   max_gain == rhs.max_gain &&
                   roi == rhs.roi;
        }
    };
 
    struct ManualWhiteBalanceConfig
    {
        float red = 1.0f;
 
        float blue = 1.0f;
 
        bool operator==(const ManualWhiteBalanceConfig &rhs) const
        {
            return red == rhs.red && blue == rhs.blue;
        }
    };
 
    struct AutoWhiteBalanceConfig
    {
        uint32_t decay = 3;
 
        float threshold = 0.5f;
 
        bool operator==(const AutoWhiteBalanceConfig &rhs) const
        {
            return decay == rhs.decay && threshold == rhs.threshold;
        }
    };
 
    struct ImageConfig
    {
        float gamma = 2.2f;
 
        bool auto_exposure_enabled = true;
 
        std::optional<ManualExposureConfig> manual_exposure = std::nullopt;
 
        std::optional<AutoExposureConfig> auto_exposure = std::nullopt;
 
        bool auto_white_balance_enabled = true;
 
        std::optional<ManualWhiteBalanceConfig> manual_white_balance = std::nullopt;
 
        std::optional<AutoWhiteBalanceConfig> auto_white_balance = std::nullopt;
 
        bool operator==(const ImageConfig& rhs) const
        {
            return gamma == rhs.gamma &&
                   auto_exposure_enabled == rhs.auto_exposure_enabled &&
                   manual_exposure == rhs.manual_exposure &&
                   auto_exposure == rhs.auto_exposure &&
                   auto_white_balance_enabled == rhs.auto_white_balance_enabled &&
                   manual_white_balance == rhs.manual_white_balance &&
                   auto_white_balance == rhs.auto_white_balance;
        }
    };
 
    struct AuxConfig
    {
        ImageConfig image_config;
 
        bool sharpening_enabled = false;
 
        float sharpening_percentage = 50.0f;
 
        uint8_t sharpening_limit = 100;
 
        bool operator==(const AuxConfig &rhs) const
        {
            return image_config == rhs.image_config &&
                   sharpening_enabled == rhs.sharpening_enabled &&
                   sharpening_percentage == rhs.sharpening_percentage &&
                   sharpening_limit == rhs.sharpening_limit;
        }
    };
 
    enum class MaxDisparities : uint8_t
    {
        D64,
        D128,
        D256
    };
 
    struct TimeConfig
    {
        bool ptp_enabled = false;
 
        bool operator==(const TimeConfig &rhs) const
        {
           return ptp_enabled == rhs.ptp_enabled;
        }
    };
 
    struct NetworkTransmissionConfig
    {
        bool packet_delay_enabled = false;
 
        bool operator==(const NetworkTransmissionConfig &rhs) const
        {
            return packet_delay_enabled == rhs.packet_delay_enabled;
        }
    };
 
    struct ImuConfig
    {
        struct OperatingMode
        {
            bool enabled = false;
 
            ImuRate rate{};
 
            ImuRange range{};
 
            bool operator==(const OperatingMode &rhs) const
            {
                return enabled == rhs.enabled && rate == rhs.rate && range == rhs.range;
            }
        };
 
        uint32_t samples_per_frame = 0;
 
        std::optional<OperatingMode> accelerometer = std::nullopt;
 
        std::optional<OperatingMode> gyroscope = std::nullopt;
 
        std::optional<OperatingMode> magnetometer = std::nullopt;
 
        bool operator==(const ImuConfig &rhs) const
        {
            return samples_per_frame == rhs.samples_per_frame &&
                   accelerometer == rhs.accelerometer &&
                   gyroscope == rhs.gyroscope &&
                   magnetometer == rhs.magnetometer;
        }
    };
 
    struct LightingConfig
    {
        struct InternalConfig
        {
            float intensity = 0.0f;
 
            bool flash = false;
 
            bool operator==(const InternalConfig &rhs) const
            {
                 return intensity == rhs.intensity && flash == rhs.flash;
            }
        };
 
        struct ExternalConfig
        {
            enum class FlashMode : uint8_t
            {
                NONE,
                SYNC_WITH_MAIN_STEREO,
                SYNC_WITH_AUX
            };
 
            float intensity = 0.0f;
 
            FlashMode flash = FlashMode::NONE;
 
            uint32_t pulses_per_exposure = 1;
 
            std::chrono::microseconds startup_time{0};
 
            bool operator==(const ExternalConfig &rhs) const
            {
                return intensity == rhs.intensity &&
                       flash == rhs.flash &&
                       pulses_per_exposure == rhs.pulses_per_exposure &&
                       startup_time == rhs.startup_time;
            }
        };
 
        std::optional<InternalConfig> internal = std::nullopt;
 
        std::optional<ExternalConfig> external = std::nullopt;
 
        bool operator==(const LightingConfig &rhs) const
        {
             return internal == rhs.internal && external == rhs.external;
        }
    };
 
    uint32_t width = 0;
 
    uint32_t height = 0;
 
    MaxDisparities disparities = MaxDisparities::D256;
 
    float frames_per_second = 10.0f;
 
    StereoConfig stereo_config;
 
    ImageConfig image_config;
 
    std::optional<AuxConfig> aux_config = std::nullopt;
 
    std::optional<TimeConfig> time_config = std::nullopt;
 
    std::optional<NetworkTransmissionConfig> network_config = std::nullopt;
 
    std::optional<ImuConfig> imu_config = std::nullopt;
 
    std::optional<LightingConfig> lighting_config = std::nullopt;
 
    bool operator==(const MultiSenseConfig &rhs) const
    {
        return width == rhs.width &&
               height == rhs.height &&
               disparities == rhs.disparities &&
               frames_per_second == rhs.frames_per_second &&
               stereo_config == rhs.stereo_config &&
               image_config == rhs.image_config &&
               aux_config == rhs.aux_config &&
               time_config == rhs.time_config &&
               network_config == rhs.network_config &&
               imu_config == rhs.imu_config &&
               lighting_config == rhs.lighting_config;
    }
};
 
struct MultiSenseStatus
{
    struct PtpStatus
    {
        bool grandmaster_present = false;
 
        std::array<uint8_t, 8> grandmaster_id{0, 0, 0, 0, 0, 0, 0, 0};
 
        std::chrono::nanoseconds grandmaster_offset{0};
 
        std::chrono::nanoseconds path_delay{0};
 
        uint16_t steps_from_local_to_grandmaster = 0;
    };
 
    struct CameraStatus
    {
        bool cameras_ok = false;
 
        bool processing_pipeline_ok = false;
    };
 
    struct TemperatureStatus
    {
        float cpu_temperature = 0.0f;
 
        float fpga_temperature = 0.0f;
 
        float left_imager_temperature = 0.0f;
 
        float right_imager_temperature = 0.0f;
    };
 
    struct PowerStatus
    {
        float input_voltage = 0.0f;
 
        float input_current = 0.0f;
 
        float fpga_power = 0.0f;
    };
 
    struct ClientNetworkStatus
    {
        size_t received_messages = 0;
 
        size_t dropped_messages = 0;
 
        size_t invalid_packets = 0;
 
        size_t unprocessed_packets = 0;
    };
 
    struct TimeStatus
    {
        std::chrono::nanoseconds camera_time{0};
 
        std::chrono::nanoseconds client_host_time{0};
 
        std::chrono::nanoseconds network_delay{0};
 
        std::chrono::nanoseconds offset_to_host() const
        {
            return (client_host_time + network_delay) - camera_time;
        }
 
        TimeT apply_offset_to_host(const TimeT &input_camera_time) const
        {
            return input_camera_time + std::chrono::duration_cast<std::chrono::system_clock::duration>(offset_to_host());
        }
    };
 
    bool system_ok = false;
 
    std::optional<PtpStatus> ptp = std::nullopt;
 
    CameraStatus camera;
 
    std::optional<TemperatureStatus> temperature = std::nullopt;
 
    std::optional<PowerStatus> power = std::nullopt;
 
    ClientNetworkStatus client_network;
 
    std::optional<TimeStatus> time = std::nullopt;
};
 
struct MultiSenseInfo
{
    struct NetworkInfo
    {
        std::string ip_address = "10.66.171.21";
 
        std::string gateway = "10.66.171.1";
 
        std::string netmask = "255.255.255.0";
    };
 
    struct DeviceInfo
    {
        struct PcbInfo
        {
            std::string name;
            uint32_t revision;
        };
 
        enum class HardwareRevision : uint8_t
        {
            UNKNOWN,
            S7,
            S21,
            ST21,
            S27,
            S30,
            KS21,
            MONOCAM,
            KS21_SILVER,
            ST25,
            KS21i
        };
 
        enum class ImagerType : uint8_t
        {
            UNKNOWN,
            CMV2000_GREY,
            CMV2000_COLOR,
            CMV4000_GREY,
            CMV4000_COLOR,
            FLIR_TAU2,
            AR0234_GREY,
            AR0239_COLOR,
            TENUM1280
        };
 
        enum class LightingType : uint8_t
        {
            NONE,
            INTERNAL,
            EXTERNAL,
            PATTERN_PROJECTOR,
            OUTPUT_TRIGGER,
            PATTERN_PROJECTOR_OUTPUT_TRIGGER
 
        };
 
        enum class LensType : uint8_t
        {
            UNKNOWN,
            STANDARD,
            FISHEYE
        };
 
        std::string camera_name{};
 
        std::string build_date{};
 
        std::string serial_number{};
 
        HardwareRevision hardware_revision{};
 
        std::vector<PcbInfo> pcb_info{};
 
        std::string imager_name{};
 
        ImagerType imager_type{};
 
        uint32_t imager_width = 0;
 
        uint32_t imager_height = 0;
 
        std::string lens_name{};
 
        LensType lens_type{};
 
        float nominal_stereo_baseline = 0.0f;
 
        float nominal_focal_length = 0.0f;
 
        float nominal_relative_aperture = 0.0f;
 
        LightingType lighting_type{};
 
        uint32_t number_of_lights = 0;
 
        constexpr bool has_aux_camera() const
        {
            switch (hardware_revision)
            {
                case HardwareRevision::S27:
                case HardwareRevision::S30:
                case HardwareRevision::KS21i:
                    return true;
                default:
                    return false;
            }
        }
 
        constexpr bool has_main_stereo_color() const
        {
            switch (hardware_revision)
            {
                case HardwareRevision::S7:
                case HardwareRevision::S21:
                case HardwareRevision::MONOCAM:
                    return true;
                default:
                    return false;
            }
        }
    };
 
    struct Version
    {
        uint32_t major = 0;
 
        uint32_t minor = 0;
 
        uint32_t patch = 0;
 
        std::string to_string() const
        {
            return std::to_string(major) + "." + std::to_string(minor) + "." + std::to_string(patch);
        }
 
        bool operator==(const Version& other) const
        {
            return std::tie(major, minor, patch) == std::tie(other.major, other.minor, other.patch);
        }
 
        bool operator<(const Version& other) const
        {
            return std::tie(major, minor, patch) < std::tie(other.major, other.minor, other.patch);
        }
 
        bool operator<=(const Version& other) const
        {
            return *this < other || *this == other;
        }
 
        bool operator>(const Version& other) const
        {
            return other < *this;
        }
 
        bool operator>=(const Version& other) const
        {
            return other <= *this;
        }
    };
 
    struct SensorVersion
    {
        std::string firmware_build_date{};
 
        MultiSenseInfo::Version firmware_version{};
 
        uint64_t hardware_version = 0;
    };
 
    struct SupportedOperatingMode
    {
        uint32_t width = 0;
 
        uint32_t height = 0;
 
        MultiSenseConfig::MaxDisparities disparities = MultiSenseConfig::MaxDisparities::D256;
 
        std::vector<DataSource> supported_sources{};
    };
 
    struct ImuInfo
    {
        struct Source
        {
            std::string name{};
 
            std::string device{};
 
            std::vector<ImuRate> rates{};
 
            std::vector<ImuRange> ranges{};
        };
 
        std::optional<Source> accelerometer = std::nullopt;
 
        std::optional<Source> gyroscope = std::nullopt;
 
        std::optional<Source> magnetometer = std::nullopt;
    };
 
    DeviceInfo device;
 
    SensorVersion version;
 
    std::vector<SupportedOperatingMode> operating_modes;
 
    std::optional<ImuInfo> imu;
 
    NetworkInfo network;
};
 
}