camera.hpp

/****************************************************************************
 *
 * Copyright 2023 Auterion AG. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 
#pragma once
 
#ifdef ROS_DISTRO_IS_ROLLING
#include <tf2_ros/transform_listener.hpp>
#else
#include <tf2_ros/transform_listener.h>
#endif
 
#include <Eigen/Eigen>
#include <auterion_core_msgs/msg/camera_info.hpp>
#include <auterion_core_msgs/msg/image1m.hpp>
#include <auterion_core_msgs/msg/image8m.hpp>
#include <chrono>
#include <functional>
#include <opencv2/core/mat.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <string>
#include <utility>
 
namespace auterion {
using namespace std::chrono_literals;
 
class SDK;
class CameraMonitorImpl;
class CameraImpl;
 
struct ImageHeader {
    ImageHeader() = default;
 
    enum class Encoding {
        Unknown,
        Y800,  
        Y16,   
        UYVY,  
        YUY2,  
        I420,  
        NV12,  
        RGB8,
        BGR8,
        BA81,  
        GBRG,  
        GRBG,  
        RGGB,  
        RGBA   
    };
 
    ImageHeader(int width_, int height_, Encoding encoding_, int step_ = 0);
 
    template <typename ImageT = auterion_core_msgs::msg::Image8m>
    explicit ImageHeader(const ImageT& image)
        : width(image.width), height(image.height), step(image.step), timestamp(image.stamp) {
        const int encoding_len =
            strnlen(reinterpret_cast<const char*>(image.encoding.data()), image.encoding.size());
        const std::string encoding_str(reinterpret_cast<const char*>(image.encoding.data()),
                                       encoding_len);
        if (encoding_str == "mono8") {
            encoding = Encoding::Y800;
        } else if (encoding_str == "mono16") {
            encoding = Encoding::Y16;
        } else if (encoding_str == "uyvy") {
            encoding = Encoding::UYVY;
        } else if (encoding_str == "yuyv") {
            encoding = Encoding::YUY2;
        } else if (encoding_str == "i420") {
            // (This does not exist in ROS:
            // https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/include/sensor_msgs/image_encodings.hpp)
            encoding = Encoding::I420;
        } else if (encoding_str == "nv12") {
            // Does not exist in ROS 2
            encoding = Encoding::NV12;
        } else if (encoding_str == "rgb8") {
            encoding = Encoding::RGB8;
        } else if (encoding_str == "rgba8") {
            encoding = Encoding::RGBA;
        } else if (encoding_str == "bgr8") {
            encoding = Encoding::BGR8;
        } else if (encoding_str == "bayer_bggr8") {
            encoding = Encoding::BA81;
        } else if (encoding_str == "bayer_gbrg8") {
            encoding = Encoding::GBRG;
        } else if (encoding_str == "bayer_grbg8") {
            encoding = Encoding::GRBG;
        } else if (encoding_str == "bayer_rggb8") {
            encoding = Encoding::RGGB;
        }
    }
 
    template <typename ImageT = auterion_core_msgs::msg::Image8m>
    void toImage(ImageT& image) {
        image.width = width;
        image.height = height;
        image.step = step;
        image.stamp = timestamp;
        switch (encoding) {
            case Encoding::Unknown:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "unknown",
                        image.encoding.size());
                break;
            case Encoding::Y800:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "mono8",
                        image.encoding.size());
                break;
            case Encoding::Y16:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "mono16",
                        image.encoding.size());
                break;
            case Encoding::UYVY:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "uyvy",
                        image.encoding.size());
                break;
            case Encoding::YUY2:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "yuyv",
                        image.encoding.size());
                break;
            case Encoding::I420:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "i420",
                        image.encoding.size());
                break;
            case Encoding::NV12:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "nv12",
                        image.encoding.size());
                break;
            case Encoding::RGB8:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "rgb8",
                        image.encoding.size());
                break;
            case Encoding::BGR8:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bgr8",
                        image.encoding.size());
                break;
            case Encoding::RGBA:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "rgba8",
                        image.encoding.size());
                break;
            case Encoding::BA81:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bayer_bggr8",
                        image.encoding.size());
                break;
            case Encoding::GBRG:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bayer_gbrg8",
                        image.encoding.size());
                break;
            case Encoding::GRBG:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bayer_grbg8",
                        image.encoding.size());
                break;
            case Encoding::RGGB:
                strncpy(reinterpret_cast<char*>(image.encoding.data()), "bayer_rggb8",
                        image.encoding.size());
                break;
        }
    }
 
    size_t planeOffset(int plane) const;
    size_t planeSizeBytes(int plane) const;
    size_t imageSizeBytes() const;
 
    int width{};
    int height{};
    int step{};  
    Encoding encoding{Encoding::Unknown};
    rclcpp::Time timestamp{};
};
 
std::ostream& operator<<(std::ostream& os, const ImageHeader::Encoding& encoding);
 
class Image {
   public:
    Image(ImageHeader header, uint8_t* data) : _header(std::move(header)), _data(data) {}
 
    const ImageHeader& header() const { return _header; }
 
    uint8_t* data() { return _data; }
    const uint8_t* data() const { return _data; }
 
    int width() const { return _header.width; }
    int height() const { return _header.height; }
 
    cv::Mat asOpenCVMat(int plane = -1) const;
 
   private:
    ImageHeader _header{};
    uint8_t* _data{nullptr};
};
 
struct CameraDescriptor {
    enum class SensorType {
        Unknown = 0,
        Monochrome = 1,
        RGB = 2,
        Depth = 3,
        RGBD = 4,
        IR = 5,
    };
    enum class MountOrientation {
        Unknown = 0,
        Dynamic = 1,
        Forward = 2,
        Backward = 3,
        Upward = 4,
        Downward = 5,
        Leftward = 6,
        Rightward = 7,
    };
    enum class MountOrientationDetail {
        Unknown = 0,
        Left = 1,
        Right = 2,
        Upper = 3,
        Lower = 4,
    };
    enum class PrimaryPurpose {
        Unknown = 0,
        Generic = 1,
        OpticalFlow = 2,
        VIO = 3,
        FPV = 4,
        Survey = 5,
        Tracking = 6,
    };
    struct ExtrinsicCalibration {
        Eigen::Quaterniond rotation{-0.5, 0.5, -0.5, 0.5};  // {q, x, y, z} default front facing
        Eigen::Vector3d translation{0.0, 0.0, 0.0};
    };
 
    CameraDescriptor() = default;
    explicit CameraDescriptor(const auterion_core_msgs::msg::CameraInfo& camera_info);
 
    operator auterion_core_msgs::msg::CameraInfo() const;
 
    std::string unique_name;
 
    std::string camera_model;
 
    SensorType sensor_type{SensorType::Unknown};
    MountOrientation mount_orientation{MountOrientation::Unknown};
    MountOrientationDetail mount_orientation_detail{MountOrientationDetail::Unknown};
    PrimaryPurpose primary_purpose{PrimaryPurpose::Unknown};
    ExtrinsicCalibration extrinsic_calibration;
};
 
class CameraMonitor {
   public:
    using NotificationCallback = std::function<void(const CameraDescriptor&)>;
 
    CameraMonitor(SDK& sdk, const NotificationCallback& on_camera_added,
                  const NotificationCallback& on_camera_removed = nullptr);
 
    ~CameraMonitor() = default;
 
   private:
    std::shared_ptr<CameraMonitorImpl> _impl;
};
 
class Camera {
   public:
    Camera(SDK& sdk, const CameraDescriptor& camera_descriptor);
    ~Camera() = default;
 
    static Camera waitForCamera(SDK& sdk,
                                const std::function<bool(const CameraDescriptor&)>& find_callback);
 
    [[deprecated(
        "waitForCamera with 'camera_descriptor' not used anymore. This will be removed in some "
        "later release.")]] static Camera
    waitForCamera(SDK& sdk,
                  std::optional<auterion::CameraDescriptor> camera_descriptor = std::nullopt);
 
    static std::optional<Camera> open(SDK& sdk, const std::string& unique_name,
                                      const std::chrono::seconds& timeout = 30s);
    static std::optional<Camera> open(
        SDK& sdk, const std::function<bool(const CameraDescriptor&)>& find_callback,
        const std::chrono::seconds& timeout = 30s);
    static std::optional<Camera> openFirst(SDK& sdk, const std::chrono::seconds& timeout = 30s);
 
    using ImageCallback = std::function<void(const Image& image)>;
 
    void subscribeImage(const ImageCallback& callback);
 
    void subscribeCameraInfo(
        std::function<void(const sensor_msgs::msg::CameraInfo::SharedPtr)> callback);
 
    bool cameraInfoValid() const;
 
    std::optional<cv::Size> getLastImageSize() const;
 
    Eigen::Vector3d unprojectImagePoint(const cv::Point& image_point) const;
 
    bool getPoseBodyCamera(Eigen::Quaterniond& q_body_camera, Eigen::Vector3d& t_body_camera) const;
 
    bool getPoseWorldCamera(Eigen::Quaterniond& q_enu_camera, Eigen::Vector3d& t_enu_camera,
                            const tf2::TimePoint& time = tf2::TimePointZero) const;
 
    bool getDeltaRotationWorldCamera(Eigen::Quaterniond& q_delta_world_camera,
                                     tf2::TimePoint& time_start, tf2::TimePoint& time_end);
 
    bool getCameraInfo(sensor_msgs::msg::CameraInfo& camera_info) const;
 
    const CameraDescriptor& descriptor() const;
 
   private:
    std::shared_ptr<CameraImpl> _impl;
};
 
}  // namespace auterion