mirror of
https://github.com/opencv/opencv.git
synced 2026-07-30 15:53:03 +04:00
Extension of the camera distortion model for tilted image sensors (Scheimpflug condition) including test
This commit is contained in:
Thomas Dunker
@@ -42,6 +42,7 @@
|
||||
|
||||
#include "precomp.hpp"
|
||||
#include "opencv2/imgproc/imgproc_c.h"
|
||||
#include "opencv2/imgproc/detail/distortion_model.hpp"
|
||||
#include "opencv2/calib3d/calib3d_c.h"
|
||||
#include <stdio.h>
|
||||
#include <iterator>
|
||||
@@ -523,7 +524,7 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
|
||||
}
|
||||
|
||||
|
||||
static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12 or 12x1 floating-point vector";
|
||||
static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
|
||||
|
||||
CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
|
||||
const CvMat* r_vec,
|
||||
@@ -542,7 +543,10 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
|
||||
int calc_derivatives;
|
||||
const CvPoint3D64f* M;
|
||||
CvPoint2D64f* m;
|
||||
double r[3], R[9], dRdr[27], t[3], a[9], k[12] = {0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
|
||||
double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
|
||||
Matx33d matTilt = Matx33d::eye();
|
||||
Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
|
||||
Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
|
||||
CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
|
||||
CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
|
||||
double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
|
||||
@@ -646,12 +650,18 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
|
||||
(distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
|
||||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
|
||||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
|
||||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12) )
|
||||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
|
||||
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
|
||||
CV_Error( CV_StsBadArg, cvDistCoeffErr );
|
||||
|
||||
_k = cvMat( distCoeffs->rows, distCoeffs->cols,
|
||||
CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
|
||||
cvConvert( distCoeffs, &_k );
|
||||
if(k[12] != 0 || k[13] != 0)
|
||||
{
|
||||
detail::computeTiltProjectionMatrix(k[12], k[13],
|
||||
&matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
|
||||
}
|
||||
}
|
||||
|
||||
if( dpdr )
|
||||
@@ -728,8 +738,8 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
|
||||
{
|
||||
if( !CV_IS_MAT(dpdk) ||
|
||||
(CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
|
||||
dpdk->rows != count*2 || (dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
|
||||
CV_Error( CV_StsBadArg, "dp/df must be 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
|
||||
dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
|
||||
CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
|
||||
|
||||
if( !distCoeffs )
|
||||
CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
|
||||
@@ -753,7 +763,11 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
|
||||
double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
|
||||
double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
|
||||
double r2, r4, r6, a1, a2, a3, cdist, icdist2;
|
||||
double xd, yd;
|
||||
double xd, yd, xd0, yd0, invProj;
|
||||
Vec3d vecTilt;
|
||||
Vec3d dVecTilt;
|
||||
Matx22d dMatTilt;
|
||||
Vec2d dXdYd;
|
||||
|
||||
z = z ? 1./z : 1;
|
||||
x *= z; y *= z;
|
||||
@@ -766,8 +780,14 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
|
||||
a3 = r2 + 2*y*y;
|
||||
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
|
||||
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
|
||||
xd = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
|
||||
yd = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
|
||||
xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
|
||||
yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
|
||||
|
||||
// additional distortion by projecting onto a tilt plane
|
||||
vecTilt = matTilt*Vec3d(xd0, yd0, 1);
|
||||
invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
|
||||
xd = invProj * vecTilt(0);
|
||||
yd = invProj * vecTilt(1);
|
||||
|
||||
m[i].x = xd*fx + cx;
|
||||
m[i].y = yd*fy + cy;
|
||||
@@ -798,42 +818,75 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
|
||||
}
|
||||
dpdf_p += dpdf_step*2;
|
||||
}
|
||||
|
||||
for (int row = 0; row < 2; ++row)
|
||||
for (int col = 0; col < 2; ++col)
|
||||
dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
|
||||
- matTilt(2,col)*vecTilt(row);
|
||||
double invProjSquare = (invProj*invProj);
|
||||
dMatTilt *= invProjSquare;
|
||||
if( dpdk_p )
|
||||
{
|
||||
dpdk_p[0] = fx*x*icdist2*r2;
|
||||
dpdk_p[1] = fx*x*icdist2*r4;
|
||||
dpdk_p[dpdk_step] = fy*y*icdist2*r2;
|
||||
dpdk_p[dpdk_step+1] = fy*y*icdist2*r4;
|
||||
dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
|
||||
dpdk_p[0] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step] = fy*dXdYd(1);
|
||||
dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
|
||||
dpdk_p[1] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+1] = fy*dXdYd(1);
|
||||
if( _dpdk->cols > 2 )
|
||||
{
|
||||
dpdk_p[2] = fx*a1;
|
||||
dpdk_p[3] = fx*a2;
|
||||
dpdk_p[dpdk_step+2] = fy*a3;
|
||||
dpdk_p[dpdk_step+3] = fy*a1;
|
||||
dXdYd = dMatTilt*Vec2d(a1, a3);
|
||||
dpdk_p[2] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+2] = fy*dXdYd(1);
|
||||
dXdYd = dMatTilt*Vec2d(a2, a1);
|
||||
dpdk_p[3] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+3] = fy*dXdYd(1);
|
||||
if( _dpdk->cols > 4 )
|
||||
{
|
||||
dpdk_p[4] = fx*x*icdist2*r6;
|
||||
dpdk_p[dpdk_step+4] = fy*y*icdist2*r6;
|
||||
dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
|
||||
dpdk_p[4] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+4] = fy*dXdYd(1);
|
||||
|
||||
if( _dpdk->cols > 5 )
|
||||
{
|
||||
dpdk_p[5] = fx*x*cdist*(-icdist2)*icdist2*r2;
|
||||
dpdk_p[dpdk_step+5] = fy*y*cdist*(-icdist2)*icdist2*r2;
|
||||
dpdk_p[6] = fx*x*cdist*(-icdist2)*icdist2*r4;
|
||||
dpdk_p[dpdk_step+6] = fy*y*cdist*(-icdist2)*icdist2*r4;
|
||||
dpdk_p[7] = fx*x*cdist*(-icdist2)*icdist2*r6;
|
||||
dpdk_p[dpdk_step+7] = fy*y*cdist*(-icdist2)*icdist2*r6;
|
||||
dXdYd = dMatTilt*Vec2d(
|
||||
x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
|
||||
dpdk_p[5] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+5] = fy*dXdYd(1);
|
||||
dXdYd = dMatTilt*Vec2d(
|
||||
x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
|
||||
dpdk_p[6] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+6] = fy*dXdYd(1);
|
||||
dXdYd = dMatTilt*Vec2d(
|
||||
x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
|
||||
dpdk_p[7] = fx*dXdYd(0);
|
||||
dpdk_p[dpdk_step+7] = fy*dXdYd(1);
|
||||
if( _dpdk->cols > 8 )
|
||||
{
|
||||
dpdk_p[8] = fx*r2; //s1
|
||||
dpdk_p[9] = fx*r4; //s2
|
||||
dpdk_p[10] = 0;//s3
|
||||
dpdk_p[11] = 0;//s4
|
||||
dpdk_p[dpdk_step+8] = 0; //s1
|
||||
dpdk_p[dpdk_step+9] = 0; //s2
|
||||
dpdk_p[dpdk_step+10] = fy*r2; //s3
|
||||
dpdk_p[dpdk_step+11] = fy*r4; //s4
|
||||
dXdYd = dMatTilt*Vec2d(r2, 0);
|
||||
dpdk_p[8] = fx*dXdYd(0); //s1
|
||||
dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
|
||||
dXdYd = dMatTilt*Vec2d(r4, 0);
|
||||
dpdk_p[9] = fx*dXdYd(0); //s2
|
||||
dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
|
||||
dXdYd = dMatTilt*Vec2d(0, r2);
|
||||
dpdk_p[10] = fx*dXdYd(0);//s3
|
||||
dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
|
||||
dXdYd = dMatTilt*Vec2d(0, r4);
|
||||
dpdk_p[11] = fx*dXdYd(0);//s4
|
||||
dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
|
||||
if( _dpdk->cols > 12 )
|
||||
{
|
||||
dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
|
||||
dpdk_p[12] = fx * invProjSquare * (
|
||||
dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
|
||||
dpdk_p[dpdk_step+12] = fy*invProjSquare * (
|
||||
dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
|
||||
dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
|
||||
dpdk_p[13] = fx * invProjSquare * (
|
||||
dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
|
||||
dpdk_p[dpdk_step+13] = fy * invProjSquare * (
|
||||
dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -850,12 +903,13 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
|
||||
double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
|
||||
double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
|
||||
double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
|
||||
double dmxdt = fx*(dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
|
||||
double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
|
||||
k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
|
||||
double dmydt = fy*(dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
|
||||
double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
|
||||
k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
|
||||
dpdt_p[j] = dmxdt;
|
||||
dpdt_p[dpdt_step+j] = dmydt;
|
||||
dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
|
||||
dpdt_p[j] = fx*dXdYd(0);
|
||||
dpdt_p[dpdt_step+j] = fy*dXdYd(1);
|
||||
}
|
||||
dpdt_p += dpdt_step*2;
|
||||
}
|
||||
@@ -885,15 +939,16 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
|
||||
double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
|
||||
double dydr = z*(dy0dr[j] - y*dz0dr[j]);
|
||||
double dr2dr = 2*x*dxdr + 2*y*dydr;
|
||||
double dcdist_dr = k[0]*dr2dr + 2*k[1]*r2*dr2dr + 3*k[4]*r4*dr2dr;
|
||||
double dicdist2_dr = -icdist2*icdist2*(k[5]*dr2dr + 2*k[6]*r2*dr2dr + 3*k[7]*r4*dr2dr);
|
||||
double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
|
||||
double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
|
||||
double da1dr = 2*(x*dydr + y*dxdr);
|
||||
double dmxdr = fx*(dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
|
||||
k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr) + k[8]*dr2dr + 2*r2*k[9]*dr2dr);
|
||||
double dmydr = fy*(dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
|
||||
k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr + k[10]*dr2dr + 2*r2*k[11]*dr2dr);
|
||||
dpdr_p[j] = dmxdr;
|
||||
dpdr_p[dpdr_step+j] = dmydr;
|
||||
double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
|
||||
k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
|
||||
double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
|
||||
k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
|
||||
dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
|
||||
dpdr_p[j] = fx*dXdYd(0);
|
||||
dpdr_p[dpdr_step+j] = fy*dXdYd(1);
|
||||
}
|
||||
dpdr_p += dpdr_step*2;
|
||||
}
|
||||
@@ -1231,11 +1286,11 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
|
||||
CvSize imageSize, CvMat* cameraMatrix, CvMat* distCoeffs,
|
||||
CvMat* rvecs, CvMat* tvecs, int flags, CvTermCriteria termCrit )
|
||||
{
|
||||
const int NINTRINSIC = 16;
|
||||
const int NINTRINSIC = 18;
|
||||
double reprojErr = 0;
|
||||
|
||||
Matx33d A;
|
||||
double k[12] = {0};
|
||||
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.;
|
||||
@@ -1252,9 +1307,19 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
|
||||
(npoints->rows != 1 && npoints->cols != 1) )
|
||||
CV_Error( CV_StsUnsupportedFormat,
|
||||
"the array of point counters must be 1-dimensional integer vector" );
|
||||
//when the thin prism model is used the distortion coefficients matrix must have 12 parameters
|
||||
if((flags & CALIB_THIN_PRISM_MODEL) && (distCoeffs->cols*distCoeffs->rows != 12))
|
||||
CV_Error( CV_StsBadArg, "Thin prism model must have 12 parameters in the distortion matrix" );
|
||||
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);
|
||||
@@ -1293,7 +1358,8 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
|
||||
(distCoeffs->cols*distCoeffs->rows != 4 &&
|
||||
distCoeffs->cols*distCoeffs->rows != 5 &&
|
||||
distCoeffs->cols*distCoeffs->rows != 8 &&
|
||||
distCoeffs->cols*distCoeffs->rows != 12) )
|
||||
distCoeffs->cols*distCoeffs->rows != 12 &&
|
||||
distCoeffs->cols*distCoeffs->rows != 14) )
|
||||
CV_Error( CV_StsBadArg, cvDistCoeffErr );
|
||||
|
||||
for( i = 0; i < nimages; i++ )
|
||||
@@ -1398,7 +1464,7 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
|
||||
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 + 12, param + 4);
|
||||
std::copy(k, k + 14, param + 4);
|
||||
|
||||
if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
|
||||
mask[0] = mask[1] = 0;
|
||||
@@ -1413,6 +1479,8 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
|
||||
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);
|
||||
@@ -1428,6 +1496,11 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
|
||||
mask[14] = 0;
|
||||
mask[15] = 0;
|
||||
}
|
||||
if(flags & CALIB_FIX_TAUX_TAUY)
|
||||
{
|
||||
mask[16] = 0;
|
||||
mask[17] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// 2. initialize extrinsic parameters
|
||||
@@ -1461,7 +1534,7 @@ CV_IMPL double cvCalibrateCamera2( const CvMat* objectPoints,
|
||||
}
|
||||
|
||||
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 + 12, k);
|
||||
std::copy(param + 4, param + 4 + 14, k);
|
||||
|
||||
if( !proceed )
|
||||
break;
|
||||
@@ -1636,11 +1709,11 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
int flags,
|
||||
CvTermCriteria termCrit )
|
||||
{
|
||||
const int NINTRINSIC = 16;
|
||||
const int NINTRINSIC = 18;
|
||||
Ptr<CvMat> npoints, err, J_LR, Je, Ji, imagePoints[2], objectPoints, RT0;
|
||||
double reprojErr = 0;
|
||||
|
||||
double A[2][9], dk[2][12]={{0,0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0,0}}, rlr[9];
|
||||
double A[2][9], dk[2][14]={{0,0,0,0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0,0,0,0}}, rlr[9];
|
||||
CvMat K[2], Dist[2], om_LR, T_LR;
|
||||
CvMat R_LR = cvMat(3, 3, CV_64F, rlr);
|
||||
int i, k, p, ni = 0, ofs, nimages, pointsTotal, maxPoints = 0;
|
||||
@@ -1686,7 +1759,7 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
(_imagePoints1->rows == 1 && _imagePoints1->cols == pointsTotal && cn == 2)) );
|
||||
|
||||
K[k] = cvMat(3,3,CV_64F,A[k]);
|
||||
Dist[k] = cvMat(1,12,CV_64F,dk[k]);
|
||||
Dist[k] = cvMat(1,14,CV_64F,dk[k]);
|
||||
|
||||
imagePoints[k].reset(cvCreateMat( points->rows, points->cols, CV_64FC(CV_MAT_CN(points->type))));
|
||||
cvConvert( points, imagePoints[k] );
|
||||
@@ -1752,6 +1825,8 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
flags |= CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6;
|
||||
if( !(flags & CV_CALIB_THIN_PRISM_MODEL) )
|
||||
flags |= CV_CALIB_FIX_S1_S2_S3_S4;
|
||||
if( !(flags & CV_CALIB_TILTED_MODEL) )
|
||||
flags |= CV_CALIB_FIX_TAUX_TAUY;
|
||||
if( flags & CV_CALIB_FIX_ASPECT_RATIO )
|
||||
imask[0] = imask[NINTRINSIC] = 0;
|
||||
if( flags & CV_CALIB_FIX_FOCAL_LENGTH )
|
||||
@@ -1779,6 +1854,11 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
imask[14] = imask[NINTRINSIC+14] = 0;
|
||||
imask[15] = imask[NINTRINSIC+15] = 0;
|
||||
}
|
||||
if( flags & CV_CALIB_FIX_TAUX_TAUY )
|
||||
{
|
||||
imask[16] = imask[NINTRINSIC+16] = 0;
|
||||
imask[17] = imask[NINTRINSIC+17] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -1857,6 +1937,8 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
iparam[13] = dk[k][9];
|
||||
iparam[14] = dk[k][10];
|
||||
iparam[15] = dk[k][11];
|
||||
iparam[16] = dk[k][12];
|
||||
iparam[17] = dk[k][13];
|
||||
}
|
||||
|
||||
om_LR = cvMat(3, 1, CV_64F, solver.param->data.db);
|
||||
@@ -1927,6 +2009,8 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
dk[k][9] = iparam[k*NINTRINSIC+13];
|
||||
dk[k][10] = iparam[k*NINTRINSIC+14];
|
||||
dk[k][11] = iparam[k*NINTRINSIC+15];
|
||||
dk[k][12] = iparam[k*NINTRINSIC+16];
|
||||
dk[k][13] = iparam[k*NINTRINSIC+17];
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3026,15 +3110,17 @@ static Mat prepareCameraMatrix(Mat& cameraMatrix0, int rtype)
|
||||
|
||||
static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype)
|
||||
{
|
||||
Mat distCoeffs = Mat::zeros(distCoeffs0.cols == 1 ? Size(1, 12) : Size(12, 1), 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(12, 1) ||
|
||||
distCoeffs0.size() == Size(14, 1) )
|
||||
{
|
||||
Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows));
|
||||
distCoeffs0.convertTo(dstCoeffs, rtype);
|
||||
@@ -3219,7 +3305,9 @@ double cv::calibrateCamera( InputArrayOfArrays _objectPoints,
|
||||
cameraMatrix = prepareCameraMatrix(cameraMatrix, rtype);
|
||||
Mat distCoeffs = _distCoeffs.getMat();
|
||||
distCoeffs = prepareDistCoeffs(distCoeffs, rtype);
|
||||
if( !(flags & CALIB_RATIONAL_MODEL) &&(!(flags & CALIB_THIN_PRISM_MODEL)))
|
||||
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());
|
||||
@@ -3314,7 +3402,9 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
|
||||
distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype);
|
||||
distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype);
|
||||
|
||||
if( !(flags & CALIB_RATIONAL_MODEL) &&(!(flags & CALIB_THIN_PRISM_MODEL)))
|
||||
if( !(flags & CALIB_RATIONAL_MODEL) &&
|
||||
(!(flags & CALIB_THIN_PRISM_MODEL)) &&
|
||||
(!(flags & CALIB_TILTED_MODEL)))
|
||||
{
|
||||
distCoeffs1 = distCoeffs1.rows == 1 ? distCoeffs1.colRange(0, 5) : distCoeffs1.rowRange(0, 5);
|
||||
distCoeffs2 = distCoeffs2.rows == 1 ? distCoeffs2.colRange(0, 5) : distCoeffs2.rowRange(0, 5);
|
||||
|
||||
Reference in New Issue
Block a user