SLAM十四讲与VINS重投影误差计算的对比

今天是2021年8月1日，刚看了东奥男子100米决赛的比赛，心情还久久不能平静，尽管我比赛前比运动员还紧张，比赛后心脏还一直激动，心情难以平复，但我还是要大喊一声，苏神牛逼！这是历史的一刻，致敬中国速度！

下面开始正文

投影模型与代价函数

在高博《VSLAM十四讲》第二版的242页，推导重投影代价函数时，使用了三步推导：

• 1.将路标点的世界坐标转换为局部坐标，并将局部坐标投影至归一化平面坐标。
• 2.归一化坐标加上畸变映射，转为相机成像时的归一化坐标。（实际图像在相机中会有畸变）
• 3.畸变图像坐标（归一化平面上的成像坐标），投影为相机的图像坐标。

假设上述变换用一个函数$h(T,p)$表示，$T$表示相机位姿，p为路标点，这样转换后，路标点的当前观测$z={[u_s,v_s]}^T$的误差可以写为:
$$e=z-h(T,p)$$
将所有位姿$T_i$下对路标点$p_j$观测进行累计，就可以获得总的误差：
$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{\Vert e_{ij}\Vert }^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{\Vert z_{ij}-h(T_i,p_j)\Vert }^2$$

在我看来，这个推导很有误导性，主要有以下几个方面。

• 1.图像坐标系中数据确实是相机的直接观测，但是图像坐标的单位是像素，是正整数，这个特性对接下来的最小二乘法优化其实是不利的，添加了坐标的正整数限制。
• 2.从归一化平面再投影到图像坐标其实是多余的，既然都有了归一化平面上的观测点数据，那么此刻相机对路标的测量（作为预测点），可以直接逆投影到归一化平面上，此时同样可以算误差，并且少了像素坐标的限制。
• 相机畸变过程是一个高度非线性过程，这里直接使用了简单的径向畸变变换$u_d=u(1+k_1{r}_c^2+k_2{r}_c^4)$来处理。由于观测数据的处理直接影响最后优化结果的好坏，这里粗糙的做法必然带来不好的效果。

但我们知道，高博这本书主要是入门书籍，所以很多地方都是以讲概念为主，让大家知道这里要做什么，目的是什么，所以还是不能吹毛求疵。

工程上的做法，以VINS-MONO为例

vins中的重投影误差：

重投影误差图示：

解释：vins中的重投影误差，同样也是观测值减去预测值（后面对路标的观测称为预测值，第一次对路标的观测称为观测值），其思想是将第一次测量得到的观测值$p_l^i$转换到当前计算预测值的相机坐标系下（公式17的第三个式子，最长那个），此时是局部坐标系下的坐标$p_l^j$（三维点）。
设第一次观测的相机位姿为$T_i$,此时为$T_j$,对路标点在当前相机坐标系下坐标$p_l^j$，计算其单位方向向量，而对当前的预测值${\hat{p}}_l^j$的观测，是图像坐标，将其逆投影至单位圆面上（理解为与观测值的单位向量同方向的单位向量就行），计算这两个单位向量的向量差（图中红色箭头部分），然后将向量差分别投影到${\hat{p}}_l^j$逆投影后单位向量所在球面的切平面的两个正交基上，这两个投影就是此次重投影的二维误差。

概括来说，就是图像预测向单位球面投影，观测值也向单位球面投影，然后计算这两者投影之差（投影误差向切平面正交基投影），这里的单位圆可以等同于单位平面。都是在空间中拿了一个中间过渡层来计算误差，不过单位圆模型比单位平面适应相机的范围更广。

而对于畸变的处理，查看vins-mono源码如下

/**
 * \brief Lifts a point from the image plane to its projective ray
 * 该函数为逆投影，即把图像点坐标投影到相机归一化平面
 * \param p image coordinates
 * \param P coordinates of the projective ray
 */
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
    //m_inv_k11为fx的倒数，m_in_k13为-cx/fx
    mx_d = m_inv_K11 * p(0) + m_inv_K13;//归一化平面的X
    my_d = m_inv_K22 * p(1) + m_inv_K23;//归一化平面的y

    if (m_noDistortion)
    {
        mx_u = mx_d;
        my_u = my_d;
    }
    else
    {
        if (0)
        {
            //获取配置文件中的畸变参数，k1,k2径向畸变，p1,p2切向畸变参数
            double k1 = mParameters.k1();
            double k2 = mParameters.k2();
            double p1 = mParameters.p1();
            double p2 = mParameters.p2();

            // Apply inverse distortion model
            // proposed by Heikkila
            mx2_d = mx_d*mx_d;//x^2
            my2_d = my_d*my_d;//y^2
            mxy_d = mx_d*my_d;//xy
            rho2_d = mx2_d+my2_d;//x^2+y^2,r^2
            rho4_d = rho2_d*rho2_d;//r^4
            radDist_d = k1*rho2_d+k2*rho4_d;
            Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;//畸变量
            Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;//畸变量
            inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);

            mx_u = mx_d - inv_denom_d*Dx_d;//原x减畸变量，那么图像是校准后的图像
            my_u = my_d - inv_denom_d*Dy_d;
        }
        else
        {
            // Recursive distortion model
            int n = 8;
            Eigen::Vector2d d_u;
            distortion(Eigen::Vector2d(mx_d, my_d), d_u);
            // Approximate value
            mx_u = mx_d - d_u(0);
            my_u = my_d - d_u(1);

            for (int i = 1; i < n; ++i)
            {
                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;
}

可以看到作者对畸变处理进行了改进，第一次是使用函数近视处理的方法（if分支），这里同时考虑了径向畸变和切向畸变，但估计这样效果不好，于是作者改为使用迭代畸变模型，循环迭代八次，这样效果比函数近视好，而速度也比opencv的API快。

在作者保留代码的变动过程中，我们也可也看到，工程中去除畸变的过程不是那么简单的。