1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. 14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved. 15 // Third party copyrights are property of their respective owners. 16 // 17 // Redistribution and use in source and binary forms, with or without modification, 18 // are permitted provided that the following conditions are met: 19 // 20 // * Redistribution's of source code must retain the above copyright notice, 21 // this list of conditions and the following disclaimer. 22 // 23 // * Redistribution's in binary form must reproduce the above copyright notice, 24 // this list of conditions and the following disclaimer in the documentation 25 // and/or other materials provided with the distribution. 26 // 27 // * The name of the copyright holders may not be used to endorse or promote products 28 // derived from this software without specific prior written permission. 29 // 30 // This software is provided by the copyright holders and contributors "as is" and 31 // any express or implied warranties, including, but not limited to, the implied 32 // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 // In no event shall the Intel Corporation or contributors be liable for any direct, 34 // indirect, incidental, special, exemplary, or consequential damages 35 // (including, but not limited to, procurement of substitute goods or services; 36 // loss of use, data, or profits; or business interruption) however caused 37 // and on any theory of liability, whether in contract, strict liability, 38 // or tort (including negligence or otherwise) arising in any way out of 39 // the use of this software, even if advised of the possibility of such damage. 40 // 41 //M*/ 42 43 #include "precomp.hpp" 44 #include "upnp.h" 45 #include "dls.h" 46 #include "epnp.h" 47 #include "p3p.h" 48 #include "opencv2/calib3d/calib3d_c.h" 49 50 #include <iostream> 51 52 namespace cv 53 { 54 55 bool solvePnP( InputArray _opoints, InputArray _ipoints, 56 InputArray _cameraMatrix, InputArray _distCoeffs, 57 OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) 58 { 59 Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); 60 int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); 61 CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); 62 63 Mat rvec, tvec; 64 if( flags != SOLVEPNP_ITERATIVE ) 65 useExtrinsicGuess = false; 66 67 if( useExtrinsicGuess ) 68 { 69 int rtype = _rvec.type(), ttype = _tvec.type(); 70 Size rsize = _rvec.size(), tsize = _tvec.size(); 71 CV_Assert( (rtype == CV_32F || rtype == CV_64F) && 72 (ttype == CV_32F || ttype == CV_64F) ); 73 CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) && 74 (tsize == Size(1, 3) || tsize == Size(3, 1)) ); 75 } 76 else 77 { 78 _rvec.create(3, 1, CV_64F); 79 _tvec.create(3, 1, CV_64F); 80 } 81 rvec = _rvec.getMat(); 82 tvec = _tvec.getMat(); 83 84 Mat cameraMatrix0 = _cameraMatrix.getMat(); 85 Mat distCoeffs0 = _distCoeffs.getMat(); 86 Mat cameraMatrix = Mat_<double>(cameraMatrix0); 87 Mat distCoeffs = Mat_<double>(distCoeffs0); 88 bool result = false; 89 90 if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) 91 { 92 Mat undistortedPoints; 93 undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); 94 epnp PnP(cameraMatrix, opoints, undistortedPoints); 95 96 Mat R; 97 PnP.compute_pose(R, tvec); 98 Rodrigues(R, rvec); 99 result = true; 100 } 101 else if (flags == SOLVEPNP_P3P) 102 { 103 CV_Assert( npoints == 4); 104 Mat undistortedPoints; 105 undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); 106 p3p P3Psolver(cameraMatrix); 107 108 Mat R; 109 result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); 110 if (result) 111 Rodrigues(R, rvec); 112 } 113 else if (flags == SOLVEPNP_ITERATIVE) 114 { 115 CvMat c_objectPoints = opoints, c_imagePoints = ipoints; 116 CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; 117 CvMat c_rvec = rvec, c_tvec = tvec; 118 cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix, 119 c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0, 120 &c_rvec, &c_tvec, useExtrinsicGuess ); 121 result = true; 122 } 123 /*else if (flags == SOLVEPNP_DLS) 124 { 125 Mat undistortedPoints; 126 undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); 127 128 dls PnP(opoints, undistortedPoints); 129 130 Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); 131 bool result = PnP.compute_pose(R, tvec); 132 if (result) 133 Rodrigues(R, rvec); 134 return result; 135 } 136 else if (flags == SOLVEPNP_UPNP) 137 { 138 upnp PnP(cameraMatrix, opoints, ipoints); 139 140 Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); 141 PnP.compute_pose(R, tvec); 142 Rodrigues(R, rvec); 143 return true; 144 }*/ 145 else 146 CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS"); 147 return result; 148 } 149 150 class PnPRansacCallback : public PointSetRegistrator::Callback 151 { 152 153 public: 154 155 PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=SOLVEPNP_ITERATIVE, 156 bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() ) 157 : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess), 158 rvec(_rvec), tvec(_tvec) {} 159 160 /* Pre: True */ 161 /* Post: compute _model with given points an return number of found models */ 162 int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const 163 { 164 Mat opoints = _m1.getMat(), ipoints = _m2.getMat(); 165 166 bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs, 167 rvec, tvec, useExtrinsicGuess, flags ); 168 169 Mat _local_model; 170 hconcat(rvec, tvec, _local_model); 171 _local_model.copyTo(_model); 172 173 return correspondence; 174 } 175 176 /* Pre: True */ 177 /* Post: fill _err with projection errors */ 178 void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const 179 { 180 181 Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat(); 182 183 int i, count = opoints.checkVector(3); 184 Mat _rvec = model.col(0); 185 Mat _tvec = model.col(1); 186 187 188 Mat projpoints(count, 2, CV_32FC1); 189 projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints); 190 191 const Point2f* ipoints_ptr = ipoints.ptr<Point2f>(); 192 const Point2f* projpoints_ptr = projpoints.ptr<Point2f>(); 193 194 _err.create(count, 1, CV_32FC1); 195 float* err = _err.getMat().ptr<float>(); 196 197 for ( i = 0; i < count; ++i) 198 err[i] = (float)norm( ipoints_ptr[i] - projpoints_ptr[i] ); 199 200 } 201 202 203 Mat cameraMatrix; 204 Mat distCoeffs; 205 int flags; 206 bool useExtrinsicGuess; 207 Mat rvec; 208 Mat tvec; 209 }; 210 211 bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, 212 InputArray _cameraMatrix, InputArray _distCoeffs, 213 OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, 214 int iterationsCount, float reprojectionError, double confidence, 215 OutputArray _inliers, int flags) 216 { 217 218 Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat(); 219 Mat opoints, ipoints; 220 if( opoints0.depth() == CV_64F || !opoints0.isContinuous() ) 221 opoints0.convertTo(opoints, CV_32F); 222 else 223 opoints = opoints0; 224 if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() ) 225 ipoints0.convertTo(ipoints, CV_32F); 226 else 227 ipoints = ipoints0; 228 229 int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); 230 CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); 231 232 CV_Assert(opoints.isContinuous()); 233 CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F); 234 CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3); 235 CV_Assert(ipoints.isContinuous()); 236 CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F); 237 CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2); 238 239 _rvec.create(3, 1, CV_64FC1); 240 _tvec.create(3, 1, CV_64FC1); 241 242 Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1); 243 Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1); 244 Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); 245 246 int model_points = 5; 247 int ransac_kernel_method = SOLVEPNP_EPNP; 248 249 if( npoints == 4 ) 250 { 251 model_points = 4; 252 ransac_kernel_method = SOLVEPNP_P3P; 253 } 254 255 Ptr<PointSetRegistrator::Callback> cb; // pointer to callback 256 cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec); 257 258 double param1 = reprojectionError; // reprojection error 259 double param2 = confidence; // confidence 260 int param3 = iterationsCount; // number maximum iterations 261 262 Mat _local_model(3, 2, CV_64FC1); 263 Mat _mask_local_inliers(1, opoints.rows, CV_8UC1); 264 265 // call Ransac 266 int result = createRANSACPointSetRegistrator(cb, model_points, 267 param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers); 268 269 if( result > 0 ) 270 { 271 vector<Point3d> opoints_inliers; 272 vector<Point2d> ipoints_inliers; 273 opoints.convertTo(opoints_inliers, CV_64F); 274 ipoints.convertTo(ipoints_inliers, CV_64F); 275 276 const uchar* mask = _mask_local_inliers.ptr<uchar>(); 277 int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints); 278 compressElems(&ipoints_inliers[0], mask, 1, npoints); 279 280 opoints_inliers.resize(npoints1); 281 ipoints_inliers.resize(npoints1); 282 result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix, 283 distCoeffs, rvec, tvec, false, flags == SOLVEPNP_P3P ? SOLVEPNP_EPNP : flags) ? 1 : -1; 284 } 285 286 if( result <= 0 || _local_model.rows <= 0) 287 { 288 _rvec.assign(rvec); // output rotation vector 289 _tvec.assign(tvec); // output translation vector 290 291 if( _inliers.needed() ) 292 _inliers.release(); 293 294 return false; 295 } 296 else 297 { 298 _rvec.assign(_local_model.col(0)); // output rotation vector 299 _tvec.assign(_local_model.col(1)); // output translation vector 300 } 301 302 if(_inliers.needed()) 303 { 304 Mat _local_inliers; 305 for (int i = 0; i < npoints; ++i) 306 { 307 if((int)_mask_local_inliers.at<uchar>(i) != 0) // inliers mask 308 _local_inliers.push_back(i); // output inliers vector 309 } 310 _local_inliers.copyTo(_inliers); 311 } 312 return true; 313 } 314 315 } 316
