stereo_camera_model.cpp
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 bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& left,
00021                                        const sensor_msgs::CameraInfo& right)
00022 {
00023   bool changed_left  = left_.fromCameraInfo(left);
00024   bool changed_right = right_.fromCameraInfo(right);
00025   bool changed = changed_left || changed_right;
00026 
00027   // Note: don't require identical time stamps to allow imperfectly synced stereo.
00028   assert( left_.tfFrame() == right_.tfFrame() );
00029   assert( left_.fx() == right_.fx() );
00030   assert( left_.fy() == right_.fy() );
00031   assert( left_.cy() == right_.cy() );
00032   // cx may differ for verged cameras
00033 
00034   if (changed)
00035     updateQ();
00036 
00037   return changed;
00038 }
00039 
00040 bool StereoCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left,
00041                                        const sensor_msgs::CameraInfoConstPtr& right)
00042 {
00043   return fromCameraInfo(*left, *right);
00044 }
00045 
00046 void StereoCameraModel::updateQ()
00047 {
00048   // Update variable fields of reprojection matrix
00049   /*
00050     From Springer Handbook of Robotics, p. 524:
00051         [ Fx 0  Cx -FxTx ]
00052     P = [ 0  Fy Cy   0   ]
00053         [ 0  0  1    0   ]
00054 
00055         [ 1 0   0      -Cx      ]
00056     Q = [ 0 1   0      -Cy      ]
00057         [ 0 0   0       Fx      ]
00058         [ 0 0 -1/Tx (Cx-Cx')/Tx ]
00059     where primed parameters are from the left projection matrix, unprimed from the right.
00060 
00061     Disparity = x_left - x_right
00062    */
00063   double Tx = baseline();
00064   Q_(3,2) = 1.0 / Tx;
00065   Q_(0,3) = -right_.cx();
00066   Q_(1,3) = -right_.cy();
00067   Q_(2,3) = right_.fx();
00068   Q_(3,3) = (right_.cx() - left_.cx()) / Tx; // zero when disparities are pre-adjusted
00069 }
00070 
00071 void StereoCameraModel::projectDisparityTo3d(const cv::Point2d& left_uv_rect, float disparity,
00072                                              cv::Point3d& xyz) const
00073 {
00074   assert( initialized() );
00075 
00076   // Do the math inline:
00077   // [X Y Z W]^T = Q * [u v d 1]^T
00078   // Point = (X/W, Y/W, Z/W)
00079   // cv::perspectiveTransform could be used but with more overhead.
00080   double u = left_uv_rect.x, v = left_uv_rect.y;
00081   cv::Point3d XYZ(u + Q_(0,3), v + Q_(1,3), Q_(2,3));
00082   double W = Q_(3,2)*disparity + Q_(3,3);
00083   xyz = XYZ * (1.0/W);
00084 }
00085 
00086 const double StereoCameraModel::MISSING_Z = 10000.;
00087 
00088 void StereoCameraModel::projectDisparityImageTo3d(const cv::Mat& disparity, cv::Mat& point_cloud,
00089                                                   bool handleMissingValues) const
00090 {
00091   assert( initialized() );
00092 
00093   cv::reprojectImageTo3D(disparity, point_cloud, Q_, handleMissingValues);
00094 }
00095 
00096 } //namespace image_geometry


image_geometry
Author(s): Patrick Mihelich
autogenerated on Sat Dec 28 2013 17:44:01