camera_utilities.h

Go to the documentation of this file.

 
#ifndef MULTISENSE_ROS_CAMERA_UTILITIES_H
#define MULTISENSE_ROS_CAMERA_UTILITIES_H
 
#include <tuple>
#include <mutex>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <opencv2/opencv.hpp>
 
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
 
#include <multisense_lib/MultiSenseChannel.hh>
#include <multisense_lib/MultiSenseTypes.hh>
 
namespace multisense_ros {
 
static constexpr size_t S30_AUX_CAM_WIDTH = 1920;
static constexpr size_t S30_AUX_CAM_HEIGHT = 1188;
 
struct OperatingResolutionT
{
    size_t width = 0;
    size_t height = 0;
};
 
enum class BorderClip {NONE, RECTANGULAR, CIRCULAR};
 
struct RectificationRemapT
{
    cv::Mat map1;
    cv::Mat map2;
};
 
template <typename T>
class BufferWrapper
{
public:
    BufferWrapper(crl::multisense::Channel* driver,
                 const T &data):
        driver_(driver),
        callback_buffer_(driver->reserveCallbackBuffer()),
        data_(data)
    {
    }
 
    ~BufferWrapper()
    {
        driver_->releaseCallbackBuffer(callback_buffer_);
    }
 
    const T &data() const noexcept
    {
        return data_;
    }
 
private:
 
    BufferWrapper(const BufferWrapper&) = delete;
    BufferWrapper operator=(const BufferWrapper&) = delete;
 
    crl::multisense::Channel * driver_;
    void* callback_buffer_;
    const T data_;
 
};
 
template <typename T>
constexpr Eigen::Matrix<T, 3, 1> ycbcrToBgr(const crl::multisense::image::Header &luma,
                                            const crl::multisense::image::Header &chroma,
                                            size_t u,
                                            size_t v)
{
    const uint8_t *lumaP = reinterpret_cast<const uint8_t*>(luma.imageDataP);
    const uint8_t *chromaP = reinterpret_cast<const uint8_t*>(chroma.imageDataP);
 
    const size_t luma_offset = (v * luma.width) + u;
    const size_t chroma_offset = 2 * (((v/2) * (luma.width/2)) + (u/2));
 
    const float px_y = static_cast<float>(lumaP[luma_offset]);
    const float px_cb = static_cast<float>(chromaP[chroma_offset+0]) - 128.0f;
    const float px_cr = static_cast<float>(chromaP[chroma_offset+1]) - 128.0f;
 
    float px_r  = px_y + 1.402f   * px_cr;
    float px_g  = px_y - 0.34414f * px_cb - 0.71414f * px_cr;
    float px_b  = px_y + 1.772f   * px_cb;
 
    if (px_r < 0.0f)        px_r = 0.0f;
    else if (px_r > 255.0f) px_r = 255.0f;
    if (px_g < 0.0f)        px_g = 0.0f;
    else if (px_g > 255.0f) px_g = 255.0f;
    if (px_b < 0.0f)        px_b = 0.0f;
    else if (px_b > 255.0f) px_b = 255.0f;
 
    return Eigen::Matrix<T, 3, 1>{static_cast<T>(px_b), static_cast<T>(px_g), static_cast<T>(px_r)};
}
 
void ycbcrToBgr(const crl::multisense::image::Header &luma,
                const crl::multisense::image::Header &chroma,
                uint8_t *output);
 
Eigen::Matrix4d makeQ(const crl::multisense::image::Config& config,
                      const crl::multisense::image::Calibration& calibration,
                      const crl::multisense::system::DeviceInfo& device_info);
 
sensor_msgs::CameraInfo makeCameraInfo(const crl::multisense::image::Config& config,
                                       const crl::multisense::image::Calibration::Data& calibration,
                                       const crl::multisense::system::DeviceInfo& device_info,
                                       bool scale_calibration);
 
RectificationRemapT makeRectificationRemap(const crl::multisense::image::Config& config,
                                           const crl::multisense::image::Calibration::Data& calibration,
                                           const crl::multisense::system::DeviceInfo& device_info);
 
class StereoCalibrationManager
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    StereoCalibrationManager(const crl::multisense::image::Config& config,
                             const crl::multisense::image::Calibration& calibration,
                             const crl::multisense::system::DeviceInfo& device_info);
 
    void updateConfig(const crl::multisense::image::Config& config);
 
    crl::multisense::image::Config config() const;
 
    Eigen::Matrix4d Q() const;
 
    double T() const;
 
    Eigen::Vector3d aux_T() const;
 
    Eigen::Vector3f reproject(size_t u, size_t v, double d) const;
 
    static Eigen::Vector3f reproject(size_t u,
                                     size_t v,
                                     double d,
                                     const sensor_msgs::CameraInfo &left_camera_info,
                                     const sensor_msgs::CameraInfo &right_camera_info);
 
    static bool isValidReprojectedPoint(const Eigen::Vector3f& pt, const float squared_max_range);
 
    Eigen::Vector2f rectifiedAuxProject(const Eigen::Vector3f &left_rectified_point) const;
 
    static Eigen::Vector2f rectifiedAuxProject(const Eigen::Vector3f &left_rectified_point,
                                               const sensor_msgs::CameraInfo &aux_camera_info);
 
    OperatingResolutionT operatingStereoResolution() const;
 
    OperatingResolutionT operatingAuxResolution() const;
 
    bool validRight() const;
 
    bool validAux() const;
 
    sensor_msgs::CameraInfo leftCameraInfo(const std::string& frame_id, const ros::Time& stamp) const;
    sensor_msgs::CameraInfo rightCameraInfo(const std::string& frame_id, const ros::Time& stamp) const;
    sensor_msgs::CameraInfo auxCameraInfo(const std::string& frame_id,
                                          const ros::Time& stamp,
                                          const OperatingResolutionT& resolution) const;
    sensor_msgs::CameraInfo auxCameraInfo(const std::string& frame_id,
                                          const ros::Time& stamp,
                                          size_t width,
                                          size_t height) const;
 
    std::shared_ptr<RectificationRemapT> leftRemap() const;
    std::shared_ptr<RectificationRemapT> rightRemap() const;
 
private:
 
    crl::multisense::image::Config config_;
    const crl::multisense::image::Calibration calibration_;
    const crl::multisense::system::DeviceInfo& device_info_;
 
    //
    // Protect our cache during calibration updates
 
    mutable std::mutex mutex_;
 
    //
    // Cache for quick queries
 
    Eigen::Matrix4d q_matrix_;
 
    sensor_msgs::CameraInfo left_camera_info_;
    sensor_msgs::CameraInfo right_camera_info_;
    sensor_msgs::CameraInfo aux_camera_info_;
 
    std::shared_ptr<RectificationRemapT> left_remap_;
    std::shared_ptr<RectificationRemapT> right_remap_;
};
 
bool clipPoint(const BorderClip& borderClipType,
               double borderClipValue,
               size_t height,
               size_t width,
               size_t u,
               size_t v);
 
cv::Vec3b interpolateColor(const Eigen::Vector2f &pixel, const cv::Mat &image);
 
}// namespace
 
#endif