VINS-Mono源码分析4— vins_estimator3(非线性优化)

if(result)
{
    solver_flag = NON_LINEAR;
    solveOdometry();
    slideWindow();
    f_manager.removeFailures();
    ROS_INFO("Initialization finish!");
    last_R = Rs[WINDOW_SIZE];
    last_P = Ps[WINDOW_SIZE];
    last_R0 = Rs[0];
    last_P0 = Ps[0];
    
}

将solver_flag的状态置为NON_LINEAR后，就会执行后端非线性优化solveOdometry()函数和滑窗处理slideWindow()函数，并且删除特征点管理器f_manager中深度为负值的特征点，最后记录last_R、last_P、last_R0和last_P0。

接下来详细分析一下solveOdometry()函数，

void Estimator::solveOdometry()
{
    if (frame_count < WINDOW_SIZE)
        return;
    if (solver_flag == NON_LINEAR)
    {
        TicToc t_tri;
        f_manager.triangulate(Ps, tic, ric);
        ROS_DEBUG("triangulation costs %f", t_tri.toc());
        optimization();
    }
}

triangulate()函数在《VINS-Mono源码分析3— vins_estimator2》中已经分析过了，这里不再赘述了，不过要强调一下，这里再次调用此函数的目的是更新特征点的深度到世界坐标系下的图像帧中。接下来分析optimization()函数，

ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);

构建ceres库优化问题，定义损失函数为柯西核函数，

for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
    problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS); 
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
    //这里在默认情况下是执行的
    if (!ESTIMATE_EXTRINSIC)
    {
        ROS_DEBUG("fix extinsic param");
        problem.SetParameterBlockConstant(para_Ex_Pose[i]);
    }
    else
        ROS_DEBUG("estimate extinsic param");
}

添加参数块，包括滑窗内所有帧的位姿、IMU的速度和加计陀螺偏置信息以及IMU-相机间的外参位姿，并将IMU-相机间的外参位姿设为固定参数块。

if (ESTIMATE_TD)
{
    problem.AddParameterBlock(para_Td[0], 1);
}

这段代码默认不执行。

vector2double();

void Estimator::vector2double()
{
    ......
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < f_manager.getFeatureCount(); i++)
        para_Feature[i][0] = dep(i);
    if (ESTIMATE_TD)
        para_Td[0][0] = td;
}

这段代码就是给para_Pose、para_SpeedBias和para_Ex_Pose赋值，最重要的是得到逆深度para_Feature。其实这里我一直不太明白，按理说应该先给para_Pose、para_SpeedBias和para_Ex_Pose赋值，然后再添加参数块才对，但不知为何这样写程序也能正常执行？

if (last_marginalization_info)
{
    MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
    problem.AddResidualBlock(marginalization_factor, NULL,
                             last_marginalization_parameter_blocks);
}

定义边缘化残差信息，添加边缘化残差块，注意边缘化在优化过程中只是起到了一个约束的作用。关于边缘化这部分专门写了一篇博客《VINS-Mono源码分析5— vins_estimator4》来介绍，这里就不多讲了。

for (int i = 0; i < WINDOW_SIZE; i++)
{
    int j = i + 1;
    //个人感觉这句代码意义不大
    if (pre_integrations[j]->sum_dt > 10.0)
        continue;
    IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
    problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}

定义IMU残差信息，添加IMU残差块。这部分具体实现代码在factor文件夹下的IMU_factor.h文件中，

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    //传参
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
    Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

    Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
    Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
    Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);
    //残差方程
    Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
    residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);

    Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
        //sqrt_info.setIdentity();
    residual = sqrt_info * residual;
    //雅克比矩阵的计算
    if (jacobians)
    {
        ......
    }

    return true;
}

上面的代码要结合着integration_base.h的代码阅读，理论知识参考崔神的文章《VINS论文推导及代码解析》中3.3节的内容。

int f_m_cnt = 0;
int feature_index = -1;
for (auto &it_per_id : f_manager.feature)
{
    it_per_id.used_num = it_per_id.feature_per_frame.size();
    if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
        continue;

    ++feature_index;

    int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
    
    Vector3d pts_i = it_per_id.feature_per_frame[0].point;

    for (auto &it_per_frame : it_per_id.feature_per_frame)
    {
        imu_j++;
        if (imu_i == imu_j)
        {
            continue;
        }
        Vector3d pts_j = it_per_frame.point;
        //默认不执行
        if (ESTIMATE_TD)
        {
               ......
        }
        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]);
        }
        f_m_cnt++;
    }
}

定义视觉重投影残差信息,添加视觉重投影残差块。这部分具体实现代码在factor文件夹下的projection_factor.cpp文件中，

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    TicToc tic_toc;
    //传参
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    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);
//默认执行else的代码
#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;
    //计算雅克比矩阵
    if (jacobians)
    {
        ......
    }
    sum_t += tic_toc.toc();

    return true;
}

理论知识参考崔神的文章《VINS论文推导及代码解析》中3.4节的内容。

if(relocalization_info)
    {
        ......
    }

重定位相关的一些处理，默认不会执行，除非将参数文件下fast_relocalization的值置为1才会在检测到回环时执行。

ceres::Solver::Options options;

options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
if (marginalization_flag == MARGIN_OLD)
    options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
    options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

配置ceres求解的一些参数，进行迭代优化，寻找待优化参数的最优结果。

double2vector();

void Estimator::double2vector()
{
    ......
    //滑窗优化前后，滑窗内第0帧的航向角作差
    double y_diff = origin_R0.x() - origin_R00.x();
    //欧拉角转换到旋转矩阵
    Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0));
    if (abs(abs(origin_R0.y()) - 90) < 1.0 || abs(abs(origin_R00.y()) - 90) < 1.0)
    {
        ROS_DEBUG("euler singular point!");
        rot_diff = Rs[0] * Quaterniond(para_Pose[0][6],
                                       para_Pose[0][3],
                                       para_Pose[0][4],
                                       para_Pose[0][5]).toRotationMatrix().transpose();
    }

    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        ......
        //以优化前的第0帧航向为参考航向，依次传参
    }

    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ......
        //优化后的外参传递
    }
    //逆深度传递
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < f_manager.getFeatureCount(); i++)
        dep(i) = para_Feature[i][0];
    f_manager.setDepth(dep);
    //默认不执行
    if (ESTIMATE_TD)
        td = para_Td[0][0];

    // 不执行，因为参数文件euroc_config.yaml下的fast_relocalization值为0
    if(relocalization_info)
    { 
       ......
    }
}

代码分析写在注释里了。

if (marginalization_flag == MARGIN_OLD)
{
    ......
}
else
{
    ......
}

这里面的代码很重要，为下一次的边缘化做准备工作，主要就是更新线性化残差$r_p$ 和线性化雅克比矩阵$J_p$，它们可以理解为边缘化的约束，参考崔神的文章《VINS论文推导及代码解析》中3.2节的目标函数中的$r_p$和$J_p$。关于这部分专门写了一篇博客《VINS-Mono源码分析5— vins_estimator4》来介绍，这里就不多讲了。

接下来返回到processImage()函数中，分析slideWindow()函数，当边缘化最老帧时，

double t_0 = Headers[0].stamp.toSec();
back_R0 = Rs[0];
back_P0 = Ps[0];

将滑窗中第0帧的时间戳、旋转和平移信息存储下来。

for (int i = 0; i < WINDOW_SIZE; i++)
{
    Rs[i].swap(Rs[i + 1]);

    std::swap(pre_integrations[i], pre_integrations[i + 1]);

    dt_buf[i].swap(dt_buf[i + 1]);
    linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
    angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);

    Headers[i] = Headers[i + 1];
    Ps[i].swap(Ps[i + 1]);
    Vs[i].swap(Vs[i + 1]);
    Bas[i].swap(Bas[i + 1]);
    Bgs[i].swap(Bgs[i + 1]);
}
Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];

delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};

dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();

滑动窗口顺序移动，此时第10帧还没有到来，所以注意一下第10帧的初始处理方式。

if (true || solver_flag == INITIAL)
{
    map<double, ImageFrame>::iterator it_0;
    it_0 = all_image_frame.find(t_0);
    
    delete it_0->second.pre_integration;
    it_0->second.pre_integration = nullptr;
    for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it)
    {
        if (it->second.pre_integration)
            delete it->second.pre_integration;
        it->second.pre_integration = NULL;
    }
    
    all_image_frame.erase(all_image_frame.begin(), it_0);
    all_image_frame.erase(t_0);
}

把滑窗外的所有帧的预积分pre_integration信息删除置空，滑窗外的所有帧删除掉，

slideWindowOld();

void Estimator::slideWindowOld()
{
    sum_of_back++;//边缘化最老帧的次数记录

    bool shift_depth = solver_flag == NON_LINEAR ? true : false;
    //初始化成功后
    if (shift_depth)
    {
        Matrix3d R0, R1;
        Vector3d P0, P1;
        R0 = back_R0 * ric[0];//原滑窗第0帧的旋转
        R1 = Rs[0] * ric[0];//原滑窗第1帧，现滑窗第0帧
        P0 = back_P0 + back_R0 * tic[0];
        P1 = Ps[0] + Rs[0] * tic[0];
        f_manager.removeBackShiftDepth(R0, P0, R1, P1);
    }
    //初始化凉凉
    else
        f_manager.removeBack();
}

滑窗结束后，特征点管理器f_manager中存储的相关帧信息需要更新，具体代码实现就是removeBackShiftDepth()和removeBack()函数，

void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P)
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        //特征点it的初始帧不是第0帧,则减减
        if (it->start_frame != 0)
            it->start_frame--;
        else
        {
            //删除特征点it之前的初始帧
            Eigen::Vector3d uv_i = it->feature_per_frame[0].point;  
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            //特征点it现在的帧观测数小于2,则删除it
            if (it->feature_per_frame.size() < 2)
            {
                feature.erase(it);
                continue;
            }
            else
            {
                //更新特征点it的深度值
                Eigen::Vector3d pts_i = uv_i * it->estimated_depth;
                Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;
                Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);
                double dep_j = pts_j(2);
                if (dep_j > 0)
                    it->estimated_depth = dep_j;
                else
                    it->estimated_depth = INIT_DEPTH;
            }
        }
    }
}

void FeatureManager::removeBack()
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        //特征点it的初始帧不是第0帧,则减减
        if (it->start_frame != 0)
            it->start_frame--;
        else
        {
            //删除特征点it之前的初始帧
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            //特征点it现在的帧观测数等于0,则删除it
            if (it->feature_per_frame.size() == 0)
                feature.erase(it);
        }
    }
}

简略分析写在注释里了，接着往下读吧！

当边缘化次新帧时，

for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
{
    double tmp_dt = dt_buf[frame_count][i];
    Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
    Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];

    pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);

    dt_buf[frame_count - 1].push_back(tmp_dt);
    linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);
    angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
}

Headers[frame_count - 1] = Headers[frame_count];
Ps[frame_count - 1] = Ps[frame_count];
Vs[frame_count - 1] = Vs[frame_count];
Rs[frame_count - 1] = Rs[frame_count];
Bas[frame_count - 1] = Bas[frame_count];
Bgs[frame_count - 1] = Bgs[frame_count];

delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};

dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();

把滑窗处理前第10帧和第9帧的一些信息合二为一，当做滑窗处理后第9帧的一些信息，然后滑窗处理前第10帧的一些其它信息赋给滑窗处理后的第9帧，最后对滑窗处理后的第10帧做一些初始处理，

slideWindowNew()

void Estimator::slideWindowNew()
{
    sum_of_front++;
    f_manager.removeFront(frame_count);
}

void FeatureManager::removeFront(int frame_count)
{
    for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)
    {
        it_next++;
        //特征点it的初始帧是第10帧,则减减
        if (it->start_frame == frame_count)
        {
            it->start_frame--;
        }
        else
        {
            //删除能被第9帧观测到的特征点it的对应观测
            int j = WINDOW_SIZE - 1 - it->start_frame;
            if (it->endFrame() < frame_count - 1)
                continue;
            it->feature_per_frame.erase(it->feature_per_frame.begin() + j);
             //特征点it现在的帧观测数等于0,则删除it
            if (it->feature_per_frame.size() == 0)
                feature.erase(it);
        }
    }
}

滑窗结束后，特征点管理器f_manager中存储的相关帧信息需要更新。

接着返回processImage()函数中，往下分析

f_manager.removeFailures();

void FeatureManager::removeFailures()
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        if (it->solve_flag == 2)
            feature.erase(it);
    }
}

清除特征点管理器中所有深度值为负值的特征点。