畸变模型的求解

畸变模型求解

畸变模型

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

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

$$x_{undist}=f(x_{dist},y_{dist})\text{\hspace{1em}(1)}$$
$$x_{dist}=g(x_{undist},y_{undist})\text{\hspace{1em}(2)}$$

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

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

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

undistortPoints()

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

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

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

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

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

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

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

迭代法求解非线性方程组

对于相机坐标系下的畸变坐标$(x_{dist},y_{dist})$，我们有如下畸变方程：
$$x_{dist}=x\ast \frac{1+k1\ast r^2+k2\ast r^4+k3\ast r^6}{1+k4\ast r^2+k5\ast r^4+k6\ast r^6}+p1\ast (2\ast x\ast y)+p2\ast (r^2+2\ast x^2)+s1\ast r^2+s2\ast r^4\text{\hspace{1em}(3)}$$
简化一下上式，令：
$$\begin{gathered} A=\frac{1+k1\ast r^2+k2\ast r^4+k3\ast r^6}{1+k4\ast r^2+k5\ast r^4+k6\ast r^6} \\ B=p1\ast (2\ast x\ast y)+p2\ast (r^2+2\ast x^2)+s1\ast r^2+s2\ast r^4 \\ C=p1\ast (r^2+2\ast y^2)+p2\ast (2\ast x\ast y)+s3\ast r^2+s4\ast r^4\text{\hspace{1em}(4)} \end{gathered}$$
那么，畸变方程简化为：
$$x_{dist}=x\ast A+B\text{\hspace{1em}(5)}$$
迭代法求解非线性方程组，首先就是要分离出$x$和$y$，即
$$\begin{gathered} x=(x_{dist}-B)/A \\ y=(y_{dist}-C)/A\text{\hspace{1em}(6)} \end{gathered}$$
这相当于我们利用畸变坐标，得到了去畸变后的坐标位置。但是，这个结果并不足够准确，需要进行多次迭代。

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

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

残差

每一次迭代，我们通过公式（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中给出的这种方法，直观、收敛速度快、精度比较高，且实现起来比较容易（牛顿法需要求雅可比矩阵和海森矩阵）。