55 const auto crop = config.
camMode() == 2000 &&
60 const auto imagerHeight = crop ? 1088 : device_info.
imagerHeight;
63 static_cast<double>(config.
width())));
65 const double y_scale = 1.0 / ((
static_cast<double>(imagerHeight) /
66 static_cast<double>(config.
height())));
76 crop ? -config.
offset()*y_scale : 0.0};
85 const size_t rgb_stride = luma.
width * 3;
87 for(uint32_t y=0; y< luma.
height; ++y)
89 const size_t row_offset = y * rgb_stride;
91 for(uint32_t x=0; x< luma.
width; ++x)
93 memcpy(output + row_offset + (3 * x), ycbcrToBgr<uint8_t>(luma, chroma, x, y).data(), 3);
102 Eigen::Matrix4d q_matrix = Eigen::Matrix4d::Zero();
104 const auto scale = compute_scale(config, device_info);
115 const auto fx = calibration.
left.
P[0][0] * scale.x_scale;
116 const auto cx = calibration.
left.
P[0][2] * scale.x_scale + scale.cx_offset;
117 const auto fy = calibration.
left.
P[1][1] * scale.y_scale;
118 const auto cy = calibration.
left.
P[1][2] * scale.y_scale + scale.cy_offset;
119 const auto tx = calibration.
right.
P[0][3] / calibration.
right.
P[0][0];
120 const auto cx_prime = calibration.
left.
P[0][2] * scale.x_scale + scale.cx_offset;
122 q_matrix(0,0) = fy * tx;
123 q_matrix(1,1) = fx * tx;
124 q_matrix(0,3) = -fy * cx * tx;
125 q_matrix(1,3) = -fx * cy * tx;
126 q_matrix(2,3) = fx * fy * tx;
128 q_matrix(3,3) = fy * (cx - cx_prime);
137 const auto scale = compute_scale(config, device_info);
139 sensor_msgs::CameraInfo camera_info;
141 camera_info.width = config.
width();
142 camera_info.height = config.
height();
144 camera_info.P[0] = calibration.
P[0][0] * scale.x_scale;
145 camera_info.P[1] = calibration.
P[0][1];
146 camera_info.P[2] = calibration.
P[0][2] * scale.x_scale + scale.cx_offset;
147 camera_info.P[3] = calibration.
P[0][3] * scale.x_scale;
148 camera_info.P[4] = calibration.
P[1][0];
149 camera_info.P[5] = calibration.
P[1][1] * scale.y_scale;
150 camera_info.P[6] = calibration.
P[1][2] * scale.y_scale + scale.cy_offset;
151 camera_info.P[7] = calibration.
P[1][3];
152 camera_info.P[8] = calibration.
P[2][0];
153 camera_info.P[9] = calibration.
P[2][1];
154 camera_info.P[10] = calibration.
P[2][2];
155 camera_info.P[11] = calibration.
P[2][3];
157 camera_info.K[0] = calibration.
M[0][0] * scale.x_scale;
158 camera_info.K[1] = calibration.
M[0][1];
159 camera_info.K[2] = calibration.
M[0][2] * scale.x_scale + scale.cx_offset;
160 camera_info.K[3] = calibration.
M[1][0];
161 camera_info.K[4] = calibration.
M[1][1] * scale.y_scale;
162 camera_info.K[5] = calibration.
M[1][2] * scale.y_scale + scale.cy_offset;
163 camera_info.K[6] = calibration.
M[2][0];
164 camera_info.K[7] = calibration.
M[2][1];
165 camera_info.K[8] = calibration.
M[2][2];
167 camera_info.R[0] = calibration.
R[0][0];
168 camera_info.R[1] = calibration.
R[0][1];
169 camera_info.R[2] = calibration.
R[0][2];
170 camera_info.R[3] = calibration.
R[1][0];
171 camera_info.R[4] = calibration.
R[1][1];
172 camera_info.R[5] = calibration.
R[1][2];
173 camera_info.R[6] = calibration.
R[2][0];
174 camera_info.R[7] = calibration.
R[2][1];
175 camera_info.R[8] = calibration.
R[2][2];
181 camera_info.D.resize(8);
182 for (
size_t i=0 ; i < 8 ; ++i)
184 camera_info.D[i] = calibration.
D[i];
193 if (calibration.
D[7] == 0.0 && calibration.
D[6] == 0.0 && calibration.
D[5] == 0.0)
211 const auto scale = compute_scale(config, device_info);
213 const cv::Matx33d K(calibration.
M[0][0] * scale.x_scale,
215 calibration.
M[0][2] * scale.x_scale + scale.cx_offset,
217 calibration.
M[1][1] * scale.y_scale,
218 calibration.
M[1][2] * scale.y_scale + scale.cy_offset,
221 calibration.
M[2][2]);
223 const cv::Matx33d R(calibration.
R[0][0],
231 calibration.
R[2][2]);
233 const cv::Matx34d P(calibration.
P[0][0] * scale.x_scale,
235 calibration.
P[0][2] * scale.x_scale + scale.cx_offset,
236 calibration.
P[0][3] * scale.x_scale,
238 calibration.
P[1][1] * scale.y_scale,
239 calibration.
P[1][2] * scale.y_scale + scale.cy_offset,
244 calibration.
P[2][3]);
246 const bool plumbob = calibration.
D[7] == 0.0 && calibration.
D[6] == 0.0 && calibration.
D[5] == 0.0;
248 cv::Mat D(plumbob ? 5 : 8, 1, CV_64FC1);
249 for (
int i = 0; i < D.rows ; ++i)
251 D.at<
double>(i) = calibration.
D[i];
254 cv::initUndistortRectifyMap(K, D, R, P, cv::Size(config.
width(), config.
height()), CV_32FC1, remap.
map1, remap.
map2);
263 calibration_(calibration),
264 device_info_(device_info),
265 q_matrix_(
makeQ(config_, calibration_, device_info_)),
266 left_camera_info_(
makeCameraInfo(config_, calibration_.left, device_info_)),
267 right_camera_info_(
makeCameraInfo(config_, calibration_.right, device_info_)),
268 aux_camera_info_(
makeCameraInfo(config_, calibration_.aux, device_info_)),
281 std::lock_guard<std::mutex> lock(
mutex_);
293 std::lock_guard<std::mutex> lock(
mutex_);
305 std::lock_guard<std::mutex> lock(
mutex_);
312 std::lock_guard<std::mutex> lock(
mutex_);
319 std::lock_guard<std::mutex> lock(
mutex_);
335 std::lock_guard<std::mutex> lock(
mutex_);
351 return std::isfinite(
aux_T());
357 std::lock_guard<std::mutex> lock(
mutex_);
360 camera_info.header.frame_id = frame_id;
361 camera_info.header.stamp = stamp;
369 std::lock_guard<std::mutex> lock(
mutex_);
372 camera_info.header.frame_id = frame_id;
373 camera_info.header.stamp = stamp;
381 std::lock_guard<std::mutex> lock(
mutex_);
384 camera_info.header.frame_id = frame_id;
385 camera_info.header.stamp = stamp;
392 std::lock_guard<std::mutex> lock(
mutex_);
399 std::lock_guard<std::mutex> lock(
mutex_);
double T() const
Translation which transforms points from the right camera frame into the left camera frame...
sensor_msgs::CameraInfo left_camera_info_
sensor_msgs::CameraInfo auxCameraInfo(const std::string &frame_id, const ros::Time &stamp) const
std::shared_ptr< RectificationRemapT > right_remap_
const std::string RATIONAL_POLYNOMIAL
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)
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV4000_COLOR
sensor_msgs::CameraInfo leftCameraInfo(const std::string &frame_id, const ros::Time &stamp) const
RectificationRemapT makeRectificationRemap(const crl::multisense::image::Config &config, const crl::multisense::image::Calibration::Data &calibration, const crl::multisense::system::DeviceInfo &device_info)
bool validAux() const
Determine if the Aux calibration is valid.
crl::multisense::image::Config config_
crl::multisense::image::Config config() const
sensor_msgs::CameraInfo rightCameraInfo(const std::string &frame_id, const ros::Time &stamp) const
void updateConfig(const crl::multisense::image::Config &config)
const crl::multisense::system::DeviceInfo & device_info_
ROSCPP_DECL std::string remap(const std::string &name)
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)
static CRL_CONSTEXPR uint32_t IMAGER_TYPE_CMV4000_GREY
const crl::multisense::image::Calibration calibration_
const std::string PLUMB_BOB
Eigen::Matrix4d q_matrix_
double aux_T() const
Translation which transforms points from the aux camera frame into the left camera frame...
std::shared_ptr< RectificationRemapT > rightRemap() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW StereoCalibrationManger(const crl::multisense::image::Config &config, const crl::multisense::image::Calibration &calibration, const crl::multisense::system::DeviceInfo &device_info)
std::shared_ptr< RectificationRemapT > left_remap_
std::shared_ptr< RectificationRemapT > leftRemap() const
Eigen::Matrix4d Q() const
Q stereo reprojection matrix see: https://docs.opencv.org/4.3.0/d9/d0c/group__calib3d.html#ga1bc1152bd57d63bc524204f21fde6e02.
sensor_msgs::CameraInfo right_camera_info_