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 00019 StereoCameraModel& operator=(const StereoCameraModel& other); 00020 00024 bool fromCameraInfo(const sensor_msgs::CameraInfo& left, 00025 const sensor_msgs::CameraInfo& right); 00026 00030 bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left, 00031 const sensor_msgs::CameraInfoConstPtr& right); 00032 00036 const PinholeCameraModel& left() const; 00037 00041 const PinholeCameraModel& right() const; 00042 00049 std::string tfFrame() const; 00050 00054 void projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity, cv::Point3d& xyz) const; 00055 00062 void projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud, 00063 bool handleMissingValues = false) const; 00064 static const double MISSING_Z; 00065 00069 const cv::Matx44d& reprojectionMatrix() const; 00070 00074 double baseline() const; 00075 00081 double getZ(double disparity) const; 00082 00088 double getDisparity(double Z) const; 00089 00093 bool initialized() const { return left_.initialized() && right_.initialized(); } 00094 protected: 00095 PinholeCameraModel left_, right_; 00096 cv::Matx44d Q_; 00097 00098 void updateQ(); 00099 }; 00100 00101 00102 /* Trivial inline functions */ 00103 inline const PinholeCameraModel& StereoCameraModel::left() const { return left_; } 00104 inline const PinholeCameraModel& StereoCameraModel::right() const { return right_; } 00105 00106 inline std::string StereoCameraModel::tfFrame() const { return left_.tfFrame(); } 00107 00108 inline const cv::Matx44d& StereoCameraModel::reprojectionMatrix() const { return Q_; } 00109 00110 inline double StereoCameraModel::baseline() const 00111 { 00113 return -right_.Tx() / right_.fx(); 00114 } 00115 00116 inline double StereoCameraModel::getZ(double disparity) const 00117 { 00118 assert( initialized() ); 00119 return -right_.Tx() / (disparity - (left().cx() - right().cx())); 00120 } 00121 00122 inline double StereoCameraModel::getDisparity(double Z) const 00123 { 00124 assert( initialized() ); 00125 return -right_.Tx() / Z + (left().cx() - right().cx()); ; 00126 } 00127 00128 } //namespace image_geometry 00129 00130 #endif