VINS-Mono代码-视觉特征部分-笔记

引言

视觉特征从提取到构造视觉误差再到优化，是VINS中重要的一部分。所采用的特征法，光流跟踪，重投影误差，基于归一化平面的残差计算等等都值得具体的分析与思考。下面我们将从整个代码的主函数开始，沿着特征处理进行介绍。

主函数

整个系统的主函数在estimator_node.cpp中，进一步的IMU数据和视觉数据的处理是在process()函数中，数据的获取是在getMeasurements()函数中，
传入进来的视觉特征数据包括归一化特征点坐标，像素坐标，特征点速度，特征索引，具体如下：

int v = img_msg->channels[0].values[i] + 0.5;
                int feature_id = v / NUM_OF_CAM;
                int camera_id = v % NUM_OF_CAM;
                double x = img_msg->points[i].x;
                double y = img_msg->points[i].y;
                double z = img_msg->points[i].z;
                double p_u = img_msg->channels[1].values[i];
                double p_v = img_msg->channels[2].values[i];
                double velocity_x = img_msg->channels[3].values[i];
                double velocity_y = img_msg->channels[4].values[i];
                ROS_ASSERT(z == 1);
                Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
                xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
                image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);

下面进入

estimator.processImage(image, img_msg->header);

首先初始化各种参数：边缘化选项/关键帧/重新进行预积分

然后进行视觉SFM初始化，通过与IMU数据进行对准，实现对尺度，IMU位姿，特征深度，IMU偏置的初始化。

然后进入

solveOdometry();

执行优化，这里的优化就是构建当前窗口内的信息的误差函数。优化之前，多特征队列中的满足一定条件的特征进行三角化，更新对应特征的深度信息。特征的选择条件如下

if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;

        if (it_per_id.estimated_depth > 0)
            continue;

在void Estimator::optimization()函数中，加入各种参数和残差项，针对视觉残差，分为两种情况，一是IMU和视觉的时间偏移已经被同步(ESTIMATE_TD=0),二是二者还没有被同步，需要同时对TD进行优化(ESTIMATE_TD=1)，对应代码为

if (ESTIMATE_TD)
            {
                    ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                                                     it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                                                                     it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
                    problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
                    /*
                    double **para = new double *[5];
                    para[0] = para_Pose[imu_i];
                    para[1] = para_Pose[imu_j];
                    para[2] = para_Ex_Pose[0];
                    para[3] = para_Feature[feature_index];
                    para[4] = para_Td[0];
                    f_td->check(para);
                    */
            }
            else
            {
                ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
            }

首先介绍已经同步的情况
首先，新建ProjectionFactor对象

ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);

其对应的构造函数为

ProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j)
{
#ifdef UNIT_SPHERE_ERROR
    Eigen::Vector3d b1, b2;
    Eigen::Vector3d a = pts_j.normalized();
    Eigen::Vector3d tmp(0, 0, 1);
    if(a == tmp)
        tmp << 1, 0, 0;
    b1 = (tmp - a * (a.transpose() * tmp)).normalized();
    b2 = a.cross(b1);
    tangent_base.block<1, 3>(0, 0) = b1.transpose();
    tangent_base.block<1, 3>(1, 0) = b2.transpose();
#endif
};

这里是其计算其正切平面上的两个正交向量$b_1,b_2$，首先对j特征的位置向量进行归一化，这里的正切平面向量的计算利用的三角形的相似性，两个直角三角形相似。具体过程如下
$$\begin{gathered} a=[a,b,c{]}^T \\ b1=[0,0,1{]}^T-[a,b,c{]}^T\cdot c=[-ac,-bc,1-c^2{]}^T \\ b1\cdot a=-a^{2}c-b^{2}c+c-c^3=0 \\ ⟹b1\perp a \end{gathered}$$

接下来，在Evaluate中计算残差

 double inv_dep_i = parameters[3][0];

    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
    Eigen::Map<Eigen::Vector2d> residual(residuals);

所对应的计算公式为
$$\begin{gathered} [x_{c_j},y_{c_j},z_{c_j},1{]}^T=T_{bc}^{-1}{T}_{w{b}_j}^{-1}{T}_{w{b}_i}Tbc[\frac{1}{\lambda }{u}_{c_i},\frac{1}{\lambda }{v}_{c_i},\frac{1}{\lambda },1{]}^T \\ {\mathbf{r}}_c=[\frac{x_{c_j}}{z_{c_j}}-u_{c_j},\frac{y_{c_j}}{z_{c_j}}-v_{c_j}{]}^T \end{gathered}$$
代码里计算了两种残差，切平面残差和直接残差

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

    residual = sqrt_info * residual; //The sqrt_infor is stastic

接下来计算视觉惨差对各参数的雅克比矩阵块。
下面的check()函数是用来自动计算雅克比矩阵的。