畸变模型的求解

畸变模型求解

畸变模型

在上一篇博客，我提到了我们用的畸变模型大多数是布朗模型的一种近似（当然还存在很多畸变模型，如鱼眼模型、根据镜头特点提出的模型等等）。

同时，在上一篇博客也指出了以下两种模型其实是等价的：

x u n d i s t = f ( x d i s t , y d i s t ) (1) x_{undist} = f(x_{dist},y_{dist}) \tag{1} xundist​=f(xdist​,ydist​)(1)

x d i s t = g ( x u n d i s t , y u n d i s t ) (2) x_{dist} = g(x_{undist},y_{undist}) \tag{2} xdist​=g(xundist​,yundist​)(2)

对于opencv，其采用的是第二种，原因是利用反向映射进行去畸变，一方面避免求解上述二元高次方程，另一方面可以避免第一种模型去畸变后产生“奶酪图”。

那么，如果我们要想求解第二种模型的方程，应该怎么做呢？

在opencv中给出了一种迭代法求解的方法，且收敛速度非常快。答案就在函数

undistortPoints()

中。

undistortPoints()

对于一些场景，我们并不需要对整幅图进行去畸变。比如对于光流法中我们跟踪的一些点（a sparse set of points），我们只需要对这些点进行畸变矫正即可，相比全图去畸变，节约算力。

同时，该函数还是

projectPoints

的逆变换。因为投影时，3D点经过刚体变换后的坐标就是理想的坐标，经过透镜后的坐标才是畸变后的坐标，进而通过乘以内参矩阵，得到像素点坐标。

对于给定的畸变点 ( u , v ) (u,v) (u,v)，计算其去畸变后的坐标点 ( x ′ ′ , y ′ ′ ) (x^{''},y^{''}) (x′′,y′′)，主要步骤如下：

1、根据相机内参，将坐标转化到相机坐标系下，即

2、迭代求解畸变模型，得到去畸变后的坐标，即

3、再根据相机内参，将坐标系转化为像素坐标系，得到去畸变后的像素坐标，即

之所以要经过上述一圈，是因为我们在相机坐标系下，利用透镜前（无畸变）和透镜后（畸变）对应的坐标点进行优化得到畸变系数。

迭代法求解非线性方程组

对于相机坐标系下的畸变坐标 ( x d i s t , y d i s t ) (x_{dist},y_{dist}) (xdist​,ydist​)，我们有如下畸变方程：

x d i s t = x ∗ 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 6 + p 1 ∗ ( 2 ∗ x ∗ y ) + p 2 ∗ ( r 2 + 2 ∗ x 2 ) + s 1 ∗ r 2 + s 2 ∗ r 4 (3) x_{dist} = x * \frac{1 + k1 * r^2 + k2 * r^4 + k3 * r^6}{1 + k4* r^2 + k5 * r^4 + k6 * r^6} + p1 * (2*x*y)+p2*(r^2+2*x^2)+s1*r^2+s2*r^4 \tag{3} xdist​=x∗1+k4∗r2+k5∗r4+k6∗r61+k1∗r2+k2∗r4+k3∗r6​+p1∗(2∗x∗y)+p2∗(r2+2∗x2)+s1∗r2+s2∗r4(3)

简化一下上式，令：

A = 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 6 B = p 1 ∗ ( 2 ∗ x ∗ y ) + p 2 ∗ ( r 2 + 2 ∗ x 2 ) + s 1 ∗ r 2 + s 2 ∗ r 4 C = p 1 ∗ ( r 2 + 2 ∗ y 2 ) + p 2 ∗ ( 2 ∗ x ∗ y ) + s 3 ∗ r 2 + s 4 ∗ r 4 (4) A = \frac{1 + k1 * r^2 + k2 * r^4 + k3 * r^6}{1 + k4* r^2 + k5 * r^4 + k6 * r^6} \\ B = p1 * (2*x*y)+p2*(r^2+2*x^2)+s1*r^2+s2*r^4 \\ C = p1 * (r^2+2*y^2)+p2*(2*x*y)+s3*r^2+s4*r^4 \tag{4} A=1+k4∗r2+k5∗r4+k6∗r61+k1∗r2+k2∗r4+k3∗r6​B=p1∗(2∗x∗y)+p2∗(r2+2∗x2)+s1∗r2+s2∗r4C=p1∗(r2+2∗y2)+p2∗(2∗x∗y)+s3∗r2+s4∗r4(4)

那么，畸变方程简化为：

x d i s t = x ∗ A + B (5) x_{dist} = x * A + B \tag{5} xdist​=x∗A+B(5)

迭代法求解非线性方程组，首先就是要分离出 x x x和 y y y，即

x = ( x d i s t − B ) / A y = ( y d i s t − C ) / A (6) x = (x_{dist} - B) / A \\ y = (y_{dist} - C) / A \tag{6} x=(xdist​−B)/Ay=(ydist​−C)/A(6)

这相当于我们利用畸变坐标，得到了去畸变后的坐标位置。但是，这个结果并不足够准确，需要进行多次迭代。

既然需要迭代进行求解，我们就需要设定迭代结束条件，即：

• 满足残差
• 达到最大迭代次数

残差

每一次迭代，我们通过公式（6）得到了“去畸变后的坐标位置”。

那么，将其带入到公式（2），我们就可以得到新的”畸变坐标位置“。

再借助相机内参，将其转化到像素坐标系下，这样我们就可以计算和真实的畸变坐标位置的欧氏距离作为最终的error，也就是重投影误差。

代码

void undistorPointCore(Point2f& point)
{
    // point 畸变点坐标
    cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS, 50, 0.0001);
    double k[3] = { 8.754e-05,3.669e-07,-1.36e-07 }; // x
    //double k[3] = { 0.0001003,-6.175e-07,-1.186e-07 }; // y

    double xu, yu, xd = 0, yd = 0;
    xd = xu = point.x;
    yd = yu = point.y;

    double error = std::numeric_limits<double>::max();

    for (int j = 0; ; j++)
    {
		if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
		{
			//cout << __LINE__ << " exceed max iterative num" << endl;
			break;
		}
		if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)
		{
			//cout << __LINE__ << " reach residual error!" << endl;
			break;
		}
        double r2 = xu * xu + yu * yu;
        double icdist = 1 + ((k[2] * r2 + k[1]) * r2 + k[0]) * r2;
        if (icdist < 0)  
        {
            xu = point.x;
            yu = point.y;
            break;
        }
        // xu yu 可以认为是去畸变后的坐标位置
        // xd yd 可以认为是畸变前的坐标位置
        // xd = xu(1+k1*rd^2+k2*rd^4+k3*rd^6)
        // rd = sqrt(xu * xu + yu * yu)
        xu = xd / icdist;
        yu = yd / icdist;

        // 然后再对上述去畸变后的坐标再加畸变，然后再转换到像素坐标系，并计算误差
        if (criteria.type & cv::TermCriteria::EPS)
        {
            double r4, r6, cdist;
            double xu0, yu0;

            r2 = xu * xu + yu * yu;
            r4 = r2 * r2;
            r6 = r4 * r2;
            cdist = 1 + k[0] * r2 + k[1] * r4 + k[2] * r6;
            xd0 = xu * cdist;
            yd0 = yu * cdist;

            error = sqrt(pow(xd0 - point.x, 2) + pow(yd0 - point.y, 2));
			//cout << j << " " << error << endl;
        }
    }

	cout << point.x << " " << point.y << " " << xu << " " << yu << endl;
	point.x = xu;
	point.y = yu;
    // point 去畸变点坐标
}
           

opencv中

undistortPoints()

在内部调用

cvUndistortPointsInternal()

，上述代码做了一些精简，比如：

• 舍弃了大量的类型检查，以及Mat与cvMat之间的转化
• 将畸变模型简化，只考虑三阶径向模型

这种方法在去畸变的时候，收敛速度非常快，一般几次就能得到去畸变后的坐标位置。opencv中提到，默认迭代次数为5次。

Default version of undistortPoints does 5 iterations to compute undistorted points

beacuse, criteria = cv::TermCriteria(cv::TermCriteria::EPS, 5, 0.01);

最后

刚开始在做反投影的时候，卡在了畸变方程的求解。于是乎，就在网上找了一个牛顿法求解非线性方程组的demo。相比牛顿法去求解，opencv中给出的这种方法，直观、收敛速度快、精度比较高，且实现起来比较容易（牛顿法需要求雅可比矩阵和海森矩阵）。