Program Listing for File pinhole_camera_model.hpp
↰ Return to documentation for file (include/image_geometry/pinhole_camera_model.hpp
)
#ifndef IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_HPP_
#define IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_HPP_
#include "image_geometry/visibility_control.hpp"
#include <sensor_msgs/msg/camera_info.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <stdexcept>
#include <string>
#include <math.h>
namespace image_geometry {
class Exception : public std::runtime_error
{
public:
Exception(const std::string& description) : std::runtime_error(description) {}
};
class PinholeCameraModel
{
public:
IMAGE_GEOMETRY_PUBLIC
PinholeCameraModel();
IMAGE_GEOMETRY_PUBLIC
PinholeCameraModel(const PinholeCameraModel& other);
IMAGE_GEOMETRY_PUBLIC
PinholeCameraModel& operator=(const PinholeCameraModel& other);
IMAGE_GEOMETRY_PUBLIC
bool fromCameraInfo(const sensor_msgs::msg::CameraInfo& msg);
IMAGE_GEOMETRY_PUBLIC
bool fromCameraInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg);
IMAGE_GEOMETRY_PUBLIC
std::string tfFrame() const;
IMAGE_GEOMETRY_PUBLIC
builtin_interfaces::msg::Time stamp() const;
IMAGE_GEOMETRY_PUBLIC
cv::Size fullResolution() const;
IMAGE_GEOMETRY_PUBLIC
cv::Size reducedResolution() const;
IMAGE_GEOMETRY_PUBLIC
cv::Point2d toFullResolution(const cv::Point2d& uv_reduced) const;
IMAGE_GEOMETRY_PUBLIC
cv::Rect toFullResolution(const cv::Rect& roi_reduced) const;
IMAGE_GEOMETRY_PUBLIC
cv::Point2d toReducedResolution(const cv::Point2d& uv_full) const;
IMAGE_GEOMETRY_PUBLIC
cv::Rect toReducedResolution(const cv::Rect& roi_full) const;
IMAGE_GEOMETRY_PUBLIC
cv::Rect rawRoi() const;
IMAGE_GEOMETRY_PUBLIC
cv::Rect rectifiedRoi() const;
IMAGE_GEOMETRY_PUBLIC
cv::Point2d project3dToPixel(const cv::Point3d& xyz) const;
IMAGE_GEOMETRY_PUBLIC
cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect) const;
cv::Point3d projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const;
IMAGE_GEOMETRY_PUBLIC
void rectifyImage(const cv::Mat& raw, cv::Mat& rectified,
int interpolation = cv::INTER_LINEAR) const;
IMAGE_GEOMETRY_PUBLIC
void unrectifyImage(const cv::Mat& rectified, cv::Mat& raw,
int interpolation = cv::INTER_LINEAR) const;
IMAGE_GEOMETRY_PUBLIC
cv::Point2d rectifyPoint(const cv::Point2d& uv_raw) const;
cv::Point2d rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const;
IMAGE_GEOMETRY_PUBLIC
cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect) const;
cv::Point2d unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const;
IMAGE_GEOMETRY_PUBLIC
cv::Rect rectifyRoi(const cv::Rect& roi_raw) const;
IMAGE_GEOMETRY_PUBLIC
cv::Rect unrectifyRoi(const cv::Rect& roi_rect) const;
IMAGE_GEOMETRY_PUBLIC
const sensor_msgs::msg::CameraInfo& cameraInfo() const;
IMAGE_GEOMETRY_PUBLIC
const cv::Matx33d& intrinsicMatrix() const;
IMAGE_GEOMETRY_PUBLIC
const cv::Mat_<double>& distortionCoeffs() const;
IMAGE_GEOMETRY_PUBLIC
const cv::Matx33d& rotationMatrix() const;
IMAGE_GEOMETRY_PUBLIC
const cv::Matx34d& projectionMatrix() const;
IMAGE_GEOMETRY_PUBLIC
const cv::Matx33d& fullIntrinsicMatrix() const;
IMAGE_GEOMETRY_PUBLIC
const cv::Matx34d& fullProjectionMatrix() const;
IMAGE_GEOMETRY_PUBLIC
double fx() const;
IMAGE_GEOMETRY_PUBLIC
double fy() const;
IMAGE_GEOMETRY_PUBLIC
double cx() const;
IMAGE_GEOMETRY_PUBLIC
double cy() const;
IMAGE_GEOMETRY_PUBLIC
double Tx() const;
IMAGE_GEOMETRY_PUBLIC
double Ty() const;
double fovX() const;
double fovY() const;
IMAGE_GEOMETRY_PUBLIC
uint32_t binningX() const;
IMAGE_GEOMETRY_PUBLIC
uint32_t binningY() const;
IMAGE_GEOMETRY_PUBLIC
double getDeltaU(double deltaX, double Z) const;
IMAGE_GEOMETRY_PUBLIC
double getDeltaV(double deltaY, double Z) const;
IMAGE_GEOMETRY_PUBLIC
double getDeltaX(double deltaU, double Z) const;
IMAGE_GEOMETRY_PUBLIC
double getDeltaY(double deltaV, double Z) const;
IMAGE_GEOMETRY_PUBLIC
bool initialized() const { return (bool)cache_; }
protected:
sensor_msgs::msg::CameraInfo cam_info_;
cv::Mat_<double> D_; // Unaffected by binning, ROI
cv::Matx33d R_; // Unaffected by binning, ROI
cv::Matx33d K_; // Describe current image (includes binning, ROI)
cv::Matx34d P_; // Describe current image (includes binning, ROI)
cv::Matx33d K_full_; // Describe full-res image, needed for full maps
cv::Matx34d P_full_; // Describe full-res image, needed for full maps
// Use PIMPL here so we can change internals in patch updates if needed
struct Cache;
std::shared_ptr<Cache> cache_; // Holds cached data for internal use
IMAGE_GEOMETRY_PUBLIC
void initRectificationMaps() const;
void initUnrectificationMaps() const;
friend class StereoCameraModel;
};
/* Trivial inline functions */
IMAGE_GEOMETRY_PUBLIC
inline std::string PinholeCameraModel::tfFrame() const
{
assert( initialized() );
return cam_info_.header.frame_id;
}
IMAGE_GEOMETRY_PUBLIC
inline builtin_interfaces::msg::Time PinholeCameraModel::stamp() const
{
assert( initialized() );
return cam_info_.header.stamp;
}
IMAGE_GEOMETRY_PUBLIC
inline const sensor_msgs::msg::CameraInfo& PinholeCameraModel::cameraInfo() const { return cam_info_; }
IMAGE_GEOMETRY_PUBLIC
inline const cv::Matx33d& PinholeCameraModel::intrinsicMatrix() const { return K_; }
IMAGE_GEOMETRY_PUBLIC
inline const cv::Mat_<double>& PinholeCameraModel::distortionCoeffs() const { return D_; }
IMAGE_GEOMETRY_PUBLIC
inline const cv::Matx33d& PinholeCameraModel::rotationMatrix() const { return R_; }
IMAGE_GEOMETRY_PUBLIC
inline const cv::Matx34d& PinholeCameraModel::projectionMatrix() const { return P_; }
IMAGE_GEOMETRY_PUBLIC
inline const cv::Matx33d& PinholeCameraModel::fullIntrinsicMatrix() const { return K_full_; }
IMAGE_GEOMETRY_PUBLIC
inline const cv::Matx34d& PinholeCameraModel::fullProjectionMatrix() const { return P_full_; }
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::fx() const { return P_(0,0); }
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::fy() const { return P_(1,1); }
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::cx() const { return P_(0,2); }
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::cy() const { return P_(1,2); }
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::Tx() const { return P_(0,3); }
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::Ty() const { return P_(1,3); }
inline double PinholeCameraModel::fovX() const {
return 2 * atan(rawRoi().width / (2 * fx()));
}
inline double PinholeCameraModel::fovY() const {
return 2 * atan(rawRoi().height / (2 * fy()));
}
IMAGE_GEOMETRY_PUBLIC
inline uint32_t PinholeCameraModel::binningX() const { return cam_info_.binning_x; }
IMAGE_GEOMETRY_PUBLIC
inline uint32_t PinholeCameraModel::binningY() const { return cam_info_.binning_y; }
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::getDeltaU(double deltaX, double Z) const
{
assert( initialized() );
return fx() * deltaX / Z;
}
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::getDeltaV(double deltaY, double Z) const
{
assert( initialized() );
return fy() * deltaY / Z;
}
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::getDeltaX(double deltaU, double Z) const
{
assert( initialized() );
return Z * deltaU / fx();
}
IMAGE_GEOMETRY_PUBLIC
inline double PinholeCameraModel::getDeltaY(double deltaV, double Z) const
{
assert( initialized() );
return Z * deltaV / fy();
}
} // namespace image_geometry
#endif // IMAGE_GEOMETRY__PINHOLE_CAMERA_MODEL_HPP_