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)+s1r2+s2r4ydistorted =y1+k4r2+k5r4+k6rr61+k1r2+k2r4+k3r6+p1(r2+2y2)+2p2xy+s3r2+s4r4.
- 将畸变后的点通过内参矩阵投影到像素平面，得到该点在图像上的正确位置。
{ u = f x x distorted + c x v = f y y distorted + c y . \left\{\begin{array}{l} u=f_x x_{\text {distorted }}+c_x \\ v=f_y y_{\text {distorted }}+c_y \end{array}\right. \text {. } {u=fxxdistorted +cxv=fyydistorted +cy.
其中 [ x distorted , y distorted ] T \left[x_{\text {distorted }}, y_{\text {distorted }}\right]^{\mathrm{T}} [xdistorted ,ydistorted ]T是有畸变点的归一化坐标， [ x , y ] T \left[x_{\text { }}, y_{\text { }}\right]^{\mathrm{T}} [x ,y ]T是无畸变点的归一化坐标。
迭代法原理
opencv对特征点去畸变的原理是迭代法的一种：不动点迭代求解非线性方程的根。
不动点迭代法是将 f ( x ) = 0 f(x)=0 f(x)=0等价转换为 x = φ ( x ) x=\varphi(x) x=φ(x)的形式，进而构造出迭代式的方法，但并不是所有的迭代形式都可以，需要满足迭代收敛的条件 ∣ φ ′ ( x ) ∣ < 1 \left|\varphi^{\prime}(\mathrm{x})\right|<1 ∣φ′(x)∣<1。
将opencv从无畸变点到有畸变点的过程表达为如下：
{
x
′
=
x
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
+
[
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
]
y
′
=
y
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
+
[
p
1
(
r
2
+
2
y
2
)
+
2
p
2
x
y
]
(1)
\left\{\begin{array}{l} \mathrm{x}^{\prime}=\mathrm{x}\left(1+\mathrm{k}_1 \mathrm{r}^2+\mathrm{k}_2 \mathrm{r}^4+\mathrm{k}_3 \mathrm{r}^6\right)+\left[2 \mathrm{p}_1 \mathrm{xy}+\mathrm{p}_2\left(\mathrm{r}^2+2 \mathrm{x}^2\right)\right] \\ \mathrm{y}^{\prime}=\mathrm{y}\left(1+\mathrm{k}_1 \mathrm{r}^2+\mathrm{k}_2 \mathrm{r}^4+\mathrm{k}_3 \mathrm{r}^6\right)+\left[\mathrm{p}_1\left(\mathrm{r}^2+2 \mathrm{y}^2\right)+2 \mathrm{p}_2 \mathrm{xy}\right] \end{array}\right.\tag{1}
{x′=x(1+k1r2+k2r4+k3r6)+[2p1xy+p2(r2+2x2)]y′=y(1+k1r2+k2r4+k3r6)+[p1(r2+2y2)+2p2xy](1)
其中
x
′
,
y
′
{x}^{\prime} , y^{\prime}
x′,y′为有畸变点，
x
,
y
{x} , y
x,y为无畸变点，。
r
2
=
x
2
+
y
2
\mathrm{r}^2=\mathrm{x}^2+\mathrm{y}^2
r2=x2+y2。写成矩阵的形式为：
[
x
′
y
′
]
=
[
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
]
[
x
y
]
+
[
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
p
1
(
r
2
+
2
y
2
)
+
2
p
2
x
y
]
(2)
\left[\begin{array}{l} \mathrm{x}^{\prime} \\ \mathrm{y}^{\prime} \end{array}\right]=\left[1+\mathrm{k}_1 \mathrm{r}^2+\mathrm{k}_2 \mathrm{r}^4+\mathrm{k}_3 \mathrm{r}^6\right]\left[\begin{array}{l} \mathrm{x} \\ \mathrm{y} \end{array}\right]+\left[\begin{array}{l} 2 \mathrm{p}_1 \mathrm{xy}+\mathrm{p}_2\left(\mathrm{r}^2+2 \mathrm{x}^2\right) \\ \mathrm{p}_1\left(\mathrm{r}^2+2 \mathrm{y}^2\right)+2 \mathrm{p}_2 \mathrm{xy} \end{array}\right]\tag{2}
[x′y′]=[1+k1r2+k2r4+k3r6][xy]+[2p1xy+p2(r2+2x2)p1(r2+2y2)+2p2xy](2)
简化形式为：
F
(
X
)
⋅
X
+
G
(
X
)
−
X
′
=
0
(3)
\mathrm{F}(\mathrm{X}) \cdot \mathrm{X}+\mathrm{G}(\mathrm{X})-\mathrm{X}^{\prime}=0\tag{3}
F(X)⋅X+G(X)−X′=0(3)
其中，
X
=
[
x
y
]
,
X
′
=
[
x
′
y
′
]
(4)
\mathrm{X}=\left[\begin{array}{l} \mathrm{x} \\ \mathrm{y} \end{array}\right], \mathrm{X}^{\prime}=\left[\begin{array}{l} \mathrm{x}^{\prime} \\ \mathrm{y}^{\prime} \end{array}\right]\tag{4}
X=[xy],X′=[x′y′](4)
F ( X ) = 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 (5) \mathrm{F}(\mathrm{X})=1+\mathrm{k}_1 \mathrm{r}^2+\mathrm{k}_2 \mathrm{r}^4+\mathrm{k}_3 \mathrm{r}^6\tag{5} F(X)=1+k1r2+k2r4+k3r6(5)
G ( X ) = [ 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y ] (6) \mathrm{G}(\mathrm{X})=\left[\begin{array}{l} 2 \mathrm{p}_1 \mathrm{xy}+\mathrm{p}_2\left(\mathrm{r}^2+2 \mathrm{x}^2\right) \\ \mathrm{p}_1\left(\mathrm{r}^2+2 \mathrm{y}^2\right)+2 \mathrm{p}_2 \mathrm{xy} \end{array}\right]\tag{6} G(X)=[2p1xy+p2(r2+2x2)p1(r2+2y2)+2p2xy](6)
根据式（3）构造不动点等价形式为：
X
=
X
′
−
G
(
X
)
F
(
X
)
(7)
\mathrm{X}=\frac{\mathrm{X}^{\prime}-\mathrm{G}(\mathrm{X})}{\mathrm{F}(\mathrm{X})}\tag{7}
X=F(X)X′−G(X)(7)
构造迭代形式为：
X
k
+
1
=
X
′
−
G
(
X
k
)
F
(
X
k
)
(8)
\mathrm{X}_{\mathrm{k}+1}=\frac{\mathrm{X}^{\prime}-\mathrm{G}\left(\mathrm{X}_{\mathrm{k}}\right)}{\mathrm{F}\left(\mathrm{X}_{\mathrm{k}}\right)}\tag{8}
Xk+1=F(Xk)X′−G(Xk)(8)
在去畸变迭代计算时，将当前已知的有畸变点
X
′
X^{\prime}
X′作为初始值，即
X
0
=
X
′
X_0 = X^{\prime}
X0=X′ ，然后根据式（8）迭代计算无畸变的点，直到满足迭代误差或者最大迭代次数条件。
注意点
注意点1：相机畸变参数不准确，可能是迭代没有收敛。
畸变模型通常是非线性的、非单凸的。
如果不收敛的话，可能的解决方法有：
- 更准确的变量初始值，通过其他测量得到某些变量的粗略值；
- 畸变模型变量太多导致收敛困难，可以先简化模型、或者固定某些参数，只调整部分参数，降低非线性求解的压力。
Reference links:
1.OpenCV迭代去畸变算法undistortPoints()
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
