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 00092 bool initialized() const { return left_.initialized() && right_.initialized(); } 00093 protected: 00094 PinholeCameraModel left_, right_; 00095 cv::Mat_<double> Q_; 00096 00097 void updateQ(); 00098 }; 00099 00100 00101 /* Trivial inline functions */ 00102 inline const PinholeCameraModel& StereoCameraModel::left() const { return left_; } 00103 inline const PinholeCameraModel& StereoCameraModel::right() const { return right_; } 00104 00105 inline std::string StereoCameraModel::tfFrame() const { return left_.tfFrame(); } 00106 00107 inline const cv::Mat_<double>& StereoCameraModel::reprojectionMatrix() const { return Q_; } 00108 00109 inline double StereoCameraModel::baseline() const 00110 { 00112 return -right_.Tx() / right_.fx(); 00113 } 00114 00115 inline double StereoCameraModel::getZ(double disparity) const 00116 { 00117 assert( initialized() ); 00118 return -right_.Tx() / (disparity - (left().cx() - right().cx())); 00119 } 00120 00121 inline double StereoCameraModel::getDisparity(double Z) const 00122 { 00123 assert( initialized() ); 00124 return -right_.Tx() / Z + (left().cx() - right().cx()); ; 00125 } 00126 00127 } //namespace image_geometry 00128 00129 #endif