3 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED 4 #include <boost/make_shared.hpp> 27 : full_maps_dirty(true),
28 reduced_maps_dirty(true),
29 rectified_roi_dirty(true)
53 bool update(
const T& new_val, T& my_val)
55 if (my_val == new_val)
62 template<
typename MatT>
63 bool updateMat(
const MatT& new_mat, MatT& my_mat, cv::Mat_<double>& cv_mat,
int rows,
int cols)
65 if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
69 cv_mat = (my_mat.size() == 0) ? cv::Mat_<double>() : cv::Mat_<double>(rows, cols, &my_mat[0]);
73 template<
typename MatT,
typename MatU>
74 bool updateMat(
const MatT& new_mat, MatT& my_mat, MatU& cv_mat)
76 if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
80 cv_mat = MatU(&my_mat[0]);
88 #ifdef BOOST_SHARED_PTR_HPP_INCLUDED 89 cache_ = boost::make_shared<Cache>();
91 cache_ = std::make_shared<Cache>();
95 uint32_t binning_x = msg.binning_x ? msg.binning_x : 1;
96 uint32_t binning_y = msg.binning_y ? msg.binning_y : 1;
99 sensor_msgs::RegionOfInterest roi = msg.roi;
100 if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) {
101 roi.width = msg.width;
102 roi.height = msg.height;
110 bool &full_dirty = cache_->full_maps_dirty;
113 full_dirty |=
update(msg.distortion_model,
cam_info_.distortion_model);
123 bool &reduced_dirty = cache_->reduced_maps_dirty;
124 reduced_dirty = full_dirty;
131 cache_->rectified_roi_dirty = reduced_dirty;
137 cache_->distortion_state =
NONE;
138 for (
size_t i = 0; i <
cam_info_.D.size(); ++i)
148 cache_->distortion_state =
UNKNOWN;
152 bool adjust_binning = (binning_x > 1) || (binning_y > 1);
153 bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0);
155 if (!adjust_binning && !adjust_roi) {
167 K_(0,2) -= roi.x_offset;
168 K_(1,2) -= roi.y_offset;
169 P_(0,2) -= roi.x_offset;
170 P_(1,2) -= roi.y_offset;
174 double scale_x = 1.0 / binning_x;
182 double scale_y = 1.0 / binning_y;
191 return reduced_dirty;
216 return cv::Point2d(uv_reduced.x *
binningX() + roi.x,
223 return cv::Rect(roi_reduced.x *
binningX() + roi.x,
232 return cv::Point2d((uv_full.x - roi.x) /
binningX(),
239 return cv::Rect((roi_full.x - roi.x) /
binningX(),
257 if (
cache_->rectified_roi_dirty)
263 cache_->rectified_roi_dirty =
false;
265 return cache_->rectified_roi;
271 assert(
P_(2, 3) == 0.0);
277 uv_rect.x = (
fx()*xyz.x +
Tx()) / xyz.z +
cx();
278 uv_rect.y = (
fy()*xyz.y +
Ty()) / xyz.z +
cy();
287 ray.x = (uv_rect.x -
cx() -
Tx()) /
fx();
288 ray.y = (uv_rect.y -
cy() -
Ty()) /
fy();
297 switch (
cache_->distortion_state) {
299 raw.copyTo(rectified);
303 if (raw.depth() == CV_32F || raw.depth() == CV_64F)
305 cv::remap(raw, rectified,
cache_->reduced_map1,
cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
308 cv::remap(raw, rectified,
cache_->reduced_map1,
cache_->reduced_map2, interpolation);
312 assert(
cache_->distortion_state == UNKNOWN);
313 throw Exception(
"Cannot call rectifyImage when distortion is unknown.");
321 throw Exception(
"PinholeCameraModel::unrectifyImage is unimplemented.");
337 if (
cache_->distortion_state == UNKNOWN)
338 throw Exception(
"Cannot call rectifyPoint when distortion is unknown.");
342 cv::Point2f raw32 = uv_raw, rect32;
343 const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x);
344 cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x);
345 cv::undistortPoints(src_pt, dst_pt,
K_,
D_,
R_,
P_);
355 if (
cache_->distortion_state == UNKNOWN)
356 throw Exception(
"Cannot call unrectifyPoint when distortion is unknown.");
363 cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
364 cv::Rodrigues(
R_.t(), r_vec);
365 std::vector<cv::Point2d> image_point;
366 cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec,
K_,
D_, image_point);
368 return image_point[0];
378 cv::Point2d rect_tl =
rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y));
379 cv::Point2d rect_tr =
rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y));
380 cv::Point2d rect_br =
rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width,
381 roi_raw.y + roi_raw.height));
382 cv::Point2d rect_bl =
rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height));
384 cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)),
385 std::ceil (std::min(rect_tl.y, rect_tr.y)));
386 cv::Point roi_br(std::floor(std::max(rect_tr.x, rect_br.x)),
387 std::floor(std::max(rect_bl.y, rect_br.y)));
389 return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
399 cv::Point2d raw_tl =
unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y));
400 cv::Point2d raw_tr =
unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, roi_rect.y));
401 cv::Point2d raw_br =
unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width,
402 roi_rect.y + roi_rect.height));
403 cv::Point2d raw_bl =
unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y + roi_rect.height));
405 cv::Point roi_tl(std::floor(std::min(raw_tl.x, raw_bl.x)),
406 std::floor(std::min(raw_tl.y, raw_tr.y)));
407 cv::Point roi_br(std::ceil (std::max(raw_tr.x, raw_br.x)),
408 std::ceil (std::max(raw_bl.y, raw_br.y)));
410 return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
418 if (
cache_->full_maps_dirty) {
422 binned_resolution.width /=
binningX();
423 binned_resolution.height /=
binningY();
453 cv::initUndistortRectifyMap(K_binned,
D_,
R_, P_binned, binned_resolution,
455 cache_->full_maps_dirty =
false;
458 if (
cache_->reduced_maps_dirty) {
462 if (roi.x != 0 || roi.y != 0 ||
472 cache_->reduced_map1 =
cache_->full_map1(roi) - cv::Scalar(roi.x, roi.y);
480 cache_->reduced_maps_dirty =
false;
cv::Rect rectifyRoi(const cv::Rect &roi_raw) const
Compute the rectified ROI best fitting a raw ROI.
cv::Size fullResolution() const
The resolution at which the camera was calibrated.
const std::string RATIONAL_POLYNOMIAL
bool initialized() const
Returns true if the camera has been initialized.
double Ty() const
Returns the y-translation term of the projection matrix.
void unrectifyImage(const cv::Mat &rectified, cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Apply camera distortion to a rectified image.
const sensor_msgs::CameraInfo & cameraInfo() const
Returns the camera info message held internally.
PinholeCameraModel & operator=(const PinholeCameraModel &other)
cv::Point2d toReducedResolution(const cv::Point2d &uv_full) const
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
Project a rectified pixel to a 3d ray.
cv::Point2d toFullResolution(const cv::Point2d &uv_reduced) const
bool update(const T &new_val, T &my_val)
cv::Size reducedResolution() const
The resolution of the current rectified image.
cv::Rect unrectifyRoi(const cv::Rect &roi_rect) const
Compute the raw ROI best fitting a rectified ROI.
double cx() const
Returns the x coordinate of the optical center.
DistortionState distortion_state
cv::Mat_< double > K_binned
double cy() const
Returns the y coordinate of the optical center.
uint32_t binningX() const
Returns the number of columns in each bin.
cv::Point2d unrectifyPoint(const cv::Point2d &uv_rect) const
Compute the raw image coordinates of a pixel in the rectified image.
cv::Mat_< double > P_binned
void initRectificationMaps() const
sensor_msgs::CameraInfo cam_info_
void rectifyImage(const cv::Mat &raw, cv::Mat &rectified, int interpolation=cv::INTER_LINEAR) const
Rectify a raw camera image.
bool updateMat(const MatT &new_mat, MatT &my_mat, cv::Mat_< double > &cv_mat, int rows, int cols)
double Tx() const
Returns the x-translation term of the projection matrix.
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
Set the camera parameters from the sensor_msgs/CameraInfo message.
std::shared_ptr< Cache > cache_
const std::string PLUMB_BOB
Simplifies interpreting images geometrically using the parameters from sensor_msgs/CameraInfo.
cv::Rect rawRoi() const
The current raw ROI, as used for capture by the camera driver.
cv::Point2d rectifyPoint(const cv::Point2d &uv_raw) const
Compute the rectified image coordinates of a pixel in the raw image.
double fy() const
Returns the focal length (pixels) in y direction of the rectified image.
uint32_t binningY() const
Returns the number of rows in each bin.
cv::Point2d project3dToPixel(const cv::Point3d &xyz) const
Project a 3d point to rectified pixel coordinates.
double fx() const
Returns the focal length (pixels) in x direction of the rectified image.
cv::Rect rectifiedRoi() const
The current rectified ROI, which best fits the raw ROI.