OpenCV undistortPoints源码解析
本文围绕OpenCV中undistortPoints源码展开，介绍了R、P参数含义及停止条件。阐述计算过程，包括投影到归一化平面、计算畸变、投影到像素平面。讲解迭代法原理，即不动点迭代求解非线性方程根。还提及相机畸变参数不准确可能导致迭代不收敛及解决办法。
undistortPoints源码理解
undistortPoints源代码
CV_EXPORTS_W void undistortPoints( InputArray src, OutputArray dst,
InputArray cameraMatrix, InputArray distCoeffs,
InputArray R = noArray(), InputArray P = noArray());
- R参数用在双目里，单目里置为空矩阵；
- P参数为空，表示结果点是归一化坐标；P参数不为空，表示结果点是像素坐标
void cv::undistortPoints( InputArray _src, OutputArray _dst,
InputArray _cameraMatrix,
InputArray _distCoeffs,
InputArray _Rmat,
InputArray _Pmat )
{
undistortPoints(_src, _dst, _cameraMatrix, _distCoeffs, _Rmat, _Pmat, TermCriteria(TermCriteria::MAX_ITER, 5, 0.01));
}
cv::TermCriteria是 OpenCV 中用于定义停止条件的类，用于控制迭代算法的终止。
-
cv::TermCriteria::MAX_ITER：指定迭代停止的条件为达到最大迭代次数。 -
5：最大迭代次数为 5。 -
0.01：指定精度要求，当迭代算法的变化小于等于 0.01 时，迭代停止。
综合起来，这个停止条件表示当迭代次数达到 5 次或迭代算法的变化小于等于 0.01 时，迭代算法将停止执行。
void cv::undistortPoints( InputArray _src, OutputArray _dst,
InputArray _cameraMatrix,
InputArray _distCoeffs,
InputArray _Rmat,
InputArray _Pmat,
TermCriteria criteria)
{
// 确保以下变量是一个 cv::Mat 对象
cv::Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat();
cv::Mat distCoeffs = _distCoeffs.getMat(), R = _Rmat.getMat(), P = _Pmat.getMat();
//1.检查输入的图像矩阵 src 是否是连续存储的；2.检查输入图像矩阵 src 的深度（数据类型）；3.检查输入图像矩阵 src 的尺寸和通道数
CV_Assert(src.isContinuous() && (src.depth() == CV_32F || src.depth() == CV_64F) &&
((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
// 创建新对象_dst
_dst.create(src.size(), src.type(), -1, true); // 大小和类型与 src 相同,-1表示使用默认的像素值初始化新创建的图像。参数 true 表示新创建的 _dst 是连续存储的。
cv::Mat dst = _dst.getMat(); // 确保 dst 是一个 cv::Mat 对象
// 将变量转换为 CvMat 类型
CvMat _csrc = cvMat(src), _cdst = cvMat(dst), _ccameraMatrix = cvMat(cameraMatrix);
CvMat matR, matP, _cdistCoeffs, *pR = 0, *pP = 0, *pD = 0;
// 检查变量是否为空
if (!R.empty())
pR = &(matR = cvMat(R)); // 如果 R 非空，则将其转换为 CvMat 类型，并将结果赋值给 matR，然后将 pR 指向 matR
if (!P.empty())
pP = &(matP = cvMat(P));
if (!distCoeffs.empty())
pD = &(_cdistCoeffs = cvMat(distCoeffs));
cvUndistortPointsInternal(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP, criteria);
}
static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
const CvMat* _distCoeffs,
const CvMat* matR, const CvMat* matP, cv::TermCriteria criteria)
{
// 判断迭代条件是否有效
CV_Assert(criteria.isValid());
// 定义中间变量 -- A相机内参数组，和matA共享内存；RR-矫正变换数组，和_RR共享内存
// k-畸变系数数组
double A[3][3], RR[3][3], k[14] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0 };
CvMat matA = cvMat(3, 3, CV_64F, A), _Dk;
CvMat _RR = cvMat(3, 3, CV_64F, RR);
cv::Matx33d invMatTilt = cv::Matx33d::eye();
cv::Matx33d matTilt = cv::Matx33d::eye();
// 检查输入变量是否有效
CV_Assert(CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
(_src->rows == 1 || _src->cols == 1) &&
(_dst->rows == 1 || _dst->cols == 1) &&
_src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 &&
(CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
(CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
CV_Assert(CV_IS_MAT(_cameraMatrix) &&
_cameraMatrix->rows == 3 && _cameraMatrix->cols == 3);
// 将 _cameraMatrix 的数据类型转换为与 matA 相同的数据类型
cvConvert(_cameraMatrix, &matA); // _cameraMatrix <--> matA / A
// 判断输入的畸变系数是否有效
if (_distCoeffs)
{
CV_Assert(CV_IS_MAT(_distCoeffs) &&
(_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
(_distCoeffs->rows*_distCoeffs->cols == 4 ||
_distCoeffs->rows*_distCoeffs->cols == 5 ||
_distCoeffs->rows*_distCoeffs->cols == 8 ||
_distCoeffs->rows*_distCoeffs->cols == 12 ||
_distCoeffs->rows*_distCoeffs->cols == 14));
_Dk = cvMat(_distCoeffs->rows, _distCoeffs->cols,
CV_MAKETYPE(CV_64F, CV_MAT_CN(_distCoeffs->type)), k); // _Dk和数组k共享内存指针
cvConvert(_distCoeffs, &_Dk);
if (k[12] != 0 || k[13] != 0)
{
computeTiltProjectionMatrix<double>(k[12], k[13], NULL, NULL, NULL, &invMatTilt);
computeTiltProjectionMatrix<double>(k[12], k[13], &matTilt, NULL, NULL);
}
}
// 判断matR是否为空
if (matR)
{
// 如果非空，检查matR的类型和维度，并将 matR 转换为 _RR
CV_Assert(CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3);
cvConvert(matR, &_RR); // matR和_RR共享内存指针
}
else // 如果为空，使用将 _RR 设置为单位矩阵
cvSetIdentity(&_RR);
if (matP)
{
double PP[3][3];
CvMat _P3x3, _PP = cvMat(3, 3, CV_64F, PP);
CV_Assert(CV_IS_MAT(matP) && matP->rows == 3 && (matP->cols == 3 || matP->cols == 4));
cvConvert(cvGetCols(matP, &_P3x3, 0, 3), &_PP); // _PP和数组PP共享内存指针
cvMatMul(&_PP, &_RR, &_RR); // _RR=_PP*_RR 放在一起计算比较高效
}
// 指针和变量的初始化
const CvPoint2D32f* srcf = (const CvPoint2D32f*)_src->data.ptr;
const CvPoint2D64f* srcd = (const CvPoint2D64f*)_src->data.ptr;
CvPoint2D32f* dstf = (CvPoint2D32f*)_dst->data.ptr;
CvPoint2D64f* dstd = (CvPoint2D64f*)_dst->data.ptr;
int stype = CV_MAT_TYPE(_src->type);
int dtype = CV_MAT_TYPE(_dst->type);
int sstep = _src->rows == 1 ? 1 : _src->step / CV_ELEM_SIZE(stype);
int dstep = _dst->rows == 1 ? 1 : _dst->step / CV_ELEM_SIZE(dtype);
double fx = A[0][0];
double fy = A[1][1];
double ifx = 1. / fx;
double ify = 1. / fy;
double cx = A[0][2];
double cy = A[1][2];
int n = _src->rows + _src->cols - 1;
// 开始对所有点遍历
for (int i = 0; i < n; i++)
{
double x, y, x0 = 0, y0 = 0, u, v;
if (stype == CV_32FC2) // 从源矩阵中提取坐标值 x 和 y
{
x = srcf[i*sstep].x;
y = srcf[i*sstep].y;
}
else
{
x = srcd[i*sstep].x;
y = srcd[i*sstep].y;
}
u = x; v = y;
x = (x - cx)*ifx; // 转换到归一化图像坐标系（含有畸变）
y = (y - cy)*ify;
//进行畸变矫正
if (_distCoeffs) {
// compensate tilt distortion -- 该部分系数用来弥补沙姆镜头畸变
cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1);
double invProj = vecUntilt(2) ? 1. / vecUntilt(2) : 1;
x0 = x = invProj * vecUntilt(0);
y0 = y = invProj * vecUntilt(1);
double error = std::numeric_limits<double>::max(); // error设定为系统最大值
// compensate distortion iteratively
// 迭代去除镜头畸变
// 迭代公式 x′= (x−2p1 xy−p2 (r^2 + 2x^2))∕( 1 + k1*r^2 + k2*r^4 + k3*r^6)
// y′= (y−2p2 xy−p1 (r^2 + 2y^2))∕( 1 + k1*r^2 + k2*r^4 + k3*r^6)
for (int j = 0; ; j++)
{
if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
break;
if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)
break;
double r2 = x * x + y * y;
// k=(k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]])
// icdist = 1.0∕( 1 + k1*r^2 + k2*r^4 + k3*r^6)
double icdist = (1 + ((k[7] * r2 + k[6])*r2 + k[5])*r2) / (1 + ((k[4] * r2 + k[1])*r2 + k[0])*r2);
// deltaX = 2*p1*xy + p2*(r^2+2*x*x)
double deltaX = 2 * k[2] * x*y + k[3] * (r2 + 2 * x*x) + k[8] * r2 + k[9] * r2*r2;
// deltaY = p1*(r^2+2*y*y) + 2*p2*x*y
double deltaY = k[2] * (r2 + 2 * y*y) + 2 * k[3] * x*y + k[10] * r2 + k[11] * r2*r2;
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
// 对当前迭代的坐标加畸变，计算误差error用于判断迭代条件
if (criteria.type & cv::TermCriteria::EPS)
{
double r4, r6, a1, a2, a3, cdist, icdist2;
double xd, yd, xd0, yd0;
cv::Vec3d vecTilt;
r2 = x * x + y * y;
r4 = r2 * r2;
r6 = r4 * r2;
a1 = 2 * x*y;
a2 = r2 + 2 * x*x;
a3 = r2 + 2 * y*y;
// k=[k1,k2,p1,p2,k3]
// cdist = 1 + k1*r^2 + k2*r^4 +k3*r^6
cdist = 1 + k[0] * r2 + k[1] * r4 + k[4] * r6;
// icdist2 = 1.
icdist2 = 1. / (1 + k[5] * r2 + k[6] * r4 + k[7] * r6);
// k[2]*a1=2*p1*x*y, k[3]*a2=p2*(r^2+2*x*x)
xd0 = x * cdist*icdist2 + k[2] * a1 + k[3] * a2 + k[8] * r2 + k[9] * r4;
// k[2]*a3=p1*(r^2+2*y*y), k[3]*a1=2*p2*x*y
yd0 = y * cdist*icdist2 + k[2] * a3 + k[3] * a1 + k[10] * r2 + k[11] * r4;
vecTilt = matTilt * cv::Vec3d(xd0, yd0, 1);
invProj = vecTilt(2) ? 1. / vecTilt(2) : 1;
xd = invProj * vecTilt(0);
yd = invProj * vecTilt(1);
double x_proj = xd * fx + cx;
double y_proj = yd * fy + cy;
error = sqrt(pow(x_proj - u, 2) + pow(y_proj - v, 2));
}
}
}
// 将坐标从归一化图像坐标系转换到成像平面坐标系
// RR=[[1 0 0 ], [0 1 0], [0 0 1]]
double xx = RR[0][0] * x + RR[0][1] * y + RR[0][2]; // x
double yy = RR[1][0] * x + RR[1][1] * y + RR[1][2]; // y
double ww = 1. / (RR[2][0] * x + RR[2][1] * y + RR[2][2]); // 1.
x = xx * ww;
y = yy * ww;
if (dtype == CV_32FC2)
{
dstf[i*dstep].x = (float)x;
dstf[i*dstep].y = (float)y;
}
else
{
dstd[i*dstep].x = x;
dstd[i*dstep].y = y;
}
}
}
1.世界坐标系，P点所在坐标系
2.相机坐标系[x, y, z]
3.归一化坐标系[x’, y’]
4.像素坐标系（图像坐标系）[u, v]
计算过程
- 将图像点投影到归一化图像平面。设它的归一化坐标为 [ x , y ] T [x, y]^{\mathrm{T}} [x,y]T。
- 对归一化平面上的点计算径向畸变、切向畸变、薄棱镜畸变。
{ x distorted = x 1 + k 1 r 2 + k 2 r 4 + k 3 r r 6 1 + k 4 r 2 + k 5 r 4 + k 6 r r 6 + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) + s 1 r 2 + s 2 r 4 y distorted = y 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 1 + k 4 r 2 + k 5 r 4 + k 6 r r 6 + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y + s 3 r 2 + s 4 r 4 . \left\{\begin{array}{l} x_{\text {distorted }}=x \frac{1+k 1 r^2+k 2 r^4+k_{3 r} r^6}{1+k_4 r^2+k 5 r^4+k_{6 r} r^6}+2 p_1 x y+p_2\left(r^2+2 x^{ 2}\right)+s_1 r^2+s_2 r^4 \\ y_{\text {distorted }}=y \frac{1+k 1 r^2+k 2 r^4+k 3 r^6}{1+k_4 r^2+k_5 r^4+k_{6 r} r^6}+p_1\left(r^2+2 y^{ 2}\right)+2 p_2 x y+s_3 r^2+s_4 r^4 \end{array} .\right. { xdistorted =x1+k4r2+k5r4+k6rr61+k1r2+k2r4+k3rr6+2p1xy+p2(r2+2x2)+s1r
最低0.47元/天 解锁文章
扫一扫
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
