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 
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


image_geometry
Author(s): Patrick Mihelich
autogenerated on Wed Aug 9 2017 02:51:50