mirror of
https://github.com/opencv/opencv.git
synced 2024-12-05 01:39:13 +08:00
initIntrisicParams2D() refactored
variables refactoring levmarq fix initIntrinsicParams2D() refactoring undo LM fix cameraCalcJErr: made a lambda; warnings fixed; vars rearranged & renamed, jacobian buffers, perViewErrors and allErrors fix, etc. stereoCalibrate: internal vars in callback stereoCalibrate: capture only useful variables stereoCalibrate: perViewError fix + minors rvecs and tvecs are not pointers anymore no extra lambda newObjPoints: no pointers stdDevs: no pointers _Jo removed: not used Range::all() -> rowRange, colRange param and mask are std::vectors now indices shortened tabs less func-scoped vars; TODOs less formatting & renaming changes trailing whitespaces less diff less changes less changes Range::all() back perViewErr ptr fix NINTRINSIC captured try to fix warning trying to fix a warning fix warnings, another attempt
This commit is contained in:
Rostislav Vasilikhin
parent
f199cf91f3
commit
9ba4bb7355
@ -300,6 +300,7 @@ LevMarq::Report detail::LevMarqBase::optimize()
|
|||||||
lambdaLevMarq *= 1.0 / settings.initialLmDownFactor;
|
lambdaLevMarq *= 1.0 / settings.initialLmDownFactor;
|
||||||
lmUpFactor = settings.initialLmUpFactor;
|
lmUpFactor = settings.initialLmUpFactor;
|
||||||
|
|
||||||
|
// Once set, these flags will be activated until next successful LM iteration - this is not a bug
|
||||||
smallGradient = (gradientMax < settings.minGradientTolerance);
|
smallGradient = (gradientMax < settings.minGradientTolerance);
|
||||||
smallStep = (xNorm < settings.stepNormTolerance);
|
smallStep = (xNorm < settings.stepNormTolerance);
|
||||||
smallEnergyDelta = (costChange / energy < settings.relEnergyDeltaTolerance);
|
smallEnergyDelta = (costChange / energy < settings.relEnergyDeltaTolerance);
|
||||||
|
|||||||
@ -61,13 +61,6 @@ static void initIntrinsicParams2D( const Mat& objectPoints,
|
|||||||
Size imageSize, OutputArray cameraMatrix,
|
Size imageSize, OutputArray cameraMatrix,
|
||||||
double aspectRatio )
|
double aspectRatio )
|
||||||
{
|
{
|
||||||
int i, j, pos;
|
|
||||||
double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
|
|
||||||
double H[9] = {0}, f[2] = {0};
|
|
||||||
Mat _a( 3, 3, CV_64F, a );
|
|
||||||
Mat matH( 3, 3, CV_64F, H );
|
|
||||||
Mat _f( 2, 1, CV_64F, f );
|
|
||||||
|
|
||||||
CV_Assert(npoints.type() == CV_32SC1 && (npoints.rows == 1 || npoints.cols == 1) && npoints.isContinuous());
|
CV_Assert(npoints.type() == CV_32SC1 && (npoints.rows == 1 || npoints.cols == 1) && npoints.isContinuous());
|
||||||
int nimages = npoints.rows + npoints.cols - 1;
|
int nimages = npoints.rows + npoints.cols - 1;
|
||||||
|
|
||||||
@ -79,34 +72,34 @@ static void initIntrinsicParams2D( const Mat& objectPoints,
|
|||||||
if( objectPoints.rows != 1 || imagePoints.rows != 1 )
|
if( objectPoints.rows != 1 || imagePoints.rows != 1 )
|
||||||
CV_Error( CV_StsBadSize, "object points and image points must be a single-row matrices" );
|
CV_Error( CV_StsBadSize, "object points and image points must be a single-row matrices" );
|
||||||
|
|
||||||
Mat matA( 2*nimages, 2, CV_64F );
|
Mat_<double> matA(2*nimages, 2);
|
||||||
Mat _b( 2*nimages, 1, CV_64F );
|
Mat_<double> matb(2*nimages, 1, CV_64F );
|
||||||
a[2] = (!imageSize.width) ? 0.5 : (imageSize.width - 1)*0.5;
|
double fx, fy, cx, cy;
|
||||||
a[5] = (!imageSize.height) ? 0.5 : (imageSize.height - 1)*0.5;
|
cx = (!imageSize.width ) ? 0.5 : (imageSize.width - 1)*0.5;
|
||||||
Mat _allH( nimages, 9, CV_64F );
|
cy = (!imageSize.height) ? 0.5 : (imageSize.height - 1)*0.5;
|
||||||
|
|
||||||
// extract vanishing points in order to obtain initial value for the focal length
|
// extract vanishing points in order to obtain initial value for the focal length
|
||||||
for( i = 0, pos = 0; i < nimages; i++ )
|
int pos = 0;
|
||||||
|
for(int i = 0; i < nimages; i++ )
|
||||||
{
|
{
|
||||||
double* Ap = (double*)matA.data + i*4;
|
|
||||||
double* bp = (double*)_b.data + i*2;
|
|
||||||
int ni = npoints.at<int>(i);
|
int ni = npoints.at<int>(i);
|
||||||
double h[3], v[3], d1[3], d2[3];
|
|
||||||
double n[4] = {0,0,0,0};
|
|
||||||
Mat matM = objectPoints.colRange(pos, pos + ni);
|
Mat matM = objectPoints.colRange(pos, pos + ni);
|
||||||
Mat _m = imagePoints.colRange(pos, pos + ni);
|
Mat _m = imagePoints.colRange(pos, pos + ni);
|
||||||
pos += ni;
|
pos += ni;
|
||||||
|
|
||||||
|
Matx33d H;
|
||||||
Mat matH0 = findHomography(matM, _m);
|
Mat matH0 = findHomography(matM, _m);
|
||||||
CV_Assert(matH0.size() == Size(3, 3));
|
CV_Assert(matH0.size() == Size(3, 3));
|
||||||
matH0.convertTo(matH, CV_64F);
|
matH0.convertTo(H, CV_64F);
|
||||||
|
|
||||||
H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
|
H(0, 0) -= H(2, 0)*cx; H(0, 1) -= H(2, 1)*cx; H(0, 2) -= H(2, 2)*cx;
|
||||||
H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];
|
H(1, 0) -= H(2, 0)*cy; H(1, 1) -= H(2, 1)*cy; H(1, 2) -= H(2, 2)*cy;
|
||||||
|
|
||||||
for( j = 0; j < 3; j++ )
|
Vec3d h, v, d1, d2;
|
||||||
|
Vec4d n;
|
||||||
|
for(int j = 0; j < 3; j++ )
|
||||||
{
|
{
|
||||||
double t0 = H[j*3], t1 = H[j*3+1];
|
double t0 = H(j, 0), t1 = H(j, 1);
|
||||||
h[j] = t0; v[j] = t1;
|
h[j] = t0; v[j] = t1;
|
||||||
d1[j] = (t0 + t1)*0.5;
|
d1[j] = (t0 + t1)*0.5;
|
||||||
d2[j] = (t0 - t1)*0.5;
|
d2[j] = (t0 - t1)*0.5;
|
||||||
@ -114,31 +107,34 @@ static void initIntrinsicParams2D( const Mat& objectPoints,
|
|||||||
n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
|
n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
|
||||||
}
|
}
|
||||||
|
|
||||||
for( j = 0; j < 4; j++ )
|
for(int j = 0; j < 4; j++ )
|
||||||
n[j] = 1./std::sqrt(n[j]);
|
n[j] = 1./std::sqrt(n[j]);
|
||||||
|
|
||||||
for( j = 0; j < 3; j++ )
|
for(int j = 0; j < 3; j++ )
|
||||||
{
|
{
|
||||||
h[j] *= n[0]; v[j] *= n[1];
|
h[j] *= n[0]; v[j] *= n[1];
|
||||||
d1[j] *= n[2]; d2[j] *= n[3];
|
d1[j] *= n[2]; d2[j] *= n[3];
|
||||||
}
|
}
|
||||||
|
|
||||||
Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
|
matA(i*2+0, 0) = h[0]*v[0]; matA(i*2+0, 1) = h[1]*v[1];
|
||||||
Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
|
matA(i*2+1, 0) = d1[0]*d2[0]; matA(i*2+1, 1) = d1[1]*d2[1];
|
||||||
bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
|
matb(i*2+0) = -h[2]*v[2];
|
||||||
|
matb(i*2+1) = -d1[2]*d2[2];
|
||||||
}
|
}
|
||||||
|
|
||||||
solve(matA, _b, _f, DECOMP_NORMAL + DECOMP_SVD);
|
Vec2d f;
|
||||||
CV_Assert((double*)_f.data == f);
|
solve(matA, matb, f, DECOMP_NORMAL + DECOMP_SVD);
|
||||||
a[0] = std::sqrt(fabs(1./f[0]));
|
fx = std::sqrt(fabs(1./f[0]));
|
||||||
a[4] = std::sqrt(fabs(1./f[1]));
|
fy = std::sqrt(fabs(1./f[1]));
|
||||||
if( aspectRatio != 0 )
|
if( aspectRatio != 0 )
|
||||||
{
|
{
|
||||||
double tf = (a[0] + a[4])/(aspectRatio + 1.);
|
double tf = (fx + fy)/(aspectRatio + 1.);
|
||||||
a[0] = aspectRatio*tf;
|
fx = aspectRatio*tf;
|
||||||
a[4] = tf;
|
fy = tf;
|
||||||
}
|
}
|
||||||
_a.copyTo(cameraMatrix);
|
Matx33d(fx, 0, cx,
|
||||||
|
0, fy, cy,
|
||||||
|
0, 0, 1).copyTo(cameraMatrix);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void subMatrix(const Mat& src, Mat& dst,
|
static void subMatrix(const Mat& src, Mat& dst,
|
||||||
@ -165,152 +161,14 @@ static void subMatrix(const Mat& src, Mat& dst,
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void cameraCalcJErr(const Mat& objectPoints, const Mat& imagePoints,
|
|
||||||
const Mat& npoints, Mat& allErrors,
|
|
||||||
Mat& _param, bool calcJ, Mat& JtErr, Mat& JtJ, double& errnorm,
|
|
||||||
double aspectRatio, Mat& perViewErrors,
|
|
||||||
int flags, bool optimizeObjPoints)
|
|
||||||
{
|
|
||||||
const int NINTRINSIC = CALIB_NINTRINSIC;
|
|
||||||
int ni = 0, nimages = (int)npoints.total();
|
|
||||||
double k[14] = {0};
|
|
||||||
Mat _k(14, 1, CV_64F, k);
|
|
||||||
double* param = _param.ptr<double>();
|
|
||||||
int nparams = (int)_param.total();
|
|
||||||
int ni0 = npoints.at<int>(0);
|
|
||||||
Mat _Je(ni0*2, 6, CV_64F), _Ji(ni0*2, NINTRINSIC, CV_64F), _Jo, _err(ni*2, 1, CV_64F);
|
|
||||||
|
|
||||||
if( flags & CALIB_FIX_ASPECT_RATIO )
|
|
||||||
{
|
|
||||||
param[0] = param[1]*aspectRatio;
|
|
||||||
//pparam[0] = pparam[1]*aspectRatio;
|
|
||||||
}
|
|
||||||
|
|
||||||
Matx33d A(param[0], 0, param[2],
|
|
||||||
0, param[1], param[3],
|
|
||||||
0, 0, 1);
|
|
||||||
std::copy(param + 4, param + 4 + 14, k);
|
|
||||||
|
|
||||||
JtJ.setZero();
|
|
||||||
JtErr.setZero();
|
|
||||||
|
|
||||||
if(optimizeObjPoints)
|
|
||||||
_Jo.create(ni0*2, ni0*3, CV_64F);
|
|
||||||
|
|
||||||
double reprojErr = 0;
|
|
||||||
int maxPoints = 0;
|
|
||||||
for( int i = 0; i < nimages; i++ )
|
|
||||||
maxPoints = max(maxPoints, npoints.at<int>(i));
|
|
||||||
|
|
||||||
for( int i = 0, pos = 0; i < nimages; i++, pos += ni )
|
|
||||||
{
|
|
||||||
ni = npoints.at<int>(i);
|
|
||||||
Mat _ri = _param.rowRange(NINTRINSIC + i*6, NINTRINSIC + i*6 + 3);
|
|
||||||
Mat _ti = _param.rowRange(NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6);
|
|
||||||
|
|
||||||
Mat _Mi = objectPoints.colRange(pos, pos + ni);
|
|
||||||
if( optimizeObjPoints )
|
|
||||||
{
|
|
||||||
_Mi = _param.rowRange(NINTRINSIC + nimages * 6,
|
|
||||||
NINTRINSIC + nimages * 6 + ni * 3);
|
|
||||||
_Mi = _Mi.reshape(3, 1);
|
|
||||||
}
|
|
||||||
Mat _mi = imagePoints.colRange(pos, pos + ni);
|
|
||||||
Mat _me = allErrors.colRange(pos, pos + ni);
|
|
||||||
|
|
||||||
_Je.resize(ni*2);
|
|
||||||
_Ji.resize(ni*2);
|
|
||||||
_err.resize(ni*2);
|
|
||||||
if (optimizeObjPoints)
|
|
||||||
_Jo.resize(ni*2);
|
|
||||||
Mat _mp = _err.reshape(2, 1);
|
|
||||||
|
|
||||||
if( calcJ )
|
|
||||||
{
|
|
||||||
Mat _dpdr = _Je.colRange(0, 3);
|
|
||||||
Mat _dpdt = _Je.colRange(3, 6);
|
|
||||||
Mat _dpdf = _Ji.colRange(0, 2);
|
|
||||||
Mat _dpdc = _Ji.colRange(2, 4);
|
|
||||||
Mat _dpdk = _Ji.colRange(4, NINTRINSIC);
|
|
||||||
Mat _dpdo = _Jo.empty() ? Mat() : _Jo.colRange(0, ni * 3);
|
|
||||||
double* dpdr_p = _dpdr.ptr<double>();
|
|
||||||
double* dpdt_p = _dpdt.ptr<double>();
|
|
||||||
double* dpdf_p = _dpdf.ptr<double>();
|
|
||||||
double* dpdc_p = _dpdc.ptr<double>();
|
|
||||||
double* dpdk_p = _dpdk.ptr<double>();
|
|
||||||
double* dpdo_p = _dpdo.ptr<double>();
|
|
||||||
|
|
||||||
projectPoints(_Mi, _ri, _ti, A, _k, _mp, _dpdr, _dpdt,
|
|
||||||
(flags & CALIB_FIX_FOCAL_LENGTH) ? _OutputArray() : _OutputArray(_dpdf),
|
|
||||||
(flags & CALIB_FIX_PRINCIPAL_POINT) ? _OutputArray() : _OutputArray(_dpdc),
|
|
||||||
_dpdk, _Jo.empty() ? _OutputArray() : _OutputArray(_dpdo),
|
|
||||||
(flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0.);
|
|
||||||
CV_Assert(_mp.ptr<double>() == _err.ptr<double>() &&
|
|
||||||
dpdr_p == _dpdr.ptr<double>() && dpdt_p == _dpdt.ptr<double>() &&
|
|
||||||
dpdf_p == _dpdf.ptr<double>() && dpdc_p == _dpdc.ptr<double>() &&
|
|
||||||
dpdk_p == _dpdk.ptr<double>() && dpdo_p == _dpdo.ptr<double>());
|
|
||||||
}
|
|
||||||
else
|
|
||||||
projectPoints( _Mi, _ri, _ti, A, _k, _mp,
|
|
||||||
noArray(), noArray(), noArray(),
|
|
||||||
noArray(), noArray(), noArray(), 0.);
|
|
||||||
|
|
||||||
subtract( _mp, _mi, _mp );
|
|
||||||
_mp.copyTo(_me);
|
|
||||||
|
|
||||||
if( calcJ )
|
|
||||||
{
|
|
||||||
// see HZ: (A6.14) for details on the structure of the Jacobian
|
|
||||||
JtJ(Rect(0, 0, NINTRINSIC, NINTRINSIC)) += _Ji.t() * _Ji;
|
|
||||||
JtJ(Rect(NINTRINSIC + i * 6, NINTRINSIC + i * 6, 6, 6)) = _Je.t() * _Je;
|
|
||||||
JtJ(Rect(NINTRINSIC + i * 6, 0, 6, NINTRINSIC)) = _Ji.t() * _Je;
|
|
||||||
if( optimizeObjPoints )
|
|
||||||
{
|
|
||||||
JtJ(Rect(NINTRINSIC + nimages * 6, 0, maxPoints * 3, NINTRINSIC)) += _Ji.t() * _Jo;
|
|
||||||
JtJ(Rect(NINTRINSIC + nimages * 6, NINTRINSIC + i * 6, maxPoints * 3, 6))
|
|
||||||
+= _Je.t() * _Jo;
|
|
||||||
JtJ(Rect(NINTRINSIC + nimages * 6, NINTRINSIC + nimages * 6, maxPoints * 3, maxPoints * 3))
|
|
||||||
+= _Jo.t() * _Jo;
|
|
||||||
}
|
|
||||||
|
|
||||||
JtErr.rowRange(0, NINTRINSIC) += _Ji.t() * _err;
|
|
||||||
JtErr.rowRange(NINTRINSIC + i * 6, NINTRINSIC + (i + 1) * 6) = _Je.t() * _err;
|
|
||||||
if( optimizeObjPoints )
|
|
||||||
{
|
|
||||||
JtErr.rowRange(NINTRINSIC + nimages * 6, nparams) += _Jo.t() * _err;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
double viewErr = norm(_err, NORM_L2SQR);
|
|
||||||
/*if (i == 0 || i == nimages-1) {
|
|
||||||
printf("image %d.", i);
|
|
||||||
for(int j = 0; j < 10; j++) {
|
|
||||||
printf(" %.2g", _err.at<double>(j));
|
|
||||||
}
|
|
||||||
printf("\n");
|
|
||||||
}*/
|
|
||||||
|
|
||||||
if( !perViewErrors.empty() )
|
|
||||||
perViewErrors.at<double>(i) = std::sqrt(viewErr / ni);
|
|
||||||
|
|
||||||
reprojErr += viewErr;
|
|
||||||
}
|
|
||||||
|
|
||||||
errnorm = reprojErr;
|
|
||||||
}
|
|
||||||
|
|
||||||
static double calibrateCameraInternal( const Mat& objectPoints,
|
static double calibrateCameraInternal( const Mat& objectPoints,
|
||||||
const Mat& imagePoints, const Mat& npoints,
|
const Mat& imagePoints, const Mat& npoints,
|
||||||
Size imageSize, int iFixedPoint, Mat& cameraMatrix, Mat& distCoeffs,
|
Size imageSize, int iFixedPoint, Mat& cameraMatrix, Mat& distCoeffs,
|
||||||
Mat* rvecs, Mat* tvecs, Mat* newObjPoints, Mat* stdDevs,
|
Mat rvecs, Mat tvecs, Mat newObjPoints, Mat stdDevs,
|
||||||
Mat* perViewErrors, int flags, const TermCriteria& termCrit )
|
Mat perViewErr, int flags, const TermCriteria& termCrit )
|
||||||
{
|
{
|
||||||
const int NINTRINSIC = CALIB_NINTRINSIC;
|
int NINTRINSIC = CALIB_NINTRINSIC;
|
||||||
|
|
||||||
Matx33d A;
|
|
||||||
double k[14] = {0};
|
|
||||||
Mat matA(3, 3, CV_64F, A.val);
|
|
||||||
int i, maxPoints = 0, ni = 0, pos, total = 0, nparams, cn;
|
|
||||||
double aspectRatio = 0.;
|
double aspectRatio = 0.;
|
||||||
int nimages = npoints.checkVector(1, CV_32S);
|
int nimages = npoints.checkVector(1, CV_32S);
|
||||||
CV_Assert(nimages >= 1);
|
CV_Assert(nimages >= 1);
|
||||||
@ -335,22 +193,22 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
CV_Error( CV_StsBadArg, "Thin prism model must have 12 parameters in the distortion matrix" );
|
CV_Error( CV_StsBadArg, "Thin prism model must have 12 parameters in the distortion matrix" );
|
||||||
}
|
}
|
||||||
|
|
||||||
if( rvecs )
|
if( !rvecs.empty() )
|
||||||
{
|
{
|
||||||
cn = rvecs->channels();
|
int cn = rvecs.channels();
|
||||||
CV_Assert(rvecs->depth() == CV_32F || rvecs->depth() == CV_64F);
|
CV_Assert(rvecs.depth() == CV_32F || rvecs.depth() == CV_64F);
|
||||||
CV_Assert(rvecs->rows == nimages);
|
CV_Assert(rvecs.rows == nimages);
|
||||||
CV_Assert((rvecs->rows == nimages && (rvecs->cols*cn == 3 || rvecs->cols*cn == 3)) ||
|
CV_Assert((rvecs.rows == nimages && (rvecs.cols*cn == 3 || rvecs.cols*cn == 3)) ||
|
||||||
(rvecs->rows == 1 && rvecs->cols == nimages && cn == 3));
|
(rvecs.rows == 1 && rvecs.cols == nimages && cn == 3));
|
||||||
}
|
}
|
||||||
|
|
||||||
if( tvecs )
|
if( !tvecs.empty() )
|
||||||
{
|
{
|
||||||
cn = tvecs->channels();
|
int cn = tvecs.channels();
|
||||||
CV_Assert(tvecs->depth() == CV_32F || tvecs->depth() == CV_64F);
|
CV_Assert(tvecs.depth() == CV_32F || tvecs.depth() == CV_64F);
|
||||||
CV_Assert(tvecs->rows == nimages);
|
CV_Assert(tvecs.rows == nimages);
|
||||||
CV_Assert((tvecs->rows == nimages && tvecs->cols*cn == 3) ||
|
CV_Assert((tvecs.rows == nimages && tvecs.cols*cn == 3) ||
|
||||||
(tvecs->rows == 1 && tvecs->cols == nimages && cn == 3));
|
(tvecs.rows == 1 && tvecs.cols == nimages && cn == 3));
|
||||||
}
|
}
|
||||||
|
|
||||||
CV_Assert(cameraMatrix.type() == CV_32F || cameraMatrix.type() == CV_64F);
|
CV_Assert(cameraMatrix.type() == CV_32F || cameraMatrix.type() == CV_64F);
|
||||||
@ -361,9 +219,10 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
CV_Assert(ndistCoeffs == 4 || ndistCoeffs == 5 || ndistCoeffs == 8 ||
|
CV_Assert(ndistCoeffs == 4 || ndistCoeffs == 5 || ndistCoeffs == 8 ||
|
||||||
ndistCoeffs == 12 || ndistCoeffs == 14);
|
ndistCoeffs == 12 || ndistCoeffs == 14);
|
||||||
|
|
||||||
for( i = 0; i < nimages; i++ )
|
int total = 0, maxPoints = 0;
|
||||||
|
for(int i = 0; i < nimages; i++ )
|
||||||
{
|
{
|
||||||
ni = npoints.at<int>(i);
|
int ni = npoints.at<int>(i);
|
||||||
if( ni < 4 )
|
if( ni < 4 )
|
||||||
{
|
{
|
||||||
CV_Error_( CV_StsOutOfRange, ("The number of points in the view #%d is < 4", i));
|
CV_Error_( CV_StsOutOfRange, ("The number of points in the view #%d is < 4", i));
|
||||||
@ -372,23 +231,23 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
total += ni;
|
total += ni;
|
||||||
}
|
}
|
||||||
|
|
||||||
if( newObjPoints )
|
if( !newObjPoints.empty() )
|
||||||
{
|
{
|
||||||
cn = newObjPoints->channels();
|
int cn = newObjPoints.channels();
|
||||||
CV_Assert(newObjPoints->depth() == CV_32F || newObjPoints->depth() == CV_64F);
|
CV_Assert(newObjPoints.depth() == CV_32F || newObjPoints.depth() == CV_64F);
|
||||||
CV_Assert(rvecs->rows == nimages);
|
CV_Assert(rvecs.rows == nimages);
|
||||||
CV_Assert((newObjPoints->rows == maxPoints && newObjPoints->cols*cn == 3) ||
|
CV_Assert((newObjPoints.rows == maxPoints && newObjPoints.cols*cn == 3) ||
|
||||||
(newObjPoints->rows == 1 && newObjPoints->cols == maxPoints && cn == 3));
|
(newObjPoints.rows == 1 && newObjPoints.cols == maxPoints && cn == 3));
|
||||||
}
|
}
|
||||||
|
|
||||||
if( stdDevs )
|
if( !stdDevs.empty() )
|
||||||
{
|
{
|
||||||
cn = stdDevs->channels();
|
int cn = stdDevs.channels();
|
||||||
CV_Assert(stdDevs->depth() == CV_32F || stdDevs->depth() == CV_64F);
|
CV_Assert(stdDevs.depth() == CV_32F || stdDevs.depth() == CV_64F);
|
||||||
int nstddev = nimages*6 + NINTRINSIC + (releaseObject ? maxPoints*3 : 0);
|
int nstddev = nimages*6 + NINTRINSIC + (releaseObject ? maxPoints*3 : 0);
|
||||||
|
|
||||||
CV_Assert((stdDevs->rows == nstddev && stdDevs->cols*cn == 1) ||
|
CV_Assert((stdDevs.rows == nstddev && stdDevs.cols*cn == 1) ||
|
||||||
(stdDevs->rows == 1 && stdDevs->cols == nstddev && cn == 1));
|
(stdDevs.rows == 1 && stdDevs.cols == nstddev && cn == 1));
|
||||||
}
|
}
|
||||||
|
|
||||||
Mat matM( 1, total, CV_64FC3 );
|
Mat matM( 1, total, CV_64FC3 );
|
||||||
@ -405,11 +264,12 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
else
|
else
|
||||||
convertPointsFromHomogeneous(imagePoints, _m, CV_64F);
|
convertPointsFromHomogeneous(imagePoints, _m, CV_64F);
|
||||||
|
|
||||||
nparams = NINTRINSIC + nimages*6;
|
int nparams = NINTRINSIC + nimages*6;
|
||||||
if( releaseObject )
|
if( releaseObject )
|
||||||
nparams += maxPoints * 3;
|
nparams += maxPoints * 3;
|
||||||
|
|
||||||
Mat _k( distCoeffs.rows, distCoeffs.cols, CV_64F, k);
|
std::vector<double> k(14, 0.0);
|
||||||
|
Mat _k( distCoeffs.rows, distCoeffs.cols, CV_64F, k.data());
|
||||||
if( distCoeffs.total() < 8 )
|
if( distCoeffs.total() < 8 )
|
||||||
{
|
{
|
||||||
if( distCoeffs.total() < 5 )
|
if( distCoeffs.total() < 5 )
|
||||||
@ -419,7 +279,8 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
const double minValidAspectRatio = 0.01;
|
const double minValidAspectRatio = 0.01;
|
||||||
const double maxValidAspectRatio = 100.0;
|
const double maxValidAspectRatio = 100.0;
|
||||||
|
|
||||||
cameraMatrix.convertTo(matA, CV_64F);
|
Matx33d A;
|
||||||
|
cameraMatrix.convertTo(A, CV_64F);
|
||||||
|
|
||||||
// 1. initialize intrinsic parameters & LM solver
|
// 1. initialize intrinsic parameters & LM solver
|
||||||
if( flags & CALIB_USE_INTRINSIC_GUESS )
|
if( flags & CALIB_USE_INTRINSIC_GUESS )
|
||||||
@ -455,7 +316,7 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
if( fabs(mean[2]) > 1e-5 || fabs(sdv[2]) > 1e-5 )
|
if( fabs(mean[2]) > 1e-5 || fabs(sdv[2]) > 1e-5 )
|
||||||
CV_Error( CV_StsBadArg,
|
CV_Error( CV_StsBadArg,
|
||||||
"For non-planar calibration rigs the initial intrinsic matrix must be specified" );
|
"For non-planar calibration rigs the initial intrinsic matrix must be specified" );
|
||||||
for( i = 0; i < total; i++ )
|
for(int i = 0; i < total; i++ )
|
||||||
matM.at<Point3d>(i).z = 0.;
|
matM.at<Point3d>(i).z = 0.;
|
||||||
|
|
||||||
if( flags & CALIB_FIX_ASPECT_RATIO )
|
if( flags & CALIB_FIX_ASPECT_RATIO )
|
||||||
@ -472,14 +333,10 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
//std::cout << "A0: " << A << std::endl;
|
//std::cout << "A0: " << A << std::endl;
|
||||||
//std::cout << "dist0:" << _k << std::endl;
|
//std::cout << "dist0:" << _k << std::endl;
|
||||||
|
|
||||||
Mat _Ji( maxPoints*2, NINTRINSIC, CV_64FC1, Scalar(0));
|
std::vector<double> param(nparams, 0.0);
|
||||||
Mat _Je( maxPoints*2, 6, CV_64FC1 );
|
Mat paramM(param, false);
|
||||||
Mat _err( maxPoints*2, 1, CV_64FC1 );
|
std::vector<uchar> mask(nparams, (uchar)1);
|
||||||
Mat param0( nparams, 1, CV_64FC1 );
|
|
||||||
Mat mask0 = Mat::ones( nparams, 1, CV_8UC1 );
|
|
||||||
|
|
||||||
const bool allocJo = stdDevs || releaseObject;
|
|
||||||
Mat _Jo = allocJo ? Mat( maxPoints*2, maxPoints*3, CV_64FC1, Scalar(0) ) : Mat();
|
|
||||||
int solveMethod = DECOMP_EIG;
|
int solveMethod = DECOMP_EIG;
|
||||||
|
|
||||||
if(flags & CALIB_USE_LU) {
|
if(flags & CALIB_USE_LU) {
|
||||||
@ -489,11 +346,8 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
solveMethod = DECOMP_QR;
|
solveMethod = DECOMP_QR;
|
||||||
}
|
}
|
||||||
|
|
||||||
double* param = param0.ptr<double>();
|
|
||||||
uchar* mask = mask0.ptr<uchar>();
|
|
||||||
|
|
||||||
param[0] = A(0, 0); param[1] = A(1, 1); param[2] = A(0, 2); param[3] = A(1, 2);
|
param[0] = A(0, 0); param[1] = A(1, 1); param[2] = A(0, 2); param[3] = A(1, 2);
|
||||||
std::copy(k, k + 14, param + 4);
|
std::copy(k.begin(), k.end(), param.begin() + 4);
|
||||||
|
|
||||||
if(flags & CALIB_FIX_ASPECT_RATIO)
|
if(flags & CALIB_FIX_ASPECT_RATIO)
|
||||||
mask[0] = 0;
|
mask[0] = 0;
|
||||||
@ -535,30 +389,36 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
if(releaseObject)
|
if(releaseObject)
|
||||||
{
|
{
|
||||||
// copy object points
|
// copy object points
|
||||||
|
int s = NINTRINSIC + nimages * 6;
|
||||||
|
|
||||||
std::copy( matM.ptr<double>(), matM.ptr<double>( 0, maxPoints - 1 ) + 3,
|
std::copy( matM.ptr<double>(), matM.ptr<double>( 0, maxPoints - 1 ) + 3,
|
||||||
param + NINTRINSIC + nimages * 6 );
|
param.begin() + NINTRINSIC + nimages * 6 );
|
||||||
|
|
||||||
// fix points
|
// fix points
|
||||||
mask[NINTRINSIC + nimages * 6] = 0;
|
mask[s + 0] = 0;
|
||||||
mask[NINTRINSIC + nimages * 6 + 1] = 0;
|
mask[s + 1] = 0;
|
||||||
mask[NINTRINSIC + nimages * 6 + 2] = 0;
|
mask[s + 2] = 0;
|
||||||
mask[NINTRINSIC + nimages * 6 + iFixedPoint * 3] = 0;
|
mask[s + iFixedPoint * 3 + 0] = 0;
|
||||||
mask[NINTRINSIC + nimages * 6 + iFixedPoint * 3 + 1] = 0;
|
mask[s + iFixedPoint * 3 + 1] = 0;
|
||||||
mask[NINTRINSIC + nimages * 6 + iFixedPoint * 3 + 2] = 0;
|
mask[s + iFixedPoint * 3 + 2] = 0;
|
||||||
mask[nparams - 1] = 0;
|
mask[nparams - 1] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 2. initialize extrinsic parameters
|
// 2. initialize extrinsic parameters
|
||||||
for( i = 0, pos = 0; i < nimages; i++, pos += ni )
|
for(int i = 0, pos = 0; i < nimages; i++ )
|
||||||
{
|
{
|
||||||
ni = npoints.at<int>(i);
|
int ni = npoints.at<int>(i);
|
||||||
|
|
||||||
Mat _ri = param0.rowRange(NINTRINSIC + i*6, NINTRINSIC + i*6 + 3);
|
int s = NINTRINSIC + i*6;
|
||||||
Mat _ti = param0.rowRange(NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6);
|
Mat _ri = paramM.rowRange(s, s + 3);
|
||||||
|
Mat _ti = paramM.rowRange(s + 3, s + 6);
|
||||||
|
|
||||||
Mat _Mi = matM.colRange(pos, pos + ni);
|
Mat _Mi = matM.colRange(pos, pos + ni);
|
||||||
Mat _mi = _m.colRange(pos, pos + ni);
|
Mat _mi = _m.colRange(pos, pos + ni);
|
||||||
|
|
||||||
solvePnP(_Mi, _mi, matA, _k, _ri, _ti, false);
|
solvePnP(_Mi, _mi, A, _k, _ri, _ti, false);
|
||||||
|
|
||||||
|
pos += ni;
|
||||||
}
|
}
|
||||||
|
|
||||||
//std::cout << "single camera calib. param before LM: " << param0.t() << "\n";
|
//std::cout << "single camera calib. param before LM: " << param0.t() << "\n";
|
||||||
@ -566,43 +426,159 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
|
|
||||||
// 3. run the optimization
|
// 3. run the optimization
|
||||||
|
|
||||||
Mat perViewErr = perViewErrors ? *perViewErrors : Mat();
|
Mat errBuf( maxPoints*2, 1, CV_64FC1 );
|
||||||
auto calibCameraLMCallback = [matM, _m, npoints, &allErrors, aspectRatio, &perViewErr, flags, releaseObject]
|
Mat JiBuf ( maxPoints*2, NINTRINSIC, CV_64FC1, Scalar(0));
|
||||||
(InputOutputArray _param, OutputArray JtErr, OutputArray JtJ, double& errnorm) -> bool
|
Mat JeBuf ( maxPoints*2, 6, CV_64FC1 );
|
||||||
|
Mat JoBuf;
|
||||||
|
if (releaseObject)
|
||||||
|
JoBuf = Mat( maxPoints*2, maxPoints*3, CV_64FC1);
|
||||||
|
|
||||||
|
auto cameraCalcJErr = [matM, _m, npoints, nimages, flags, releaseObject, nparams, maxPoints, aspectRatio, NINTRINSIC,
|
||||||
|
&JeBuf, &JiBuf, &JoBuf, &errBuf, &allErrors, &perViewErr]
|
||||||
|
(InputOutputArray _param, OutputArray _JtErr, OutputArray _JtJ, double& errnorm) -> bool
|
||||||
{
|
{
|
||||||
Mat jterr = JtErr.getMat(), jtj = JtJ.getMat();
|
bool optimizeObjPoints = releaseObject;
|
||||||
Mat mparam = _param.getMat();
|
|
||||||
cameraCalcJErr(matM, _m, npoints, allErrors, mparam, /* calcJ */ JtErr.needed() && JtJ.needed(),
|
Mat_<double> param_m = _param.getMat();
|
||||||
jterr, jtj, errnorm,
|
param_m = param_m.reshape(1, max(param_m.rows, param_m.cols));
|
||||||
aspectRatio, perViewErr, flags, releaseObject);
|
|
||||||
|
if( flags & CALIB_FIX_ASPECT_RATIO )
|
||||||
|
{
|
||||||
|
param_m(0) = param_m(1)*aspectRatio;
|
||||||
|
}
|
||||||
|
|
||||||
|
double fx = param_m(0), fy = param_m(1), cx = param_m(2), cy = param_m(3);
|
||||||
|
Matx33d intrin(fx, 0, cx,
|
||||||
|
0, fy, cy,
|
||||||
|
0, 0, 1);
|
||||||
|
Mat dist = param_m.rowRange(4, 4+14);
|
||||||
|
|
||||||
|
bool calcJ = !_JtErr.empty() && !_JtJ.empty();
|
||||||
|
Mat JtErr, JtJ;
|
||||||
|
if (calcJ)
|
||||||
|
{
|
||||||
|
JtErr = _JtErr.getMat();
|
||||||
|
JtJ = _JtJ.getMat();
|
||||||
|
JtJ.setZero();
|
||||||
|
JtErr.setZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
double reprojErr = 0;
|
||||||
|
|
||||||
|
int so = NINTRINSIC + nimages * 6;
|
||||||
|
int pos = 0;
|
||||||
|
for( int i = 0; i < nimages; i++ )
|
||||||
|
{
|
||||||
|
int si = NINTRINSIC + i * 6;
|
||||||
|
|
||||||
|
int ni = npoints.at<int>(i);
|
||||||
|
Mat _ri = param_m.rowRange(si, si + 3);
|
||||||
|
Mat _ti = param_m.rowRange(si + 3, si + 6);
|
||||||
|
|
||||||
|
Mat _Mi = matM.colRange(pos, pos + ni);
|
||||||
|
if( optimizeObjPoints )
|
||||||
|
{
|
||||||
|
_Mi = param_m.rowRange(so, so + ni * 3);
|
||||||
|
_Mi = _Mi.reshape(3, 1);
|
||||||
|
}
|
||||||
|
Mat _mi = _m.colRange(pos, pos + ni);
|
||||||
|
Mat _me = allErrors.colRange(pos, pos + ni);
|
||||||
|
|
||||||
|
Mat Jo;
|
||||||
|
if (optimizeObjPoints)
|
||||||
|
Jo = JoBuf(Range(0, ni*2), Range(0, ni*3));
|
||||||
|
|
||||||
|
Mat Je = JeBuf.rowRange(0, ni*2);
|
||||||
|
Mat Ji = JiBuf.rowRange(0, ni*2);
|
||||||
|
Mat err = errBuf.rowRange(0, ni*2);
|
||||||
|
Mat _mp = err.reshape(2, 1);
|
||||||
|
|
||||||
|
if( calcJ )
|
||||||
|
{
|
||||||
|
Mat _dpdr = Je.colRange(0, 3);
|
||||||
|
Mat _dpdt = Je.colRange(3, 6);
|
||||||
|
Mat _dpdf = (flags & CALIB_FIX_FOCAL_LENGTH) ? Mat() : Ji.colRange(0, 2);
|
||||||
|
Mat _dpdc = (flags & CALIB_FIX_PRINCIPAL_POINT) ? Mat() : Ji.colRange(2, 4);
|
||||||
|
Mat _dpdk = Ji.colRange(4, NINTRINSIC);
|
||||||
|
Mat _dpdo = Jo.empty() ? Mat() : Jo.colRange(0, ni * 3);
|
||||||
|
|
||||||
|
projectPoints(_Mi, _ri, _ti, intrin, dist, _mp, _dpdr, _dpdt,
|
||||||
|
(_dpdf.empty() ? noArray() : _dpdf),
|
||||||
|
(_dpdc.empty() ? noArray() : _dpdc),
|
||||||
|
_dpdk, (_dpdo.empty() ? noArray() : _dpdo),
|
||||||
|
(flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0.);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
projectPoints( _Mi, _ri, _ti, intrin, dist, _mp);
|
||||||
|
|
||||||
|
subtract( _mp, _mi, _mp );
|
||||||
|
_mp.copyTo(_me);
|
||||||
|
|
||||||
|
if( calcJ )
|
||||||
|
{
|
||||||
|
// see HZ: (A6.14) for details on the structure of the Jacobian
|
||||||
|
JtJ(Rect(0, 0, NINTRINSIC, NINTRINSIC)) += Ji.t() * Ji;
|
||||||
|
JtJ(Rect(si, si, 6, 6)) = Je.t() * Je;
|
||||||
|
JtJ(Rect(si, 0, 6, NINTRINSIC)) = Ji.t() * Je;
|
||||||
|
if( optimizeObjPoints )
|
||||||
|
{
|
||||||
|
JtJ(Rect(so, 0, maxPoints * 3, NINTRINSIC)) += Ji.t() * Jo;
|
||||||
|
JtJ(Rect(so, si, maxPoints * 3, 6)) += Je.t() * Jo;
|
||||||
|
JtJ(Rect(so, so, maxPoints * 3, maxPoints * 3)) += Jo.t() * Jo;
|
||||||
|
}
|
||||||
|
|
||||||
|
JtErr.rowRange(0, NINTRINSIC) += Ji.t() * err;
|
||||||
|
JtErr.rowRange(si, si + 6) = Je.t() * err;
|
||||||
|
if( optimizeObjPoints )
|
||||||
|
{
|
||||||
|
JtErr.rowRange(so, nparams) += Jo.t() * err;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
double viewErr = norm(err, NORM_L2SQR);
|
||||||
|
if( !perViewErr.empty() )
|
||||||
|
perViewErr.at<double>(i) = std::sqrt(viewErr / ni);
|
||||||
|
|
||||||
|
reprojErr += viewErr;
|
||||||
|
pos += ni;
|
||||||
|
}
|
||||||
|
|
||||||
|
errnorm = reprojErr;
|
||||||
return true;
|
return true;
|
||||||
};
|
};
|
||||||
|
|
||||||
LevMarq solver(param0, calibCameraLMCallback,
|
LevMarq solver(param, cameraCalcJErr,
|
||||||
LevMarq::Settings()
|
LevMarq::Settings()
|
||||||
.setMaxIterations((unsigned int)termCrit.maxCount)
|
.setMaxIterations((unsigned int)termCrit.maxCount)
|
||||||
.setStepNormTolerance(termCrit.epsilon)
|
.setStepNormTolerance(termCrit.epsilon)
|
||||||
.setSmallEnergyTolerance(termCrit.epsilon * termCrit.epsilon),
|
.setSmallEnergyTolerance(termCrit.epsilon * termCrit.epsilon),
|
||||||
mask0, MatrixType::AUTO, VariableType::LINEAR, /* LtoR */ false, solveMethod);
|
mask, MatrixType::AUTO, VariableType::LINEAR, /* LtoR */ false, solveMethod);
|
||||||
// geodesic is not supported for normal callbacks
|
// geodesic is not supported for normal callbacks
|
||||||
solver.optimize();
|
solver.optimize();
|
||||||
|
|
||||||
//std::cout << "single camera calib. param after LM: " << param0.t() << "\n";
|
//std::cout << "single camera calib. param after LM: " << param0.t() << "\n";
|
||||||
|
|
||||||
// If solver failed, then the last calculated perViewErr can be wrong & should be recalculated
|
// If solver failed, then the last calculated perViewErr can be wrong & should be recalculated
|
||||||
Mat JtErr(nparams, 1, CV_64F), JtJ(nparams, nparams, CV_64F), JtJinv, JtJN;
|
Mat JtErr, JtJ, JtJinv, JtJN;
|
||||||
double reprojErr = 0;
|
double reprojErr = 0;
|
||||||
JtErr.setZero(); JtJ.setZero();
|
if (!stdDevs.empty())
|
||||||
cameraCalcJErr(matM, _m, npoints, allErrors, param0, (stdDevs != nullptr),
|
|
||||||
JtErr, JtJ, reprojErr,
|
|
||||||
aspectRatio, perViewErr, flags, releaseObject);
|
|
||||||
if (stdDevs)
|
|
||||||
{
|
{
|
||||||
int nparams_nz = countNonZero(mask0);
|
JtErr = Mat(nparams, 1, CV_64F);
|
||||||
|
JtJ = Mat(nparams, nparams, CV_64F);
|
||||||
|
JtErr.setZero();
|
||||||
|
JtJ.setZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
cameraCalcJErr(param, JtErr, JtJ, reprojErr);
|
||||||
|
|
||||||
|
if (!stdDevs.empty())
|
||||||
|
{
|
||||||
|
int nparams_nz = countNonZero(mask);
|
||||||
JtJN.create(nparams_nz, nparams_nz, CV_64F);
|
JtJN.create(nparams_nz, nparams_nz, CV_64F);
|
||||||
|
|
||||||
subMatrix(JtJ, JtJN, mask0, mask0);
|
subMatrix(JtJ, JtJN, mask, mask);
|
||||||
completeSymm(JtJN, false);
|
completeSymm(JtJN, false);
|
||||||
|
//TODO: try DECOMP_CHOLESKY maybe?
|
||||||
cv::invert(JtJN, JtJinv, DECOMP_EIG);
|
cv::invert(JtJN, JtJinv, DECOMP_EIG);
|
||||||
// sigma2 is deviation of the noise
|
// sigma2 is deviation of the noise
|
||||||
// see any papers about variance of the least squares estimator for
|
// see any papers about variance of the least squares estimator for
|
||||||
@ -610,50 +586,53 @@ static double calibrateCameraInternal( const Mat& objectPoints,
|
|||||||
double sigma2 = norm(allErrors, NORM_L2SQR) / (total - nparams_nz);
|
double sigma2 = norm(allErrors, NORM_L2SQR) / (total - nparams_nz);
|
||||||
int j = 0;
|
int j = 0;
|
||||||
for ( int s = 0; s < nparams; s++ )
|
for ( int s = 0; s < nparams; s++ )
|
||||||
if( mask0.at<uchar>(s) )
|
if( mask[s] )
|
||||||
{
|
{
|
||||||
stdDevs->at<double>(s) = std::sqrt(JtJinv.at<double>(j,j) * sigma2);
|
stdDevs.at<double>(s) = std::sqrt(JtJinv.at<double>(j,j) * sigma2);
|
||||||
j++;
|
j++;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
stdDevs->at<double>(s) = 0.;
|
stdDevs.at<double>(s) = 0.;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 4. store the results
|
// 4. store the results
|
||||||
A = Matx33d(param[0], 0, param[2], 0, param[1], param[3], 0, 0, 1);
|
A = Matx33d(param[0], 0, param[2], 0, param[1], param[3], 0, 0, 1);
|
||||||
A.convertTo(cameraMatrix, cameraMatrix.type());
|
A.convertTo(cameraMatrix, cameraMatrix.type());
|
||||||
_k = Mat(distCoeffs.size(), CV_64F, param + 4);
|
_k = Mat(distCoeffs.size(), CV_64F, param.data() + 4);
|
||||||
_k.convertTo(distCoeffs, distCoeffs.type());
|
_k.convertTo(distCoeffs, distCoeffs.type());
|
||||||
if( newObjPoints && releaseObject )
|
|
||||||
|
if( !newObjPoints.empty() && releaseObject )
|
||||||
{
|
{
|
||||||
Mat _Mi = param0.rowRange(NINTRINSIC + nimages * 6,
|
int s = NINTRINSIC + nimages * 6;
|
||||||
NINTRINSIC + nimages * 6 + maxPoints * 3);
|
Mat _Mi = paramM.rowRange(s, s + maxPoints * 3);
|
||||||
_Mi.reshape(3, 1).convertTo(*newObjPoints, newObjPoints->type());
|
_Mi.reshape(3, 1).convertTo(newObjPoints, newObjPoints.type());
|
||||||
}
|
}
|
||||||
|
|
||||||
for( i = 0, pos = 0; i < nimages; i++ )
|
for(int i = 0; i < nimages; i++ )
|
||||||
{
|
{
|
||||||
if( rvecs )
|
if( !rvecs.empty() )
|
||||||
{
|
{
|
||||||
Mat src = Mat(3, 1, CV_64F, param + NINTRINSIC + i*6);
|
//TODO: fix it
|
||||||
if( rvecs->rows == nimages && rvecs->cols*rvecs->channels() == 9 )
|
Mat src = Mat(3, 1, CV_64F, param.data() + NINTRINSIC + i*6);
|
||||||
|
if( rvecs.rows == nimages && rvecs.cols*rvecs.channels() == 9 )
|
||||||
{
|
{
|
||||||
Mat dst(3, 3, rvecs->depth(), rvecs->ptr(i));
|
Mat dst(3, 3, rvecs.depth(), rvecs.ptr(i));
|
||||||
Rodrigues(src, A);
|
Rodrigues(src, A);
|
||||||
A.convertTo(dst, dst.type());
|
A.convertTo(dst, dst.type());
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
Mat dst(3, 1, rvecs->depth(), rvecs->rows == 1 ?
|
Mat dst(3, 1, rvecs.depth(), rvecs.rows == 1 ?
|
||||||
rvecs->data + i*rvecs->elemSize1() : rvecs->ptr(i));
|
rvecs.data + i*rvecs.elemSize1() : rvecs.ptr(i));
|
||||||
src.convertTo(dst, dst.type());
|
src.convertTo(dst, dst.type());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if( tvecs )
|
if( !tvecs.empty() )
|
||||||
{
|
{
|
||||||
Mat src(3, 1, CV_64F, param + NINTRINSIC + i*6 + 3);
|
//TODO: fix it
|
||||||
Mat dst(3, 1, tvecs->depth(), tvecs->rows == 1 ?
|
Mat src(3, 1, CV_64F, param.data() + NINTRINSIC + i*6 + 3);
|
||||||
tvecs->data + i*tvecs->elemSize1() : tvecs->ptr(i));
|
Mat dst(3, 1, tvecs.depth(), tvecs.rows == 1 ?
|
||||||
|
tvecs.data + i*tvecs.elemSize1() : tvecs.ptr(i));
|
||||||
src.convertTo(dst, dst.type());
|
src.convertTo(dst, dst.type());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -673,7 +652,7 @@ static double stereoCalibrateImpl(
|
|||||||
Mat perViewErr, int flags,
|
Mat perViewErr, int flags,
|
||||||
TermCriteria termCrit )
|
TermCriteria termCrit )
|
||||||
{
|
{
|
||||||
const int NINTRINSIC = 18;
|
int NINTRINSIC = CALIB_NINTRINSIC;
|
||||||
|
|
||||||
double dk[2][14]={{0}};
|
double dk[2][14]={{0}};
|
||||||
Mat Dist[2];
|
Mat Dist[2];
|
||||||
@ -735,7 +714,7 @@ static double stereoCalibrateImpl(
|
|||||||
Mat matA(A[k], false);
|
Mat matA(A[k], false);
|
||||||
calibrateCameraInternal(objectPoints, imagePoints[k],
|
calibrateCameraInternal(objectPoints, imagePoints[k],
|
||||||
_npoints, imageSize, 0, matA, Dist[k],
|
_npoints, imageSize, 0, matA, Dist[k],
|
||||||
0, 0, 0, 0, 0, flags, termCrit);
|
Mat(), Mat(), Mat(), Mat(), Mat(), flags, termCrit);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -829,12 +808,12 @@ static double stereoCalibrateImpl(
|
|||||||
for(int i = 0; i < nimages; i++ )
|
for(int i = 0; i < nimages; i++ )
|
||||||
{
|
{
|
||||||
int ni = _npoints.at<int>(i);
|
int ni = _npoints.at<int>(i);
|
||||||
Mat objpt_i = objectPoints(Range::all(), Range(pos, pos + ni));
|
Mat objpt_i = objectPoints.colRange(pos, pos + ni);
|
||||||
Matx33d R[2];
|
Matx33d R[2];
|
||||||
Vec3d rv, T[2];
|
Vec3d rv, T[2];
|
||||||
for(int k = 0; k < 2; k++ )
|
for(int k = 0; k < 2; k++ )
|
||||||
{
|
{
|
||||||
Mat imgpt_ik = imagePoints[k](Range::all(), Range(pos, pos + ni));
|
Mat imgpt_ik = imagePoints[k].colRange(pos, pos + ni);
|
||||||
solvePnP(objpt_i, imgpt_ik, A[k], Dist[k], rv, T[k], false, SOLVEPNP_ITERATIVE );
|
solvePnP(objpt_i, imgpt_ik, A[k], Dist[k], rv, T[k], false, SOLVEPNP_ITERATIVE );
|
||||||
Rodrigues(rv, R[k]);
|
Rodrigues(rv, R[k]);
|
||||||
|
|
||||||
@ -916,13 +895,17 @@ static double stereoCalibrateImpl(
|
|||||||
Mat J_LRBuf( maxPoints*2, 6, CV_64F );
|
Mat J_LRBuf( maxPoints*2, 6, CV_64F );
|
||||||
Mat JiBuf( maxPoints*2, NINTRINSIC, CV_64F, Scalar(0) );
|
Mat JiBuf( maxPoints*2, NINTRINSIC, CV_64F, Scalar(0) );
|
||||||
|
|
||||||
auto lmcallback = [&](InputOutputArray _param, OutputArray JtErr_, OutputArray JtJ_, double& errnorm)
|
auto lmcallback = [errBuf, JeBuf, J_LRBuf, JiBuf, recomputeIntrinsics, nimages, flags, aspectRatio, &A, &dk, NINTRINSIC,
|
||||||
|
_npoints, objectPoints, imagePoints, Dist, &perViewErr]
|
||||||
|
(InputOutputArray _param, OutputArray JtErr_, OutputArray JtJ_, double& errnorm)
|
||||||
{
|
{
|
||||||
Mat_<double> param_m = _param.getMat();
|
Mat_<double> param_m = _param.getMat();
|
||||||
Vec3d om_LR(param_m(0), param_m(1), param_m(2));
|
Vec3d om_LR(param_m(0), param_m(1), param_m(2));
|
||||||
Vec3d T_LR(param_m(3), param_m(4), param_m(5));
|
Vec3d T_LR(param_m(3), param_m(4), param_m(5));
|
||||||
Vec3d om[2], T[2];
|
Vec3d om[2], T[2];
|
||||||
Matx33d dr3dr1, dr3dr2, dt3dr2, dt3dt1, dt3dt2;
|
Matx33d dr3dr1, dr3dr2, dt3dr2, dt3dt1, dt3dt2;
|
||||||
|
Matx33d intrin[2];
|
||||||
|
std::vector< std::vector<double> > distCoeffs(2, std::vector<double>(14, 0.0));
|
||||||
|
|
||||||
double reprojErr = 0;
|
double reprojErr = 0;
|
||||||
|
|
||||||
@ -944,11 +927,20 @@ static double stereoCalibrateImpl(
|
|||||||
{
|
{
|
||||||
double fx = param_m(idx + k*NINTRINSIC+0), fy = param_m(idx + k*NINTRINSIC+1);
|
double fx = param_m(idx + k*NINTRINSIC+0), fy = param_m(idx + k*NINTRINSIC+1);
|
||||||
double cx = param_m(idx + k*NINTRINSIC+2), cy = param_m(idx + k*NINTRINSIC+3);
|
double cx = param_m(idx + k*NINTRINSIC+2), cy = param_m(idx + k*NINTRINSIC+3);
|
||||||
A[k] = Matx33d(fx, 0, cx,
|
intrin[k] = Matx33d(fx, 0, cx,
|
||||||
0, fy, cy,
|
0, fy, cy,
|
||||||
0, 0, 1);
|
0, 0, 1);
|
||||||
for(int j = 0; j < 14; j++)
|
for(int j = 0; j < 14; j++)
|
||||||
dk[k][j] = param_m(idx + k*NINTRINSIC+4+j);
|
distCoeffs[k][j] = param_m(idx + k*NINTRINSIC+4+j);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
for (int k = 0; k < 2; k++)
|
||||||
|
{
|
||||||
|
intrin[k] = A[k];
|
||||||
|
for(int j = 0; j < 14; j++)
|
||||||
|
distCoeffs[k][j] = dk[k][j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -985,11 +977,11 @@ static double stereoCalibrateImpl(
|
|||||||
Mat imgpt_ik = imagePoints[k](Range::all(), Range(ptPos, ptPos + ni));
|
Mat imgpt_ik = imagePoints[k](Range::all(), Range(ptPos, ptPos + ni));
|
||||||
|
|
||||||
if( JtJ_.needed() || JtErr_.needed() )
|
if( JtJ_.needed() || JtErr_.needed() )
|
||||||
projectPoints(objpt_i, om[k], T[k], A[k], Dist[k],
|
projectPoints(objpt_i, om[k], T[k], intrin[k], distCoeffs[k],
|
||||||
tmpImagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk, noArray(),
|
tmpImagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk, noArray(),
|
||||||
(flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0.);
|
(flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0.);
|
||||||
else
|
else
|
||||||
projectPoints(objpt_i, om[k], T[k], A[k], Dist[k], tmpImagePoints);
|
projectPoints(objpt_i, om[k], T[k], intrin[k], distCoeffs[k], tmpImagePoints);
|
||||||
subtract( tmpImagePoints, imgpt_ik, tmpImagePoints );
|
subtract( tmpImagePoints, imgpt_ik, tmpImagePoints );
|
||||||
|
|
||||||
if( JtJ_.needed() )
|
if( JtJ_.needed() )
|
||||||
@ -1089,6 +1081,8 @@ static double stereoCalibrateImpl(
|
|||||||
reprojErr = r.energy;
|
reprojErr = r.energy;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Extract optimized params from the param vector
|
||||||
|
|
||||||
Vec3d om_LR(param[0], param[1], param[2]);
|
Vec3d om_LR(param[0], param[1], param[2]);
|
||||||
Vec3d T_LR(param[3], param[4], param[5]);
|
Vec3d T_LR(param[3], param[4], param[5]);
|
||||||
Matx33d R_LR;
|
Matx33d R_LR;
|
||||||
@ -1137,15 +1131,18 @@ static void collectCalibrationData( InputArrayOfArrays objectPoints,
|
|||||||
InputArrayOfArrays imagePoints1,
|
InputArrayOfArrays imagePoints1,
|
||||||
InputArrayOfArrays imagePoints2,
|
InputArrayOfArrays imagePoints2,
|
||||||
int iFixedPoint,
|
int iFixedPoint,
|
||||||
Mat& objPtMat, Mat& imgPtMat1, Mat* imgPtMat2,
|
OutputArray objPt, OutputArray imgPt1, OutputArray imgPt2,
|
||||||
Mat& npoints )
|
OutputArray npoints )
|
||||||
{
|
{
|
||||||
int nimages = (int)objectPoints.total();
|
int nimages = (int)objectPoints.total();
|
||||||
int total = 0;
|
int total = 0;
|
||||||
CV_Assert(nimages > 0);
|
CV_Assert(nimages > 0);
|
||||||
CV_CheckEQ(nimages, (int)imagePoints1.total(), "");
|
CV_CheckEQ(nimages, (int)imagePoints1.total(), "");
|
||||||
if (imgPtMat2)
|
if (!imagePoints2.empty())
|
||||||
|
{
|
||||||
CV_CheckEQ(nimages, (int)imagePoints2.total(), "");
|
CV_CheckEQ(nimages, (int)imagePoints2.total(), "");
|
||||||
|
CV_Assert(imgPt2.needed());
|
||||||
|
}
|
||||||
|
|
||||||
for (int i = 0; i < nimages; i++)
|
for (int i = 0; i < nimages; i++)
|
||||||
{
|
{
|
||||||
@ -1168,25 +1165,28 @@ static void collectCalibrationData( InputArrayOfArrays objectPoints,
|
|||||||
}
|
}
|
||||||
|
|
||||||
npoints.create(1, (int)nimages, CV_32S);
|
npoints.create(1, (int)nimages, CV_32S);
|
||||||
objPtMat.create(1, (int)total, CV_32FC3);
|
objPt.create(1, (int)total, CV_32FC3);
|
||||||
imgPtMat1.create(1, (int)total, CV_32FC2);
|
imgPt1.create(1, (int)total, CV_32FC2);
|
||||||
Point2f* imgPtData2 = 0;
|
Point2f* imgPtData2 = 0;
|
||||||
|
|
||||||
if (imgPtMat2)
|
Mat imgPt1Mat = imgPt1.getMat();
|
||||||
|
if (!imagePoints2.empty())
|
||||||
{
|
{
|
||||||
imgPtMat2->create(1, (int)total, CV_32FC2);
|
imgPt2.create(1, (int)total, CV_32FC2);
|
||||||
imgPtData2 = imgPtMat2->ptr<Point2f>();
|
imgPtData2 = imgPt2.getMat().ptr<Point2f>();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Mat nPointsMat = npoints.getMat();
|
||||||
|
Mat objPtMat = objPt.getMat();
|
||||||
Point3f* objPtData = objPtMat.ptr<Point3f>();
|
Point3f* objPtData = objPtMat.ptr<Point3f>();
|
||||||
Point2f* imgPtData1 = imgPtMat1.ptr<Point2f>();
|
Point2f* imgPtData1 = imgPt1.getMat().ptr<Point2f>();
|
||||||
|
|
||||||
for (int i = 0, j = 0; i < nimages; i++)
|
for (int i = 0, j = 0; i < nimages; i++)
|
||||||
{
|
{
|
||||||
Mat objpt = objectPoints.getMat(i);
|
Mat objpt = objectPoints.getMat(i);
|
||||||
Mat imgpt1 = imagePoints1.getMat(i);
|
Mat imgpt1 = imagePoints1.getMat(i);
|
||||||
int numberOfObjectPoints = objpt.checkVector(3, CV_32F);
|
int numberOfObjectPoints = objpt.checkVector(3, CV_32F);
|
||||||
npoints.at<int>(i) = numberOfObjectPoints;
|
nPointsMat.at<int>(i) = numberOfObjectPoints;
|
||||||
for (int n = 0; n < numberOfObjectPoints; ++n)
|
for (int n = 0; n < numberOfObjectPoints; ++n)
|
||||||
{
|
{
|
||||||
objPtData[j + n] = objpt.ptr<Point3f>()[n];
|
objPtData[j + n] = objpt.ptr<Point3f>()[n];
|
||||||
@ -1207,14 +1207,14 @@ static void collectCalibrationData( InputArrayOfArrays objectPoints,
|
|||||||
j += numberOfObjectPoints;
|
j += numberOfObjectPoints;
|
||||||
}
|
}
|
||||||
|
|
||||||
int ni = npoints.at<int>(0);
|
int ni = nPointsMat.at<int>(0);
|
||||||
bool releaseObject = iFixedPoint > 0 && iFixedPoint < ni - 1;
|
bool releaseObject = iFixedPoint > 0 && iFixedPoint < ni - 1;
|
||||||
// check object points. If not qualified, report errors.
|
// check object points. If not qualified, report errors.
|
||||||
if( releaseObject )
|
if( releaseObject )
|
||||||
{
|
{
|
||||||
for (int i = 1; i < nimages; i++)
|
for (int i = 1; i < nimages; i++)
|
||||||
{
|
{
|
||||||
if( npoints.at<int>(i) != ni )
|
if( nPointsMat.at<int>(i) != ni )
|
||||||
{
|
{
|
||||||
CV_Error( CV_StsBadArg, "All objectPoints[i].size() should be equal when "
|
CV_Error( CV_StsBadArg, "All objectPoints[i].size() should be equal when "
|
||||||
"object-releasing method is requested." );
|
"object-releasing method is requested." );
|
||||||
@ -1233,10 +1233,10 @@ static void collectCalibrationData( InputArrayOfArrays objectPoints,
|
|||||||
static void collectCalibrationData( InputArrayOfArrays objectPoints,
|
static void collectCalibrationData( InputArrayOfArrays objectPoints,
|
||||||
InputArrayOfArrays imagePoints1,
|
InputArrayOfArrays imagePoints1,
|
||||||
InputArrayOfArrays imagePoints2,
|
InputArrayOfArrays imagePoints2,
|
||||||
Mat& objPtMat, Mat& imgPtMat1, Mat* imgPtMat2,
|
OutputArray objPt, OutputArray imgPtMat1, OutputArray imgPtMat2,
|
||||||
Mat& npoints )
|
OutputArray npoints )
|
||||||
{
|
{
|
||||||
collectCalibrationData( objectPoints, imagePoints1, imagePoints2, -1, objPtMat, imgPtMat1,
|
collectCalibrationData( objectPoints, imagePoints1, imagePoints2, -1, objPt, imgPtMat1,
|
||||||
imgPtMat2, npoints );
|
imgPtMat2, npoints );
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1258,7 +1258,7 @@ Mat initCameraMatrix2D( InputArrayOfArrays objectPoints,
|
|||||||
|
|
||||||
Mat objPt, imgPt, npoints, cameraMatrix;
|
Mat objPt, imgPt, npoints, cameraMatrix;
|
||||||
collectCalibrationData( objectPoints, imagePoints, noArray(),
|
collectCalibrationData( objectPoints, imagePoints, noArray(),
|
||||||
objPt, imgPt, 0, npoints );
|
objPt, imgPt, noArray(), npoints );
|
||||||
initIntrinsicParams2D( objPt, imgPt, npoints, imageSize, cameraMatrix, aspectRatio );
|
initIntrinsicParams2D( objPt, imgPt, npoints, imageSize, cameraMatrix, aspectRatio );
|
||||||
return cameraMatrix;
|
return cameraMatrix;
|
||||||
}
|
}
|
||||||
@ -1390,7 +1390,7 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints,
|
|||||||
}
|
}
|
||||||
|
|
||||||
collectCalibrationData( _objectPoints, _imagePoints, noArray(), iFixedPoint,
|
collectCalibrationData( _objectPoints, _imagePoints, noArray(), iFixedPoint,
|
||||||
objPt, imgPt, 0, npoints );
|
objPt, imgPt, noArray(), npoints );
|
||||||
bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints.at<int>(0) - 1;
|
bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints.at<int>(0) - 1;
|
||||||
|
|
||||||
newobj_needed = newobj_needed && releaseObject;
|
newobj_needed = newobj_needed && releaseObject;
|
||||||
@ -1405,10 +1405,8 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints,
|
|||||||
bool stddev_any_needed = stddev_needed || stddev_ext_needed || stddev_obj_needed;
|
bool stddev_any_needed = stddev_needed || stddev_ext_needed || stddev_obj_needed;
|
||||||
if( stddev_any_needed )
|
if( stddev_any_needed )
|
||||||
{
|
{
|
||||||
if( releaseObject )
|
int sz = nimages*6 + CALIB_NINTRINSIC + (releaseObject ? np * 3 : 0);
|
||||||
stdDeviationsM.create(nimages*6 + CALIB_NINTRINSIC + np * 3, 1, CV_64F);
|
stdDeviationsM.create(sz, 1, CV_64F);
|
||||||
else
|
|
||||||
stdDeviationsM.create(nimages*6 + CALIB_NINTRINSIC, 1, CV_64F);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if( errors_needed )
|
if( errors_needed )
|
||||||
@ -1420,39 +1418,26 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints,
|
|||||||
double reprojErr = calibrateCameraInternal(
|
double reprojErr = calibrateCameraInternal(
|
||||||
objPt, imgPt, npoints, imageSize, iFixedPoint,
|
objPt, imgPt, npoints, imageSize, iFixedPoint,
|
||||||
cameraMatrix, distCoeffs,
|
cameraMatrix, distCoeffs,
|
||||||
rvecs_needed ? &rvecM : NULL,
|
rvecM, tvecM,
|
||||||
tvecs_needed ? &tvecM : NULL,
|
newObjPt,
|
||||||
newobj_needed ? &newObjPt : NULL,
|
stdDeviationsM,
|
||||||
stddev_any_needed ? &stdDeviationsM : NULL,
|
errorsM, flags, cvTermCriteria(criteria));
|
||||||
errors_needed ? &errorsM : NULL, flags, cvTermCriteria(criteria));
|
|
||||||
|
|
||||||
if( newobj_needed )
|
|
||||||
newObjPt.copyTo(newObjPoints);
|
|
||||||
|
|
||||||
if( stddev_needed )
|
if( stddev_needed )
|
||||||
{
|
{
|
||||||
stdDeviationsIntrinsics.create(CALIB_NINTRINSIC, 1, CV_64F);
|
stdDeviationsM.rowRange(0, CALIB_NINTRINSIC).copyTo(stdDeviationsIntrinsics);
|
||||||
Mat stdDeviationsIntrinsicsMat = stdDeviationsIntrinsics.getMat();
|
|
||||||
std::memcpy(stdDeviationsIntrinsicsMat.ptr(), stdDeviationsM.ptr(),
|
|
||||||
CALIB_NINTRINSIC*sizeof(double));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if ( stddev_ext_needed )
|
if ( stddev_ext_needed )
|
||||||
{
|
{
|
||||||
stdDeviationsExtrinsics.create(nimages*6, 1, CV_64F);
|
int s = CALIB_NINTRINSIC;
|
||||||
Mat stdDeviationsExtrinsicsMat = stdDeviationsExtrinsics.getMat();
|
stdDeviationsM.rowRange(s, s + nimages*6).copyTo(stdDeviationsExtrinsics);
|
||||||
std::memcpy(stdDeviationsExtrinsicsMat.ptr(),
|
|
||||||
stdDeviationsM.ptr() + CALIB_NINTRINSIC*sizeof(double),
|
|
||||||
nimages*6*sizeof(double));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if( stddev_obj_needed )
|
if( stddev_obj_needed )
|
||||||
{
|
{
|
||||||
stdDeviationsObjPoints.create( np * 3, 1, CV_64F );
|
int s = CALIB_NINTRINSIC + nimages*6;
|
||||||
Mat stdDeviationsObjPointsMat = stdDeviationsObjPoints.getMat();
|
stdDeviationsM.rowRange(s, s + np*3).copyTo(stdDeviationsObjPoints);
|
||||||
std::memcpy( stdDeviationsObjPointsMat.ptr(), stdDeviationsM.ptr()
|
|
||||||
+ ( CALIB_NINTRINSIC + nimages * 6 ) * sizeof( double ),
|
|
||||||
np * 3 * sizeof( double ) );
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// overly complicated and inefficient rvec/ tvec handling to support vector<Mat>
|
// overly complicated and inefficient rvec/ tvec handling to support vector<Mat>
|
||||||
@ -1577,7 +1562,7 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints,
|
|||||||
Mat objPt, imgPt, imgPt2, npoints;
|
Mat objPt, imgPt, imgPt2, npoints;
|
||||||
|
|
||||||
collectCalibrationData( _objectPoints, _imagePoints1, _imagePoints2,
|
collectCalibrationData( _objectPoints, _imagePoints1, _imagePoints2,
|
||||||
objPt, imgPt, &imgPt2, npoints );
|
objPt, imgPt, imgPt2, npoints );
|
||||||
Mat matR = _Rmat.getMat(), matT = _Tmat.getMat();
|
Mat matR = _Rmat.getMat(), matT = _Tmat.getMat();
|
||||||
|
|
||||||
bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed();
|
bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed();
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user