stereo_camera_model.h

Go to the documentation of this file.

#ifndef IMAGE_GEOMETRY_STEREO_CAMERA_MODEL_H
#define IMAGE_GEOMETRY_STEREO_CAMERA_MODEL_H
 
#include "image_geometry/pinhole_camera_model.h"
#include "exports.h"
 
namespace image_geometry {
 
class IMAGE_GEOMETRY_DECL StereoCameraModel
{
public:
  StereoCameraModel();
 
  StereoCameraModel(const StereoCameraModel& other);
 
  StereoCameraModel& operator=(const StereoCameraModel& other);
 
  bool fromCameraInfo(const sensor_msgs::CameraInfo& left,
                      const sensor_msgs::CameraInfo& right);
 
  bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left,
                      const sensor_msgs::CameraInfoConstPtr& right);
 
  const PinholeCameraModel& left() const;
 
  const PinholeCameraModel& right() const;
 
  std::string tfFrame() const;
 
  void projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity, cv::Point3d& xyz) const;
 
  void projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud,
                                 bool handleMissingValues = false) const;
  static const double MISSING_Z;
  
  const cv::Matx44d& reprojectionMatrix() const;
 
  double baseline() const;
 
  double getZ(double disparity) const;
 
  double getDisparity(double Z) const;
 
  bool initialized() const { return left_.initialized() && right_.initialized(); }
protected:
  PinholeCameraModel left_, right_;
  cv::Matx44d Q_;
 
  void updateQ();
};
 
 
/* Trivial inline functions */
inline const PinholeCameraModel& StereoCameraModel::left() const  { return left_; }
inline const PinholeCameraModel& StereoCameraModel::right() const { return right_; }
 
inline std::string StereoCameraModel::tfFrame() const { return left_.tfFrame(); }
 
inline const cv::Matx44d& StereoCameraModel::reprojectionMatrix() const { return Q_; }
 
inline double StereoCameraModel::baseline() const
{
  return -right_.Tx() / right_.fx();
}
 
inline double StereoCameraModel::getZ(double disparity) const
{
  assert( initialized() );
  return -right_.Tx() / (disparity - (left().cx() - right().cx()));
}
 
inline double StereoCameraModel::getDisparity(double Z) const
{
  assert( initialized() );
  return -right_.Tx() / Z + (left().cx() - right().cx()); ;
}
 
} //namespace image_geometry
 
#endif