图像畸变方程

径向畸变

通过畸变前的归一化坐标求取畸变后的像素坐标

如果已知相机坐标系中的一点 $P(X,Y,Z)$ ,我们就可以通过五个畸变系数找到这个点在像素平面上的正确位置:

① 将三维空间点投影到归一化图像平面。设它的无畸变归一化坐标为 $[x,y]^T$

② 对归一化平面上的点进行径向畸变和切向畸变纠正得到畸变后的归一化坐标为$[x_{distorted},y_{distorted}]^T$。

$$ \left\{ \begin{array}{L} x_{distorted}=x(1+k_1r^2+k_2r^4+k_3r^6)+2p_1xy+p_2(r^2+2x^2)\\ y_{distorted}=y(1+k_1r^2+k_2r^4+k_3r^6)+p_1(r^2+2y^2)+2p_2xy \end{array} \right.\ \tag{1} $$

由于这里主要讨论单目相机,参数 $k_3$ 只与鱼眼相机相关;所以忽略参数 $k_3$:

$$ \left\{ \begin{array}{L} x_{distorted}=x(1+k_1r^2+k_2r^4)+2p_1xy+p_2(r^2+2x^2)\\ y_{distorted}=y(1+k_1r^2+k_2r^4)+p_1(r^2+2y^2)+2p_2xy \end{array} \right.\ \tag{2} $$

其中,$r^2=x^2+y^2$;

③ 将纠正后的点通过内参数矩阵投影到像素平面,得到该点在图像上的正确位置 :

$$ \left\{ \begin{array}{L} u=f_x x_{distorted}+c_x\\ v=f_y y_{distorted}+c_y \end{array} \right.\tag{3} $$

上面这个过程是求 无畸变的相机归一化坐标在经过相机之后发生畸变,最终在像素平面的像素坐标,可以发现,这个像素坐标是发生畸变后得到的;

但是,实际上是一个逆向的过程,即目前实验已知的是发生畸变后得到的像素 $[u,v]^T$,所以我们去畸变需要做的就是,根据发生畸变的像素坐标求原始的无畸变的相机归一化坐标;

为了后面介绍的方便,定义畸变坐标 $[x_{distorted},y_{distorted}]^T = [x_d,y_d]^T$;

OpenCV去畸变

不动点迭代法(undistortpoints)

对于公式 $(1)$ 进行如下的变化:

$$ \begin{bmatrix}x_d\\y_d \end{bmatrix}= [1+k_1r^2+k_2r^4+k_3r^6] \begin{bmatrix}x\\y \end{bmatrix}+ \begin{bmatrix}2p_1xy+p_2(r^2+2x^2)\\p_1(r^2+2y^2)+2p_2xy \end{bmatrix}\tag{4} $$

简化形式为:

$$ X_d=F(X)\cdot X+G(X)\tag{5} $$

其中;

$$ \begin{align*} X_d &=\begin{bmatrix}x_d\\y_d \end{bmatrix}\\ X&=\begin{bmatrix}x\\y \end{bmatrix}\\ F(X)&=1+k_1r^2+k_2r^4+k_3r^6\\ G(X)&=\begin{bmatrix}2p_1xy+p_2(r^2+2x^2)\\p_1(r^2+2y^2)+2p_2xy \end{bmatrix} \end{align*} \tag{6} $$

由公式 $(5)$ 构造:

$$ F(X)\cdot X+G(X)-X_d=0\tag{7} $$

根据式 $(7)$ 构造不动点等价形式为:

$$ X=\frac{X_d-G(X)}{F(X)}\tag{8} $$

构造迭代形式为:

$$ X_{k+1}=\frac{X_d-G(X_k)}{F(X_k)}\tag{9} $$

该迭代式子是否满足收敛条件我还没有进行验证,在去畸变迭代计算时,将当前已知的有畸变点 $X_d$ 作为初始值, 即 $X_0=X_d$,然后根据式 $(8)$ 迭代计算无畸变的点,直到满足迭代误差或者最大迭代次数条件。

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:形如式(9)的迭代
        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));
        }
    }
}

高斯牛顿法

对于畸变模型做如下处理,构建出残差:

$$ \begin{align*} &\left\{ \begin{array}{L} x_{d}=x(1+k_1r^2+k_2r^4)+2p_1xy+p_2(r^2+2x^2)\\ y_{d}=y(1+k_1r^2+k_2r^4)+p_1(r^2+2y^2)+2p_2xy \end{array} \right.\\ \Rightarrow &\left\{ \begin{array}{L} x_{d}=x+x(k_1r^2+k_2r^4)+2p_1xy+p_2(r^2+2x^2)\\ y_{d}=y+y(k_1r^2+k_2r^4)+p_1(r^2+2y^2)+2p_2xy \end{array} \right.\\ \Rightarrow &\left\{ \begin{array}{L} x_{d}=x+f(x,y)\\ y_{d}=y+g(x,y) \end{array} \right.\\ \Rightarrow &\left\{ \begin{array}{L} F(x,y)=x+f(x,y)-x_{d}\\ G(x,y)=y+g(x,y)-y_{d} \end{array} \right.\\ \Rightarrow &R(x,y)=\begin{bmatrix}F(x,y)\\G(x,y)\end{bmatrix} \end{align*} \tag{10} $$

然后构建最小二乘:

$$ (x,y)=\arg\min\frac{1}{2}\parallel R(x,y)\parallel^2\tag{11} $$

可以到正规方程:

$$ \begin{align*} &\left[J(x,y)^T J(x,y)\right]\begin{bmatrix}\Delta x\\ \Delta y\end{bmatrix}= -J(x,y)^TR(x,y)\\ \Rightarrow &\mathbf{H} \Delta\beta=\mathbf{g}\\ \Rightarrow &\Delta\beta=\left[J(x,y)^T J(x,y)\right]^{-1}[-J(x,y)^TR(x,y)] \end{align*}\tag{12} $$

其中,

$$ J(x)=\begin{bmatrix} \frac{\partial F(x,y)}{\partial x}&\frac{\partial G(x,y)}{\partial x}\\ \frac{\partial F(x,y)}{\partial y}&\frac{\partial G(x,y)}{\partial y}\\ \end{bmatrix}\tag{13} $$

迭代过程如下:

  1. 给定初始值 $x_0$
  2. 对于第 $k$ 次迭代,求出当前的雅可比矩阵 $J(x_k,y_k)$和误差 $R(x_k,y_k)$;
  3. 求解增量方程:$\mathbf{H} \Delta\beta_k=\mathbf{g}$
  4. 若 $\Delta\beta_k$ 足够小,则停止。否则,令 $\beta_{k+1}=\beta_k+\Delta\beta_k$

VINS去畸变

首先,我们需要把畸变后的像素坐标转化为畸变后的归一化坐标

$$ \left\{ \begin{array}{L} x_{d}=\frac{u-c_x}{f_x}\\ y_{d}=\frac{v-c_y}{f_y}\\ \end{array} \right.\ \tag{14} $$

然后反求未发生畸变的归一化坐标 $[x,y]^T$,但是,通过公式 $(1)$ 不难发现,它是一个高度非线性化的表达式,如果反求的话,不是很容易,而且非常小消耗资源。

那么VINS怎么计算的呢?

这里用的方法和OpenCV不同,假设现在求 $A_d$ 点的去畸变点 $A$ 的坐标,那么我们将 $A_d$ 的坐标直接代入畸变模型中,求得再次畸变的点 $A_{d1}$ 坐标,并求得点 $A_{d1}$ 和 $A_d$ 之间的距离 $d_1$ ,因为越靠近中心这个 $d$ 越小,此时肯定小于真实点 $A$ 的坐标到 $A_d$ 的距离 $d_t$。所以我们在朝真实畸变方向(再次畸变的反方向)上加上这个 $d_1$,得到一个靠近真实值的方向,在以这个点再进行一次畸变求得距离 $d_2$ , $d_2$ 肯定大于 $d_1$ 小于 $d_t$ , $d_2$ 在加到坐标 $A_d$ 上得到更靠近的结果,以此迭代,如图所示,迭代三次的示意图;代码中一共进行了8次迭代。由于VINS这种方法是正向计算的,并没对高度非线性的畸变方程进行求解,因此,这种方法比OpenCV中的方法更加快捷;

VINS中的去畸变迭代过程

//首先从函数参数来看,它的输入是一个二维的像素坐标,它的输出是一个三维的无畸变的归一化坐标
void PinholeCamera::liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const
{
    double mx_d, my_d, mx2_d, mxy_d, my2_d, mx_u, my_u;
    double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;
    // double lambda;

    // Lift points to normalised plane
    // 这里的mx_d和my_d就是最初的、有畸变的归一化坐标[x_d,y_d]^T
    mx_d = m_inv_K11 * p(0) + m_inv_K13; // x_d = (u - cx)/ fx
    my_d = m_inv_K22 * p(1) + m_inv_K23; // y_d = (v - cy)/ fy

    if (m_noDistortion) //如果没有畸变,则直接返回归一化坐标
    {
        mx_u = mx_d;
        my_u = my_d;
    }
    else //如果有畸变,则计算无畸变的归一化坐标
    {
        if (0)
        {
            //......
        }
        else
        {
            // 递归失真模型
            int n = 8; //迭代8次
            Eigen::Vector2d d_u;

            // distortion()函数的作用是计算公式 \Delta x 和 \Delta x; d_u=[\Delta x,\Delta y]
            distortion(Eigen::Vector2d(mx_d, my_d), d_u);

            // 近似值
            // 此处的mx_d和my_d相当A_d,它们在迭代过程保持不变
            mx_u = mx_d - d_u(0); //x = x_d - \Delta x
            my_u = my_d - d_u(1); //y = y_d - \Delta y

            //此时的mx_u和my_u相当于被更新了一次的点A_1
            
            for (int i = 1; i < n; ++i)
            { //上面已经迭代了1次,for循环中迭代7次
                distortion(Eigen::Vector2d(mx_u, my_u), d_u);  
                mx_u = mx_d - d_u(0);
                my_u = my_d - d_u(1);
            }
        }
    }

    // Obtain a projective ray
    P << mx_u, my_u, 1.0;
}

对于函数 distortion() :

//计算 \Delta x 和 \Delta x
void PinholeCamera::distortion(const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u) const
{
    // 畸变参数
    double k1 = mParameters.k1();
    double k2 = mParameters.k2();
    double p1 = mParameters.p1();
    double p2 = mParameters.p2();

    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;

    mx2_u = p_u(0) * p_u(0); //x^2
    my2_u = p_u(1) * p_u(1); //y^2
    mxy_u = p_u(0) * p_u(1); //xy
    rho2_u = mx2_u + my2_u;  //r^2=x^2+y^2
    rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; // k1 * r^2 + k2 * r^4
    d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),// \Delta x = x(k1 * r^2 + k2 * r^4)+2 * p1 * xy +p2(r^2+2x^2)
    p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);   // \Delta y = y(k1 * r^2 + k2 * r^4)+2 * p2 * xy +p1(r^2+2y^2)
}

参考链接

VINS_Mono代码阅读记录--liftProjective函数

高斯牛顿法去畸变(C++实现)

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/57.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 06 月 10 日
赏杯咖啡