stereo_camera_model.h
Go to the documentation of this file.00001 #ifndef IMAGE_GEOMETRY_STEREO_CAMERA_MODEL_H 00002 #define IMAGE_GEOMETRY_STEREO_CAMERA_MODEL_H 00003 00004 #include "image_geometry/pinhole_camera_model.h" 00005 00006 namespace image_geometry { 00007 00012 class StereoCameraModel 00013 { 00014 public: 00015 StereoCameraModel(); 00016 00017 StereoCameraModel(const StereoCameraModel& other); 00018 00022 bool fromCameraInfo(const sensor_msgs::CameraInfo& left, 00023 const sensor_msgs::CameraInfo& right); 00024 00028 bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left, 00029 const sensor_msgs::CameraInfoConstPtr& right); 00030 00034 const PinholeCameraModel& left() const; 00035 00039 const PinholeCameraModel& right() const; 00040 00047 std::string tfFrame() const; 00048 00052 void projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity, cv::Point3d& xyz) const; 00053 00060 void projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud, 00061 bool handleMissingValues = false) const; 00062 static const double MISSING_Z; 00063 00067 const cv::Mat_<double>& reprojectionMatrix() const; 00068 00072 double baseline() const; 00073 00079 double getZ(double disparity) const; 00080 00086 double getDisparity(double Z) const; 00087 00088 protected: 00089 PinholeCameraModel left_, right_; 00090 cv::Mat_<double> Q_; 00091 00092 void updateQ(); 00093 00094 bool initialized() const { return left_.initialized() && right_.initialized(); } 00095 }; 00096 00097 00098 /* Trivial inline functions */ 00099 inline const PinholeCameraModel& StereoCameraModel::left() const { return left_; } 00100 inline const PinholeCameraModel& StereoCameraModel::right() const { return right_; } 00101 00102 inline std::string StereoCameraModel::tfFrame() const { return left_.tfFrame(); } 00103 00104 inline const cv::Mat_<double>& StereoCameraModel::reprojectionMatrix() const { return Q_; } 00105 00106 inline double StereoCameraModel::baseline() const 00107 { 00109 return -right_.Tx() / right_.fx(); 00110 } 00111 00112 inline double StereoCameraModel::getZ(double disparity) const 00113 { 00114 assert( initialized() ); 00115 return -right_.Tx() / disparity; 00116 } 00117 00118 inline double StereoCameraModel::getDisparity(double Z) const 00119 { 00120 assert( initialized() ); 00121 return -right_.Tx() / Z; 00122 } 00123 00124 } //namespace image_geometry 00125 00126 #endif