VINS-Mono工程笔记（八）：纯视觉SFM及视觉IMU对齐

1.视觉SFM理论及流程

1) 理论及流程

        假设滑窗中一共有11帧，首先需要选取一个枢纽帧，利用枢纽帧和最后一帧通过对极约束求出这两帧之间的相对位姿。对枢纽帧的要求是：一方面要求枢纽帧离最后一帧尽可能远，因为离最后一帧比较近时，二者之间的平移比较小，使得三角化精度较差，甚至会无法进行对极约束（平移为零的情况）。另一方面，要求枢纽帧与最后一帧之间不能距离太远，否则会使得二者之间关联的特征点比较少，导致求解出的位姿可信度不高，并且无法三角化出尽可能多的点。因此，在保证匹配点足够多的情况下，枢纽帧离最后一帧应该尽可能远，如下图中选取第4帧作为枢纽帧。

​        将枢纽帧设为参考帧，其位姿设置：旋转为单位阵I II，平移为0。通过对极几何求出枢纽阵(4)和最后一帧(10)之间的相对位姿R和t（尺度未知）。由于第4帧的位姿为( I , 0 ) (I,0)(I,0)，那么第10帧的位姿就是二者之间的相对位姿( R , t ) (R,t)(R,t)。然后，在已知位姿之后，通过三角化求出这两帧之间的共视点（带尺度的）。由于光流追踪的性质，求出的第4帧与第10帧之间的共视点也会被中间的第5—9帧所看到，那么在已知这些共视点（3D）和第4帧的关键点（2D）之后，通过PnP的方法（3D-2D）求解出第5帧的位姿。按照同样的方法，通过三角化获取更多的共视点，通过PnP求出枢纽帧(4)和最后一帧(10)之间的每一帧(5—9)的位姿。

​        枢纽帧(4)与第一帧(0)之间的位姿和共视点求取按照同样的方法，先根据对极几何求出第0帧和第4帧的相对位姿，由于第4帧的位姿为( I , 0 ) (I,0)(I,0)，那么求出的相对位姿就是第0帧的位姿。然后通过三角化求出第0帧与第4帧之间的共视点，根据光流追踪的性质，这些点都会被第1—3帧所看到。然后通过PnP的方法求出第1帧的位姿，以此类推，通过三角化获取更多的共视点，通过PnP求出第2、3帧的位姿。

​        通过对滑窗进行上述操作，可求出滑窗中每一帧的位姿和所看到的3D共视点。

2) VINS流程

（1）通过加速度标准差判断滑窗内是否具有足够运动。 

//1.1.check imu observibility 通过计算标准差来进行判断
    {
        map<double, ImageFrame>::iterator frame_it;
        Vector3d sum_g;
        //遍历除了第一帧图像外的所有图像帧
        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
        {
            double dt = frame_it->second.pre_integration->sum_dt;
            //计算每一帧图像对应的加速度
            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
            //图像的加速度之累加和
            sum_g += tmp_g;
        }
        Vector3d aver_g;
        //计算平均加速度。因为上边计算的是除了第一帧图像之外的其他所有图像帧对应的加速度之和，所以这里要减去1
        aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
        double var = 0;
        for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
        {
            double dt = frame_it->second.pre_integration->sum_dt;
            //计算获得每一帧的加速度
            Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
            //加速度减去平均加速度的差值的平方的累加
            var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
            //cout << "frame g " << tmp_g.transpose() << endl;
        }
        /**
         * 简单来说，标准差是一组数值自平均值分散开来的程度的一种测量观念。
         * 一个较大的标准差，代表大部分的数值和其平均值之间差异较大；一个较小的标准差，代表这些数值较接近平均值。
         * 方差公式：s^2=[(x1-x)^2 +...(xn-x)^2]/n 
         * 标准差等于方差的算数平方根，标准差公式：s=sqrt(s^2) 
        */
        var = sqrt(var / ((int)all_image_frame.size() - 1));
        //ROS_WARN("IMU variation %f!", var);
        //通过加速度标准差判断IMU是否有充分运动，标准差必须大于等于0.25
        if(var < 0.25)
        {
            ROS_INFO("IMU excitation not enouth!");
            //return false;
        }
    }

判断加速度的方差保证具有足够的运动。

(2)  遍历feature， 将feature的id以及有哪些帧可以观测到该feature以及在图像帧中的坐标。存储到SFMFeature中。

/**
     * 1.2.将f_manager.feature中的feature存储到sfm_f中
    */
    for (auto &it_per_id : f_manager.feature)
    {
        int imu_j = it_per_id.start_frame - 1;
        SFMFeature tmp_feature;
        tmp_feature.state = false;
        tmp_feature.id = it_per_id.feature_id;
        //遍历每一个能观察到该feature的frame
        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
            imu_j++;
            Vector3d pts_j = it_per_frame.point;
            tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));
        }
        sfm_f.push_back(tmp_feature);
    }

(3)  求解枢纽帧l及与最后一帧的relative_pose

/**
     * 1.3.位姿求解
    */
    Matrix3d relative_R;
    Vector3d relative_T;
    int l;
    //通过求取本质矩阵来求解出位姿
    /**
     * 这里的l表示滑动窗口中第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，
     * 会作为参考帧到下面的全局sfm使用，得到的Rt为当前帧到第l帧的坐标系变换Rt，存储在relative_R和relative_T当中
     * */
    if (!relativePose(relative_R, relative_T, l))
    {
        ROS_INFO("Not enough features or parallax; Move device around");
        return false;
    }

/**
 * 这里的第l帧是从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧，
 * 会作为参考帧到下面的全局sfm使用，得到的Rt为当前帧到第l帧的坐标系变换Rt
 * 该函数判断滑动窗口中第一个到窗口最后一帧对应特征点的平均视差大于30，且内点数目大于12的帧，此时可进行初始化，同时返回当前帧到第l帧的坐标系变换R和T
 * */
bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
    // find previous frame which contians enough correspondance and parallex with newest frame
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        vector<pair<Vector3d, Vector3d>> corres;
        //寻找第i帧到窗口最后一帧的对应特征点，存放在corres中
        corres = f_manager.getCorresponding(i, WINDOW_SIZE);
        //匹配的特征点对要大于20
        if (corres.size() > 20)
        {
            //计算平均视差
            double sum_parallax = 0;
            double average_parallax;
            for (int j = 0; j < int(corres.size()); j++)
            {
                //第j个对应点在第i帧和最后一帧的(x,y)
                Vector2d pts_0(corres[j].first(0), corres[j].first(1));
                Vector2d pts_1(corres[j].second(0), corres[j].second(1));
                //计算视差
                double parallax = (pts_0 - pts_1).norm();
                //计算视差的总和