Go to the documentation of this file.
2 #include <opencv2/calib3d/calib3d.hpp>
9 Q_(0,0) =
Q_(1,1) = 1.0;
13 : left_(other.left_), right_(other.right_),
16 Q_(0,0) =
Q_(1,1) = 1.0;
29 const sensor_msgs::CameraInfo& right)
33 bool changed = changed_left || changed_right;
49 const sensor_msgs::CameraInfoConstPtr& right)
117 cv::Point3d& xyz)
const
125 double u = left_uv_rect.x, v = left_uv_rect.y;
126 cv::Point3d XYZ( (
Q_(0,0) * u) +
Q_(0,3), (
Q_(1,1) * v) +
Q_(1,3),
Q_(2,3));
127 double W =
Q_(3,2)*disparity +
Q_(3,3);
134 bool handleMissingValues)
const
138 cv::reprojectImageTo3D(disparity, point_cloud,
Q_, handleMissingValues);
StereoCameraModel & operator=(const StereoCameraModel &other)
bool initialized() const
Returns true if the camera has been initialized.
std::string tfFrame() const
Get the name of the camera coordinate frame in tf.
const sensor_msgs::CameraInfo & cameraInfo() const
Returns the camera info message held internally.
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
Set the camera parameters from the sensor_msgs/CameraInfo message.
double fx() const
Returns the focal length (pixels) in x direction of the rectified image.
Simplifies interpreting stereo image pairs geometrically using the parameters from the left and right...
const PinholeCameraModel & right() const
Get the right monocular camera model.
double cy() const
Returns the y coordinate of the optical center.
static const double MISSING_Z
void projectDisparityImageTo3d(const cv::Mat &disparity, cv::Mat &point_cloud, bool handleMissingValues=false) const
Project a disparity image to a 3d point cloud.
PinholeCameraModel right_
void projectDisparityTo3d(const cv::Point2d &left_uv_rect, float disparity, cv::Point3d &xyz) const
Project a rectified pixel with disparity to a 3d point.
double fy() const
Returns the focal length (pixels) in y direction of the rectified image.
double cx() const
Returns the x coordinate of the optical center.
double baseline() const
Returns the horizontal baseline in world coordinates.
bool fromCameraInfo(const sensor_msgs::CameraInfo &left, const sensor_msgs::CameraInfo &right)
Set the camera parameters from the sensor_msgs/CameraInfo messages.
const PinholeCameraModel & left() const
Get the left monocular camera model.
image_geometry
Author(s): Patrick Mihelich
autogenerated on Wed Aug 21 2024 02:47:03