34 #ifndef MULTISENSE_ROS_CAMERA_UTILITIES_H 35 #define MULTISENSE_ROS_CAMERA_UTILITIES_H 41 #include <opencv2/opencv.hpp> 44 #include <sensor_msgs/CameraInfo.h> 46 #include <multisense_lib/MultiSenseChannel.hh> 47 #include <multisense_lib/MultiSenseTypes.hh> 66 callback_buffer_(driver->reserveCallbackBuffer()),
73 driver_->releaseCallbackBuffer(callback_buffer_);
76 const T &
data() const noexcept
98 const uint8_t *lumaP =
reinterpret_cast<const uint8_t*
>(luma.
imageDataP);
99 const uint8_t *chromaP =
reinterpret_cast<const uint8_t*
>(chroma.
imageDataP);
101 const size_t luma_offset = (v * luma.
width) + u;
102 const size_t chroma_offset = 2 * (((v/2) * (luma.
width/2)) + (u/2));
104 const float px_y =
static_cast<float>(lumaP[luma_offset]);
105 const float px_cb =
static_cast<float>(chromaP[chroma_offset+0]) - 128.0
f;
106 const float px_cr =
static_cast<float>(chromaP[chroma_offset+1]) - 128.0
f;
108 float px_r = px_y + 1.402f * px_cr;
109 float px_g = px_y - 0.34414f * px_cb - 0.71414f * px_cr;
110 float px_b = px_y + 1.772f * px_cb;
112 if (px_r < 0.0
f) px_r = 0.0f;
113 else if (px_r > 255.0
f) px_r = 255.0f;
114 if (px_g < 0.0
f) px_g = 0.0f;
115 else if (px_g > 255.0
f) px_g = 255.0f;
116 if (px_b < 0.0
f) px_b = 0.0f;
117 else if (px_b > 255.0
f) px_b = 255.0f;
119 return Eigen::Matrix<T, 3, 1>{
static_cast<T
>(px_b), static_cast<T>(px_g),
static_cast<T
>(px_r)};
133 bool scale_calibration);
141 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
155 Eigen::Matrix4d Q()
const;
165 double aux_T()
const;
170 bool validAux()
const;
172 sensor_msgs::CameraInfo leftCameraInfo(
const std::string& frame_id,
const ros::Time& stamp)
const;
173 sensor_msgs::CameraInfo rightCameraInfo(
const std::string& frame_id,
const ros::Time& stamp)
const;
174 sensor_msgs::CameraInfo auxCameraInfo(
const std::string& frame_id,
const ros::Time& stamp)
const;
176 std::shared_ptr<RectificationRemapT> leftRemap()
const;
177 std::shared_ptr<RectificationRemapT> rightRemap()
const;
sensor_msgs::CameraInfo left_camera_info_
std::shared_ptr< RectificationRemapT > right_remap_
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)
crl::multisense::Channel * driver_
crl::multisense::image::Config config_
const crl::multisense::system::DeviceInfo & device_info_
sensor_msgs::CameraInfo aux_camera_info_
Eigen::Matrix4d makeQ(const crl::multisense::image::Config &config, const crl::multisense::image::Calibration &calibration, const crl::multisense::system::DeviceInfo &device_info)
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 crl::multisense::image::Calibration calibration_
const T & data() const noexcept
Eigen::Matrix4d q_matrix_
BufferWrapper(crl::multisense::Channel *driver, const T &data)
std::shared_ptr< RectificationRemapT > left_remap_
sensor_msgs::CameraInfo right_camera_info_