VINS-Fusion源码逐行解析：对外点操作的一系列函数outliersRejection()、reprojectionError()、removeOutliers()

当初始化完位姿，优化完位姿、设定位姿等等操作做完后，接下来就是清算了，将外点删除掉。

首先outliersRejection()函数，函数作用是对所有特征点进行遍历，通过计算累计的重投影误差，根据设定的阈值来将不符合要求的特征点给个标签removeIndex，然后分单双目情况，单目是同一特征点在不同帧中的观测来计算重投影误差，双目是将右目当作单目，同一特征点在不同帧中的重投影误差，还有就是同一特征点在左右目中的重投影误差一同累计。计算重投影误差的函数为reprojectionError()

我们看下源码：

//初始化：设置特征点索引为-1，初始化误差和误差计数器。
//遍历特征点：对于每一个特征点，如果其使用的帧数少于4，则跳过。
//计算重投影误差：遍历特征点在每一帧中的信息，计算重投影误差并累加。
//处理双目立体视觉：如果是双目立体视觉系统，计算右图像中的重投影误差。
//判断外点：计算平均误差，如果平均误差乘以焦距大于3，则认为该特征点为外点，添加到移除索引集合中。
void Estimator::outliersRejection(set<int> &removeIndex)
{
    //return;
    // 初始化特征点索引为-1
    int feature_index = -1;
    // 遍历所有特征点
    for (auto &it_per_id : f_manager.feature)
    {
        // 初始化误差值和误差计数器
        double err = 0;
        int errCnt = 0;
        // 计算当前特征点被使用的帧数
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        // 如果使用的帧数少于4，跳过该特征点
        if (it_per_id.used_num < 4)
            continue;
        // 增加特征点索引
        feature_index ++;
        // 初始化IMU帧索引
        //imu_i 是特征点在当前帧的索引，表示该特征点在滑动窗口中的起始帧。
        //imu_j 是特征点在遍历过程中的帧索引，初始值为 imu_i - 1，并在每次遍历中递增。
        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
        // 获取当前特征点在第一帧中的坐标
        Vector3d pts_i = it_per_id.feature_per_frame[0].point;
        // 获取当前特征点的估计深度
        double depth = it_per_id.estimated_depth;
        // 遍历当前特征点在每一帧中的信息
        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
            // 增加IMU帧索引
            imu_j++;
            // 如果不是同一帧,也就是特征点在至少两帧中有观测
            if (imu_i != imu_j)
            {
                // 获取特征点在当前帧中的坐标
                Vector3d pts_j = it_per_frame.point;  
                // 计算重投影误差           
                double tmp_error = reprojectionError(Rs[imu_i], Ps[imu_i], ric[0], tic[0], 
                                                    Rs[imu_j], Ps[imu_j], ric[0], tic[0],
                                                    depth, pts_i, pts_j);
                // 累加误差值
                err += tmp_error;
                // 增加误差计数器
                errCnt++;
                //printf("tmp_error %f\n", FOCAL_LENGTH / 1.5 * tmp_error);
            }
            // need to rewrite projecton factor.........
            // 如果是双目立体视觉，并且该帧有右图像的特征点
            if(STEREO && it_per_frame.is_stereo)
            {
                // 获取右图像中的特征点坐标
                Vector3d pts_j_right = it_per_frame.pointRight;
                // 如果不是同一帧
                //计算不同帧之间的右目图像的重投影误差。
                if(imu_i != imu_j)
                {            
                    // 计算右图像中的重投影误差
                    double tmp_error = reprojectionError(Rs[imu_i], Ps[imu_i], ric[0], tic[0], 
                                                        Rs[imu_j], Ps[imu_j], ric[1], tic[1],
                                                        depth, pts_i, pts_j_right);
                    // 累加误差值
                    err += tmp_error;
                    // 增加误差计数器
                    errCnt++;
                    //printf("tmp_error %f\n", FOCAL_LENGTH / 1.5 * tmp_error);
                }
                else
                {
                    // 计算同一帧中的右图像重投影误差
                    double tmp_error = reprojectionError(Rs[imu_i], Ps[imu_i], ric[0], tic[0], 
                                                        Rs[imu_j], Ps[imu_j], ric[1], tic[1],
                                                        depth, pts_i, pts_j_right);
                    // 累加误差值
                    err += tmp_error;
                    // 增加误差计数器
                    errCnt++;
                    //printf("tmp_error %f\n", FOCAL_LENGTH / 1.5 * tmp_error);
                }       
            }
        }
        // 计算平均误差
        double ave_err = err / errCnt;
        // 如果平均误差乘以焦距大于3，则认为该特征点为外点，添加到移除索引集合中
        if(ave_err * FOCAL_LENGTH > 3)
            removeIndex.insert(it_per_id.feature_id);

    }
}

 看下reprojectionError()，该函数是计算重投影误差的函数，将一个3D点从一个相机帧（i）到另一个相机帧（j），将转换后的3D点投影到2D平面，计算投影点和实际观测点之间的误差，返回误差的欧几里得距离。

看下源码：

//该函数计算了一个3D点从一个相机帧（i）到另一个相机帧（j）之间的重投影误差
//根据深度信息，将特征点从相机坐标系 i 转换到世界坐标系。
//将点从世界坐标系转换到相机坐标系 j。
//将转换后的3D点投影到2D平面，计算投影点和实际观测点之间的误差。
//返回误差的欧几里得距离。
double Estimator::reprojectionError(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici,
                                 Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj, 
                                 double depth, Vector3d &uvi, Vector3d &uvj)
{
    // 将特征点从相机坐标系i转换到世界坐标系
    // depth * uvi 将单位归一化平面上的特征点 uvi 转换为深度为 depth 的3D点
    // rici * (depth * uvi) + tici 将这个3D点从相机坐标系i转换到IMU坐标系i
    // Ri * (...) + Pi 将点从IMU坐标系i转换到世界坐标系
    Vector3d pts_w = Ri * (rici * (depth * uvi) + tici) + Pi;
    // 将特征点从世界坐标系转换到相机坐标系j
    // Rj.transpose() * (pts_w - Pj) 将点从世界坐标系转换到IMU坐标系j
    // ricj.transpose() * (...) - ticj 将点从IMU坐标系j转换到相机坐标系j
    Vector3d pts_cj = ricj.transpose() * (Rj.transpose() * (pts_w - Pj) - ticj);
    // 计算重投影误差
    // pts_cj / pts_cj.z() 将3D点投影到2D平面
    // (pts_cj / pts_cj.z()).head<2>() 提取投影后的2D坐标
    // residual 是投影点和观测点 uvj 之间的误差
    Vector2d residual = (pts_cj / pts_cj.z()).head<2>() - uvj.head<2>();

    // 计算重投影误差的欧几里得距离
    double rx = residual.x();
    double ry = residual.y();
    return sqrt(rx * rx + ry * ry);
}

 然后是移除外点的函数removeOutlier()，通过遍历特征点容器中是否被标记为 outlierIndex的，将其删除，看下源码：

//该函数removeOutlier 的作用是从 feature 容器中删除在 outlierIndex 集合中标记为外点的特征点。
void FeatureManager::removeOutlier(set<int> &outlierIndex)
{
    // 定义一个集合迭代器
    std::set<int>::iterator itSet;
    // 遍历 feature 容器中的每个特征点
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        // 将 it_next 设为下一个迭代器位置，以便在删除当前元素后继续遍历
        it_next++;
        // 获取当前特征点的 ID
        int index = it->feature_id;
        // 在 outlierIndex 集合中查找当前特征点的 ID
        itSet = outlierIndex.find(index);
        // 如果当前特征点的 ID 在 outlierIndex 中找到
        if(itSet != outlierIndex.end())
        {
            // 从 feature 容器中删除当前特征点
            feature.erase(it);
            //printf("remove outlier %d \n", index);
        }
    }
}