Go to the documentation of this file.00001 #include "image_geometry/stereo_camera_model.h"
00002
00003 namespace image_geometry {
00004
00005 StereoCameraModel::StereoCameraModel()
00006 : Q_(4, 4, 0.0)
00007 {
00008 Q_(0,0) = Q_(1,1) = 1.0;
00009 }
00010
00011 StereoCameraModel::StereoCameraModel(const StereoCameraModel& other)
00012 : left_(other.left_), right_(other.right_),
00013 Q_(4, 4, 0.0)
00014 {
00015 Q_(0,0) = Q_(1,1) = 1.0;
00016 if (other.initialized())
00017 updateQ();
00018 }
00019
00020 StereoCameraModel& StereoCameraModel::operator=(const StereoCameraModel& other)
00021 {
00022 if (other.initialized())
00023 this->fromCameraInfo(other.left_.cameraInfo(), other.right_.cameraInfo());
00024 return *this;
00025 }
00026
00027 bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& left,
00028 const sensor_msgs::CameraInfo& right)
00029 {
00030 bool changed_left = left_.fromCameraInfo(left);
00031 bool changed_right = right_.fromCameraInfo(right);
00032 bool changed = changed_left || changed_right;
00033
00034
00035 assert( left_.tfFrame() == right_.tfFrame() );
00036 assert( left_.fx() == right_.fx() );
00037 assert( left_.fy() == right_.fy() );
00038 assert( left_.cy() == right_.cy() );
00039
00040
00041 if (changed)
00042 updateQ();
00043
00044 return changed;
00045 }
00046
00047 bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left,
00048 const sensor_msgs::CameraInfoConstPtr& right)
00049 {
00050 return fromCameraInfo(*left, *right);
00051 }
00052
00053 void StereoCameraModel::updateQ()
00054 {
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070 double Tx = baseline();
00071 Q_(3,2) = 1.0 / Tx;
00072 Q_(0,3) = -right_.cx();
00073 Q_(1,3) = -right_.cy();
00074 Q_(2,3) = right_.fx();
00075 Q_(3,3) = (right_.cx() - left_.cx()) / Tx;
00076 }
00077
00078 void StereoCameraModel::projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity,
00079 cv::Point3d& xyz) const
00080 {
00081 assert( initialized() );
00082
00083
00084
00085
00086
00087 double u = left_uv_rect.x, v = left_uv_rect.y;
00088 cv::Point3d XYZ(u + Q_(0,3), v + Q_(1,3), Q_(2,3));
00089 double W = Q_(3,2)*disparity + Q_(3,3);
00090 xyz = XYZ * (1.0/W);
00091 }
00092
00093 const double StereoCameraModel::MISSING_Z = 10000.;
00094
00095 void StereoCameraModel::projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud,
00096 bool handleMissingValues) const
00097 {
00098 assert( initialized() );
00099
00100 cv::reprojectImageTo3D(disparity, point_cloud, Q_, handleMissingValues);
00101 }
00102
00103 }