Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Delete direct lapack calls, minor fixes in UI

Browse Source
pull/7133/head
Vladislav Sovrasov 8 years ago
parent f6208e3dea
commit d5603caa0b
7 changed files with 16 additions and 1405 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      apps/interactive-calibration/CMakeLists.txt
  2. 2
      apps/interactive-calibration/calibCommon.hpp
  3. 824
      apps/interactive-calibration/cvCalibrationFork.cpp
  4. 56
      apps/interactive-calibration/cvCalibrationFork.hpp
  5. 491
      apps/interactive-calibration/linalg.cpp
  6. 13
      apps/interactive-calibration/linalg.hpp
  7. 26
      apps/interactive-calibration/main.cpp

9
apps/interactive-calibration/CMakeLists.txt
Unescape View File

@ -5,15 +5,6 @@ if(NOT OCV_DEPENDENCIES_FOUND)
return() return()
endif() endif()
find_package(LAPACK)
if(LAPACK_FOUND)
find_file(LAPACK_HEADER "lapacke.h")
if(LAPACK_HEADER)
add_definitions(-DUSE_LAPACK)
link_libraries(${LAPACK_LIBRARIES})
endif()
endif()
project(interactive-calibration) project(interactive-calibration)
set(the_target opencv_interactive-calibration) set(the_target opencv_interactive-calibration)

2
apps/interactive-calibration/calibCommon.hpp
Unescape View File

@ -23,7 +23,7 @@ namespace calib
static const std::string consoleHelp = "Hot keys:\nesc - exit application\n" static const std::string consoleHelp = "Hot keys:\nesc - exit application\n"
"s - save current data to .xml file\n" "s - save current data to .xml file\n"
"r - delete last frame\n" "r - delete last frame\n"
"u - enable/disable applying undistortion" "u - enable/disable applying undistortion\n"
"d - delete all frames\n" "d - delete all frames\n"
"v - switch visualization"; "v - switch visualization";

824
apps/interactive-calibration/cvCalibrationFork.cpp
Unescape View File

@ -1,824 +0,0 @@
#include <opencv2/calib3d.hpp>
#include "linalg.hpp"
#include "cvCalibrationFork.hpp"
using namespace cv;
static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,
const std::vector<uchar>& rows);
static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
static void cvEvaluateJtJ2(CvMat* _JtJ,
const CvMat* camera_matrix,
const CvMat* distortion_coeffs,
const CvMat* object_points,
const CvMat* param,
const CvMat* npoints,
int flags, int NINTRINSIC, double aspectRatio)
{
int i, pos, ni, total = 0, npstep = 0, maxPoints = 0;
npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type);
int nimages = npoints->rows*npoints->cols;
for( i = 0; i < nimages; i++ )
{
ni = npoints->data.i[i*npstep];
if( ni < 4 )
{
CV_Error_( CV_StsOutOfRange, ("The number of points in the view #%d is < 4", i));
}
maxPoints = MAX( maxPoints, ni );
total += ni;
}
Mat _Ji( maxPoints*2, NINTRINSIC, CV_64FC1, Scalar(0));
Mat _Je( maxPoints*2, 6, CV_64FC1 );
Mat _err( maxPoints*2, 1, CV_64FC1 );
Mat _m( 1, total, CV_64FC2 );
const Mat matM = cvarrToMat(object_points);
cvZero(_JtJ);
for(i = 0, pos = 0; i < nimages; i++, pos += ni )
{
CvMat _ri, _ti;
ni = npoints->data.i[i*npstep];
cvGetRows( param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
cvGetRows( param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
CvMat _Mi(matM.colRange(pos, pos + ni));
CvMat _mi(_m.colRange(pos, pos + ni));
_Je.resize(ni*2); _Ji.resize(ni*2); _err.resize(ni*2);
CvMat _dpdr(_Je.colRange(0, 3));
CvMat _dpdt(_Je.colRange(3, 6));
CvMat _dpdf(_Ji.colRange(0, 2));
CvMat _dpdc(_Ji.colRange(2, 4));
CvMat _dpdk(_Ji.colRange(4, NINTRINSIC));
CvMat _mp(_err.reshape(2, 1));
cvProjectPoints2( &_Mi, &_ri, &_ti, camera_matrix, distortion_coeffs, &_mp, &_dpdr, &_dpdt,
(flags & CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf,
(flags & CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk,
(flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0);
cvSub( &_mp, &_mi, &_mp );
Mat JtJ(cvarrToMat(_JtJ));
// 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;
}
}
double cvfork::cvCalibrateCamera2( const CvMat* objectPoints,
const CvMat* imagePoints, const CvMat* npoints,
CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs,
CvMat* rvecs, CvMat* tvecs, CvMat* stdDevs, CvMat* perViewErrors, int flags, CvTermCriteria termCrit )
{
{
const int NINTRINSIC = CV_CALIB_NINTRINSIC;
double reprojErr = 0;
Matx33d A;
double k[14] = {0};
CvMat matA = cvMat(3, 3, CV_64F, A.val), _k;
int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn;
double aspectRatio = 0.;
// 0. check the parameters & allocate buffers
if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) ||
!CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) )
CV_Error( CV_StsBadArg, "One of required vector arguments is not a valid matrix" );
if( imageSize.width <= 0 || imageSize.height <= 0 )
CV_Error( CV_StsOutOfRange, "image width and height must be positive" );
if( CV_MAT_TYPE(npoints->type) != CV_32SC1 ||
(npoints->rows != 1 && npoints->cols != 1) )
CV_Error( CV_StsUnsupportedFormat,
"the array of point counters must be 1-dimensional integer vector" );
if(flags & CV_CALIB_TILTED_MODEL)
{
//when the tilted sensor model is used the distortion coefficients matrix must have 14 parameters
if (distCoeffs->cols*distCoeffs->rows != 14)
CV_Error( CV_StsBadArg, "The tilted sensor model must have 14 parameters in the distortion matrix" );
}
else
{
//when the thin prism model is used the distortion coefficients matrix must have 12 parameters
if(flags & CV_CALIB_THIN_PRISM_MODEL)
if (distCoeffs->cols*distCoeffs->rows != 12)
CV_Error( CV_StsBadArg, "Thin prism model must have 12 parameters in the distortion matrix" );
}
nimages = npoints->rows*npoints->cols;
npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type);
if( rvecs )
{
cn = CV_MAT_CN(rvecs->type);
if( !CV_IS_MAT(rvecs) ||
(CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) ||
((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) &&
(rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) )
CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel "
"1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" );
}
if( tvecs )
{
cn = CV_MAT_CN(tvecs->type);
if( !CV_IS_MAT(tvecs) ||
(CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) ||
((tvecs->rows != nimages || tvecs->cols*cn != 3) &&
(tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) )
CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel "
"1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
}
if( stdDevs )
{
cn = CV_MAT_CN(stdDevs->type);
if( !CV_IS_MAT(stdDevs) ||
(CV_MAT_DEPTH(stdDevs->type) != CV_32F && CV_MAT_DEPTH(stdDevs->type) != CV_64F) ||
((stdDevs->rows != (nimages*6 + NINTRINSIC) || stdDevs->cols*cn != 1) &&
(stdDevs->rows != 1 || stdDevs->cols != (nimages*6 + NINTRINSIC) || cn != 1)) )
CV_Error( CV_StsBadArg, "the output array of standard deviations vectors must be 1-channel "
"1x(n*6 + NINTRINSIC) or (n*6 + NINTRINSIC)x1 array, where n is the number of views" );
}
if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 &&
CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) ||
cameraMatrix->rows != 3 || cameraMatrix->cols != 3 )
CV_Error( CV_StsBadArg,
"Intrinsic parameters must be 3x3 floating-point matrix" );
if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 &&
CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) ||
(distCoeffs->cols != 1 && distCoeffs->rows != 1) ||
(distCoeffs->cols*distCoeffs->rows != 4 &&
distCoeffs->cols*distCoeffs->rows != 5 &&
distCoeffs->cols*distCoeffs->rows != 8 &&
distCoeffs->cols*distCoeffs->rows != 12 &&
distCoeffs->cols*distCoeffs->rows != 14) )
CV_Error( CV_StsBadArg, cvDistCoeffErr );
for( i = 0; i < nimages; i++ )
{
ni = npoints->data.i[i*npstep];
if( ni < 4 )
{
CV_Error_( CV_StsOutOfRange, ("The number of points in the view #%d is < 4", i));
}
maxPoints = MAX( maxPoints, ni );
total += ni;
}
Mat matM( 1, total, CV_64FC3 );
Mat _m( 1, total, CV_64FC2 );
if(CV_MAT_CN(objectPoints->type) == 3) {
cvarrToMat(objectPoints).convertTo(matM, CV_64F);
} else {
convertPointsHomogeneous(cvarrToMat(objectPoints), matM);
}
if(CV_MAT_CN(imagePoints->type) == 2) {
cvarrToMat(imagePoints).convertTo(_m, CV_64F);
} else {
convertPointsHomogeneous(cvarrToMat(imagePoints), _m);
}
nparams = NINTRINSIC + nimages*6;
Mat _Ji( maxPoints*2, NINTRINSIC, CV_64FC1, Scalar(0));
Mat _Je( maxPoints*2, 6, CV_64FC1 );
Mat _err( maxPoints*2, 1, CV_64FC1 );
_k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k);
if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 8 )
{
if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) < 5 )
flags |= CALIB_FIX_K3;
flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6;
}
const double minValidAspectRatio = 0.01;
const double maxValidAspectRatio = 100.0;
// 1. initialize intrinsic parameters & LM solver
if( flags & CALIB_USE_INTRINSIC_GUESS )
{
cvConvert( cameraMatrix, &matA );
if( A(0, 0) <= 0 || A(1, 1) <= 0 )
CV_Error( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" );
if( A(0, 2) < 0 || A(0, 2) >= imageSize.width ||
A(1, 2) < 0 || A(1, 2) >= imageSize.height )
CV_Error( CV_StsOutOfRange, "Principal point must be within the image" );
if( fabs(A(0, 1)) > 1e-5 )
CV_Error( CV_StsOutOfRange, "Non-zero skew is not supported by the function" );
if( fabs(A(1, 0)) > 1e-5 || fabs(A(2, 0)) > 1e-5 ||
fabs(A(2, 1)) > 1e-5 || fabs(A(2,2)-1) > 1e-5 )
CV_Error( CV_StsOutOfRange,
"The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" );
A(0, 1) = A(1, 0) = A(2, 0) = A(2, 1) = 0.;
A(2, 2) = 1.;
if( flags & CALIB_FIX_ASPECT_RATIO )
{
aspectRatio = A(0, 0)/A(1, 1);
if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio )
CV_Error( CV_StsOutOfRange,
"The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" );
}
cvConvert( distCoeffs, &_k );
}
else
{
Scalar mean, sdv;
meanStdDev(matM, mean, sdv);
if( fabs(mean[2]) > 1e-5 || fabs(sdv[2]) > 1e-5 )
CV_Error( CV_StsBadArg,
"For non-planar calibration rigs the initial intrinsic matrix must be specified" );
for( i = 0; i < total; i++ )
matM.at<Point3d>(i).z = 0.;
if( flags & CALIB_FIX_ASPECT_RATIO )
{
aspectRatio = cvmGet(cameraMatrix,0,0);
aspectRatio /= cvmGet(cameraMatrix,1,1);
if( aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio )
CV_Error( CV_StsOutOfRange,
"The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect" );
}
CvMat _matM(matM), m(_m);
cvInitIntrinsicParams2D( &_matM, &m, npoints, imageSize, &matA, aspectRatio );
}
//CvLevMarq solver( nparams, 0, termCrit );
cvfork::CvLevMarqFork solver( nparams, 0, termCrit );
Mat allErrors(1, total, CV_64FC2);
if(flags & CALIB_USE_LU) {
solver.solveMethod = DECOMP_LU;
}
else if(flags & CALIB_USE_QR)
solver.solveMethod = DECOMP_QR;
{
double* param = solver.param->data.db;
uchar* mask = solver.mask->data.ptr;
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);
if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
mask[0] = mask[1] = 0;
if( flags & CV_CALIB_FIX_PRINCIPAL_POINT )
mask[2] = mask[3] = 0;
if( flags & CV_CALIB_ZERO_TANGENT_DIST )
{
param[6] = param[7] = 0;
mask[6] = mask[7] = 0;
}
if( !(flags & CALIB_RATIONAL_MODEL) )
flags |= CALIB_FIX_K4 + CALIB_FIX_K5 + CALIB_FIX_K6;
if( !(flags & CV_CALIB_THIN_PRISM_MODEL))
flags |= CALIB_FIX_S1_S2_S3_S4;
if( !(flags & CV_CALIB_TILTED_MODEL))
flags |= CALIB_FIX_TAUX_TAUY;
mask[ 4] = !(flags & CALIB_FIX_K1);
mask[ 5] = !(flags & CALIB_FIX_K2);
mask[ 8] = !(flags & CALIB_FIX_K3);
mask[ 9] = !(flags & CALIB_FIX_K4);
mask[10] = !(flags & CALIB_FIX_K5);
mask[11] = !(flags & CALIB_FIX_K6);
if(flags & CALIB_FIX_S1_S2_S3_S4)
{
mask[12] = 0;
mask[13] = 0;
mask[14] = 0;
mask[15] = 0;
}
if(flags & CALIB_FIX_TAUX_TAUY)
{
mask[16] = 0;
mask[17] = 0;
}
}
// 2. initialize extrinsic parameters
for( i = 0, pos = 0; i < nimages; i++, pos += ni )
{
CvMat _ri, _ti;
ni = npoints->data.i[i*npstep];
cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
CvMat _Mi(matM.colRange(pos, pos + ni));
CvMat _mi(_m.colRange(pos, pos + ni));
cvFindExtrinsicCameraParams2( &_Mi, &_mi, &matA, &_k, &_ri, &_ti );
}
// 3. run the optimization
for(;;)
{
const CvMat* _param = 0;
CvMat *_JtJ = 0, *_JtErr = 0;
double* _errNorm = 0;
bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm );
double *param = solver.param->data.db, *pparam = solver.prevParam->data.db;
if( flags & CALIB_FIX_ASPECT_RATIO )
{
param[0] = param[1]*aspectRatio;
pparam[0] = pparam[1]*aspectRatio;
}
A(0, 0) = param[0]; A(1, 1) = param[1]; A(0, 2) = param[2]; A(1, 2) = param[3];
std::copy(param + 4, param + 4 + 14, k);
if( !proceed ) {
//do errors estimation
if(stdDevs) {
Ptr<CvMat> JtJ(cvCreateMat(nparams, nparams, CV_64F));
CvMat cvMatM(matM);
cvEvaluateJtJ2(JtJ, &matA, &_k, &cvMatM, solver.param, npoints, flags, NINTRINSIC, aspectRatio);
Mat mask = cvarrToMat(solver.mask);
int nparams_nz = countNonZero(mask);
Mat JtJinv, JtJN;
JtJN.create(nparams_nz, nparams_nz, CV_64F);
subMatrix(cvarrToMat(JtJ), JtJN, mask, mask);
completeSymm(JtJN, false);
#ifndef USE_LAPACK
cv::invert(JtJN, JtJinv, DECOMP_SVD);
#else
cvfork::invert(JtJN, JtJinv, DECOMP_SVD);
#endif
double sigma2 = norm(allErrors, NORM_L2SQR) / (total - nparams_nz);
Mat stdDevsM = cvarrToMat(stdDevs);
int j = 0;
for (int s = 0; s < nparams; s++)
if(mask.data[s]) {
stdDevsM.at<double>(s) = std::sqrt(JtJinv.at<double>(j,j)*sigma2);
j++;
}
else
stdDevsM.at<double>(s) = 0;
}
break;
}
reprojErr = 0;
for( i = 0, pos = 0; i < nimages; i++, pos += ni )
{
CvMat _ri, _ti;
ni = npoints->data.i[i*npstep];
cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 );
cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 );
CvMat _Mi(matM.colRange(pos, pos + ni));
CvMat _mi(_m.colRange(pos, pos + ni));
CvMat _me(allErrors.colRange(pos, pos + ni));
_Je.resize(ni*2); _Ji.resize(ni*2); _err.resize(ni*2);
CvMat _dpdr(_Je.colRange(0, 3));
CvMat _dpdt(_Je.colRange(3, 6));
CvMat _dpdf(_Ji.colRange(0, 2));
CvMat _dpdc(_Ji.colRange(2, 4));
CvMat _dpdk(_Ji.colRange(4, NINTRINSIC));
CvMat _mp(_err.reshape(2, 1));
if( solver.state == CvLevMarq::CALC_J )
{
cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp, &_dpdr, &_dpdt,
(flags & CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf,
(flags & CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk,
(flags & CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0);
}
else
cvProjectPoints2( &_Mi, &_ri, &_ti, &matA, &_k, &_mp );
cvSub( &_mp, &_mi, &_mp );
if( solver.state == CvLevMarq::CALC_J )
{
Mat JtJ(cvarrToMat(_JtJ)), JtErr(cvarrToMat(_JtErr));
// 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;
JtErr.rowRange(0, NINTRINSIC) += _Ji.t() * _err;
JtErr.rowRange(NINTRINSIC + i * 6, NINTRINSIC + (i + 1) * 6) = _Je.t() * _err;
}
if (stdDevs || perViewErrors)
cvCopy(&_mp, &_me);
reprojErr += norm(_err, NORM_L2SQR);
}
if( _errNorm )
*_errNorm = reprojErr;
}
// 4. store the results
cvConvert( &matA, cameraMatrix );
cvConvert( &_k, distCoeffs );
for( i = 0, pos = 0; i < nimages; i++)
{
CvMat src, dst;
if( perViewErrors )
{
ni = npoints->data.i[i*npstep];
perViewErrors->data.db[i] = std::sqrt(cv::norm(allErrors.colRange(pos, pos + ni), NORM_L2SQR) / ni);
pos+=ni;
}
if( rvecs )
{
src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 );
if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 )
{
dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type),
rvecs->data.ptr + rvecs->step*i );
cvRodrigues2( &src, &matA );
cvConvert( &matA, &dst );
}
else
{
dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ?
rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) :
rvecs->data.ptr + rvecs->step*i );
cvConvert( &src, &dst );
}
}
if( tvecs )
{
src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 );
dst = cvMat( 3, 1, CV_MAT_DEPTH(tvecs->type), tvecs->rows == 1 ?
tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) :
tvecs->data.ptr + tvecs->step*i );
cvConvert( &src, &dst );
}
}
return std::sqrt(reprojErr/total);
}
}
static Mat prepareCameraMatrix(Mat& cameraMatrix0, int rtype)
{
Mat cameraMatrix = Mat::eye(3, 3, rtype);
if( cameraMatrix0.size() == cameraMatrix.size() )
cameraMatrix0.convertTo(cameraMatrix, rtype);
return cameraMatrix;
}
static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype)
{
Mat distCoeffs = Mat::zeros(distCoeffs0.cols == 1 ? Size(1, 14) : Size(14, 1), rtype);
if( distCoeffs0.size() == Size(1, 4) ||
distCoeffs0.size() == Size(1, 5) ||
distCoeffs0.size() == Size(1, 8) ||
distCoeffs0.size() == Size(1, 12) ||
distCoeffs0.size() == Size(1, 14) ||
distCoeffs0.size() == Size(4, 1) ||
distCoeffs0.size() == Size(5, 1) ||
distCoeffs0.size() == Size(8, 1) ||
distCoeffs0.size() == Size(12, 1) ||
distCoeffs0.size() == Size(14, 1) )
{
Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows));
distCoeffs0.convertTo(dstCoeffs, rtype);
}
return distCoeffs;
}
static void collectCalibrationData( InputArrayOfArrays objectPoints,
InputArrayOfArrays imagePoints1,
InputArrayOfArrays imagePoints2,
Mat& objPtMat, Mat& imgPtMat1, Mat* imgPtMat2,
Mat& npoints )
{
int nimages = (int)objectPoints.total();
int i, j = 0, ni = 0, total = 0;
CV_Assert(nimages > 0 && nimages == (int)imagePoints1.total() &&
(!imgPtMat2 || nimages == (int)imagePoints2.total()));
for( i = 0; i < nimages; i++ )
{
ni = objectPoints.getMat(i).checkVector(3, CV_32F);
if( ni <= 0 )
CV_Error(CV_StsUnsupportedFormat, "objectPoints should contain vector of vectors of points of type Point3f");
int ni1 = imagePoints1.getMat(i).checkVector(2, CV_32F);
if( ni1 <= 0 )
CV_Error(CV_StsUnsupportedFormat, "imagePoints1 should contain vector of vectors of points of type Point2f");
CV_Assert( ni == ni1 );
total += ni;
}
npoints.create(1, (int)nimages, CV_32S);
objPtMat.create(1, (int)total, CV_32FC3);
imgPtMat1.create(1, (int)total, CV_32FC2);
Point2f* imgPtData2 = 0;
if( imgPtMat2 )
{
imgPtMat2->create(1, (int)total, CV_32FC2);
imgPtData2 = imgPtMat2->ptr<Point2f>();
}
Point3f* objPtData = objPtMat.ptr<Point3f>();
Point2f* imgPtData1 = imgPtMat1.ptr<Point2f>();
for( i = 0; i < nimages; i++, j += ni )
{
Mat objpt = objectPoints.getMat(i);
Mat imgpt1 = imagePoints1.getMat(i);
ni = objpt.checkVector(3, CV_32F);
npoints.at<int>(i) = ni;
memcpy( objPtData + j, objpt.ptr(), ni*sizeof(objPtData[0]) );
memcpy( imgPtData1 + j, imgpt1.ptr(), ni*sizeof(imgPtData1[0]) );
if( imgPtData2 )
{
Mat imgpt2 = imagePoints2.getMat(i);
int ni2 = imgpt2.checkVector(2, CV_32F);
CV_Assert( ni == ni2 );
memcpy( imgPtData2 + j, imgpt2.ptr(), ni*sizeof(imgPtData2[0]) );
}
}
}
double cvfork::calibrateCamera(InputArrayOfArrays _objectPoints,
InputArrayOfArrays _imagePoints,
Size imageSize, InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, OutputArray _stdDeviations, OutputArray _perViewErrors, int flags, TermCriteria criteria )
{
int rtype = CV_64F;
Mat cameraMatrix = _cameraMatrix.getMat();
cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype);
Mat distCoeffs = _distCoeffs.getMat();
distCoeffs = prepareDistCoeffs(distCoeffs, rtype);
if( !(flags & CALIB_RATIONAL_MODEL) &&
(!(flags & CALIB_THIN_PRISM_MODEL)) &&
(!(flags & CALIB_TILTED_MODEL)))
distCoeffs = distCoeffs.rows == 1 ? distCoeffs.colRange(0, 5) : distCoeffs.rowRange(0, 5);
int nimages = int(_objectPoints.total());
CV_Assert( nimages > 0 );
Mat objPt, imgPt, npoints, rvecM, tvecM, stdDeviationsM, errorsM;
bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed(),
stddev_needed = _stdDeviations.needed(), errors_needed = _perViewErrors.needed();
bool rvecs_mat_vec = _rvecs.isMatVector();
bool tvecs_mat_vec = _tvecs.isMatVector();
if( rvecs_needed ) {
_rvecs.create(nimages, 1, CV_64FC3);
if(rvecs_mat_vec)
rvecM.create(nimages, 3, CV_64F);
else
rvecM = _rvecs.getMat();
}
if( tvecs_needed ) {
_tvecs.create(nimages, 1, CV_64FC3);
if(tvecs_mat_vec)
tvecM.create(nimages, 3, CV_64F);
else
tvecM = _tvecs.getMat();
}
if( stddev_needed ) {
_stdDeviations.create(nimages*6 + CV_CALIB_NINTRINSIC, 1, CV_64F);
stdDeviationsM = _stdDeviations.getMat();
}
if( errors_needed) {
_perViewErrors.create(nimages, 1, CV_64F);
errorsM = _perViewErrors.getMat();
}
collectCalibrationData( _objectPoints, _imagePoints, noArray(),
objPt, imgPt, 0, npoints );
CvMat c_objPt = objPt, c_imgPt = imgPt, c_npoints = npoints;
CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
CvMat c_rvecM = rvecM, c_tvecM = tvecM, c_stdDev = stdDeviationsM, c_errors = errorsM;
double reprojErr = cvfork::cvCalibrateCamera2(&c_objPt, &c_imgPt, &c_npoints, imageSize,
&c_cameraMatrix, &c_distCoeffs,
rvecs_needed ? &c_rvecM : NULL,
tvecs_needed ? &c_tvecM : NULL,
stddev_needed ? &c_stdDev : NULL,
errors_needed ? &c_errors : NULL, flags, criteria );
// overly complicated and inefficient rvec/ tvec handling to support vector<Mat>
for(int i = 0; i < nimages; i++ )
{
if( rvecs_needed && rvecs_mat_vec)
{
_rvecs.create(3, 1, CV_64F, i, true);
Mat rv = _rvecs.getMat(i);
memcpy(rv.ptr(), rvecM.ptr(i), 3*sizeof(double));
}
if( tvecs_needed && tvecs_mat_vec)
{
_tvecs.create(3, 1, CV_64F, i, true);
Mat tv = _tvecs.getMat(i);
memcpy(tv.ptr(), tvecM.ptr(i), 3*sizeof(double));
}
}
cameraMatrix.copyTo(_cameraMatrix);
distCoeffs.copyTo(_distCoeffs);
return reprojErr;
}
double cvfork::calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
Ptr<aruco::CharucoBoard> &_board, Size imageSize,
InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, OutputArray _stdDeviations, OutputArray _perViewErrors,
int flags, TermCriteria criteria) {
CV_Assert(_charucoIds.total() > 0 && (_charucoIds.total() == _charucoCorners.total()));
// Join object points of charuco corners in a single vector for calibrateCamera() function
std::vector< std::vector< Point3f > > allObjPoints;
allObjPoints.resize(_charucoIds.total());
for(unsigned int i = 0; i < _charucoIds.total(); i++) {
unsigned int nCorners = (unsigned int)_charucoIds.getMat(i).total();
CV_Assert(nCorners > 0 && nCorners == _charucoCorners.getMat(i).total()); //actually nCorners must be > 3 for calibration
allObjPoints[i].reserve(nCorners);
for(unsigned int j = 0; j < nCorners; j++) {
int pointId = _charucoIds.getMat(i).ptr< int >(0)[j];
CV_Assert(pointId >= 0 && pointId < (int)_board->chessboardCorners.size());
allObjPoints[i].push_back(_board->chessboardCorners[pointId]);
}
}
return cvfork::calibrateCamera(allObjPoints, _charucoCorners, imageSize, _cameraMatrix, _distCoeffs,
_rvecs, _tvecs, _stdDeviations, _perViewErrors, flags, criteria);
}
static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,
const std::vector<uchar>& rows) {
int nonzeros_cols = cv::countNonZero(cols);
cv::Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
for (int i = 0, j = 0; i < (int)cols.size(); i++)
{
if (cols[i])
{
src.col(i).copyTo(tmp.col(j++));
}
}
int nonzeros_rows = cv::countNonZero(rows);
dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1);
for (int i = 0, j = 0; i < (int)rows.size(); i++)
{
if (rows[i])
{
tmp.row(i).copyTo(dst.row(j++));
}
}
}
void cvfork::CvLevMarqFork::step()
{
using namespace cv;
const double LOG10 = log(10.);
double lambda = exp(lambdaLg10*LOG10);
int nparams = param->rows;
Mat _JtJ = cvarrToMat(JtJ);
Mat _mask = cvarrToMat(mask);
int nparams_nz = countNonZero(_mask);
if(!JtJN || JtJN->rows != nparams_nz) {
// prevent re-allocation in every step
JtJN.reset(cvCreateMat( nparams_nz, nparams_nz, CV_64F ));
JtJV.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
JtJW.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
}
Mat _JtJN = cvarrToMat(JtJN);
Mat _JtErr = cvarrToMat(JtJV);
Mat_<double> nonzero_param = cvarrToMat(JtJW);
subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(1, 1), _mask);
subMatrix(_JtJ, _JtJN, _mask, _mask);
if( !err )
completeSymm( _JtJN, completeSymmFlag );
#if 1
_JtJN.diag() *= 1. + lambda;
#else
_JtJN.diag() += lambda;
#endif
#ifndef USE_LAPACK
cv::solve(_JtJN, _JtErr, nonzero_param, solveMethod);
#else
cvfork::solve(_JtJN, _JtErr, nonzero_param, solveMethod);
#endif
int j = 0;
for( int i = 0; i < nparams; i++ )
param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0);
}
cvfork::CvLevMarqFork::CvLevMarqFork(int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag)
{
init(nparams, nerrs, criteria0, _completeSymmFlag);
}
cvfork::CvLevMarqFork::~CvLevMarqFork()
{
clear();
}
bool cvfork::CvLevMarqFork::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
{
CV_Assert( !err );
if( state == DONE )
{
_param = param;
return false;
}
if( state == STARTED )
{
_param = param;
cvZero( JtJ );
cvZero( JtErr );
errNorm = 0;
_JtJ = JtJ;
_JtErr = JtErr;
_errNorm = &errNorm;
state = CALC_J;
return true;
}
if( state == CALC_J )
{
cvCopy( param, prevParam );
step();
_param = param;
prevErrNorm = errNorm;
errNorm = 0;
_errNorm = &errNorm;
state = CHECK_ERR;
return true;
}
assert( state == CHECK_ERR );
if( errNorm > prevErrNorm )
{
if( ++lambdaLg10 <= 16 )
{
step();
_param = param;
errNorm = 0;
_errNorm = &errNorm;
state = CHECK_ERR;
return true;
}
}
lambdaLg10 = MAX(lambdaLg10-1, -16);
if( ++iters >= criteria.max_iter ||
cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
{
//printf("iters %i\n", iters);
_param = param;
state = DONE;
return false;
}
prevErrNorm = errNorm;
cvZero( JtJ );
cvZero( JtErr );
_param = param;
_JtJ = JtJ;
_JtErr = JtErr;
state = CALC_J;
return true;
}

56
apps/interactive-calibration/cvCalibrationFork.hpp
Unescape View File

@ -1,56 +0,0 @@
#ifndef CV_CALIBRATION_FORK_HPP
#define CV_CALIBRATION_FORK_HPP
#include <opencv2/core.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/calib3d/calib3d_c.h>
namespace cvfork
{
using namespace cv;
#define CV_CALIB_NINTRINSIC 18
#define CALIB_USE_QR (1 << 18)
double calibrateCamera(InputArrayOfArrays objectPoints,
InputArrayOfArrays imagePoints, Size imageSize,
InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, OutputArray stdDeviations,
OutputArray perViewErrors, int flags = 0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
double cvCalibrateCamera2( const CvMat* object_points,
const CvMat* image_points,
const CvMat* point_counts,
CvSize image_size,
CvMat* camera_matrix,
CvMat* distortion_coeffs,
CvMat* rotation_vectors CV_DEFAULT(NULL),
CvMat* translation_vectors CV_DEFAULT(NULL),
CvMat* stdDeviations_vector CV_DEFAULT(NULL),
CvMat* perViewErrors_vector CV_DEFAULT(NULL),
int flags CV_DEFAULT(0),
CvTermCriteria term_crit CV_DEFAULT(cvTermCriteria(
CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON)) );
double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
Ptr<aruco::CharucoBoard> &_board, Size imageSize,
InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, OutputArray _stdDeviations, OutputArray _perViewErrors,
int flags = 0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
class CvLevMarqFork : public CvLevMarq
{
public:
CvLevMarqFork( int nparams, int nerrs, CvTermCriteria criteria=
cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
bool completeSymmFlag=false );
bool updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm );
void step();
~CvLevMarqFork();
};
}
#endif

491
apps/interactive-calibration/linalg.cpp
Unescape View File

@ -1,491 +0,0 @@
#include "linalg.hpp"
#ifdef USE_LAPACK
typedef int integer;
#include <lapacke.h>
#include <cassert>
using namespace cv;
bool cvfork::solve(InputArray _src, const InputArray _src2arg, OutputArray _dst, int method )
{
bool result = true;
Mat src = _src.getMat(), _src2 = _src2arg.getMat();
int type = src.type();
bool is_normal = (method & DECOMP_NORMAL) != 0;
CV_Assert( type == _src2.type() && (type == CV_32F || type == CV_64F) );
method &= ~DECOMP_NORMAL;
CV_Assert( (method != DECOMP_LU && method != DECOMP_CHOLESKY) ||
is_normal || src.rows == src.cols );
double rcond=-1, s1=0, work1=0, *work=0, *s=0;
float frcond=-1, fs1=0, fwork1=0, *fwork=0, *fs=0;
integer m = src.rows, m_ = m, n = src.cols, mn = std::max(m,n),
nm = std::min(m, n), nb = _src2.cols, lwork=-1, liwork=0, iwork1=0,
lda = m, ldx = mn, info=0, rank=0, *iwork=0;
int elem_size = CV_ELEM_SIZE(type);
bool copy_rhs=false;
int buf_size=0;
AutoBuffer<uchar> buffer;
uchar* ptr;
char N[] = {'N', '\0'}, L[] = {'L', '\0'};
Mat src2 = _src2;
_dst.create( src.cols, src2.cols, src.type() );
Mat dst = _dst.getMat();
if( m <= n )
is_normal = false;
else if( is_normal )
m_ = n;
buf_size += (is_normal ? n*n : m*n)*elem_size;
if( m_ != n || nb > 1 || !dst.isContinuous() )
{
copy_rhs = true;
if( is_normal )
buf_size += n*nb*elem_size;
else
buf_size += mn*nb*elem_size;
}
if( method == DECOMP_SVD || method == DECOMP_EIG )
{
integer nlvl = cvRound(std::log(std::max(std::min(m_,n)/25., 1.))/CV_LOG2) + 1;
liwork = std::min(m_,n)*(3*std::max(nlvl,(integer)0) + 11);
if( type == CV_32F )
sgelsd_(&m_, &n, &nb, (float*)src.data, &lda, (float*)dst.data, &ldx,
&fs1, &frcond, &rank, &fwork1, &lwork, &iwork1, &info);
else
dgelsd_(&m_, &n, &nb, (double*)src.data, &lda, (double*)dst.data, &ldx,
&s1, &rcond, &rank, &work1, &lwork, &iwork1, &info );
buf_size += nm*elem_size + (liwork + 1)*sizeof(integer);
}
else if( method == DECOMP_QR )
{
if( type == CV_32F )
sgels_(N, &m_, &n, &nb, (float*)src.data, &lda,
(float*)dst.data, &ldx, &fwork1, &lwork, &info );
else
dgels_(N, &m_, &n, &nb, (double*)src.data, &lda,
(double*)dst.data, &ldx, &work1, &lwork, &info );
}
else if( method == DECOMP_LU )
{
buf_size += (n+1)*sizeof(integer);
}
else if( method == DECOMP_CHOLESKY )
;
else
CV_Error( Error::StsBadArg, "Unknown method" );
assert(info == 0);
lwork = cvRound(type == CV_32F ? (double)fwork1 : work1);
buf_size += lwork*elem_size;
buffer.allocate(buf_size);
ptr = (uchar*)buffer;
Mat at(n, m_, type, ptr);
ptr += n*m_*elem_size;
if( method == DECOMP_CHOLESKY || method == DECOMP_EIG )
src.copyTo(at);
else if( !is_normal )
transpose(src, at);
else
mulTransposed(src, at, true);
Mat xt;
if( !is_normal )
{
if( copy_rhs )
{
Mat temp(nb, mn, type, ptr);
ptr += nb*mn*elem_size;
Mat bt = temp.colRange(0, m);
xt = temp.colRange(0, n);
transpose(src2, bt);
}
else
{
src2.copyTo(dst);
xt = Mat(1, n, type, dst.data);
}
}
else
{
if( copy_rhs )
{
xt = Mat(nb, n, type, ptr);
ptr += nb*n*elem_size;
}
else
xt = Mat(1, n, type, dst.data);
// (a'*b)' = b'*a
gemm( src2, src, 1, Mat(), 0, xt, GEMM_1_T );
}
lda = (int)(at.step ? at.step/elem_size : at.cols);
ldx = (int)(xt.step ? xt.step/elem_size : (!is_normal && copy_rhs ? mn : n));
if( method == DECOMP_SVD || method == DECOMP_EIG )
{
if( type == CV_32F )
{
fs = (float*)ptr;
ptr += nm*elem_size;
fwork = (float*)ptr;
ptr += lwork*elem_size;
iwork = (integer*)alignPtr(ptr, sizeof(integer));
sgelsd_(&m_, &n, &nb, (float*)at.data, &lda, (float*)xt.data, &ldx,
fs, &frcond, &rank, fwork, &lwork, iwork, &info);
}
else
{
s = (double*)ptr;
ptr += nm*elem_size;
work = (double*)ptr;
ptr += lwork*elem_size;
iwork = (integer*)alignPtr(ptr, sizeof(integer));
dgelsd_(&m_, &n, &nb, (double*)at.data, &lda, (double*)xt.data, &ldx,
s, &rcond, &rank, work, &lwork, iwork, &info);
}
}
else if( method == DECOMP_QR )
{
if( type == CV_32F )
{
fwork = (float*)ptr;
sgels_(N, &m_, &n, &nb, (float*)at.data, &lda,
(float*)xt.data, &ldx, fwork, &lwork, &info);
}
else
{
work = (double*)ptr;
dgels_(N, &m_, &n, &nb, (double*)at.data, &lda,
(double*)xt.data, &ldx, work, &lwork, &info);
}
}
else if( method == DECOMP_CHOLESKY || (method == DECOMP_LU && is_normal) )
{
if( type == CV_32F )
{
spotrf_(L, &n, (float*)at.data, &lda, &info);
if(info==0)
spotrs_(L, &n, &nb, (float*)at.data, &lda, (float*)xt.data, &ldx, &info);
}
else
{
dpotrf_(L, &n, (double*)at.data, &lda, &info);
if(info==0)
dpotrs_(L, &n, &nb, (double*)at.data, &lda, (double*)xt.data, &ldx, &info);
}
}
else if( method == DECOMP_LU )
{
iwork = (integer*)alignPtr(ptr, sizeof(integer));
if( type == CV_32F )
sgesv_(&n, &nb, (float*)at.data, &lda, iwork, (float*)xt.data, &ldx, &info );
else
dgesv_(&n, &nb, (double*)at.data, &lda, iwork, (double*)xt.data, &ldx, &info );
}
else
assert(0);
result = info == 0;
if( !result )
dst = Scalar(0);
else if( xt.data != dst.data )
transpose( xt, dst );
return result;
}
static void _SVDcompute( const InputArray _aarr, OutputArray _w,
OutputArray _u, OutputArray _vt, int flags = 0)
{
Mat a = _aarr.getMat(), u, vt;
integer m = a.rows, n = a.cols, mn = std::max(m, n), nm = std::min(m, n);
int type = a.type(), elem_size = (int)a.elemSize();
bool compute_uv = _u.needed() || _vt.needed();
if( flags & SVD::NO_UV )
{
_u.release();
_vt.release();
compute_uv = false;
}
if( compute_uv )
{
_u.create( (int)m, (int)((flags & SVD::FULL_UV) ? m : nm), type );
_vt.create( (int)((flags & SVD::FULL_UV) ? n : nm), n, type );
u = _u.getMat();
vt = _vt.getMat();
}
_w.create(nm, 1, type, -1, true);
Mat _a = a, w = _w.getMat();
CV_Assert( w.isContinuous() );
int work_ofs=0, iwork_ofs=0, buf_size = 0;
bool temp_a = false;
double u1=0, v1=0, work1=0;
float uf1=0, vf1=0, workf1=0;
integer lda, ldu, ldv, lwork=-1, iwork1=0, info=0;
char mode[] = {compute_uv ? 'S' : 'N', '\0'};
if( m != n && compute_uv && (flags & SVD::FULL_UV) )
mode[0] = 'A';
if( !(flags & SVD::MODIFY_A) )
{
if( mode[0] == 'N' || mode[0] == 'A' )
temp_a = true;
else if( compute_uv && (a.size() == vt.size() || a.size() == u.size()) && mode[0] == 'S' )
mode[0] = 'O';
}
lda = a.cols;
ldv = ldu = mn;
if( type == CV_32F )
{
sgesdd_(mode, &n, &m, (float*)a.data, &lda, (float*)w.data,
&vf1, &ldv, &uf1, &ldu, &workf1, &lwork, &iwork1, &info );
lwork = cvRound(workf1);
}
else
{
dgesdd_(mode, &n, &m, (double*)a.data, &lda, (double*)w.data,
&v1, &ldv, &u1, &ldu, &work1, &lwork, &iwork1, &info );
lwork = cvRound(work1);
}
assert(info == 0);
if( temp_a )
{
buf_size += n*m*elem_size;
}
work_ofs = buf_size;
buf_size += lwork*elem_size;
buf_size = alignSize(buf_size, sizeof(integer));
iwork_ofs = buf_size;
buf_size += 8*nm*sizeof(integer);
AutoBuffer<uchar> buf(buf_size);
uchar* buffer = (uchar*)buf;
if( temp_a )
{
_a = Mat(a.rows, a.cols, type, buffer );
a.copyTo(_a);
}
if( !(flags & SVD::MODIFY_A) && !temp_a )
{
if( compute_uv && a.size() == vt.size() )
{
a.copyTo(vt);
_a = vt;
}
else if( compute_uv && a.size() == u.size() )
{
a.copyTo(u);
_a = u;
}
}
if( compute_uv )
{
ldv = (int)(vt.step ? vt.step/elem_size : vt.cols);
ldu = (int)(u.step ? u.step/elem_size : u.cols);
}
lda = (int)(_a.step ? _a.step/elem_size : _a.cols);
if( type == CV_32F )
{
sgesdd_(mode, &n, &m, _a.ptr<float>(), &lda, w.ptr<float>(),
vt.data ? vt.ptr<float>() : (float*)&v1, &ldv,
u.data ? u.ptr<float>() : (float*)&u1, &ldu,
(float*)(buffer + work_ofs), &lwork,
(integer*)(buffer + iwork_ofs), &info );
}
else
{
dgesdd_(mode, &n, &m, _a.ptr<double>(), &lda, w.ptr<double>(),
vt.data ? vt.ptr<double>() : &v1, &ldv,
u.data ? u.ptr<double>() : &u1, &ldu,
(double*)(buffer + work_ofs), &lwork,
(integer*)(buffer + iwork_ofs), &info );
}
CV_Assert(info >= 0);
if(info != 0)
{
if( u.data )
u = Scalar(0.);
if( vt.data )
vt = Scalar(0.);
w = Scalar(0.);
}
}
//////////////////////////////////////////////////////////
template<typename T1, typename T2, typename T3> static void
MatrAXPY( int m, int n, const T1* x, int dx,
const T2* a, int inca, T3* y, int dy )
{
int i, j;
for( i = 0; i < m; i++, x += dx, y += dy )
{
T2 s = a[i*inca];
for( j = 0; j <= n - 4; j += 4 )
{
T3 t0 = (T3)(y[j] + s*x[j]);
T3 t1 = (T3)(y[j+1] + s*x[j+1]);
y[j] = t0;
y[j+1] = t1;
t0 = (T3)(y[j+2] + s*x[j+2]);
t1 = (T3)(y[j+3] + s*x[j+3]);
y[j+2] = t0;
y[j+3] = t1;
}
for( ; j < n; j++ )
y[j] = (T3)(y[j] + s*x[j]);
}
}
template<typename T> static void
SVBkSb( int m, int n, const T* w, int incw,
const T* u, int ldu, int uT,
const T* v, int ldv, int vT,
const T* b, int ldb, int nb,
T* x, int ldx, double* buffer, T eps )
{
double threshold = 0;
int udelta0 = uT ? ldu : 1, udelta1 = uT ? 1 : ldu;
int vdelta0 = vT ? ldv : 1, vdelta1 = vT ? 1 : ldv;
int i, j, nm = std::min(m, n);
if( !b )
nb = m;
for( i = 0; i < n; i++ )
for( j = 0; j < nb; j++ )
x[i*ldx + j] = 0;
for( i = 0; i < nm; i++ )
threshold += w[i*incw];
threshold *= eps;
// v * inv(w) * uT * b
for( i = 0; i < nm; i++, u += udelta0, v += vdelta0 )
{
double wi = w[i*incw];
if( wi <= threshold )
continue;
wi = 1/wi;
if( nb == 1 )
{
double s = 0;
if( b )
for( j = 0; j < m; j++ )
s += u[j*udelta1]*b[j*ldb];
else
s = u[0];
s *= wi;
for( j = 0; j < n; j++ )
x[j*ldx] = (T)(x[j*ldx] + s*v[j*vdelta1]);
}
else
{
if( b )
{
for( j = 0; j < nb; j++ )
buffer[j] = 0;
MatrAXPY( m, nb, b, ldb, u, udelta1, buffer, 0 );
for( j = 0; j < nb; j++ )
buffer[j] *= wi;
}
else
{
for( j = 0; j < nb; j++ )
buffer[j] = u[j*udelta1]*wi;
}
MatrAXPY( n, nb, buffer, 0, v, vdelta1, x, ldx );
}
}
}
static void _backSubst( const InputArray _w, const InputArray _u, const InputArray _vt,
const InputArray _rhs, OutputArray _dst )
{
Mat w = _w.getMat(), u = _u.getMat(), vt = _vt.getMat(), rhs = _rhs.getMat();
int type = w.type(), esz = (int)w.elemSize();
int m = u.rows, n = vt.cols, nb = rhs.data ? rhs.cols : m;
AutoBuffer<double> buffer(nb);
CV_Assert( u.data && vt.data && w.data );
CV_Assert( rhs.data == 0 || (rhs.type() == type && rhs.rows == m) );
_dst.create( n, nb, type );
Mat dst = _dst.getMat();
if( type == CV_32F )
SVBkSb(m, n, (float*)w.data, 1, (float*)u.data, (int)(u.step/esz), false,
(float*)vt.data, (int)(vt.step/esz), true, (float*)rhs.data, (int)(rhs.step/esz),
nb, (float*)dst.data, (int)(dst.step/esz), buffer, 10*FLT_EPSILON );
else if( type == CV_64F )
SVBkSb(m, n, (double*)w.data, 1, (double*)u.data, (int)(u.step/esz), false,
(double*)vt.data, (int)(vt.step/esz), true, (double*)rhs.data, (int)(rhs.step/esz),
nb, (double*)dst.data, (int)(dst.step/esz), buffer, 2*DBL_EPSILON );
else
CV_Error( Error::StsUnsupportedFormat, "" );
}
///////////////////////////////////////////
#define Sf( y, x ) ((float*)(srcdata + y*srcstep))[x]
#define Sd( y, x ) ((double*)(srcdata + y*srcstep))[x]
#define Df( y, x ) ((float*)(dstdata + y*dststep))[x]
#define Dd( y, x ) ((double*)(dstdata + y*dststep))[x]
double cvfork::invert( InputArray _src, OutputArray _dst, int method )
{
Mat src = _src.getMat();
int type = src.type();
CV_Assert(type == CV_32F || type == CV_64F);
size_t esz = CV_ELEM_SIZE(type);
int m = src.rows, n = src.cols;
if( method == DECOMP_SVD )
{
int nm = std::min(m, n);
AutoBuffer<uchar> _buf((m*nm + nm + nm*n)*esz + sizeof(double));
uchar* buf = alignPtr((uchar*)_buf, (int)esz);
Mat u(m, nm, type, buf);
Mat w(nm, 1, type, u.ptr() + m*nm*esz);
Mat vt(nm, n, type, w.ptr() + nm*esz);
_SVDcompute(src, w, u, vt);
_backSubst(w, u, vt, Mat(), _dst);
return type == CV_32F ?
(w.ptr<float>()[0] >= FLT_EPSILON ?
w.ptr<float>()[n-1]/w.ptr<float>()[0] : 0) :
(w.ptr<double>()[0] >= DBL_EPSILON ?
w.ptr<double>()[n-1]/w.ptr<double>()[0] : 0);
}
return 0;
}
#endif //USE_LAPACK

13
apps/interactive-calibration/linalg.hpp
Unescape View File

@ -1,13 +0,0 @@
#ifndef LINALG_HPP
#define LINALG_HPP
#include <opencv2/core.hpp>
namespace cvfork {
double invert( cv::InputArray _src, cv::OutputArray _dst, int method );
bool solve(cv::InputArray _src, cv::InputArray _src2arg, cv::OutputArray _dst, int method );
}
#endif

26
apps/interactive-calibration/main.cpp
Unescape View File

@ -12,7 +12,6 @@
#include "calibCommon.hpp" #include "calibCommon.hpp"
#include "calibPipeline.hpp" #include "calibPipeline.hpp"
#include "frameProcessor.hpp" #include "frameProcessor.hpp"
#include "cvCalibrationFork.hpp"
#include "calibController.hpp" #include "calibController.hpp"
#include "parametersController.hpp" #include "parametersController.hpp"
#include "rotationConverters.hpp" #include "rotationConverters.hpp"
@ -106,7 +105,7 @@ int main(int argc, char** argv)
if(!parser.has("v")) globalData->imageSize = capParams.cameraResolution; if(!parser.has("v")) globalData->imageSize = capParams.cameraResolution;
int calibrationFlags = 0; int calibrationFlags = 0;
if(intParams.fastSolving) calibrationFlags |= CALIB_USE_QR; if(intParams.fastSolving) calibrationFlags |= cv::CALIB_USE_QR;
cv::Ptr<calibController> controller(new calibController(globalData, calibrationFlags, cv::Ptr<calibController> controller(new calibController(globalData, calibrationFlags,
parser.get<bool>("ft"), capParams.minFramesNum)); parser.get<bool>("ft"), capParams.minFramesNum));
cv::Ptr<calibDataController> dataController(new calibDataController(globalData, capParams.maxFramesNum, cv::Ptr<calibDataController> dataController(new calibDataController(globalData, capParams.maxFramesNum,
@ -131,11 +130,16 @@ int main(int argc, char** argv)
cv::namedWindow(mainWindowName); cv::namedWindow(mainWindowName);
cv::moveWindow(mainWindowName, 10, 10); cv::moveWindow(mainWindowName, 10, 10);
#ifdef HAVE_QT #ifdef HAVE_QT
cv::createButton("Delete last frame", deleteButton, &dataController, cv::QT_PUSH_BUTTON); cv::createButton("Delete last frame", deleteButton, &dataController,
cv::createButton("Delete all frames", deleteAllButton, &dataController, cv::QT_PUSH_BUTTON); cv::QT_PUSH_BUTTON | cv::QT_NEW_BUTTONBAR);
cv::createButton("Undistort", undistortButton, &showProcessor, cv::QT_CHECKBOX, false); cv::createButton("Delete all frames", deleteAllButton, &dataController,
cv::createButton("Save current parameters", saveCurrentParamsButton, &dataController, cv::QT_PUSH_BUTTON); cv::QT_PUSH_BUTTON | cv::QT_NEW_BUTTONBAR);
cv::createButton("Switch visualisation mode", switchVisualizationModeButton, &showProcessor, cv::QT_PUSH_BUTTON); cv::createButton("Undistort", undistortButton, &showProcessor,
cv::QT_CHECKBOX | cv::QT_NEW_BUTTONBAR, false);
cv::createButton("Save current parameters", saveCurrentParamsButton, &dataController,
cv::QT_PUSH_BUTTON | cv::QT_NEW_BUTTONBAR);
cv::createButton("Switch visualisation mode", switchVisualizationModeButton, &showProcessor,
cv::QT_PUSH_BUTTON | cv::QT_NEW_BUTTONBAR);
#endif //HAVE_QT #endif //HAVE_QT
try { try {
bool pipelineFinished = false; bool pipelineFinished = false;
@ -156,10 +160,10 @@ int main(int argc, char** argv)
if(capParams.board != chAruco) { if(capParams.board != chAruco) {
globalData->totalAvgErr = globalData->totalAvgErr =
cvfork::calibrateCamera(globalData->objectPoints, globalData->imagePoints, cv::calibrateCamera(globalData->objectPoints, globalData->imagePoints,
globalData->imageSize, globalData->cameraMatrix, globalData->imageSize, globalData->cameraMatrix,
globalData->distCoeffs, cv::noArray(), cv::noArray(), globalData->distCoeffs, cv::noArray(), cv::noArray(),
globalData->stdDeviations, globalData->perViewErrors, globalData->stdDeviations, cv::noArray(), globalData->perViewErrors,
calibrationFlags, solverTermCrit); calibrationFlags, solverTermCrit);
} }
else { else {
@ -169,10 +173,10 @@ int main(int argc, char** argv)
cv::aruco::CharucoBoard::create(capParams.boardSize.width, capParams.boardSize.height, cv::aruco::CharucoBoard::create(capParams.boardSize.width, capParams.boardSize.height,
capParams.charucoSquareLenght, capParams.charucoMarkerSize, dictionary); capParams.charucoSquareLenght, capParams.charucoMarkerSize, dictionary);
globalData->totalAvgErr = globalData->totalAvgErr =
cvfork::calibrateCameraCharuco(globalData->allCharucoCorners, globalData->allCharucoIds, cv::aruco::calibrateCameraCharuco(globalData->allCharucoCorners, globalData->allCharucoIds,
charucoboard, globalData->imageSize, charucoboard, globalData->imageSize,
globalData->cameraMatrix, globalData->distCoeffs, globalData->cameraMatrix, globalData->distCoeffs,
cv::noArray(), cv::noArray(), globalData->stdDeviations, cv::noArray(), cv::noArray(), globalData->stdDeviations, cv::noArray(),
globalData->perViewErrors, calibrationFlags, solverTermCrit); globalData->perViewErrors, calibrationFlags, solverTermCrit);
} }
dataController->updateUndistortMap(); dataController->updateUndistortMap();

Write Preview
Loading…
Cancel
Save