OpenCV去畸变undistortPoints原理解析

镜头畸变模型

参考opencv文档 https://docs.opencv.org/3.1.0/d4/d94/tutorial_camera_calibration.html，opencv在相机标定时的采用的是带有三个参数的径向畸变模型和两个参数的切向畸变模型，如下：

去畸变undistortPoints

opencv对特征点去畸变的原理是迭代法的一种：不动点迭代求解非线性方程的根，当然也可以用高斯牛顿法去畸变(参见另一篇博文)。不动点迭代法是将$f(x)=0$等价转换为$x=\psi (x)$的形式，进而构造出迭代式的方法，但并不是所有的迭代形式都可以，需要满足迭代收敛的条件$|{\psi }^{\prime }(x)|<1$。

将opencv从无畸变点到有畸变点的过程表达为如下
$$\{\begin{array}{c}{x}^{\prime }=x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+[2p_{1}xy+p_2(r^2+2x^2)]\\ y^{\prime }=y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+[p_1(r^2+2y^2)+2p_{2}xy]\end{array}\text{\hspace{1em}(1)}$$
其中$x^{\prime },y^{\prime }$为有畸变点，$x,y$为无畸变点，$r^2=x^2+y^2$。写成矩阵的形式为
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right]=[1+k_1{r}^2+k_2{r}^4+k_3{r}^6]\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{c}2p_{1}xy+p_2(r^2+2x^2)\\ p_1(r^2+2y^2)+2p_{2}xy\end{array}\right]\text{\hspace{1em}(2)}$$
简化形式为
$$F(X)\cdot X+G(X)-X^{\prime }=0\text{\hspace{1em}(3)}$$
其中，$$X=\left[\begin{array}{c}x\\ y\end{array}\right],X^{\prime }=\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right]\text{\hspace{1em}(4)}$$,$$F(X)=1+k_1{r}^2+k_2{r}^4+k_3{r}^6\text{\hspace{1em}(5)}$$,
$$G(X)=\left[\begin{array}{c}2p_{1}xy+p_2(r^2+2x^2)\\ p_1(r^2+2y^2)+2p_{2}xy\end{array}\right]\text{\hspace{1em}(6)}$$
根据式（3）构造不动点等价形式为
$$X=\frac{X^{\prime }-G(X)}{F(X)}\text{\hspace{1em}(7)}$$
构造迭代形式为
$$X_{k+1}=\frac{X^{\prime }-G(X_k)}{F(X_k)}\text{\hspace{1em}(8)}$$
该迭代式子是否满足收敛条件我还没有进行验证，在去畸变迭代计算时，将当前已知的有畸变点$X’$作为初始值，即$X_0=X^{\prime }$，然后根据式（8）迭代计算无畸变的点，直到满足迭代误差或者最大迭代次数条件。

附上opencv中相关的一段代码，标注上主要的注释

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 = 1.7976931348623158e+308;
	// compensate distortion iteratively

	//注1： 开始循环迭代去除畸变
	for (int j = 0;; j++)
	{
		//注2：一个是最大迭代次数条件
		if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
			break;
		//注3：一个是迭代最大误差条件
		if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)
			break;
			
		//注4：在只有k1,k2,k3,p1,p2共5个畸变参数时，对应的k数组中只有k[0]~k[4]有值，其他都为0
		double r2 = x*x + y*y;
		double icdist = (1 + ((k[7] * r2 + k[6])*r2 + k[5])*r2) / (1 + ((k[4] * r2 + k[1])*r2 + k[0])*r2);
		double deltaX = 2 * k[2] * x*y + k[3] * (r2 + 2 * x*x) + k[8] * r2 + k[9] * r2*r2;
		double deltaY = k[2] * (r2 + 2 * y*y) + 2 * k[3] * x*y + k[10] * r2 + k[11] * r2*r2;
		//注5：形如式（8）的迭代
		x = (x0 - deltaX)*icdist;
		y = (y0 - deltaY)*icdist;

		if (criteria.type & cv::TermCriteria::EPS)
		{
			double r4, r6, a1, a2, a3, cdist, icdist2;
			double xd, yd, xd0, yd0;
			cv::Vec3d vecTilt;

			//注6：将第k次计算的无畸变点代入畸变模型，得到于当前有畸变的偏差
			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;
			cdist = 1 + k[0] * r2 + k[1] * r4 + k[4] * r6;
			icdist2 = 1. / (1 + k[5] * r2 + k[6] * r4 + k[7] * r6);
			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;

			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));
		}
	}
}