00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 00005 | | 00006 | Copyright (C) 2005-2011 University of Malaga | 00007 | | 00008 | This software was written by the Machine Perception and Intelligent | 00009 | Robotics Lab, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 00026 | | 00027 +---------------------------------------------------------------------------+ */ 00028 00029 #ifndef mrpt_vision_pinhole_H 00030 #define mrpt_vision_pinhole_H 00031 00032 #include <mrpt/utils/TCamera.h> 00033 #include <mrpt/vision/utils.h> 00034 00035 namespace mrpt 00036 { 00037 namespace vision 00038 { 00039 /** Functions related to pinhole camera models, point projections, etc. \ingroup mrpt_vision_grp */ 00040 namespace pinhole 00041 { 00042 /** \addtogroup mrpt_vision_grp 00043 * @{ */ 00044 00045 using mrpt::utils::TPixelCoordf; 00046 00047 /** Project a set of 3D points into a camera at an arbitrary 6D pose using its calibration matrix (undistorted projection model) 00048 * \param in_points_3D [IN] The list of 3D points in world coordinates (meters) to project. 00049 * \param cameraPose [IN] The pose of the camera in the world. 00050 * \param intrinsicParams [IN] The 3x3 calibration matrix. See http://www.mrpt.org/Camera_Parameters 00051 * \param projectedPoints [OUT] The list of image coordinates (in pixels) for the projected points. At output this list is resized to the same number of input points. 00052 * \param accept_points_behind [IN] See the note below. 00053 * 00054 * \note Points "behind" the camera (which couldn't be physically seen in the real world) are marked with pixel coordinates (-1,-1) to detect them as invalid, unless accept_points_behind is true. In that case they'll be projected normally. 00055 * 00056 * \sa projectPoints_with_distortion, projectPoint_no_distortion 00057 */ 00058 void VISION_IMPEXP projectPoints_no_distortion( 00059 const std::vector<mrpt::poses::CPoint3D> &in_points_3D, 00060 const mrpt::poses::CPose3D &cameraPose, 00061 const mrpt::math::CMatrixDouble33 & intrinsicParams, 00062 std::vector<TPixelCoordf> &projectedPoints, 00063 bool accept_points_behind = false 00064 ); 00065 00066 /** Project a single 3D point with global coordinates P into a camera at pose F, without distortion parameters. 00067 * The template argument INVERSE_CAM_POSE is related on how the camera pose "F" is stored: 00068 * - INVERSE_CAM_POSE:false -> The local coordinates of the feature wrt the camera F are: \f$ P \ominus F \f$ 00069 * - INVERSE_CAM_POSE:true -> The local coordinates of the feature wrt the camera F are: \f$ F \oplus P \f$ 00070 */ 00071 template <bool INVERSE_CAM_POSE> 00072 inline TPixelCoordf projectPoint_no_distortion( 00073 const mrpt::utils::TCamera &cam_params, 00074 const mrpt::poses::CPose3D &F, 00075 const mrpt::math::TPoint3D &P) 00076 { 00077 double x,y,z; // wrt cam (local coords) 00078 if (INVERSE_CAM_POSE) 00079 F.composePoint(P.x,P.y,P.z, x,y,z); 00080 else 00081 F.inverseComposePoint(P.x,P.y,P.z, x,y,z); 00082 ASSERT_(z!=0) 00083 // Pinhole model: 00084 return TPixelCoordf( 00085 cam_params.cx() + cam_params.fx() * x/z, 00086 cam_params.cy() + cam_params.fy() * y/z ); 00087 } 00088 00089 //! \overload 00090 template <typename POINT> 00091 inline void projectPoint_no_distortion( 00092 const POINT &in_point_wrt_cam, 00093 const mrpt::utils::TCamera &cam_params, 00094 TPixelCoordf &out_projectedPoints ) 00095 { 00096 ASSERT_(in_point_wrt_cam.z!=0) 00097 // Pinhole model: 00098 out_projectedPoints.x = cam_params.cx() + cam_params.fx() * in_point_wrt_cam.x/in_point_wrt_cam.z; 00099 out_projectedPoints.y = cam_params.cy() + cam_params.fy() * in_point_wrt_cam.y/in_point_wrt_cam.z; 00100 } 00101 00102 00103 /** Project a set of 3D points into a camera at an arbitrary 6D pose using its calibration matrix and distortion parameters (radial and tangential distortions projection model) 00104 * \param in_points_3D [IN] The list of 3D points in world coordinates (meters) to project. 00105 * \param cameraPose [IN] The pose of the camera in the world. 00106 * \param intrinsicParams [IN] The 3x3 calibration matrix. See http://www.mrpt.org/Camera_Parameters 00107 * \param distortionParams [IN] The 4-length vector with the distortion parameters [k1 k2 p1 p2]. See http://www.mrpt.org/Camera_Parameters 00108 * \param projectedPoints [OUT] The list of image coordinates (in pixels) for the projected points. At output this list is resized to the same number of input points. 00109 * \param accept_points_behind [IN] See the note below. 00110 * 00111 * \note Points "behind" the camera (which couldn't be physically seen in the real world) are marked with pixel coordinates (-1,-1) to detect them as invalid, unless accept_points_behind is true. In that case they'll be projected normally. 00112 * 00113 * \sa projectPoint_with_distortion, projectPoints_no_distortion 00114 */ 00115 void VISION_IMPEXP projectPoints_with_distortion( 00116 const std::vector<mrpt::poses::CPoint3D> &in_points_3D, 00117 const mrpt::poses::CPose3D &cameraPose, 00118 const mrpt::math::CMatrixDouble33 & intrinsicParams, 00119 const std::vector<double> & distortionParams, 00120 std::vector<TPixelCoordf> &projectedPoints, 00121 bool accept_points_behind = false 00122 ); 00123 00124 /** Project one 3D point into a camera using its calibration matrix and distortion parameters (radial and tangential distortions projection model) 00125 * \param in_point_wrt_cam [IN] The 3D point wrt the camera focus, with +Z=optical axis, +X=righthand in the image plane, +Y=downward in the image plane. 00126 * \param in_cam_params [IN] The camera parameters. See http://www.mrpt.org/Camera_Parameters 00127 * \param out_projectedPoints [OUT] The projected point, in pixel units. 00128 * \param accept_points_behind [IN] See the note below. 00129 * 00130 * \note Points "behind" the camera (which couldn't be physically seen in the real world) are marked with pixel coordinates (-1,-1) to detect them as invalid, unless accept_points_behind is true. In that case they'll be projected normally. 00131 * 00132 * \sa projectPoints_with_distortion 00133 */ 00134 void VISION_IMPEXP projectPoint_with_distortion( 00135 const mrpt::math::TPoint3D &in_point_wrt_cam, 00136 const mrpt::utils::TCamera &in_cam_params, 00137 TPixelCoordf &out_projectedPoints, 00138 bool accept_points_behind = false 00139 ); 00140 00141 //! \overload 00142 void VISION_IMPEXP projectPoints_with_distortion( 00143 const std::vector<mrpt::math::TPoint3D> &P, 00144 const mrpt::utils::TCamera ¶ms, 00145 const CPose3DQuat &cameraPose, 00146 std::vector<TPixelCoordf> &pixels, 00147 bool accept_points_behind = false 00148 ); 00149 00150 00151 /** Undistort a list of points given by their pixel coordinates, provided the camera matrix and distortion coefficients. 00152 * \param srcDistortedPixels [IN] The pixel coordinates as in the distorted image. 00153 * \param dstUndistortedPixels [OUT] The computed pixel coordinates without distortion. 00154 * \param intrinsicParams [IN] The 3x3 calibration matrix. See http://www.mrpt.org/Camera_Parameters 00155 * \param distortionParams [IN] The 4-length vector with the distortion parameters [k1 k2 p1 p2]. See http://www.mrpt.org/Camera_Parameters 00156 * \sa undistort_point 00157 */ 00158 void VISION_IMPEXP undistort_points( 00159 const std::vector<TPixelCoordf> &srcDistortedPixels, 00160 std::vector<TPixelCoordf> &dstUndistortedPixels, 00161 const mrpt::math::CMatrixDouble33 & intrinsicParams, 00162 const std::vector<double> & distortionParams ); 00163 00164 /** Undistort a list of points given by their pixel coordinates, provided the camera matrix and distortion coefficients. 00165 * \param srcDistortedPixels [IN] The pixel coordinates as in the distorted image. 00166 * \param dstUndistortedPixels [OUT] The computed pixel coordinates without distortion. 00167 * \param cameraModel [IN] The camera parameters. 00168 * \sa undistort_point 00169 */ 00170 void VISION_IMPEXP undistort_points( 00171 const std::vector<TPixelCoordf> &srcDistortedPixels, 00172 std::vector<TPixelCoordf> &dstUndistortedPixels, 00173 const mrpt::utils::TCamera &cameraModel); 00174 00175 /** Undistort one point given by its pixel coordinates and the camera parameters. 00176 * \sa undistort_points 00177 */ 00178 void VISION_IMPEXP undistort_point( 00179 const TPixelCoordf &inPt, 00180 TPixelCoordf &outPt, 00181 const mrpt::utils::TCamera &cameraModel); 00182 00183 /** @} */ // end of grouping 00184 } 00185 } 00186 } 00187 00188 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |