2011-03-05 08:18:49 +08:00
|
|
|
/*M///////////////////////////////////////////////////////////////////////////////////////
|
2011-04-17 21:14:45 +08:00
|
|
|
//
|
|
|
|
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
|
|
|
|
//
|
|
|
|
// By downloading, copying, installing or using the software you agree to this license.
|
|
|
|
// If you do not agree to this license, do not download, install,
|
|
|
|
// copy or use the software.
|
|
|
|
//
|
|
|
|
//
|
|
|
|
// License Agreement
|
|
|
|
// For Open Source Computer Vision Library
|
|
|
|
//
|
|
|
|
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
|
|
|
|
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
|
|
|
|
// Third party copyrights are property of their respective owners.
|
|
|
|
//
|
|
|
|
// Redistribution and use in source and binary forms, with or without modification,
|
|
|
|
// are permitted provided that the following conditions are met:
|
|
|
|
//
|
|
|
|
// * Redistribution's of source code must retain the above copyright notice,
|
|
|
|
// this list of conditions and the following disclaimer.
|
|
|
|
//
|
|
|
|
// * Redistribution's in binary form must reproduce the above copyright notice,
|
|
|
|
// this list of conditions and the following disclaimer in the documentation
|
|
|
|
// and/or other materials provided with the distribution.
|
|
|
|
//
|
|
|
|
// * The name of the copyright holders may not be used to endorse or promote products
|
|
|
|
// derived from this software without specific prior written permission.
|
|
|
|
//
|
|
|
|
// This software is provided by the copyright holders and contributors "as is" and
|
|
|
|
// any express or implied warranties, including, but not limited to, the implied
|
|
|
|
// warranties of merchantability and fitness for a particular purpose are disclaimed.
|
|
|
|
// In no event shall the Intel Corporation or contributors be liable for any direct,
|
|
|
|
// indirect, incidental, special, exemplary, or consequential damages
|
|
|
|
// (including, but not limited to, procurement of substitute goods or services;
|
|
|
|
// loss of use, data, or profits; or business interruption) however caused
|
|
|
|
// and on any theory of liability, whether in contract, strict liability,
|
|
|
|
// or tort (including negligence or otherwise) arising in any way out of
|
|
|
|
// the use of this software, even if advised of the possibility of such damage.
|
|
|
|
//
|
|
|
|
//M*/
|
2011-03-05 08:18:49 +08:00
|
|
|
|
|
|
|
#include "precomp.hpp"
|
2014-10-02 22:45:04 +08:00
|
|
|
#include "upnp.h"
|
2014-07-16 06:06:29 +08:00
|
|
|
#include "dls.h"
|
2011-12-26 20:59:07 +08:00
|
|
|
#include "epnp.h"
|
|
|
|
#include "p3p.h"
|
2017-04-07 14:48:34 +08:00
|
|
|
#include "ap3p.h"
|
2019-04-19 01:07:19 +08:00
|
|
|
#include "ippe.hpp"
|
2018-11-09 21:12:22 +08:00
|
|
|
#include "calib3d_c_api.h"
|
2013-04-11 23:27:54 +08:00
|
|
|
|
2015-03-16 21:56:26 +08:00
|
|
|
namespace cv
|
|
|
|
{
|
2019-04-19 01:07:19 +08:00
|
|
|
#if defined _DEBUG || defined CV_STATIC_ANALYSIS
|
|
|
|
static bool isPlanarObjectPoints(InputArray _objectPoints, double threshold)
|
|
|
|
{
|
|
|
|
CV_CheckType(_objectPoints.type(), _objectPoints.type() == CV_32FC3 || _objectPoints.type() == CV_64FC3,
|
|
|
|
"Type of _objectPoints must be CV_32FC3 or CV_64FC3");
|
|
|
|
Mat objectPoints;
|
|
|
|
if (_objectPoints.type() == CV_32FC3)
|
|
|
|
{
|
|
|
|
_objectPoints.getMat().convertTo(objectPoints, CV_64F);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
objectPoints = _objectPoints.getMat();
|
|
|
|
}
|
|
|
|
|
|
|
|
Scalar meanValues = mean(objectPoints);
|
|
|
|
int nbPts = objectPoints.checkVector(3, CV_64F);
|
|
|
|
Mat objectPointsCentred = objectPoints - meanValues;
|
|
|
|
objectPointsCentred = objectPointsCentred.reshape(1, nbPts);
|
|
|
|
|
|
|
|
Mat w, u, vt;
|
|
|
|
Mat MM = objectPointsCentred.t() * objectPointsCentred;
|
|
|
|
SVDecomp(MM, w, u, vt);
|
|
|
|
|
|
|
|
return (w.at<double>(2) < w.at<double>(1) * threshold);
|
|
|
|
}
|
|
|
|
|
|
|
|
static bool approxEqual(double a, double b, double eps)
|
|
|
|
{
|
|
|
|
return std::fabs(a-b) < eps;
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2018-10-25 23:57:18 +08:00
|
|
|
void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs,
|
|
|
|
InputArray rvec, InputArray tvec, float length, int thickness)
|
|
|
|
{
|
|
|
|
CV_INSTRUMENT_REGION();
|
|
|
|
|
|
|
|
int type = image.type();
|
|
|
|
int cn = CV_MAT_CN(type);
|
|
|
|
CV_CheckType(type, cn == 1 || cn == 3 || cn == 4,
|
|
|
|
"Number of channels must be 1, 3 or 4" );
|
|
|
|
|
|
|
|
CV_Assert(image.getMat().total() > 0);
|
|
|
|
CV_Assert(length > 0);
|
|
|
|
|
|
|
|
// project axes points
|
|
|
|
vector<Point3f> axesPoints;
|
|
|
|
axesPoints.push_back(Point3f(0, 0, 0));
|
|
|
|
axesPoints.push_back(Point3f(length, 0, 0));
|
|
|
|
axesPoints.push_back(Point3f(0, length, 0));
|
|
|
|
axesPoints.push_back(Point3f(0, 0, length));
|
|
|
|
vector<Point2f> imagePoints;
|
|
|
|
projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints);
|
|
|
|
|
|
|
|
// draw axes lines
|
|
|
|
line(image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255), thickness);
|
|
|
|
line(image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0), thickness);
|
|
|
|
line(image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0), thickness);
|
|
|
|
}
|
2015-03-16 21:56:26 +08:00
|
|
|
|
2019-04-19 01:07:19 +08:00
|
|
|
bool solvePnP( InputArray opoints, InputArray ipoints,
|
|
|
|
InputArray cameraMatrix, InputArray distCoeffs,
|
|
|
|
OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int flags )
|
2011-03-05 08:18:49 +08:00
|
|
|
{
|
2018-09-14 05:35:26 +08:00
|
|
|
CV_INSTRUMENT_REGION();
|
2016-08-18 14:53:00 +08:00
|
|
|
|
2019-04-19 01:07:19 +08:00
|
|
|
vector<Mat> rvecs, tvecs;
|
|
|
|
int solutions = solvePnPGeneric(opoints, ipoints, cameraMatrix, distCoeffs, rvecs, tvecs, useExtrinsicGuess, (SolvePnPMethod)flags, rvec, tvec);
|
2012-06-08 01:21:29 +08:00
|
|
|
|
2019-04-19 01:07:19 +08:00
|
|
|
if (solutions > 0)
|
2014-07-15 15:58:49 +08:00
|
|
|
{
|
2019-04-19 01:07:19 +08:00
|
|
|
int rdepth = rvec.empty() ? CV_64F : rvec.depth();
|
|
|
|
int tdepth = tvec.empty() ? CV_64F : tvec.depth();
|
|
|
|
rvecs[0].convertTo(rvec, rdepth);
|
|
|
|
tvecs[0].convertTo(tvec, tdepth);
|
2012-06-08 01:21:29 +08:00
|
|
|
}
|
2014-10-02 22:45:04 +08:00
|
|
|
|
2019-04-19 01:07:19 +08:00
|
|
|
return solutions > 0;
|
2011-03-05 08:18:49 +08:00
|
|
|
}
|
|
|
|
|
2018-03-15 21:16:55 +08:00
|
|
|
class PnPRansacCallback CV_FINAL : public PointSetRegistrator::Callback
|
2014-07-02 20:53:12 +08:00
|
|
|
{
|
2012-06-08 01:21:29 +08:00
|
|
|
|
2014-07-02 20:53:12 +08:00
|
|
|
public:
|
|
|
|
|
2015-03-16 21:56:26 +08:00
|
|
|
PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=SOLVEPNP_ITERATIVE,
|
2014-08-05 23:02:06 +08:00
|
|
|
bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
|
|
|
|
: cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
|
|
|
|
rvec(_rvec), tvec(_tvec) {}
|
2014-07-02 20:53:12 +08:00
|
|
|
|
|
|
|
/* Pre: True */
|
2016-07-26 11:56:10 +08:00
|
|
|
/* Post: compute _model with given points and return number of found models */
|
2018-03-15 21:16:55 +08:00
|
|
|
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
|
2014-07-02 20:53:12 +08:00
|
|
|
{
|
|
|
|
Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
|
|
|
|
|
2015-03-16 21:56:26 +08:00
|
|
|
bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
|
2014-08-05 23:02:06 +08:00
|
|
|
rvec, tvec, useExtrinsicGuess, flags );
|
2014-07-04 23:20:17 +08:00
|
|
|
|
2014-08-05 23:02:06 +08:00
|
|
|
Mat _local_model;
|
2015-03-16 21:56:26 +08:00
|
|
|
hconcat(rvec, tvec, _local_model);
|
2014-08-05 23:02:06 +08:00
|
|
|
_local_model.copyTo(_model);
|
2014-07-02 20:53:12 +08:00
|
|
|
|
2014-07-04 23:20:17 +08:00
|
|
|
return correspondence;
|
2014-07-02 20:53:12 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
/* Pre: True */
|
|
|
|
/* Post: fill _err with projection errors */
|
2018-03-15 21:16:55 +08:00
|
|
|
void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const CV_OVERRIDE
|
2014-07-02 20:53:12 +08:00
|
|
|
{
|
|
|
|
|
2014-07-04 23:20:17 +08:00
|
|
|
Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat();
|
2014-07-02 20:53:12 +08:00
|
|
|
|
2014-11-10 06:08:47 +08:00
|
|
|
int i, count = opoints.checkVector(3);
|
2014-07-09 16:37:37 +08:00
|
|
|
Mat _rvec = model.col(0);
|
|
|
|
Mat _tvec = model.col(1);
|
2014-07-02 20:53:12 +08:00
|
|
|
|
2014-08-04 20:12:48 +08:00
|
|
|
|
2014-07-04 23:20:17 +08:00
|
|
|
Mat projpoints(count, 2, CV_32FC1);
|
2015-03-16 21:56:26 +08:00
|
|
|
projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
|
2014-07-02 20:53:12 +08:00
|
|
|
|
|
|
|
const Point2f* ipoints_ptr = ipoints.ptr<Point2f>();
|
|
|
|
const Point2f* projpoints_ptr = projpoints.ptr<Point2f>();
|
|
|
|
|
2014-07-04 23:20:17 +08:00
|
|
|
_err.create(count, 1, CV_32FC1);
|
2014-07-02 20:53:12 +08:00
|
|
|
float* err = _err.getMat().ptr<float>();
|
|
|
|
|
|
|
|
for ( i = 0; i < count; ++i)
|
2016-12-12 18:22:44 +08:00
|
|
|
err[i] = (float)norm( Matx21f(ipoints_ptr[i] - projpoints_ptr[i]), NORM_L2SQR );
|
2014-07-02 20:53:12 +08:00
|
|
|
|
|
|
|
}
|
2014-07-04 23:20:17 +08:00
|
|
|
|
|
|
|
|
|
|
|
Mat cameraMatrix;
|
|
|
|
Mat distCoeffs;
|
|
|
|
int flags;
|
|
|
|
bool useExtrinsicGuess;
|
|
|
|
Mat rvec;
|
|
|
|
Mat tvec;
|
2014-07-02 20:53:12 +08:00
|
|
|
};
|
|
|
|
|
2015-03-16 21:56:26 +08:00
|
|
|
bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
|
2019-04-19 01:07:19 +08:00
|
|
|
InputArray _cameraMatrix, InputArray _distCoeffs,
|
|
|
|
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
|
|
|
|
int iterationsCount, float reprojectionError, double confidence,
|
|
|
|
OutputArray _inliers, int flags)
|
2011-03-05 08:18:49 +08:00
|
|
|
{
|
2018-09-14 05:35:26 +08:00
|
|
|
CV_INSTRUMENT_REGION();
|
2014-07-04 23:20:17 +08:00
|
|
|
|
2015-05-26 03:03:25 +08:00
|
|
|
Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat();
|
|
|
|
Mat opoints, ipoints;
|
|
|
|
if( opoints0.depth() == CV_64F || !opoints0.isContinuous() )
|
|
|
|
opoints0.convertTo(opoints, CV_32F);
|
|
|
|
else
|
|
|
|
opoints = opoints0;
|
|
|
|
if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() )
|
|
|
|
ipoints0.convertTo(ipoints, CV_32F);
|
|
|
|
else
|
|
|
|
ipoints = ipoints0;
|
2014-07-09 23:27:08 +08:00
|
|
|
|
|
|
|
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
|
2017-07-04 03:55:11 +08:00
|
|
|
CV_Assert( npoints >= 4 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
|
2012-06-08 01:21:29 +08:00
|
|
|
|
2011-04-17 21:14:45 +08:00
|
|
|
CV_Assert(opoints.isContinuous());
|
2014-07-08 22:07:30 +08:00
|
|
|
CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
|
2011-04-17 21:14:45 +08:00
|
|
|
CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
|
|
|
|
CV_Assert(ipoints.isContinuous());
|
2014-07-08 22:07:30 +08:00
|
|
|
CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
|
2011-04-17 21:14:45 +08:00
|
|
|
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
|
2012-06-08 01:21:29 +08:00
|
|
|
|
2014-07-04 23:20:17 +08:00
|
|
|
_rvec.create(3, 1, CV_64FC1);
|
|
|
|
_tvec.create(3, 1, CV_64FC1);
|
2014-07-03 18:23:03 +08:00
|
|
|
|
2014-07-09 16:37:37 +08:00
|
|
|
Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1);
|
|
|
|
Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
|
2014-07-09 23:27:08 +08:00
|
|
|
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
|
2014-07-02 20:53:12 +08:00
|
|
|
|
2015-03-16 21:56:26 +08:00
|
|
|
int model_points = 5;
|
|
|
|
int ransac_kernel_method = SOLVEPNP_EPNP;
|
2014-07-02 20:53:12 +08:00
|
|
|
|
2017-06-28 20:27:30 +08:00
|
|
|
if( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P)
|
|
|
|
{
|
|
|
|
model_points = 4;
|
|
|
|
ransac_kernel_method = flags;
|
|
|
|
}
|
|
|
|
else if( npoints == 4 )
|
2015-03-16 21:56:26 +08:00
|
|
|
{
|
|
|
|
model_points = 4;
|
|
|
|
ransac_kernel_method = SOLVEPNP_P3P;
|
|
|
|
}
|
|
|
|
|
2017-07-04 03:55:11 +08:00
|
|
|
if( model_points == npoints )
|
|
|
|
{
|
2019-04-29 23:23:23 +08:00
|
|
|
opoints = opoints.reshape(3);
|
|
|
|
ipoints = ipoints.reshape(2);
|
|
|
|
|
2017-07-04 03:55:11 +08:00
|
|
|
bool result = solvePnP(opoints, ipoints, cameraMatrix, distCoeffs, _rvec, _tvec, useExtrinsicGuess, ransac_kernel_method);
|
|
|
|
|
|
|
|
if(!result)
|
|
|
|
{
|
|
|
|
if( _inliers.needed() )
|
|
|
|
_inliers.release();
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
if(_inliers.needed())
|
|
|
|
{
|
|
|
|
_inliers.create(npoints, 1, CV_32S);
|
|
|
|
Mat _local_inliers = _inliers.getMat();
|
|
|
|
for(int i = 0; i < npoints; i++)
|
|
|
|
{
|
|
|
|
_local_inliers.at<int>(i) = i;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2015-03-16 21:56:26 +08:00
|
|
|
Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
|
|
|
|
cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec);
|
2014-08-19 03:46:16 +08:00
|
|
|
|
2014-08-05 23:02:06 +08:00
|
|
|
double param1 = reprojectionError; // reprojection error
|
2014-08-07 20:59:01 +08:00
|
|
|
double param2 = confidence; // confidence
|
|
|
|
int param3 = iterationsCount; // number maximum iterations
|
2012-06-08 01:21:29 +08:00
|
|
|
|
2015-03-16 21:56:26 +08:00
|
|
|
Mat _local_model(3, 2, CV_64FC1);
|
|
|
|
Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
|
2014-07-04 23:20:17 +08:00
|
|
|
|
2014-08-05 23:02:06 +08:00
|
|
|
// call Ransac
|
2015-03-16 21:56:26 +08:00
|
|
|
int result = createRANSACPointSetRegistrator(cb, model_points,
|
|
|
|
param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
|
|
|
|
|
2014-08-05 23:02:06 +08:00
|
|
|
if( result <= 0 || _local_model.rows <= 0)
|
2011-04-17 21:14:45 +08:00
|
|
|
{
|
2014-08-05 23:02:06 +08:00
|
|
|
_rvec.assign(rvec); // output rotation vector
|
|
|
|
_tvec.assign(tvec); // output translation vector
|
|
|
|
|
|
|
|
if( _inliers.needed() )
|
|
|
|
_inliers.release();
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
2017-07-04 03:55:11 +08:00
|
|
|
|
|
|
|
vector<Point3d> opoints_inliers;
|
|
|
|
vector<Point2d> ipoints_inliers;
|
|
|
|
opoints = opoints.reshape(3);
|
|
|
|
ipoints = ipoints.reshape(2);
|
|
|
|
opoints.convertTo(opoints_inliers, CV_64F);
|
|
|
|
ipoints.convertTo(ipoints_inliers, CV_64F);
|
|
|
|
|
|
|
|
const uchar* mask = _mask_local_inliers.ptr<uchar>();
|
|
|
|
int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints);
|
|
|
|
compressElems(&ipoints_inliers[0], mask, 1, npoints);
|
|
|
|
|
|
|
|
opoints_inliers.resize(npoints1);
|
|
|
|
ipoints_inliers.resize(npoints1);
|
|
|
|
result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
|
|
|
|
distCoeffs, rvec, tvec, useExtrinsicGuess,
|
|
|
|
(flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;
|
|
|
|
|
|
|
|
if( result <= 0 )
|
2014-08-05 23:02:06 +08:00
|
|
|
{
|
|
|
|
_rvec.assign(_local_model.col(0)); // output rotation vector
|
|
|
|
_tvec.assign(_local_model.col(1)); // output translation vector
|
2017-07-04 03:55:11 +08:00
|
|
|
|
|
|
|
if( _inliers.needed() )
|
|
|
|
_inliers.release();
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
_rvec.assign(rvec); // output rotation vector
|
|
|
|
_tvec.assign(tvec); // output translation vector
|
2011-04-17 21:14:45 +08:00
|
|
|
}
|
2012-06-08 01:21:29 +08:00
|
|
|
|
2014-08-05 23:02:06 +08:00
|
|
|
if(_inliers.needed())
|
2011-04-17 21:14:45 +08:00
|
|
|
{
|
2014-08-05 23:02:06 +08:00
|
|
|
Mat _local_inliers;
|
2014-11-10 06:10:43 +08:00
|
|
|
for (int i = 0; i < npoints; ++i)
|
2012-06-08 01:21:29 +08:00
|
|
|
{
|
2014-11-10 06:10:43 +08:00
|
|
|
if((int)_mask_local_inliers.at<uchar>(i) != 0) // inliers mask
|
|
|
|
_local_inliers.push_back(i); // output inliers vector
|
2012-06-08 01:21:29 +08:00
|
|
|
}
|
2014-08-05 23:02:06 +08:00
|
|
|
_local_inliers.copyTo(_inliers);
|
2011-04-17 21:14:45 +08:00
|
|
|
}
|
2014-07-09 16:37:37 +08:00
|
|
|
return true;
|
2011-03-05 08:18:49 +08:00
|
|
|
}
|
2015-03-16 21:56:26 +08:00
|
|
|
|
2017-06-28 20:27:30 +08:00
|
|
|
int solveP3P( InputArray _opoints, InputArray _ipoints,
|
|
|
|
InputArray _cameraMatrix, InputArray _distCoeffs,
|
|
|
|
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags) {
|
2018-09-14 05:35:26 +08:00
|
|
|
CV_INSTRUMENT_REGION();
|
2017-06-28 20:27:30 +08:00
|
|
|
|
|
|
|
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
|
|
|
|
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
|
2019-04-19 01:07:19 +08:00
|
|
|
CV_Assert( npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
|
|
|
|
CV_Assert( npoints == 3 || npoints == 4 );
|
2017-06-28 20:27:30 +08:00
|
|
|
CV_Assert( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P );
|
|
|
|
|
2019-04-29 23:23:23 +08:00
|
|
|
if (opoints.cols == 3)
|
|
|
|
opoints = opoints.reshape(3);
|
|
|
|
if (ipoints.cols == 2)
|
|
|
|
ipoints = ipoints.reshape(2);
|
|
|
|
|
2017-06-28 20:27:30 +08:00
|
|
|
Mat cameraMatrix0 = _cameraMatrix.getMat();
|
|
|
|
Mat distCoeffs0 = _distCoeffs.getMat();
|
|
|
|
Mat cameraMatrix = Mat_<double>(cameraMatrix0);
|
|
|
|
Mat distCoeffs = Mat_<double>(distCoeffs0);
|
|
|
|
|
|
|
|
Mat undistortedPoints;
|
|
|
|
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
|
2019-04-19 01:07:19 +08:00
|
|
|
std::vector<Mat> Rs, ts, rvecs;
|
2017-06-28 20:27:30 +08:00
|
|
|
|
|
|
|
int solutions = 0;
|
|
|
|
if (flags == SOLVEPNP_P3P)
|
|
|
|
{
|
|
|
|
p3p P3Psolver(cameraMatrix);
|
|
|
|
solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
|
|
|
|
}
|
|
|
|
else if (flags == SOLVEPNP_AP3P)
|
|
|
|
{
|
|
|
|
ap3p P3Psolver(cameraMatrix);
|
|
|
|
solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (solutions == 0) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2019-04-19 01:07:19 +08:00
|
|
|
Mat objPts, imgPts;
|
|
|
|
opoints.convertTo(objPts, CV_64F);
|
|
|
|
ipoints.convertTo(imgPts, CV_64F);
|
|
|
|
if (imgPts.cols > 1)
|
|
|
|
{
|
|
|
|
imgPts = imgPts.reshape(1);
|
|
|
|
imgPts = imgPts.t();
|
2017-06-28 20:27:30 +08:00
|
|
|
}
|
2019-04-19 01:07:19 +08:00
|
|
|
else
|
|
|
|
imgPts = imgPts.reshape(1, 2*imgPts.rows);
|
2017-06-28 20:27:30 +08:00
|
|
|
|
2019-04-19 01:07:19 +08:00
|
|
|
vector<double> reproj_errors(solutions);
|
|
|
|
for (size_t i = 0; i < reproj_errors.size(); i++)
|
|
|
|
{
|
2017-06-28 20:27:30 +08:00
|
|
|
Mat rvec;
|
|
|
|
Rodrigues(Rs[i], rvec);
|
2019-04-19 01:07:19 +08:00
|
|
|
rvecs.push_back(rvec);
|
|
|
|
|
|
|
|
Mat projPts;
|
|
|
|
projectPoints(objPts, rvec, ts[i], _cameraMatrix, _distCoeffs, projPts);
|
|
|
|
|
|
|
|
projPts = projPts.reshape(1, 2*projPts.rows);
|
|
|
|
Mat err = imgPts - projPts;
|
|
|
|
|
|
|
|
err = err.t() * err;
|
|
|
|
reproj_errors[i] = err.at<double>(0,0);
|
|
|
|
}
|
|
|
|
|
|
|
|
//sort the solutions
|
|
|
|
for (int i = 1; i < solutions; i++)
|
|
|
|
{
|
|
|
|
for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--)
|
|
|
|
{
|
|
|
|
std::swap(reproj_errors[j], reproj_errors[j-1]);
|
|
|
|
std::swap(rvecs[j], rvecs[j-1]);
|
|
|
|
std::swap(ts[j], ts[j-1]);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
int depthRot = _rvecs.fixedType() ? _rvecs.depth() : CV_64F;
|
|
|
|
int depthTrans = _tvecs.fixedType() ? _tvecs.depth() : CV_64F;
|
|
|
|
_rvecs.create(solutions, 1, CV_MAKETYPE(depthRot, _rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
|
|
|
|
_tvecs.create(solutions, 1, CV_MAKETYPE(depthTrans, _tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
|
|
|
|
|
|
|
|
for (int i = 0; i < solutions; i++)
|
|
|
|
{
|
|
|
|
Mat rvec0, tvec0;
|
|
|
|
if (depthRot == CV_64F)
|
|
|
|
rvec0 = rvecs[i];
|
|
|
|
else
|
|
|
|
rvecs[i].convertTo(rvec0, depthRot);
|
|
|
|
|
|
|
|
if (depthTrans == CV_64F)
|
|
|
|
tvec0 = ts[i];
|
|
|
|
else
|
|
|
|
ts[i].convertTo(tvec0, depthTrans);
|
|
|
|
|
|
|
|
if (_rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR)
|
|
|
|
{
|
|
|
|
Mat rref = _rvecs.getMat_();
|
|
|
|
|
|
|
|
if (_rvecs.depth() == CV_32F)
|
|
|
|
rref.at<Vec3f>(0,i) = Vec3f(rvec0.at<float>(0,0), rvec0.at<float>(1,0), rvec0.at<float>(2,0));
|
|
|
|
else
|
|
|
|
rref.at<Vec3d>(0,i) = Vec3d(rvec0.at<double>(0,0), rvec0.at<double>(1,0), rvec0.at<double>(2,0));
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
_rvecs.getMatRef(i) = rvec0;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (_tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR)
|
|
|
|
{
|
|
|
|
|
|
|
|
Mat tref = _tvecs.getMat_();
|
|
|
|
|
|
|
|
if (_tvecs.depth() == CV_32F)
|
|
|
|
tref.at<Vec3f>(0,i) = Vec3f(tvec0.at<float>(0,0), tvec0.at<float>(1,0), tvec0.at<float>(2,0));
|
|
|
|
else
|
|
|
|
tref.at<Vec3d>(0,i) = Vec3d(tvec0.at<double>(0,0), tvec0.at<double>(1,0), tvec0.at<double>(2,0));
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
_tvecs.getMatRef(i) = tvec0;
|
|
|
|
}
|
2017-06-28 20:27:30 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
return solutions;
|
|
|
|
}
|
|
|
|
|
2019-04-27 01:43:42 +08:00
|
|
|
class SolvePnPRefineLMCallback CV_FINAL : public LMSolver::Callback
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
SolvePnPRefineLMCallback(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs)
|
|
|
|
{
|
|
|
|
objectPoints = _opoints.getMat();
|
|
|
|
imagePoints = _ipoints.getMat();
|
|
|
|
npoints = std::max(objectPoints.checkVector(3, CV_32F), objectPoints.checkVector(3, CV_64F));
|
|
|
|
imagePoints0 = imagePoints.reshape(1, npoints*2);
|
|
|
|
cameraMatrix = _cameraMatrix.getMat();
|
|
|
|
distCoeffs = _distCoeffs.getMat();
|
|
|
|
}
|
|
|
|
|
|
|
|
bool compute(InputArray _param, OutputArray _err, OutputArray _Jac) const CV_OVERRIDE
|
|
|
|
{
|
|
|
|
Mat param = _param.getMat();
|
|
|
|
_err.create(npoints*2, 1, CV_64FC1);
|
|
|
|
|
|
|
|
if(_Jac.needed())
|
|
|
|
{
|
|
|
|
_Jac.create(npoints*2, param.rows, CV_64FC1);
|
|
|
|
}
|
|
|
|
|
|
|
|
Mat rvec = param(Rect(0, 0, 1, 3)), tvec = param(Rect(0, 3, 1, 3));
|
|
|
|
|
|
|
|
Mat J, projectedPts;
|
|
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedPts, _Jac.needed() ? J : noArray());
|
|
|
|
|
|
|
|
if (_Jac.needed())
|
|
|
|
{
|
|
|
|
Mat Jac = _Jac.getMat();
|
|
|
|
for (int i = 0; i < Jac.rows; i++)
|
|
|
|
{
|
|
|
|
for (int j = 0; j < Jac.cols; j++)
|
|
|
|
{
|
|
|
|
Jac.at<double>(i,j) = J.at<double>(i,j);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
Mat err = _err.getMat();
|
|
|
|
projectedPts = projectedPts.reshape(1, npoints*2);
|
|
|
|
err = projectedPts - imagePoints0;
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
Mat objectPoints, imagePoints, imagePoints0;
|
|
|
|
Mat cameraMatrix, distCoeffs;
|
|
|
|
int npoints;
|
|
|
|
};
|
|
|
|
|
|
|
|
/**
|
|
|
|
* @brief Compute the Interaction matrix and the residuals for the current pose.
|
|
|
|
* @param objectPoints 3D object points.
|
|
|
|
* @param R Current estimated rotation matrix.
|
|
|
|
* @param tvec Current estimated translation vector.
|
|
|
|
* @param L Interaction matrix for a vector of point features.
|
|
|
|
* @param s Residuals.
|
|
|
|
*/
|
|
|
|
static void computeInteractionMatrixAndResiduals(const Mat& objectPoints, const Mat& R, const Mat& tvec,
|
|
|
|
Mat& L, Mat& s)
|
|
|
|
{
|
|
|
|
Mat objectPointsInCam;
|
|
|
|
|
|
|
|
int npoints = objectPoints.rows;
|
|
|
|
for (int i = 0; i < npoints; i++)
|
|
|
|
{
|
|
|
|
Mat curPt = objectPoints.row(i);
|
|
|
|
objectPointsInCam = R * curPt.t() + tvec;
|
|
|
|
|
|
|
|
double Zi = objectPointsInCam.at<double>(2,0);
|
|
|
|
double xi = objectPointsInCam.at<double>(0,0) / Zi;
|
|
|
|
double yi = objectPointsInCam.at<double>(1,0) / Zi;
|
|
|
|
|
|
|
|
s.at<double>(2*i,0) = xi;
|
|
|
|
s.at<double>(2*i+1,0) = yi;
|
|
|
|
|
|
|
|
L.at<double>(2*i,0) = -1 / Zi;
|
|
|
|
L.at<double>(2*i,1) = 0;
|
|
|
|
L.at<double>(2*i,2) = xi / Zi;
|
|
|
|
L.at<double>(2*i,3) = xi*yi;
|
|
|
|
L.at<double>(2*i,4) = -(1 + xi*xi);
|
|
|
|
L.at<double>(2*i,5) = yi;
|
|
|
|
|
|
|
|
L.at<double>(2*i+1,0) = 0;
|
|
|
|
L.at<double>(2*i+1,1) = -1 / Zi;
|
|
|
|
L.at<double>(2*i+1,2) = yi / Zi;
|
|
|
|
L.at<double>(2*i+1,3) = 1 + yi*yi;
|
|
|
|
L.at<double>(2*i+1,4) = -xi*yi;
|
|
|
|
L.at<double>(2*i+1,5) = -xi;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* @brief The exponential map from se(3) to SE(3).
|
|
|
|
* @param twist A twist (v, w) represents the velocity of a rigid body as an angular velocity
|
|
|
|
* around an axis and a linear velocity along this axis.
|
|
|
|
* @param R1 Resultant rotation matrix from the twist.
|
|
|
|
* @param t1 Resultant translation vector from the twist.
|
|
|
|
*/
|
|
|
|
static void exponentialMapToSE3Inv(const Mat& twist, Mat& R1, Mat& t1)
|
|
|
|
{
|
|
|
|
//see Exponential Map in http://ethaneade.com/lie.pdf
|
|
|
|
/*
|
|
|
|
\begin{align*}
|
|
|
|
\boldsymbol{\delta} &= \left( \mathbf{u}, \boldsymbol{\omega} \right ) \in se(3) \\
|
|
|
|
\mathbf{u}, \boldsymbol{\omega} &\in \mathbb{R}^3 \\
|
|
|
|
\theta &= \sqrt{ \boldsymbol{\omega}^T \boldsymbol{\omega} } \\
|
|
|
|
A &= \frac{\sin \theta}{\theta} \\
|
|
|
|
B &= \frac{1 - \cos \theta}{\theta^2} \\
|
|
|
|
C &= \frac{1-A}{\theta^2} \\
|
|
|
|
\mathbf{R} &= \mathbf{I} + A \boldsymbol{\omega}_{\times} + B \boldsymbol{\omega}_{\times}^2 \\
|
|
|
|
\mathbf{V} &= \mathbf{I} + B \boldsymbol{\omega}_{\times} + C \boldsymbol{\omega}_{\times}^2 \\
|
|
|
|
\exp \begin{pmatrix}
|
|
|
|
\mathbf{u} \\
|
|
|
|
\boldsymbol{\omega}
|
|
|
|
\end{pmatrix} &=
|
|
|
|
\left(
|
|
|
|
\begin{array}{c|c}
|
|
|
|
\mathbf{R} & \mathbf{V} \mathbf{u} \\ \hline
|
|
|
|
\mathbf{0} & 1
|
|
|
|
\end{array}
|
|
|
|
\right )
|
|
|
|
\end{align*}
|
|
|
|
*/
|
|
|
|
double vx = twist.at<double>(0,0);
|
|
|
|
double vy = twist.at<double>(1,0);
|
|
|
|
double vz = twist.at<double>(2,0);
|
|
|
|
double wx = twist.at<double>(3,0);
|
|
|
|
double wy = twist.at<double>(4,0);
|
|
|
|
double wz = twist.at<double>(5,0);
|
|
|
|
|
|
|
|
Matx31d rvec(wx, wy, wz);
|
|
|
|
Mat R;
|
|
|
|
Rodrigues(rvec, R);
|
|
|
|
|
|
|
|
double theta = sqrt(wx*wx + wy*wy + wz*wz);
|
|
|
|
double sinc = std::fabs(theta) < 1e-8 ? 1 : sin(theta) / theta;
|
|
|
|
double mcosc = (std::fabs(theta) < 1e-8) ? 0.5 : (1-cos(theta)) / (theta*theta);
|
|
|
|
double msinc = (std::abs(theta) < 1e-8) ? (1/6.0) : (1-sinc) / (theta*theta);
|
|
|
|
|
|
|
|
Matx31d dt;
|
|
|
|
dt(0) = vx*(sinc + wx*wx*msinc) + vy*(wx*wy*msinc - wz*mcosc) + vz*(wx*wz*msinc + wy*mcosc);
|
|
|
|
dt(1) = vx*(wx*wy*msinc + wz*mcosc) + vy*(sinc + wy*wy*msinc) + vz*(wy*wz*msinc - wx*mcosc);
|
|
|
|
dt(2) = vx*(wx*wz*msinc - wy*mcosc) + vy*(wy*wz*msinc + wx*mcosc) + vz*(sinc + wz*wz*msinc);
|
|
|
|
|
|
|
|
R1 = R.t();
|
|
|
|
t1 = -R1 * dt;
|
|
|
|
}
|
|
|
|
|
|
|
|
enum SolvePnPRefineMethod {
|
|
|
|
SOLVEPNP_REFINE_LM = 0,
|
|
|
|
SOLVEPNP_REFINE_VVS = 1
|
|
|
|
};
|
|
|
|
|
|
|
|
static void 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)
|
|
|
|
{
|
|
|
|
CV_INSTRUMENT_REGION();
|
|
|
|
|
|
|
|
Mat opoints_ = _objectPoints.getMat(), ipoints_ = _imagePoints.getMat();
|
|
|
|
Mat opoints, ipoints;
|
|
|
|
opoints_.convertTo(opoints, CV_64F);
|
|
|
|
ipoints_.convertTo(ipoints, CV_64F);
|
|
|
|
int npoints = opoints.checkVector(3, CV_64F);
|
|
|
|
CV_Assert( npoints >= 3 && npoints == ipoints.checkVector(2, CV_64F) );
|
|
|
|
CV_Assert( !_rvec.empty() && !_tvec.empty() );
|
|
|
|
|
|
|
|
int rtype = _rvec.type(), ttype = _tvec.type();
|
|
|
|
Size rsize = _rvec.size(), tsize = _tvec.size();
|
|
|
|
CV_Assert( (rtype == CV_32FC1 || rtype == CV_64FC1) &&
|
|
|
|
(ttype == CV_32FC1 || ttype == CV_64FC1) );
|
|
|
|
CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
|
|
|
|
(tsize == Size(1, 3) || tsize == Size(3, 1)) );
|
|
|
|
|
|
|
|
Mat cameraMatrix0 = _cameraMatrix.getMat();
|
|
|
|
Mat distCoeffs0 = _distCoeffs.getMat();
|
|
|
|
Mat cameraMatrix = Mat_<double>(cameraMatrix0);
|
|
|
|
Mat distCoeffs = Mat_<double>(distCoeffs0);
|
|
|
|
|
|
|
|
if (_flags == SOLVEPNP_REFINE_LM)
|
|
|
|
{
|
|
|
|
Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
|
|
|
|
Mat rvec, tvec;
|
|
|
|
rvec0.convertTo(rvec, CV_64F);
|
|
|
|
tvec0.convertTo(tvec, CV_64F);
|
|
|
|
|
|
|
|
Mat params(6, 1, CV_64FC1);
|
|
|
|
for (int i = 0; i < 3; i++)
|
|
|
|
{
|
|
|
|
params.at<double>(i,0) = rvec.at<double>(i,0);
|
|
|
|
params.at<double>(i+3,0) = tvec.at<double>(i,0);
|
|
|
|
}
|
|
|
|
|
2019-05-07 23:51:10 +08:00
|
|
|
LMSolver::create(makePtr<SolvePnPRefineLMCallback>(opoints, ipoints, cameraMatrix, distCoeffs), _criteria.maxCount, _criteria.epsilon)->run(params);
|
2019-04-27 01:43:42 +08:00
|
|
|
|
|
|
|
params.rowRange(0, 3).convertTo(rvec0, rvec0.depth());
|
|
|
|
params.rowRange(3, 6).convertTo(tvec0, tvec0.depth());
|
|
|
|
}
|
|
|
|
else if (_flags == SOLVEPNP_REFINE_VVS)
|
|
|
|
{
|
|
|
|
Mat rvec0 = _rvec.getMat(), tvec0 = _tvec.getMat();
|
|
|
|
Mat rvec, tvec;
|
|
|
|
rvec0.convertTo(rvec, CV_64F);
|
|
|
|
tvec0.convertTo(tvec, CV_64F);
|
|
|
|
|
|
|
|
vector<Point2d> ipoints_normalized;
|
|
|
|
undistortPoints(ipoints, ipoints_normalized, cameraMatrix, distCoeffs);
|
|
|
|
Mat sd = Mat(ipoints_normalized).reshape(1, npoints*2);
|
|
|
|
Mat objectPoints0 = opoints.reshape(1, npoints);
|
|
|
|
Mat imagePoints0 = ipoints.reshape(1, npoints*2);
|
|
|
|
Mat L(npoints*2, 6, CV_64FC1), s(npoints*2, 1, CV_64FC1);
|
|
|
|
|
|
|
|
double residuals_1 = std::numeric_limits<double>::max(), residuals = 0;
|
|
|
|
Mat err;
|
|
|
|
Mat R;
|
|
|
|
Rodrigues(rvec, R);
|
|
|
|
for (int iter = 0; iter < _criteria.maxCount; iter++)
|
|
|
|
{
|
|
|
|
computeInteractionMatrixAndResiduals(objectPoints0, R, tvec, L, s);
|
|
|
|
err = s - sd;
|
|
|
|
|
|
|
|
Mat Lp = L.inv(cv::DECOMP_SVD);
|
|
|
|
Mat dq = -_vvslambda * Lp * err;
|
|
|
|
|
|
|
|
Mat R1, t1;
|
|
|
|
exponentialMapToSE3Inv(dq, R1, t1);
|
|
|
|
R = R1 * R;
|
|
|
|
tvec = R1 * tvec + t1;
|
|
|
|
|
|
|
|
residuals_1 = residuals;
|
|
|
|
Mat res = err.t()*err;
|
|
|
|
residuals = res.at<double>(0,0);
|
|
|
|
|
|
|
|
if (std::fabs(residuals - residuals_1) < _criteria.epsilon)
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
Rodrigues(R, rvec);
|
|
|
|
rvec.convertTo(rvec0, rvec0.depth());
|
|
|
|
tvec.convertTo(tvec0, tvec0.depth());
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void solvePnPRefineLM(InputArray _objectPoints, InputArray _imagePoints,
|
|
|
|
InputArray _cameraMatrix, InputArray _distCoeffs,
|
|
|
|
InputOutputArray _rvec, InputOutputArray _tvec,
|
|
|
|
TermCriteria _criteria)
|
|
|
|
{
|
|
|
|
CV_INSTRUMENT_REGION();
|
|
|
|
solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_LM, _criteria);
|
|
|
|
}
|
|
|
|
|
|
|
|
void solvePnPRefineVVS(InputArray _objectPoints, InputArray _imagePoints,
|
|
|
|
InputArray _cameraMatrix, InputArray _distCoeffs,
|
|
|
|
InputOutputArray _rvec, InputOutputArray _tvec,
|
|
|
|
TermCriteria _criteria, double _VVSlambda)
|
|
|
|
{
|
|
|
|
CV_INSTRUMENT_REGION();
|
|
|
|
solvePnPRefine(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, SOLVEPNP_REFINE_VVS, _criteria, _VVSlambda);
|
|
|
|
}
|
|
|
|
|
2019-04-19 01:07:19 +08:00
|
|
|
int solvePnPGeneric( InputArray _opoints, InputArray _ipoints,
|
|
|
|
InputArray _cameraMatrix, InputArray _distCoeffs,
|
|
|
|
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
|
|
|
|
bool useExtrinsicGuess, SolvePnPMethod flags,
|
|
|
|
InputArray _rvec, InputArray _tvec,
|
|
|
|
OutputArray reprojectionError) {
|
|
|
|
CV_INSTRUMENT_REGION();
|
|
|
|
|
|
|
|
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
|
|
|
|
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
|
|
|
|
CV_Assert( ( (npoints >= 4) || (npoints == 3 && flags == SOLVEPNP_ITERATIVE && useExtrinsicGuess) )
|
|
|
|
&& npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
|
|
|
|
|
2019-04-29 23:23:23 +08:00
|
|
|
if (opoints.cols == 3)
|
|
|
|
opoints = opoints.reshape(3);
|
|
|
|
if (ipoints.cols == 2)
|
|
|
|
ipoints = ipoints.reshape(2);
|
|
|
|
|
2019-04-19 01:07:19 +08:00
|
|
|
if( flags != SOLVEPNP_ITERATIVE )
|
|
|
|
useExtrinsicGuess = false;
|
|
|
|
|
|
|
|
if (useExtrinsicGuess)
|
|
|
|
CV_Assert( !_rvec.empty() && !_tvec.empty() );
|
|
|
|
|
|
|
|
if( useExtrinsicGuess )
|
|
|
|
{
|
|
|
|
int rtype = _rvec.type(), ttype = _tvec.type();
|
|
|
|
Size rsize = _rvec.size(), tsize = _tvec.size();
|
|
|
|
CV_Assert( (rtype == CV_32FC1 || rtype == CV_64FC1) &&
|
|
|
|
(ttype == CV_32FC1 || ttype == CV_64FC1) );
|
|
|
|
CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
|
|
|
|
(tsize == Size(1, 3) || tsize == Size(3, 1)) );
|
|
|
|
}
|
|
|
|
|
|
|
|
Mat cameraMatrix0 = _cameraMatrix.getMat();
|
|
|
|
Mat distCoeffs0 = _distCoeffs.getMat();
|
|
|
|
Mat cameraMatrix = Mat_<double>(cameraMatrix0);
|
|
|
|
Mat distCoeffs = Mat_<double>(distCoeffs0);
|
|
|
|
|
|
|
|
vector<Mat> vec_rvecs, vec_tvecs;
|
|
|
|
if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
|
|
|
|
{
|
|
|
|
Mat undistortedPoints;
|
|
|
|
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
|
|
|
|
epnp PnP(cameraMatrix, opoints, undistortedPoints);
|
|
|
|
|
|
|
|
Mat rvec, tvec, R;
|
|
|
|
PnP.compute_pose(R, tvec);
|
|
|
|
Rodrigues(R, rvec);
|
|
|
|
|
|
|
|
vec_rvecs.push_back(rvec);
|
|
|
|
vec_tvecs.push_back(tvec);
|
|
|
|
}
|
|
|
|
else if (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P)
|
|
|
|
{
|
|
|
|
vector<Mat> rvecs, tvecs;
|
|
|
|
solveP3P(_opoints, _ipoints, _cameraMatrix, _distCoeffs, rvecs, tvecs, flags);
|
|
|
|
vec_rvecs.insert(vec_rvecs.end(), rvecs.begin(), rvecs.end());
|
|
|
|
vec_tvecs.insert(vec_tvecs.end(), tvecs.begin(), tvecs.end());
|
|
|
|
}
|
|
|
|
else if (flags == SOLVEPNP_ITERATIVE)
|
|
|
|
{
|
|
|
|
Mat rvec, tvec;
|
|
|
|
if (useExtrinsicGuess)
|
|
|
|
{
|
|
|
|
rvec = _rvec.getMat();
|
|
|
|
tvec = _tvec.getMat();
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
rvec.create(3, 1, CV_64FC1);
|
|
|
|
tvec.create(3, 1, CV_64FC1);
|
|
|
|
}
|
|
|
|
|
|
|
|
CvMat c_objectPoints = cvMat(opoints), c_imagePoints = cvMat(ipoints);
|
|
|
|
CvMat c_cameraMatrix = cvMat(cameraMatrix), c_distCoeffs = cvMat(distCoeffs);
|
|
|
|
CvMat c_rvec = cvMat(rvec), c_tvec = cvMat(tvec);
|
|
|
|
cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
|
|
|
|
(c_distCoeffs.rows && c_distCoeffs.cols) ? &c_distCoeffs : 0,
|
|
|
|
&c_rvec, &c_tvec, useExtrinsicGuess );
|
|
|
|
|
|
|
|
vec_rvecs.push_back(rvec);
|
|
|
|
vec_tvecs.push_back(tvec);
|
|
|
|
}
|
|
|
|
else if (flags == SOLVEPNP_IPPE)
|
|
|
|
{
|
|
|
|
CV_DbgAssert(isPlanarObjectPoints(opoints, 1e-3));
|
|
|
|
Mat undistortedPoints;
|
|
|
|
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
|
|
|
|
|
|
|
|
IPPE::PoseSolver poseSolver;
|
|
|
|
Mat rvec1, tvec1, rvec2, tvec2;
|
|
|
|
float reprojErr1, reprojErr2;
|
|
|
|
try
|
|
|
|
{
|
|
|
|
poseSolver.solveGeneric(opoints, undistortedPoints, rvec1, tvec1, reprojErr1, rvec2, tvec2, reprojErr2);
|
|
|
|
|
|
|
|
if (reprojErr1 < reprojErr2)
|
|
|
|
{
|
|
|
|
vec_rvecs.push_back(rvec1);
|
|
|
|
vec_tvecs.push_back(tvec1);
|
|
|
|
|
|
|
|
vec_rvecs.push_back(rvec2);
|
|
|
|
vec_tvecs.push_back(tvec2);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
vec_rvecs.push_back(rvec2);
|
|
|
|
vec_tvecs.push_back(tvec2);
|
|
|
|
|
|
|
|
vec_rvecs.push_back(rvec1);
|
|
|
|
vec_tvecs.push_back(tvec1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
catch (...) { }
|
|
|
|
}
|
|
|
|
else if (flags == SOLVEPNP_IPPE_SQUARE)
|
|
|
|
{
|
|
|
|
CV_Assert(npoints == 4);
|
|
|
|
|
|
|
|
#if defined _DEBUG || defined CV_STATIC_ANALYSIS
|
|
|
|
double Xs[4][3];
|
|
|
|
if (opoints.depth() == CV_32F)
|
|
|
|
{
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
{
|
|
|
|
for (int j = 0; j < 3; j++)
|
|
|
|
{
|
|
|
|
Xs[i][j] = opoints.ptr<Vec3f>(0)[i](j);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
{
|
|
|
|
for (int j = 0; j < 3; j++)
|
|
|
|
{
|
|
|
|
Xs[i][j] = opoints.ptr<Vec3d>(0)[i](j);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
const double equalThreshold = 1e-9;
|
|
|
|
//Z must be zero
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
{
|
|
|
|
CV_DbgCheck(Xs[i][2], approxEqual(Xs[i][2], 0, equalThreshold), "Z object point coordinate must be zero!");
|
|
|
|
}
|
|
|
|
//Y0 == Y1 && Y2 == Y3
|
|
|
|
CV_DbgCheck(Xs[0][1], approxEqual(Xs[0][1], Xs[1][1], equalThreshold), "Object points must be: Y0 == Y1!");
|
|
|
|
CV_DbgCheck(Xs[2][1], approxEqual(Xs[2][1], Xs[3][1], equalThreshold), "Object points must be: Y2 == Y3!");
|
|
|
|
//X0 == X3 && X1 == X2
|
|
|
|
CV_DbgCheck(Xs[0][0], approxEqual(Xs[0][0], Xs[3][0], equalThreshold), "Object points must be: X0 == X3!");
|
|
|
|
CV_DbgCheck(Xs[1][0], approxEqual(Xs[1][0], Xs[2][0], equalThreshold), "Object points must be: X1 == X2!");
|
|
|
|
//X1 == Y1 && X3 == Y3
|
|
|
|
CV_DbgCheck(Xs[1][0], approxEqual(Xs[1][0], Xs[1][1], equalThreshold), "Object points must be: X1 == Y1!");
|
|
|
|
CV_DbgCheck(Xs[3][0], approxEqual(Xs[3][0], Xs[3][1], equalThreshold), "Object points must be: X3 == Y3!");
|
|
|
|
#endif
|
|
|
|
|
|
|
|
Mat undistortedPoints;
|
|
|
|
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
|
|
|
|
|
|
|
|
IPPE::PoseSolver poseSolver;
|
|
|
|
Mat rvec1, tvec1, rvec2, tvec2;
|
|
|
|
float reprojErr1, reprojErr2;
|
|
|
|
try
|
|
|
|
{
|
|
|
|
poseSolver.solveSquare(opoints, undistortedPoints, rvec1, tvec1, reprojErr1, rvec2, tvec2, reprojErr2);
|
|
|
|
|
|
|
|
if (reprojErr1 < reprojErr2)
|
|
|
|
{
|
|
|
|
vec_rvecs.push_back(rvec1);
|
|
|
|
vec_tvecs.push_back(tvec1);
|
|
|
|
|
|
|
|
vec_rvecs.push_back(rvec2);
|
|
|
|
vec_tvecs.push_back(tvec2);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
vec_rvecs.push_back(rvec2);
|
|
|
|
vec_tvecs.push_back(tvec2);
|
|
|
|
|
|
|
|
vec_rvecs.push_back(rvec1);
|
|
|
|
vec_tvecs.push_back(tvec1);
|
|
|
|
}
|
|
|
|
} catch (...) { }
|
|
|
|
}
|
|
|
|
/*else if (flags == SOLVEPNP_DLS)
|
|
|
|
{
|
|
|
|
Mat undistortedPoints;
|
|
|
|
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
|
|
|
|
|
|
|
|
dls PnP(opoints, undistortedPoints);
|
|
|
|
|
|
|
|
Mat rvec, tvec, R;
|
|
|
|
bool result = PnP.compute_pose(R, tvec);
|
|
|
|
if (result)
|
|
|
|
{
|
|
|
|
Rodrigues(R, rvec);
|
|
|
|
vec_rvecs.push_back(rvec);
|
|
|
|
vec_tvecs.push_back(tvec);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else if (flags == SOLVEPNP_UPNP)
|
|
|
|
{
|
|
|
|
upnp PnP(cameraMatrix, opoints, ipoints);
|
|
|
|
|
|
|
|
Mat rvec, tvec, R;
|
|
|
|
PnP.compute_pose(R, tvec);
|
|
|
|
Rodrigues(R, rvec);
|
|
|
|
vec_rvecs.push_back(rvec);
|
|
|
|
vec_tvecs.push_back(tvec);
|
|
|
|
}*/
|
|
|
|
else
|
|
|
|
CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
|
|
|
|
|
|
|
|
CV_Assert(vec_rvecs.size() == vec_tvecs.size());
|
|
|
|
|
|
|
|
int solutions = static_cast<int>(vec_rvecs.size());
|
|
|
|
|
|
|
|
int depthRot = _rvecs.fixedType() ? _rvecs.depth() : CV_64F;
|
|
|
|
int depthTrans = _tvecs.fixedType() ? _tvecs.depth() : CV_64F;
|
|
|
|
_rvecs.create(solutions, 1, CV_MAKETYPE(depthRot, _rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
|
|
|
|
_tvecs.create(solutions, 1, CV_MAKETYPE(depthTrans, _tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
|
|
|
|
|
|
|
|
for (int i = 0; i < solutions; i++)
|
|
|
|
{
|
|
|
|
Mat rvec0, tvec0;
|
|
|
|
if (depthRot == CV_64F)
|
|
|
|
rvec0 = vec_rvecs[i];
|
|
|
|
else
|
|
|
|
vec_rvecs[i].convertTo(rvec0, depthRot);
|
|
|
|
|
|
|
|
if (depthTrans == CV_64F)
|
|
|
|
tvec0 = vec_tvecs[i];
|
|
|
|
else
|
|
|
|
vec_tvecs[i].convertTo(tvec0, depthTrans);
|
|
|
|
|
|
|
|
if (_rvecs.fixedType() && _rvecs.kind() == _InputArray::STD_VECTOR)
|
|
|
|
{
|
|
|
|
Mat rref = _rvecs.getMat_();
|
|
|
|
|
|
|
|
if (_rvecs.depth() == CV_32F)
|
|
|
|
rref.at<Vec3f>(0,i) = Vec3f(rvec0.at<float>(0,0), rvec0.at<float>(1,0), rvec0.at<float>(2,0));
|
|
|
|
else
|
|
|
|
rref.at<Vec3d>(0,i) = Vec3d(rvec0.at<double>(0,0), rvec0.at<double>(1,0), rvec0.at<double>(2,0));
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
_rvecs.getMatRef(i) = rvec0;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (_tvecs.fixedType() && _tvecs.kind() == _InputArray::STD_VECTOR)
|
|
|
|
{
|
|
|
|
|
|
|
|
Mat tref = _tvecs.getMat_();
|
|
|
|
|
|
|
|
if (_tvecs.depth() == CV_32F)
|
|
|
|
tref.at<Vec3f>(0,i) = Vec3f(tvec0.at<float>(0,0), tvec0.at<float>(1,0), tvec0.at<float>(2,0));
|
|
|
|
else
|
|
|
|
tref.at<Vec3d>(0,i) = Vec3d(tvec0.at<double>(0,0), tvec0.at<double>(1,0), tvec0.at<double>(2,0));
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
_tvecs.getMatRef(i) = tvec0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (reprojectionError.needed())
|
|
|
|
{
|
|
|
|
int type = reprojectionError.type();
|
|
|
|
reprojectionError.create(solutions, 1, type);
|
|
|
|
CV_CheckType(reprojectionError.type(), type == CV_32FC1 || type == CV_64FC1,
|
|
|
|
"Type of reprojectionError must be CV_32FC1 or CV_64FC1!");
|
|
|
|
|
|
|
|
Mat objectPoints, imagePoints;
|
|
|
|
if (_opoints.depth() == CV_32F)
|
|
|
|
{
|
|
|
|
_opoints.getMat().convertTo(objectPoints, CV_64F);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
objectPoints = _opoints.getMat();
|
|
|
|
}
|
|
|
|
if (_ipoints.depth() == CV_32F)
|
|
|
|
{
|
|
|
|
_ipoints.getMat().convertTo(imagePoints, CV_64F);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
imagePoints = _ipoints.getMat();
|
|
|
|
}
|
|
|
|
|
|
|
|
for (size_t i = 0; i < vec_rvecs.size(); i++)
|
|
|
|
{
|
|
|
|
vector<Point2d> projectedPoints;
|
|
|
|
projectPoints(objectPoints, vec_rvecs[i], vec_tvecs[i], cameraMatrix, distCoeffs, projectedPoints);
|
|
|
|
double rmse = norm(projectedPoints, imagePoints, NORM_L2) / sqrt(2*projectedPoints.size());
|
|
|
|
|
|
|
|
Mat err = reprojectionError.getMat();
|
|
|
|
if (type == CV_32F)
|
|
|
|
{
|
|
|
|
err.at<float>(0,static_cast<int>(i)) = static_cast<float>(rmse);
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
err.at<double>(0,static_cast<int>(i)) = rmse;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return solutions;
|
|
|
|
}
|
|
|
|
|
2015-03-16 21:56:26 +08:00
|
|
|
}
|