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"

namespace image_geometry {

class 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