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 "ap3p.h"
49 #include "ippe.hpp"
50 #include "sqpnp.hpp"
51 #include "calib3d_c_api.h"
52
53 #include "usac.hpp"
54
55 #include <opencv2/core/utils/logger.hpp>
56
57 namespace cv
58 {
59 #if defined _DEBUG || defined CV_STATIC_ANALYSIS
isPlanarObjectPoints(InputArray _objectPoints,double threshold)60 static bool isPlanarObjectPoints(InputArray _objectPoints, double threshold)
61 {
62 CV_CheckType(_objectPoints.type(), _objectPoints.type() == CV_32FC3 || _objectPoints.type() == CV_64FC3,
63 "Type of _objectPoints must be CV_32FC3 or CV_64FC3");
64 Mat objectPoints;
65 if (_objectPoints.type() == CV_32FC3)
66 {
67 _objectPoints.getMat().convertTo(objectPoints, CV_64F);
68 }
69 else
70 {
71 objectPoints = _objectPoints.getMat();
72 }
73
74 Scalar meanValues = mean(objectPoints);
75 int nbPts = objectPoints.checkVector(3, CV_64F);
76 Mat objectPointsCentred = objectPoints - meanValues;
77 objectPointsCentred = objectPointsCentred.reshape(1, nbPts);
78
79 Mat w, u, vt;
80 Mat MM = objectPointsCentred.t() * objectPointsCentred;
81 SVDecomp(MM, w, u, vt);
82
83 return (w.at<double>(2) < w.at<double>(1) * threshold);
84 }
85
approxEqual(double a,double b,double eps)86 static bool approxEqual(double a, double b, double eps)
87 {
88 return std::fabs(a-b) < eps;
89 }
90 #endif
91
drawFrameAxes(InputOutputArray image,InputArray cameraMatrix,InputArray distCoeffs,InputArray rvec,InputArray tvec,float length,int thickness)92 void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs,
93 InputArray rvec, InputArray tvec, float length, int thickness)
94 {
95 CV_INSTRUMENT_REGION();
96
97 int type = image.type();
98 int cn = CV_MAT_CN(type);
99 CV_CheckType(type, cn == 1 || cn == 3 || cn == 4,
100 "Number of channels must be 1, 3 or 4" );
101
102 CV_Assert(image.getMat().total() > 0);
103 CV_Assert(length > 0);
104
105 // project axes points
106 vector<Point3f> axesPoints;
107 axesPoints.push_back(Point3f(0, 0, 0));
108 axesPoints.push_back(Point3f(length, 0, 0));
109 axesPoints.push_back(Point3f(0, length, 0));
110 axesPoints.push_back(Point3f(0, 0, length));
111 vector<Point2f> imagePoints;
112 projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);
113
114 // draw axes lines
115 line(image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255), thickness);
116 line(image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0), thickness);
117 line(image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0), thickness);
118 }
119
solvePnP(InputArray opoints,InputArray ipoints,InputArray cameraMatrix,InputArray distCoeffs,OutputArray rvec,OutputArray tvec,bool useExtrinsicGuess,int flags)120 bool solvePnP( InputArray opoints, InputArray ipoints,
121 InputArray cameraMatrix, InputArray distCoeffs,
122 OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int flags )
123 {
124 CV_INSTRUMENT_REGION();
125
126 vector<Mat> rvecs, tvecs;
127 int solutions = solvePnPGeneric(opoints, ipoints, cameraMatrix, distCoeffs, rvecs, tvecs, useExtrinsicGuess, (SolvePnPMethod)flags, rvec, tvec);
128
129 if (solutions > 0)
130 {
131 int rdepth = rvec.empty() ? CV_64F : rvec.depth();
132 int tdepth = tvec.empty() ? CV_64F : tvec.depth();
133 rvecs[0].convertTo(rvec, rdepth);
134 tvecs[0].convertTo(tvec, tdepth);
135 }
136
137 return solutions > 0;
138 }
139
140 class PnPRansacCallback CV_FINAL : public PointSetRegistrator::Callback
141 {
142
143 public:
144
PnPRansacCallback(Mat _cameraMatrix=Mat (3,3,CV_64F),Mat _distCoeffs=Mat (4,1,CV_64F),int _flags=SOLVEPNP_ITERATIVE,bool _useExtrinsicGuess=false,Mat _rvec=Mat (),Mat _tvec=Mat ())145 PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=SOLVEPNP_ITERATIVE,
146 bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
147 : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
148 rvec(_rvec), tvec(_tvec) {}
149
150 /* Pre: True */
151 /* Post: compute _model with given points and return number of found models */
runKernel(InputArray _m1,InputArray _m2,OutputArray _model) const152 int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
153 {
154 Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
155
156 bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
157 rvec, tvec, useExtrinsicGuess, flags );
158
159 Mat _local_model;
160 hconcat(rvec, tvec, _local_model);
161 _local_model.copyTo(_model);
162
163 return correspondence;
164 }
165
166 /* Pre: True */
167 /* Post: fill _err with projection errors */
computeError(InputArray _m1,InputArray _m2,InputArray _model,OutputArray _err) const168 void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
169 {
170
171 Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat();
172
173 int i, count = opoints.checkVector(3);
174 Mat _rvec = model.col(0);
175 Mat _tvec = model.col(1);
176
177
178 Mat projpoints(count, 2, CV_32FC1);
179 projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
180
181 const Point2f* ipoints_ptr = ipoints.ptr<Point2f>();
182 const Point2f* projpoints_ptr = projpoints.ptr<Point2f>();
183
184 _err.create(count, 1, CV_32FC1);
185 float* err = _err.getMat().ptr<float>();
186
187 for ( i = 0; i < count; ++i)
188 err[i] = (float)norm( Matx21f(ipoints_ptr[i] - projpoints_ptr[i]), NORM_L2SQR );
189
190 }
191
192
193 Mat cameraMatrix;
194 Mat distCoeffs;
195 int flags;
196 bool useExtrinsicGuess;
197 Mat rvec;
198 Mat tvec;
199 };
200
UsacParams()201 UsacParams::UsacParams()
202 {
203 confidence = 0.99;
204 isParallel = false;
205 loIterations = 5;
206 loMethod = LocalOptimMethod::LOCAL_OPTIM_INNER_LO;
207 loSampleSize = 14;
208 maxIterations = 5000;
209 neighborsSearch = NeighborSearchMethod::NEIGH_GRID;
210 randomGeneratorState = 0;
211 sampler = SamplingMethod::SAMPLING_UNIFORM;
212 score = ScoreMethod::SCORE_METHOD_MSAC;
213 threshold = 1.5;
214 }
215
solvePnPRansac(InputArray _opoints,InputArray _ipoints,InputArray _cameraMatrix,InputArray _distCoeffs,OutputArray _rvec,OutputArray _tvec,bool useExtrinsicGuess,int iterationsCount,float reprojectionError,double confidence,OutputArray _inliers,int flags)216 bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
217 InputArray _cameraMatrix, InputArray _distCoeffs,
218 OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
219 int iterationsCount, float reprojectionError, double confidence,
220 OutputArray _inliers, int flags)
221 {
222 CV_INSTRUMENT_REGION();
223
224 if (flags >= USAC_DEFAULT && flags <= USAC_MAGSAC)
225 return usac::solvePnPRansac(_opoints, _ipoints, _cameraMatrix, _distCoeffs,
226 _rvec, _tvec, useExtrinsicGuess, iterationsCount, reprojectionError,
227 confidence, _inliers, flags);
228
229 Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat();
230 Mat opoints, ipoints;
231 if( opoints0.depth() == CV_64F || !opoints0.isContinuous() )
232 opoints0.convertTo(opoints, CV_32F);
233 else
234 opoints = opoints0;
235 if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() )
236 ipoints0.convertTo(ipoints, CV_32F);
237 else
238 ipoints = ipoints0;
239
240 int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
241 CV_Assert( npoints >= 4 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
242
243 CV_Assert(opoints.isContinuous());
244 CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
245 CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
246 CV_Assert(ipoints.isContinuous());
247 CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
248 CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
249
250 _rvec.create(3, 1, CV_64FC1);
251 _tvec.create(3, 1, CV_64FC1);
252
253 Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1);
254 Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
255 Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
256
257 int model_points = 5;
258 int ransac_kernel_method = SOLVEPNP_EPNP;
259
260 if( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P)
261 {
262 model_points = 4;
263 ransac_kernel_method = flags;
264 }
265 else if( npoints == 4 )
266 {
267 model_points = 4;
268 ransac_kernel_method = SOLVEPNP_P3P;
269 }
270
271 if( model_points == npoints )
272 {
273 opoints = opoints.reshape(3);
274 ipoints = ipoints.reshape(2);
275
276 bool result = solvePnP(opoints, ipoints, cameraMatrix, distCoeffs, _rvec, _tvec, useExtrinsicGuess, ransac_kernel_method);
277
278 if(!result)
279 {
280 if( _inliers.needed() )
281 _inliers.release();
282
283 return false;
284 }
285
286 if(_inliers.needed())
287 {
288 _inliers.create(npoints, 1, CV_32S);
289 Mat _local_inliers = _inliers.getMat();
290 for(int i = 0; i < npoints; i++)
291 {
292 _local_inliers.at<int>(i) = i;
293 }
294 }
295
296 return true;
297 }
298
299 Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
300 cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec);
301
302 double param1 = reprojectionError; // reprojection error
303 double param2 = confidence; // confidence
304 int param3 = iterationsCount; // number maximum iterations
305
306 Mat _local_model(3, 2, CV_64FC1);
307 Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
308
309 // call Ransac
310 int result = createRANSACPointSetRegistrator(cb, model_points,
311 param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
312
313 if( result <= 0 || _local_model.rows <= 0)
314 {
315 _rvec.assign(rvec); // output rotation vector
316 _tvec.assign(tvec); // output translation vector
317
318 if( _inliers.needed() )
319 _inliers.release();
320
321 return false;
322 }
323
324 vector<Point3d> opoints_inliers;
325 vector<Point2d> ipoints_inliers;
326 opoints = opoints.reshape(3);
327 ipoints = ipoints.reshape(2);
328 opoints.convertTo(opoints_inliers, CV_64F);
329 ipoints.convertTo(ipoints_inliers, CV_64F);
330
331 const uchar* mask = _mask_local_inliers.ptr<uchar>();
332 int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints);
333 compressElems(&ipoints_inliers[0], mask, 1, npoints);
334
335 opoints_inliers.resize(npoints1);
336 ipoints_inliers.resize(npoints1);
337 try
338 {
339 result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
340 distCoeffs, rvec, tvec, useExtrinsicGuess,
341 (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;
342 }
343 catch (const cv::Exception& e)
344 {
345 if (flags == SOLVEPNP_ITERATIVE &&
346 npoints1 == 5 &&
347 e.what() &&
348 std::string(e.what()).find("DLT algorithm needs at least 6 points") != std::string::npos
349 )
350 {
351 CV_LOG_INFO(NULL, "solvePnPRansac(): solvePnP stage to compute the final pose using points "
352 "in the consensus set raised DLT 6 points exception, use result from MSS (Minimal Sample Sets) stage instead.");
353 rvec = _local_model.col(0); // output rotation vector
354 tvec = _local_model.col(1); // output translation vector
355 result = 1;
356 }
357 else
358 {
359 // raise other exceptions
360 throw;
361 }
362 }
363
364 if (result <= 0)
365 {
366 _rvec.assign(_local_model.col(0)); // output rotation vector
367 _tvec.assign(_local_model.col(1)); // output translation vector
368
369 if (_inliers.needed())
370 _inliers.release();
371
372 CV_LOG_DEBUG(NULL, "solvePnPRansac(): solvePnP stage to compute the final pose using points in the consensus set failed. Return false");
373 return false;
374 }
375 else
376 {
377 _rvec.assign(rvec); // output rotation vector
378 _tvec.assign(tvec); // output translation vector
379 }
380
381 if(_inliers.needed())
382 {
383 Mat _local_inliers;
384 for (int i = 0; i < npoints; ++i)
385 {
386 if((int)_mask_local_inliers.at<uchar>(i) != 0) // inliers mask
387 _local_inliers.push_back(i); // output inliers vector
388 }
389 _local_inliers.copyTo(_inliers);
390 }
391 return true;
392 }
393
394
solvePnPRansac(InputArray objectPoints,InputArray imagePoints,InputOutputArray cameraMatrix,InputArray distCoeffs,OutputArray rvec,OutputArray tvec,OutputArray inliers,const UsacParams & params)395 bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
396 InputOutputArray cameraMatrix, InputArray distCoeffs,
397 OutputArray rvec, OutputArray tvec, OutputArray inliers,
398 const UsacParams ¶ms) {
399 Ptr<usac::Model> model_params;
400 usac::setParameters(model_params, cameraMatrix.empty() ? usac::EstimationMethod::P6P :
401 usac::EstimationMethod::P3P, params, inliers.needed());
402 Ptr<usac::RansacOutput> ransac_output;
403 if (usac::run(model_params, imagePoints, objectPoints, model_params->getRandomGeneratorState(),
404 ransac_output, cameraMatrix, noArray(), distCoeffs, noArray())) {
405 if (inliers.needed()) {
406 const auto &inliers_mask = ransac_output->getInliersMask();
407 Mat inliers_;
408 for (int i = 0; i < (int)inliers_mask.size(); i++)
409 if (inliers_mask[i])
410 inliers_.push_back(i);
411 inliers_.copyTo(inliers);
412 }
413 const Mat &model = ransac_output->getModel();
414 model.col(0).copyTo(rvec);
415 model.col(1).copyTo(tvec);
416 if (cameraMatrix.empty())
417 model.colRange(2, 5).copyTo(cameraMatrix);
418 return true;
419 } else return false;
420 }
421
422
solveP3P(InputArray _opoints,InputArray _ipoints,InputArray _cameraMatrix,InputArray _distCoeffs,OutputArrayOfArrays _rvecs,OutputArrayOfArrays _tvecs,int flags)423 int solveP3P( InputArray _opoints, InputArray _ipoints,
424 InputArray _cameraMatrix, InputArray _distCoeffs,
425 OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags) {
426 CV_INSTRUMENT_REGION();
427
428 Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
429 int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
430 CV_Assert( npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
431 CV_Assert( npoints == 3 || npoints == 4 );
432 CV_Assert( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P );
433
434 if (opoints.cols == 3)
435 opoints = opoints.reshape(3);
436 if (ipoints.cols == 2)
437 ipoints = ipoints.reshape(2);
438
439 Mat cameraMatrix0 = _cameraMatrix.getMat();
440 Mat distCoeffs0 = _distCoeffs.getMat();
441 Mat cameraMatrix = Mat_<double>(cameraMatrix0);
442 Mat distCoeffs = Mat_<double>(distCoeffs0);
443
444 Mat undistortedPoints;
445 undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
446 std::vector<Mat> Rs, ts, rvecs;
447
448 int solutions = 0;
449 if (flags == SOLVEPNP_P3P)
450 {
451 p3p P3Psolver(cameraMatrix);
452 solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
453 }
454 else if (flags == SOLVEPNP_AP3P)
455 {
456 ap3p P3Psolver(cameraMatrix);
457 solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
458 }
459
460 if (solutions == 0) {
461 return 0;
462 }
463
464 Mat objPts, imgPts;
465 opoints.convertTo(objPts, CV_64F);
466 ipoints.convertTo(imgPts, CV_64F);
467 if (imgPts.cols > 1)
468 {
469 imgPts = imgPts.reshape(1);
470 imgPts = imgPts.t();
471 }
472 else
473 imgPts = imgPts.reshape(1, 2*imgPts.rows);
474
475 vector<double> reproj_errors(solutions);
476 for (size_t i = 0; i < reproj_errors.size(); i++)
477 {
478 Mat rvec;
479 Rodrigues(Rs[i], rvec);
480 rvecs.push_back(rvec);
481
482 Mat projPts;
483 projectPoints(objPts, rvec, ts[i], _cameraMatrix, _distCoeffs, projPts);
484
485 projPts = projPts.reshape(1, 2*projPts.rows);
486 Mat err = imgPts - projPts;
487
488 err = err.t() * err;
489 reproj_errors[i] = err.at<double>(0,0);
490 }
491
492 //sort the solutions
493 for (int i = 1; i < solutions; i++)
494 {
495 for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--)
496 {
497 std::swap(reproj_errors[j], reproj_errors[j-1]);
498 std::swap(rvecs[j], rvecs[j-1]);
499 std::swap(ts[j], ts[j-1]);
500 }
501 }
502
503 int depthRot = _rvecs.fixedType() ? _rvecs.depth() : CV_64F;
504 int depthTrans = _tvecs.fixedType() ? _tvecs.depth() : CV_64F;
505 _rvecs.create(solutions, 1, CV_MAKETYPE(depthRot, _rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
506 _tvecs.create(solutions, 1, CV_MAKETYPE(depthTrans, _tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
507
508 for (int i = 0; i < solutions; i++)
509 {
510 Mat rvec0, tvec0;
511 if (depthRot == CV_64F)
512 rvec0 = rvecs[i];
513 else
514 rvecs[i].convertTo(rvec0, depthRot);
515
516 if (depthTrans == CV_64F)
517 tvec0 = ts[i];
518 else
519 ts[i].convertTo(tvec0, depthTrans);
520
521 if (_rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR)
522 {
523 Mat rref = _rvecs.getMat_();
524
525 if (_rvecs.depth() == CV_32F)
526 rref.at<Vec3f>(0,i) = Vec3f(rvec0.at<float>(0,0), rvec0.at<float>(1,0), rvec0.at<float>(2,0));
527 else
528 rref.at<Vec3d>(0,i) = Vec3d(rvec0.at<double>(0,0), rvec0.at<double>(1,0), rvec0.at<double>(2,0));
529 }
530 else
531 {
532 _rvecs.getMatRef(i) = rvec0;
533 }
534
535 if (_tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR)
536 {
537
538 Mat tref = _tvecs.getMat_();
539
540 if (_tvecs.depth() == CV_32F)
541 tref.at<Vec3f>(0,i) = Vec3f(tvec0.at<float>(0,0), tvec0.at<float>(1,0), tvec0.at<float>(2,0));
542 else
543 tref.at<Vec3d>(0,i) = Vec3d(tvec0.at<double>(0,0), tvec0.at<double>(1,0), tvec0.at<double>(2,0));
544 }
545 else
546 {
547 _tvecs.getMatRef(i) = tvec0;
548 }
549 }
550
551 return solutions;
552 }
553
554 class SolvePnPRefineLMCallback CV_FINAL : public LMSolver::Callback
555 {
556 public:
SolvePnPRefineLMCallback(InputArray _opoints,InputArray _ipoints,InputArray _cameraMatrix,InputArray _distCoeffs)557 SolvePnPRefineLMCallback(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs)
558 {
559 objectPoints = _opoints.getMat();
560 imagePoints = _ipoints.getMat();
561 npoints = std::max(objectPoints.checkVector(3, CV_32F), objectPoints.checkVector(3, CV_64F));
562 imagePoints0 = imagePoints.reshape(1, npoints*2);
563 cameraMatrix = _cameraMatrix.getMat();
564 distCoeffs = _distCoeffs.getMat();
565 }
566
compute(InputArray _param,OutputArray _err,OutputArray _Jac) const567 bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
568 {
569 Mat param = _param.getMat();
570 _err.create(npoints*2, 1, CV_64FC1);
571
572 if(_Jac.needed())
573 {
574 _Jac.create(npoints*2, param.rows, CV_64FC1);
575 }
576
577 Mat rvec = param(Rect(0, 0, 1, 3)), tvec = param(Rect(0, 3, 1, 3));
578
579 Mat J, projectedPts;
580 projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPts, _Jac.needed() ? J : noArray());
581
582 if (_Jac.needed())
583 {
584 Mat Jac = _Jac.getMat();
585 for (int i = 0; i < Jac.rows; i++)
586 {
587 for (int j = 0; j < Jac.cols; j++)
588 {
589 Jac.at<double>(i,j) = J.at<double>(i,j);
590 }
591 }
592 }
593
594 Mat err = _err.getMat();
595 projectedPts = projectedPts.reshape(1, npoints*2);
596 err = projectedPts - imagePoints0;
597
598 return true;
599 }
600
601 Mat objectPoints, imagePoints, imagePoints0;
602 Mat cameraMatrix, distCoeffs;
603 int npoints;
604 };
605
606 /**
607 * @brief Compute the Interaction matrix and the residuals for the current pose.
608 * @param objectPoints 3D object points.
609 * @param R Current estimated rotation matrix.
610 * @param tvec Current estimated translation vector.
611 * @param L Interaction matrix for a vector of point features.
612 * @param s Residuals.
613 */
computeInteractionMatrixAndResiduals(const Mat & objectPoints,const Mat & R,const Mat & tvec,Mat & L,Mat & s)614 static void computeInteractionMatrixAndResiduals(const Mat& objectPoints, const Mat& R, const Mat& tvec,
615 Mat& L, Mat& s)
616 {
617 Mat objectPointsInCam;
618
619 int npoints = objectPoints.rows;
620 for (int i = 0; i < npoints; i++)
621 {
622 Mat curPt = objectPoints.row(i);
623 objectPointsInCam = R * curPt.t() + tvec;
624
625 double Zi = objectPointsInCam.at<double>(2,0);
626 double xi = objectPointsInCam.at<double>(0,0) / Zi;
627 double yi = objectPointsInCam.at<double>(1,0) / Zi;
628
629 s.at<double>(2*i,0) = xi;
630 s.at<double>(2*i+1,0) = yi;
631
632 L.at<double>(2*i,0) = -1 / Zi;
633 L.at<double>(2*i,1) = 0;
634 L.at<double>(2*i,2) = xi / Zi;
635 L.at<double>(2*i,3) = xi*yi;
636 L.at<double>(2*i,4) = -(1 + xi*xi);
637 L.at<double>(2*i,5) = yi;
638
639 L.at<double>(2*i+1,0) = 0;
640 L.at<double>(2*i+1,1) = -1 / Zi;
641 L.at<double>(2*i+1,2) = yi / Zi;
642 L.at<double>(2*i+1,3) = 1 + yi*yi;
643 L.at<double>(2*i+1,4) = -xi*yi;
644 L.at<double>(2*i+1,5) = -xi;
645 }
646 }
647
648 /**
649 * @brief The exponential map from se(3) to SE(3).
650 * @param twist A twist (v, w) represents the velocity of a rigid body as an angular velocity
651 * around an axis and a linear velocity along this axis.
652 * @param R1 Resultant rotation matrix from the twist.
653 * @param t1 Resultant translation vector from the twist.
654 */
exponentialMapToSE3Inv(const Mat & twist,Mat & R1,Mat & t1)655 static void exponentialMapToSE3Inv(const Mat& twist, Mat& R1, Mat& t1)
656 {
657 //see Exponential Map in http://ethaneade.com/lie.pdf
658 /*
659 \begin{align*}
660 \boldsymbol{\delta} &= \left( \mathbf{u}, \boldsymbol{\omega} \right ) \in se(3) \\
661 \mathbf{u}, \boldsymbol{\omega} &\in \mathbb{R}^3 \\
662 \theta &= \sqrt{ \boldsymbol{\omega}^T \boldsymbol{\omega} } \\
663 A &= \frac{\sin \theta}{\theta} \\
664 B &= \frac{1 - \cos \theta}{\theta^2} \\
665 C &= \frac{1-A}{\theta^2} \\
666 \mathbf{R} &= \mathbf{I} + A \boldsymbol{\omega}_{\times} + B \boldsymbol{\omega}_{\times}^2 \\
667 \mathbf{V} &= \mathbf{I} + B \boldsymbol{\omega}_{\times} + C \boldsymbol{\omega}_{\times}^2 \\
668 \exp \begin{pmatrix}
669 \mathbf{u} \\
670 \boldsymbol{\omega}
671 \end{pmatrix} &=
672 \left(
673 \begin{array}{c|c}
674 \mathbf{R} & \mathbf{V} \mathbf{u} \\ \hline
675 \mathbf{0} & 1
676 \end{array}
677 \right )
678 \end{align*}
679 */
680 double vx = twist.at<double>(0,0);
681 double vy = twist.at<double>(1,0);
682 double vz = twist.at<double>(2,0);
683 double wx = twist.at<double>(3,0);
684 double wy = twist.at<double>(4,0);
685 double wz = twist.at<double>(5,0);
686
687 Matx31d rvec(wx, wy, wz);
688 Mat R;
689 Rodrigues(rvec, R);
690
691 double theta = sqrt(wx*wx + wy*wy + wz*wz);
692 double sinc = std::fabs(theta) < 1e-8 ? 1 : sin(theta) / theta;
693 double mcosc = (std::fabs(theta) < 1e-8) ? 0.5 : (1-cos(theta)) / (theta*theta);
694 double msinc = (std::abs(theta) < 1e-8) ? (1/6.0) : (1-sinc) / (theta*theta);
695
696 Matx31d dt;
697 dt(0) = vx*(sinc + wx*wx*msinc) + vy*(wx*wy*msinc - wz*mcosc) + vz*(wx*wz*msinc + wy*mcosc);
698 dt(1) = vx*(wx*wy*msinc + wz*mcosc) + vy*(sinc + wy*wy*msinc) + vz*(wy*wz*msinc - wx*mcosc);
699 dt(2) = vx*(wx*wz*msinc - wy*mcosc) + vy*(wy*wz*msinc + wx*mcosc) + vz*(sinc + wz*wz*msinc);
700
701 R1 = R.t();
702 t1 = -R1 * dt;
703 }
704
705 enum SolvePnPRefineMethod {
706 SOLVEPNP_REFINE_LM = 0,
707 SOLVEPNP_REFINE_VVS = 1
708 };
709
solvePnPRefine(InputArray _objectPoints,InputArray _imagePoints,InputArray _cameraMatrix,InputArray _distCoeffs,InputOutputArray _rvec,InputOutputArray _tvec,SolvePnPRefineMethod _flags,TermCriteria _criteria=TermCriteria (TermCriteria::EPS+TermCriteria::COUNT,20,FLT_EPSILON),double _vvslambda=1)710 static void solvePnPRefine(InputArray _objectPoints, InputArray _imagePoints,
711 InputArray _cameraMatrix, InputArray _distCoeffs,
712 InputOutputArray _rvec, InputOutputArray _tvec,
713 SolvePnPRefineMethod _flags,
714 TermCriteria _criteria=TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 20, FLT_EPSILON),
715 double _vvslambda=1)
716 {
717 CV_INSTRUMENT_REGION();
718
719 Mat opoints_ = _objectPoints.getMat(), ipoints_ = _imagePoints.getMat();
720 Mat opoints, ipoints;
721 opoints_.convertTo(opoints, CV_64F);
722 ipoints_.convertTo(ipoints, CV_64F);
723 int npoints = opoints.checkVector(3, CV_64F);
724 CV_Assert( npoints >= 3 && npoints == ipoints.checkVector(2, CV_64F) );
725 CV_Assert( !_rvec.empty() && !_tvec.empty() );
726
727 int rtype = _rvec.type(), ttype = _tvec.type();
728 Size rsize = _rvec.size(), tsize = _tvec.size();
729 CV_Assert( (rtype == CV_32FC1 || rtype == CV_64FC1) &&
730 (ttype == CV_32FC1 || ttype == CV_64FC1) );
731 CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
732 (tsize == Size(1, 3) || tsize == Size(3, 1)) );
733
734 Mat cameraMatrix0 = _cameraMatrix.getMat();
735 Mat distCoeffs0 = _distCoeffs.getMat();
736 Mat cameraMatrix = Mat_<double>(cameraMatrix0);
737 Mat distCoeffs = Mat_<double>(distCoeffs0);
738
739 if (_flags == SOLVEPNP_REFINE_LM)
740 {
741 Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
742 Mat rvec, tvec;
743 rvec0.convertTo(rvec, CV_64F);
744 tvec0.convertTo(tvec, CV_64F);
745
746 Mat params(6, 1, CV_64FC1);
747 for (int i = 0; i < 3; i++)
748 {
749 params.at<double>(i,0) = rvec.at<double>(i,0);
750 params.at<double>(i+3,0) = tvec.at<double>(i,0);
751 }
752
753 LMSolver::create(makePtr<SolvePnPRefineLMCallback>(opoints, ipoints, cameraMatrix, distCoeffs), _criteria.maxCount, _criteria.epsilon)->run(params);
754
755 params.rowRange(0, 3).convertTo(rvec0, rvec0.depth());
756 params.rowRange(3, 6).convertTo(tvec0, tvec0.depth());
757 }
758 else if (_flags == SOLVEPNP_REFINE_VVS)
759 {
760 Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
761 Mat rvec, tvec;
762 rvec0.convertTo(rvec, CV_64F);
763 tvec0.convertTo(tvec, CV_64F);
764
765 vector<Point2d> ipoints_normalized;
766 undistortPoints(ipoints, ipoints_normalized, cameraMatrix, distCoeffs);
767 Mat sd = Mat(ipoints_normalized).reshape(1, npoints*2);
768 Mat objectPoints0 = opoints.reshape(1, npoints);
769 Mat imagePoints0 = ipoints.reshape(1, npoints*2);
770 Mat L(npoints*2, 6, CV_64FC1), s(npoints*2, 1, CV_64FC1);
771
772 double residuals_1 = std::numeric_limits<double>::max(), residuals = 0;
773 Mat err;
774 Mat R;
775 Rodrigues(rvec, R);
776 for (int iter = 0; iter < _criteria.maxCount; iter++)
777 {
778 computeInteractionMatrixAndResiduals(objectPoints0, R, tvec, L, s);
779 err = s - sd;
780
781 Mat Lp = L.inv(cv::DECOMP_SVD);
782 Mat dq = -_vvslambda * Lp * err;
783
784 Mat R1, t1;
785 exponentialMapToSE3Inv(dq, R1, t1);
786 R = R1 * R;
787 tvec = R1 * tvec + t1;
788
789 residuals_1 = residuals;
790 Mat res = err.t()*err;
791 residuals = res.at<double>(0,0);
792
793 if (std::fabs(residuals - residuals_1) < _criteria.epsilon)
794 break;
795 }
796
797 Rodrigues(R, rvec);
798 rvec.convertTo(rvec0, rvec0.depth());
799 tvec.convertTo(tvec0, tvec0.depth());
800 }
801 }
802
solvePnPRefineLM(InputArray _objectPoints,InputArray _imagePoints,InputArray _cameraMatrix,InputArray _distCoeffs,InputOutputArray _rvec,InputOutputArray _tvec,TermCriteria _criteria)803 void solvePnPRefineLM(InputArray _objectPoints, InputArray _imagePoints,
804 InputArray _cameraMatrix, InputArray _distCoeffs,
805 InputOutputArray _rvec, InputOutputArray _tvec,
806 TermCriteria _criteria)
807 {
808 CV_INSTRUMENT_REGION();
809 solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_LM, _criteria);
810 }
811
solvePnPRefineVVS(InputArray _objectPoints,InputArray _imagePoints,InputArray _cameraMatrix,InputArray _distCoeffs,InputOutputArray _rvec,InputOutputArray _tvec,TermCriteria _criteria,double _VVSlambda)812 void solvePnPRefineVVS(InputArray _objectPoints, InputArray _imagePoints,
813 InputArray _cameraMatrix, InputArray _distCoeffs,
814 InputOutputArray _rvec, InputOutputArray _tvec,
815 TermCriteria _criteria, double _VVSlambda)
816 {
817 CV_INSTRUMENT_REGION();
818 solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_VVS, _criteria, _VVSlambda);
819 }
820
solvePnPGeneric(InputArray _opoints,InputArray _ipoints,InputArray _cameraMatrix,InputArray _distCoeffs,OutputArrayOfArrays _rvecs,OutputArrayOfArrays _tvecs,bool useExtrinsicGuess,SolvePnPMethod flags,InputArray _rvec,InputArray _tvec,OutputArray reprojectionError)821 int solvePnPGeneric( InputArray _opoints, InputArray _ipoints,
822 InputArray _cameraMatrix, InputArray _distCoeffs,
823 OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
824 bool useExtrinsicGuess, SolvePnPMethod flags,
825 InputArray _rvec, InputArray _tvec,
826 OutputArray reprojectionError) {
827 CV_INSTRUMENT_REGION();
828
829 Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
830 int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
831 CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess)
832 || (npoints >= 3 && flags == SOLVEPNP_SQPNP) )
833 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
834
835 opoints = opoints.reshape(3, npoints);
836 ipoints = ipoints.reshape(2, npoints);
837
838 if( flags != SOLVEPNP_ITERATIVE )
839 useExtrinsicGuess = false;
840
841 if (useExtrinsicGuess)
842 CV_Assert( !_rvec.empty() && !_tvec.empty() );
843
844 if( useExtrinsicGuess )
845 {
846 int rtype = _rvec.type(), ttype = _tvec.type();
847 Size rsize = _rvec.size(), tsize = _tvec.size();
848 CV_Assert( (rtype == CV_32FC1 || rtype == CV_64FC1) &&
849 (ttype == CV_32FC1 || ttype == CV_64FC1) );
850 CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
851 (tsize == Size(1, 3) || tsize == Size(3, 1)) );
852 }
853
854 Mat cameraMatrix0 = _cameraMatrix.getMat();
855 Mat distCoeffs0 = _distCoeffs.getMat();
856 Mat cameraMatrix = Mat_<double>(cameraMatrix0);
857 Mat distCoeffs = Mat_<double>(distCoeffs0);
858
859 vector<Mat> vec_rvecs, vec_tvecs;
860 if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
861 {
862 if (flags == SOLVEPNP_DLS)
863 {
864 CV_LOG_DEBUG(NULL, "Broken implementation for SOLVEPNP_DLS. Fallback to EPnP.");
865 }
866 else if (flags == SOLVEPNP_UPNP)
867 {
868 CV_LOG_DEBUG(NULL, "Broken implementation for SOLVEPNP_UPNP. Fallback to EPnP.");
869 }
870
871 Mat undistortedPoints;
872 undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
873 epnp PnP(cameraMatrix, opoints, undistortedPoints);
874
875 Mat rvec, tvec, R;
876 PnP.compute_pose(R, tvec);
877 Rodrigues(R, rvec);
878
879 vec_rvecs.push_back(rvec);
880 vec_tvecs.push_back(tvec);
881 }
882 else if (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P)
883 {
884 vector<Mat> rvecs, tvecs;
885 solveP3P(opoints, ipoints, _cameraMatrix, _distCoeffs, rvecs, tvecs, flags);
886 vec_rvecs.insert(vec_rvecs.end(), rvecs.begin(), rvecs.end());
887 vec_tvecs.insert(vec_tvecs.end(), tvecs.begin(), tvecs.end());
888 }
889 else if (flags == SOLVEPNP_ITERATIVE)
890 {
891 Mat rvec, tvec;
892 if (useExtrinsicGuess)
893 {
894 rvec = _rvec.getMat();
895 tvec = _tvec.getMat();
896 }
897 else
898 {
899 rvec.create(3, 1, CV_64FC1);
900 tvec.create(3, 1, CV_64FC1);
901 }
902
903 CvMat c_objectPoints = cvMat(opoints), c_imagePoints = cvMat(ipoints);
904 CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs);
905 CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec);
906 cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
907 (c_distCoeffs.rows && c_distCoeffs.cols) ? &c_distCoeffs : 0,
908 &c_rvec, &c_tvec, useExtrinsicGuess );
909
910 vec_rvecs.push_back(rvec);
911 vec_tvecs.push_back(tvec);
912 }
913 else if (flags == SOLVEPNP_IPPE)
914 {
915 CV_DbgAssert(isPlanarObjectPoints(opoints, 1e-3));
916 Mat undistortedPoints;
917 undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
918
919 IPPE::PoseSolver poseSolver;
920 Mat rvec1, tvec1, rvec2, tvec2;
921 float reprojErr1, reprojErr2;
922 try
923 {
924 poseSolver.solveGeneric(opoints, undistortedPoints, rvec1, tvec1, reprojErr1, rvec2, tvec2, reprojErr2);
925
926 if (reprojErr1 < reprojErr2)
927 {
928 vec_rvecs.push_back(rvec1);
929 vec_tvecs.push_back(tvec1);
930
931 vec_rvecs.push_back(rvec2);
932 vec_tvecs.push_back(tvec2);
933 }
934 else
935 {
936 vec_rvecs.push_back(rvec2);
937 vec_tvecs.push_back(tvec2);
938
939 vec_rvecs.push_back(rvec1);
940 vec_tvecs.push_back(tvec1);
941 }
942 }
943 catch (...) { }
944 }
945 else if (flags == SOLVEPNP_IPPE_SQUARE)
946 {
947 CV_Assert(npoints == 4);
948
949 #if defined _DEBUG || defined CV_STATIC_ANALYSIS
950 double Xs[4][3];
951 if (opoints.depth() == CV_32F)
952 {
953 for (int i = 0; i < 4; i++)
954 {
955 for (int j = 0; j < 3; j++)
956 {
957 Xs[i][j] = opoints.ptr<Vec3f>(0)[i](j);
958 }
959 }
960 }
961 else
962 {
963 for (int i = 0; i < 4; i++)
964 {
965 for (int j = 0; j < 3; j++)
966 {
967 Xs[i][j] = opoints.ptr<Vec3d>(0)[i](j);
968 }
969 }
970 }
971
972 const double equalThreshold = 1e-9;
973 //Z must be zero
974 for (int i = 0; i < 4; i++)
975 {
976 CV_DbgCheck(Xs[i][2], approxEqual(Xs[i][2], 0, equalThreshold), "Z object point coordinate must be zero!");
977 }
978 //Y0 == Y1 && Y2 == Y3
979 CV_DbgCheck(Xs[0][1], approxEqual(Xs[0][1], Xs[1][1], equalThreshold), "Object points must be: Y0 == Y1!");
980 CV_DbgCheck(Xs[2][1], approxEqual(Xs[2][1], Xs[3][1], equalThreshold), "Object points must be: Y2 == Y3!");
981 //X0 == X3 && X1 == X2
982 CV_DbgCheck(Xs[0][0], approxEqual(Xs[0][0], Xs[3][0], equalThreshold), "Object points must be: X0 == X3!");
983 CV_DbgCheck(Xs[1][0], approxEqual(Xs[1][0], Xs[2][0], equalThreshold), "Object points must be: X1 == X2!");
984 //X1 == Y1 && X3 == Y3
985 CV_DbgCheck(Xs[1][0], approxEqual(Xs[1][0], Xs[1][1], equalThreshold), "Object points must be: X1 == Y1!");
986 CV_DbgCheck(Xs[3][0], approxEqual(Xs[3][0], Xs[3][1], equalThreshold), "Object points must be: X3 == Y3!");
987 #endif
988
989 Mat undistortedPoints;
990 undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
991
992 IPPE::PoseSolver poseSolver;
993 Mat rvec1, tvec1, rvec2, tvec2;
994 float reprojErr1, reprojErr2;
995 try
996 {
997 poseSolver.solveSquare(opoints, undistortedPoints, rvec1, tvec1, reprojErr1, rvec2, tvec2, reprojErr2);
998
999 if (reprojErr1 < reprojErr2)
1000 {
1001 vec_rvecs.push_back(rvec1);
1002 vec_tvecs.push_back(tvec1);
1003
1004 vec_rvecs.push_back(rvec2);
1005 vec_tvecs.push_back(tvec2);
1006 }
1007 else
1008 {
1009 vec_rvecs.push_back(rvec2);
1010 vec_tvecs.push_back(tvec2);
1011
1012 vec_rvecs.push_back(rvec1);
1013 vec_tvecs.push_back(tvec1);
1014 }
1015 } catch (...) { }
1016 }
1017 else if (flags == SOLVEPNP_SQPNP)
1018 {
1019 Mat undistortedPoints;
1020 undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
1021
1022 sqpnp::PoseSolver solver;
1023 solver.solve(opoints, undistortedPoints, vec_rvecs, vec_tvecs);
1024 }
1025 /*else if (flags == SOLVEPNP_DLS)
1026 {
1027 Mat undistortedPoints;
1028 undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
1029
1030 dls PnP(opoints, undistortedPoints);
1031
1032 Mat rvec, tvec, R;
1033 bool result = PnP.compute_pose(R, tvec);
1034 if (result)
1035 {
1036 Rodrigues(R, rvec);
1037 vec_rvecs.push_back(rvec);
1038 vec_tvecs.push_back(tvec);
1039 }
1040 }
1041 else if (flags == SOLVEPNP_UPNP)
1042 {
1043 upnp PnP(cameraMatrix, opoints, ipoints);
1044
1045 Mat rvec, tvec, R;
1046 PnP.compute_pose(R, tvec);
1047 Rodrigues(R, rvec);
1048 vec_rvecs.push_back(rvec);
1049 vec_tvecs.push_back(tvec);
1050 }*/
1051 else
1052 CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, "
1053 "SOLVEPNP_EPNP, SOLVEPNP_DLS, SOLVEPNP_UPNP, SOLVEPNP_AP3P, SOLVEPNP_IPPE, SOLVEPNP_IPPE_SQUARE or SOLVEPNP_SQPNP");
1054
1055 CV_Assert(vec_rvecs.size() == vec_tvecs.size());
1056
1057 int solutions = static_cast<int>(vec_rvecs.size());
1058
1059 int depthRot = _rvecs.fixedType() ? _rvecs.depth() : CV_64F;
1060 int depthTrans = _tvecs.fixedType() ? _tvecs.depth() : CV_64F;
1061 _rvecs.create(solutions, 1, CV_MAKETYPE(depthRot, _rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
1062 _tvecs.create(solutions, 1, CV_MAKETYPE(depthTrans, _tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
1063
1064 for (int i = 0; i < solutions; i++)
1065 {
1066 Mat rvec0, tvec0;
1067 if (depthRot == CV_64F)
1068 rvec0 = vec_rvecs[i];
1069 else
1070 vec_rvecs[i].convertTo(rvec0, depthRot);
1071
1072 if (depthTrans == CV_64F)
1073 tvec0 = vec_tvecs[i];
1074 else
1075 vec_tvecs[i].convertTo(tvec0, depthTrans);
1076
1077 if (_rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR)
1078 {
1079 Mat rref = _rvecs.getMat_();
1080
1081 if (_rvecs.depth() == CV_32F)
1082 rref.at<Vec3f>(0,i) = Vec3f(rvec0.at<float>(0,0), rvec0.at<float>(1,0), rvec0.at<float>(2,0));
1083 else
1084 rref.at<Vec3d>(0,i) = Vec3d(rvec0.at<double>(0,0), rvec0.at<double>(1,0), rvec0.at<double>(2,0));
1085 }
1086 else
1087 {
1088 _rvecs.getMatRef(i) = rvec0;
1089 }
1090
1091 if (_tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR)
1092 {
1093
1094 Mat tref = _tvecs.getMat_();
1095
1096 if (_tvecs.depth() == CV_32F)
1097 tref.at<Vec3f>(0,i) = Vec3f(tvec0.at<float>(0,0), tvec0.at<float>(1,0), tvec0.at<float>(2,0));
1098 else
1099 tref.at<Vec3d>(0,i) = Vec3d(tvec0.at<double>(0,0), tvec0.at<double>(1,0), tvec0.at<double>(2,0));
1100 }
1101 else
1102 {
1103 _tvecs.getMatRef(i) = tvec0;
1104 }
1105 }
1106
1107 if (reprojectionError.needed())
1108 {
1109 int type = (reprojectionError.fixedType() || !reprojectionError.empty())
1110 ? reprojectionError.type()
1111 : (max(_ipoints.depth(), _opoints.depth()) == CV_64F ? CV_64F : CV_32F);
1112
1113 reprojectionError.create(solutions, 1, type);
1114 CV_CheckType(reprojectionError.type(), type == CV_32FC1 || type == CV_64FC1,
1115 "Type of reprojectionError must be CV_32FC1 or CV_64FC1!");
1116
1117 Mat objectPoints, imagePoints;
1118 if (opoints.depth() == CV_32F)
1119 {
1120 opoints.convertTo(objectPoints, CV_64F);
1121 }
1122 else
1123 {
1124 objectPoints = opoints;
1125 }
1126 if (ipoints.depth() == CV_32F)
1127 {
1128 ipoints.convertTo(imagePoints, CV_64F);
1129 }
1130 else
1131 {
1132 imagePoints = ipoints;
1133 }
1134
1135 for (size_t i = 0; i < vec_rvecs.size(); i++)
1136 {
1137 vector<Point2d> projectedPoints;
1138 projectPoints(objectPoints, vec_rvecs[i], vec_tvecs[i], cameraMatrix, distCoeffs, projectedPoints);
1139 double rmse = norm(Mat(projectedPoints, false), imagePoints, NORM_L2) / sqrt(2*projectedPoints.size());
1140
1141 Mat err = reprojectionError.getMat();
1142 if (type == CV_32F)
1143 {
1144 err.at<float>(static_cast<int>(i)) = static_cast<float>(rmse);
1145 }
1146 else
1147 {
1148 err.at<double>(static_cast<int>(i)) = rmse;
1149 }
1150 }
1151 }
1152
1153 return solutions;
1154 }
1155
1156 }
1157