vins前端中的去畸变方法解析

最新推荐文章于 2023-10-24 08:27:47 发布

weixin_45807759

最新推荐文章于 2023-10-24 08:27:47 发布

阅读量148

点赞数

文章标签：前端

本文链接：https://blog.csdn.net/weixin_45807759/article/details/129859600

版权

本文详细解读了VINS_Mono中的liftProjective函数，该函数用于处理Pinhole相机模型的畸变校正。通过递归失真模型，在无畸变和有畸变的情况下计算无畸变的归一化坐标，涉及的关键步骤包括计算畸变参数和应用distortion函数进行校正。

摘要由CSDN通过智能技术生成

Vins 前端中高效的去畸变的方式解析
 VINS_Mono代码阅读记录–liftProjective函数
 VinsFusion中的MEI模型解析
 vins 解读_代码解读 | VINS_Mono中的鱼眼相机模型

以下简单记录
在这里插入图片描述

//首先从函数参数来看，它的输入是一个二维的像素坐标，它的输出是一个三维的无畸变的归一化坐标
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;
}

//计算 \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)
}