stereo_camera_model.cpp
Go to the documentation of this file.
2 #include <opencv2/calib3d/calib3d.hpp>
3 
4 namespace image_geometry {
5 
7  : Q_(0.0)
8 {
9  Q_(0,0) = Q_(1,1) = 1.0;
10 }
11 
13  : left_(other.left_), right_(other.right_),
14  Q_(0.0)
15 {
16  Q_(0,0) = Q_(1,1) = 1.0;
17  if (other.initialized())
18  updateQ();
19 }
20 
22 {
23  if (other.initialized())
24  this->fromCameraInfo(other.left_.cameraInfo(), other.right_.cameraInfo());
25  return *this;
26 }
27 
28 bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& left,
29  const sensor_msgs::CameraInfo& right)
30 {
31  bool changed_left = left_.fromCameraInfo(left);
32  bool changed_right = right_.fromCameraInfo(right);
33  bool changed = changed_left || changed_right;
34 
35  // Note: don't require identical time stamps to allow imperfectly synced stereo.
36  assert( left_.tfFrame() == right_.tfFrame() );
37  assert( left_.fx() == right_.fx() );
38  assert( left_.fy() == right_.fy() );
39  assert( left_.cy() == right_.cy() );
40  // cx may differ for verged cameras
41 
42  if (changed)
43  updateQ();
44 
45  return changed;
46 }
47 
48 bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left,
49  const sensor_msgs::CameraInfoConstPtr& right)
50 {
51  return fromCameraInfo(*left, *right);
52 }
53 
55 {
56  // Update variable fields of reprojection matrix
57  /*
58  From Springer Handbook of Robotics, p. 524:
59 
60  [ Fx 0 Cx 0 ]
61  P = [ 0 Fy Cy 0 ]
62  [ 0 0 1 0 ]
63 
64  [ Fx 0 Cx' FxTx ]
65  P' = [ 0 Fy Cy 0 ]
66  [ 0 0 1 0 ]
67  where primed parameters are from the left projection matrix, unprimed from the right.
68 
69  [u v 1]^T = P * [x y z 1]^T
70  [u-d v 1]^T = P' * [x y z 1]^T
71 
72  Combining the two equations above results in the following equation
73 
74  [u v u-d 1]^T = [ Fx 0 Cx 0 ] * [ x y z 1]^T
75  [ 0 Fy Cy 0 ]
76  [ Fx 0 Cx' FxTx ]
77  [ 0 0 1 0 ]
78 
79  Subtracting the 3rd from from the first and inverting the expression
80  results in the following equation.
81 
82  [x y z 1]^T = Q * [u v d 1]^T
83 
84  Where Q is defined as
85 
86  Q = [ FyTx 0 0 -FyCxTx ]
87  [ 0 FxTx 0 -FxCyTx ]
88  [ 0 0 0 FxFyTx ]
89  [ 0 0 -Fy Fy(Cx-Cx') ]
90 
91  Using the assumption Fx = Fy Q can be simplified to the following. But for
92  compatibility with stereo cameras with different focal lengths we will use
93  the full Q matrix.
94 
95  [ 1 0 0 -Cx ]
96  Q = [ 0 1 0 -Cy ]
97  [ 0 0 0 Fx ]
98  [ 0 0 -1/Tx (Cx-Cx')/Tx ]
99 
100  Disparity = x_left - x_right
101 
102  For compatibility with stereo cameras with different focal lengths we will use
103  the full Q matrix.
104 
105  */
106  double Tx = -baseline(); // The baseline member negates our Tx. Undo this negation
107  Q_(0,0) = left_.fy() * Tx;
108  Q_(0,3) = -left_.fy() * left_.cx() * Tx;
109  Q_(1,1) = left_.fx() * Tx;
110  Q_(1,3) = -left_.fx() * left_.cy() * Tx;
111  Q_(2,3) = left_.fx() * left_.fy() * Tx;
112  Q_(3,2) = -left_.fy();
113  Q_(3,3) = left_.fy() * (left_.cx() - right_.cx()); // zero when disparities are pre-adjusted
114 }
115 
116 void StereoCameraModel::projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity,
117  cv::Point3d& xyz) const
118 {
119  assert( initialized() );
120 
121  // Do the math inline:
122  // [X Y Z W]^T = Q * [u v d 1]^T
123  // Point = (X/W, Y/W, Z/W)
124  // cv::perspectiveTransform could be used but with more overhead.
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);
128  xyz = XYZ * (1.0/W);
129 }
130 
131 const double StereoCameraModel::MISSING_Z = 10000.;
132 
133 void StereoCameraModel::projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud,
134  bool handleMissingValues) const
135 {
136  assert( initialized() );
137 
138  cv::reprojectImageTo3D(disparity, point_cloud, Q_, handleMissingValues);
139 }
140 
141 } //namespace image_geometry
image_geometry::StereoCameraModel::operator=
StereoCameraModel & operator=(const StereoCameraModel &other)
Definition: stereo_camera_model.cpp:21
image_geometry::StereoCameraModel::initialized
bool initialized() const
Returns true if the camera has been initialized.
Definition: stereo_camera_model.h:94
image_geometry
Definition: pinhole_camera_model.h:12
image_geometry::PinholeCameraModel::tfFrame
std::string tfFrame() const
Get the name of the camera coordinate frame in tf.
Definition: pinhole_camera_model.h:302
stereo_camera_model.h
image_geometry::PinholeCameraModel::cameraInfo
const sensor_msgs::CameraInfo & cameraInfo() const
Returns the camera info message held internally.
Definition: pinhole_camera_model.h:314
image_geometry::StereoCameraModel::Q_
cv::Matx44d Q_
Definition: stereo_camera_model.h:97
image_geometry::PinholeCameraModel::fromCameraInfo
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
Set the camera parameters from the sensor_msgs/CameraInfo message.
Definition: pinhole_camera_model.cpp:97
image_geometry::PinholeCameraModel::fx
double fx() const
Returns the focal length (pixels) in x direction of the rectified image.
Definition: pinhole_camera_model.h:322
image_geometry::StereoCameraModel::StereoCameraModel
StereoCameraModel()
Definition: stereo_camera_model.cpp:6
image_geometry::StereoCameraModel
Simplifies interpreting stereo image pairs geometrically using the parameters from the left and right...
Definition: stereo_camera_model.h:13
image_geometry::StereoCameraModel::right
const PinholeCameraModel & right() const
Get the right monocular camera model.
Definition: stereo_camera_model.h:105
image_geometry::PinholeCameraModel::cy
double cy() const
Returns the y coordinate of the optical center.
Definition: pinhole_camera_model.h:325
image_geometry::StereoCameraModel::MISSING_Z
static const double MISSING_Z
Definition: stereo_camera_model.h:65
image_geometry::StereoCameraModel::updateQ
void updateQ()
Definition: stereo_camera_model.cpp:54
image_geometry::StereoCameraModel::projectDisparityImageTo3d
void projectDisparityImageTo3d(const cv::Mat &disparity, cv::Mat &point_cloud, bool handleMissingValues=false) const
Project a disparity image to a 3d point cloud.
Definition: stereo_camera_model.cpp:133
image_geometry::StereoCameraModel::right_
PinholeCameraModel right_
Definition: stereo_camera_model.h:96
image_geometry::StereoCameraModel::left_
PinholeCameraModel left_
Definition: stereo_camera_model.h:96
image_geometry::StereoCameraModel::projectDisparityTo3d
void projectDisparityTo3d(const cv::Point2d &left_uv_rect, float disparity, cv::Point3d &xyz) const
Project a rectified pixel with disparity to a 3d point.
Definition: stereo_camera_model.cpp:116
image_geometry::PinholeCameraModel::fy
double fy() const
Returns the focal length (pixels) in y direction of the rectified image.
Definition: pinhole_camera_model.h:323
image_geometry::PinholeCameraModel::cx
double cx() const
Returns the x coordinate of the optical center.
Definition: pinhole_camera_model.h:324
image_geometry::StereoCameraModel::baseline
double baseline() const
Returns the horizontal baseline in world coordinates.
Definition: stereo_camera_model.h:111
image_geometry::StereoCameraModel::fromCameraInfo
bool fromCameraInfo(const sensor_msgs::CameraInfo &left, const sensor_msgs::CameraInfo &right)
Set the camera parameters from the sensor_msgs/CameraInfo messages.
Definition: stereo_camera_model.cpp:28
image_geometry::StereoCameraModel::left
const PinholeCameraModel & left() const
Get the left monocular camera model.
Definition: stereo_camera_model.h:104


image_geometry
Author(s): Patrick Mihelich
autogenerated on Wed Aug 21 2024 02:47:03