Program Listing for File stereo_camera_model.h

↰ Return to documentation for file (/tmp/ws/src/vision_opencv/image_geometry/include/image_geometry/stereo_camera_model.h)

#ifndef IMAGE_GEOMETRY__STEREO_CAMERA_MODEL_H
#define IMAGE_GEOMETRY__STEREO_CAMERA_MODEL_H

#include "image_geometry/pinhole_camera_model.h"
#include "image_geometry/visibility_control.hpp"

namespace image_geometry {

class StereoCameraModel
{
public:
  IMAGE_GEOMETRY_PUBLIC
  StereoCameraModel();

  IMAGE_GEOMETRY_PUBLIC
  StereoCameraModel(const StereoCameraModel& other);

  IMAGE_GEOMETRY_PUBLIC
  StereoCameraModel& operator=(const StereoCameraModel& other);

  IMAGE_GEOMETRY_PUBLIC
  bool fromCameraInfo(const sensor_msgs::msg::CameraInfo& left,
                      const sensor_msgs::msg::CameraInfo& right);

  IMAGE_GEOMETRY_PUBLIC
  bool fromCameraInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& left,
                      const sensor_msgs::msg::CameraInfo::ConstSharedPtr& right);

  IMAGE_GEOMETRY_PUBLIC
  const PinholeCameraModel& left() const;

  IMAGE_GEOMETRY_PUBLIC
  const PinholeCameraModel& right() const;

  IMAGE_GEOMETRY_PUBLIC
  std::string tfFrame() const;

  IMAGE_GEOMETRY_PUBLIC
  void projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity, cv::Point3d& xyz) const;

  IMAGE_GEOMETRY_PUBLIC
  void projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud,
                                 bool handleMissingValues = false) const;
  static const double MISSING_Z;

  IMAGE_GEOMETRY_PUBLIC
  const cv::Matx44d& reprojectionMatrix() const;

  IMAGE_GEOMETRY_PUBLIC
  double baseline() const;

  IMAGE_GEOMETRY_PUBLIC
  double getZ(double disparity) const;

  IMAGE_GEOMETRY_PUBLIC
  double getDisparity(double Z) const;

  IMAGE_GEOMETRY_PUBLIC
  bool initialized() const { return left_.initialized() && right_.initialized(); }
protected:
  PinholeCameraModel left_, right_;
  cv::Matx44d Q_;

  IMAGE_GEOMETRY_PUBLIC
  void updateQ();
};


/* Trivial inline functions */
IMAGE_GEOMETRY_PUBLIC
inline const PinholeCameraModel& StereoCameraModel::left() const  { return left_; }
IMAGE_GEOMETRY_PUBLIC
inline const PinholeCameraModel& StereoCameraModel::right() const { return right_; }

IMAGE_GEOMETRY_PUBLIC
inline std::string StereoCameraModel::tfFrame() const { return left_.tfFrame(); }

IMAGE_GEOMETRY_PUBLIC
inline const cv::Matx44d& StereoCameraModel::reprojectionMatrix() const { return Q_; }

IMAGE_GEOMETRY_PUBLIC
inline double StereoCameraModel::baseline() const
{
  return -right_.Tx() / right_.fx();
}

IMAGE_GEOMETRY_PUBLIC
inline double StereoCameraModel::getZ(double disparity) const
{
  assert( initialized() );
  return -right_.Tx() / (disparity - (left().cx() - right().cx()));
}

IMAGE_GEOMETRY_PUBLIC
inline double StereoCameraModel::getDisparity(double Z) const
{
  assert( initialized() );
  return -right_.Tx() / Z + (left().cx() - right().cx()); ;
}

}  // namespace image_geometry

#endif