VIR-SLAM代码分析3——VIR_VINS详解之estimator.cpp/.h

前言

续接上一篇，本本篇接着介绍VIR-SLAM中estimator.cpp/.h文件的函数，尤其是和UWB相关的相比于VINS改动过的函数，仍然以具体功能情况+代码注释的形式进行介绍。

重点函数介绍

优化函数，代码是先优化，后边缘化。

void Estimator::optimization()
{
    ceres::Problem problem;
    ceres::LossFunction *loss_function;
    //loss_function = new ceres::HuberLoss(1.0);
    loss_function = new ceres::CauchyLoss(1.0);
    for (int i = 0; i < WINDOW_SIZE + 1; i++)//添加各种待优化量X——位姿优化量，还包括最新的第11帧
    {
        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++)//7维、相机IMU外参
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
        if (!ESTIMATE_EXTRINSIC)//如果IMU-相机外参不需要标定
        {
            ROS_DEBUG("fix extinsic param");
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);//这个变量固定为constant
        }
        else
            ROS_DEBUG("estimate extinsic param");
    }
    if (ESTIMATE_TD)//IMU-image时间同步误差，1维，标定同步时间
    {
        problem.AddParameterBlock(para_Td[0], 1);
        //problem.SetParameterBlockConstant(para_Td[0]);
    }

    TicToc t_whole, t_prepare;
    vector2double();  // 因为ceres用的是double数组,所以下面用vector2double做类型转换

    if (last_marginalization_info)//添加先验信残差
    {
        // construct new marginlization_factor, for the prior element.
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }

    for (int i = 0; i < WINDOW_SIZE; i++)//添加IMU残差
    {
        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]);
    }

    if(USE_UWB && PsLong.size() == (WINDOW_SIZE_LONG) && KNOWN_ANCHOR == 1)//添加UWB残差
    {  
        //add edge for long window


        for (int i = 0; i < PsLong.size(); i++)
        {
            ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
            problem.AddParameterBlock(para_Pose_Long[i], SIZE_POSE, local_parameterization);
        }

        for (int i = 1; i < PsLong.size(); i++)
        {
            for (int j = 1; j < 5; j++)
            {
                int neibLink = i-j;
                if (neibLink >0)
                {
                    //cout<<" Add residual in pslong "<< i << " "<< neibLink << " pslong size "<<  PsLong.size() << " "<< endl;
                    ceres::CostFunction* cost_function = LongWindowError::Create(PsLong.at(neibLink), PsLong.at(i), RsLong.at(neibLink), RsLong.at(i), LINK_W);
                    problem.AddResidualBlock(cost_function, NULL, para_Pose_Long[neibLink], para_Pose_Long[i]);
                }
            }

            ceres::CostFunction* cost_function = movingError::Create(PsLong.at(i),  MOVE_W);
            problem.AddResidualBlock(cost_function, NULL, para_Pose_Long[i]);

        }

        // //Link between pose in window and long window

        for (int i = 0; i < WINDOW_SIZE; i++)
        {
            for (int j = 1; j <WINDOW_SIZE_LONG; j++)
            {   
                unsigned neibLink = PsLong.size() + i-j;
                if (neibLink<PsLong.size())
                {
                    ceres::CostFunction* cost_function = LongWindowError::Create( PsLong.at(neibLink),Ps[i], RsLong.at(neibLink), Rs[i], 1);
                    problem.AddResidualBlock(cost_function, NULL, para_Pose_Long[neibLink], para_Pose[i]);
                }
            }
        }

        //cout<<" Opt with UWB  uwb_keymeas size "<<uwb_keymeas.size()<<" ; "<< endl;

        double uwbResidual = 0;
        for (int i = WINDOW_SIZE+WINDOW_SIZE_LONG-1; i >=0; i--)
        {
            //cout<<" For uwb meas i-1 "<< i <<" "<< uwb_keymeas.at(i) <<endl;
            if (uwb_keymeas.at(i)>1.5)
            {
                double avgWindow = 4;
                std::deque<double> temp_keymeas = uwb_keymeas;
                for (int k = 0; k<avgWindow/2; k++)
                {
                    temp_keymeas.push_front(uwb_keymeas.front());
                    temp_keymeas.push_back(uwb_keymeas.back());
                }

                int posID = i - WINDOW_SIZE_LONG; 

                if (posID >= 0)
                {
                    auto st = temp_keymeas.begin()+ i ;
                    auto ed = temp_keymeas.begin()+ i + avgWindow;

                    double sum = 0;
                    int nums = 0;
                    for( auto iit = st; iit < ed; iit ++)
                    {
                        if (*iit != -1)
                        {
                            sum+= *iit;
                            nums ++;
                        }
                    }
                    double average = sum/nums;
                    //cout<<"avg start "<<*st << " end "<< *ed << " sum "<< sum << " "<< nums << endl;

                    //double average = std::accumulate(st, ed, 0.0) / avgWindow;
                    // //double res = Ps[posID].norm()-uwb_keymeas.at(i);
                    //cout<<" Short window "<< i <<" "<< uwb_keymeas.at(i) << " avg "<< average<<" ; pos " << posID << " (" << para_Pose[posID][0]<<" " << para_Pose[posID][1]<<" " << para_Pose[posID][2]<< " ); " <<endl;
                    UWBFactor* uwb_factor = new UWBFactor(average,RANGE_W);
                    //UWBFactor* uwb_factor = new UWBFactor(uwb_keymeas.at(i),RANGE_W);
                    // //ceres::LossFunction *loss_function;
                    // //loss_function = new ceres::HuberLoss(2);
                    problem.AddResidualBlock(uwb_factor, NULL, para_Pose[posID], anchor_pos);  
                    double res = Ps[posID].norm()-average;
                    uwbResidual+=abs(res);

                }
                else
                {
                    int idx_in_long = i;

                    auto st = temp_keymeas.begin()+ i ;
                    auto ed = temp_keymeas.begin()+ i + avgWindow;


                    double sum = 0;
                    int nums = 0;
                    for( auto iit = st; iit < ed; iit ++)
                    {
                        if (*iit != -1)
                        {
                            sum+= *iit;
                            nums ++;
                        }
                    }
                    double average = sum/nums;

                    /*** Block following module to test algorithm without long window optimization ***/
                    // if ((idx_in_long%1)==0)
                    // {
                    //     //double res = PsLong[idx_in_long].norm()-uwb_keymeas.at(i);
                    //     double res = PsLong[idx_in_long].norm()-average;
                    //     //cout<<" Long window " << i <<" "<< uwb_keymeas[i]<< " avg " << average <<" ; pos ("<< idx_in_long << " " << para_Pose_Long[idx_in_long][0]<<" " << para_Pose_Long[idx_in_long][1]<<" " << para_Pose_Long[idx_in_long][2]<< " ); " <<endl;
                    //     UWBFactor* uwb_factor = new UWBFactor(average, RANGE_W);
                    //     //UWBFactor* uwb_factor = new UWBFactor(uwb_keymeas.at(i), RANGE_W);
                    //     //ceres::LossFunction *loss_function;
                    //     //loss_function = new ceres::CauchyLoss(1.0) ;//new ceres::HuberLoss(0.1);
                    //     problem.AddResidualBlock(uwb_factor, NULL, para_Pose_Long[idx_in_long], anchor_pos); 
                    //     uwbResidual+=abs(res);
                    // }
                    

                }
            }
        }
        //cout<<">>>>>>Before optimization: Total uwb residuals "<< uwbResidual <<" anchor pos "<< anchor_pos[0] <<" "<< anchor_pos[1] <<" "<< anchor_pos[2] <<" ;"<<endl; 
    }

	//添加视觉残差
    int f_m_cnt = 0;
    int feature_index = -1;
    for (auto &it_per_id : f_manager.feature)//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)) // 如果这个特征点被观测的次数大于等于2 并且首次观测到该特征点的帧小于滑动窗口倒数第3帧,这个特征点就可以建立一个残差
            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)
            {
                    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]);
            }
            f_m_cnt++;
        }
    }

    ROS_DEBUG("visual measurement count: %d", f_m_cnt);
    ROS_DEBUG("prepare for ceres: %f", t_prepare.toc());

    if(relocalization_info)
    {
        //printf("set relocalization factor! \n");
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);
        int retrive_feature_index = 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 start = it_per_id.start_frame;
            if(start <= relo_frame_local_index)
            {   
                while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id)
                {
                    retrive_feature_index++;
                }
                if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id)
                {
                    Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);
                    Vector3d pts_i = it_per_id.feature_per_frame[0].point;
                    
                    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                    problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
                    retrive_feature_index++;
                }     
            }
        }

    }

    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);
    cout << summary.BriefReport() << endl;
    ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
    ROS_DEBUG("solver costs: %f", t_solver.toc());

    double2vector();

    // calculate the residual of all uwb measurements
    if ( USE_UWB && PsLong.size() == (WINDOW_SIZE_LONG))
    {
        double uwbResidual = 0;

        for (int i = WINDOW_SIZE+WINDOW_SIZE_LONG-1; i >=0; i--)
        {
            //cout<<" For uwb meas i-1 "<< i <<" "<< uwb_keymeas.at(i) <<endl;
            if (uwb_keymeas.at(i)>1.5)
            {
                int posID = i - WINDOW_SIZE_LONG; 
                if (posID >= 0)
                {
                    double res = Ps[posID].norm()-uwb_keymeas.at(i);
                    uwbResidual+=abs(res);
                }
                else
                {
                    int idx_in_long = i;
                    if ((idx_in_long%1)==0)
                    {
                        double res = PsLong_result[idx_in_long].norm()-uwb_keymeas.at(i);
                        uwbResidual+=abs(res);
                    }
                }
            }
        }
        //cout<<"after optimization: Total uwb residuals "<< uwbResidual <<" anchor pos "<< anchor_pos[0] <<" "<< anchor_pos[1] <<" "<< anchor_pos[2] <<" ;"<<endl; 
    }

	//Step3：marg部分
    //1.把之前存的残差部分加进来
    //2.把与当前要marg掉的帧的所有相关残差项加进来,IMU,vision.
    //3.preMarginalize-> 调用Evaluate计算所有ResidualBlock的残差和雅克比,parameter_block_data是margniliazation中存参数块的容器
    //4.多线程构造Hx=b的结构,H是边缘化后的结果,First Estimate Jacobian,在X0处进行线性化,需要去看!!!!???????????????????????????
    //5.marg结束,调整参数块在下一次window中对应的位置
    TicToc t_whole_marginalization;
    if (marginalization_flag == MARGIN_OLD)
    {
        MarginalizationInfo *marginalization_info = new MarginalizationInfo();
        vector2double();
		//! 先验误差会一直保存，而不是只使用一次
        //! 如果上一次边缘化的信息存在
        //! 要边缘化的参数块是 para_Pose[0] para_SpeedBias[0] 以及 para_Feature[feature_index](滑窗内的第feature_index个点的逆深度)
      
        if (last_marginalization_info)
        {
            vector<int> drop_set;
            for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
            {// 查询last_marginalization_parameter_blocks中是首帧状态量的序号
                if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
                    last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
                    drop_set.push_back(i);
            }
            // construct new marginlization_factor，//! 构造边缘化的的Factor
            MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
            //添加上一次边缘化的参数块
            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                           last_marginalization_parameter_blocks,
                                                                           drop_set);

            marginalization_info->addResidualBlockInfo(residual_block_info);
        }

        {
            if (pre_integrations[1]->sum_dt < 10.0)//添加IMU的先验，只包含边缘化帧的IMU测量残差
            {
                IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
                ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
                                                                           vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
                                                                           vector<int>{0, 1});
                marginalization_info->addResidualBlockInfo(residual_block_info);
            }
        }

        {//添加视觉的先验，只添加起始帧是旧帧且观测次数大于2的Features
            int feature_index = -1;
            for (auto &it_per_id : f_manager.feature)//遍历滑窗内所有的Features
            {
                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))//Feature的观测次数不小于2次，且起始帧不属于最后两帧
                    continue;

                ++feature_index;

                int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;//只选择被边缘化的帧的Features
                if (imu_i != 0)
                    continue;

                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;//得到该Feature在起始下的归一化坐标
                    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());
                        ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function,
                                                                                        vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},
                                                                                        vector<int>{0, 3});
                        marginalization_info->addResidualBlockInfo(residual_block_info);
                    }
                    else
                    {
                        ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                        ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
                                                                                       vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
                                                                                       vector<int>{0, 3});
                        marginalization_info->addResidualBlockInfo(residual_block_info);
                    }
                }
            }
        }
		//将三个ResidualBlockInfo中的参数块综合到marginalization_info中
        //  计算所有ResidualBlock(残差项)的残差和雅克比,parameter_block_data是参数块的容器

        TicToc t_pre_margin;
        marginalization_info->preMarginalize();
        ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
        
        TicToc t_margin;
        marginalization_info->marginalize();
        ROS_DEBUG("marginalization %f ms", t_margin.toc());

        std::unordered_map<long, double *> addr_shift;
        for (int i = 1; i <= WINDOW_SIZE; i++)
        {
            addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
            addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
        }
        for (int i = 0; i < NUM_OF_CAM; i++)
            addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
        if (ESTIMATE_TD)
        {
            addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
        }
        vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);

        if (last_marginalization_info)
            delete last_marginalization_info;
        last_marginalization_info = marginalization_info;
        last_marginalization_parameter_blocks = parameter_blocks;
        
    }
    else //MARGIN_SECOND_NEW边缘化倒数第二帧
    //如果倒数第二帧不是关键帧
    //1.保留该帧的IMU测量,去掉该帧的visual,代码中都没有写.
    //2.premarg
    //3.marg
    //4.滑动窗口移动
    {
        if (last_marginalization_info &&
            std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))
        {

            MarginalizationInfo *marginalization_info = new MarginalizationInfo();
            vector2double();
            if (last_marginalization_info)
            {
                vector<int> drop_set;
                for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
                {
                    ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
                    if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])
                        drop_set.push_back(i);
                }
                // construct new marginlization_factor
                MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
                ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                               last_marginalization_parameter_blocks,
                                                                               drop_set);

                marginalization_info->addResidualBlockInfo(residual_block_info);
            }

            TicToc t_pre_margin;
            ROS_DEBUG("begin marginalization");
            marginalization_info->preMarginalize();
            ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());

            TicToc t_margin;
            ROS_DEBUG("begin marginalization");
            marginalization_info->marginalize();
            ROS_DEBUG("end marginalization, %f ms", t_margin.toc());
            
            std::unordered_map<long, double *> addr_shift;
            for (int i = 0; i <= WINDOW_SIZE; i++)
            {
                if (i == WINDOW_SIZE - 1)
                    continue;
                else if (i == WINDOW_SIZE)
                {
                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
                }
                else
                {
                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
                }
            }
            for (int i = 0; i < NUM_OF_CAM; i++)
                addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
            if (ESTIMATE_TD)
            {
                addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
            }
            
            vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
            if (last_marginalization_info)
                delete last_marginalization_info;
            last_marginalization_info = marginalization_info;
            last_marginalization_parameter_blocks = parameter_blocks;
            
        }
    }
    ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc());
    
    ROS_DEBUG("whole time for ceres: %f", t_whole.toc());
}

滑动窗口函数
实际滑动窗口的地方，如果第二最新帧是关键帧的话，那么这个关键帧就会留在滑动窗口中，时间最长的一帧和其测量值就会被边缘化掉；如果第二最新帧不是关键帧的话，则把这帧的视觉测量舍弃掉而保留IMU测量值在滑动窗口中,这样的策略会保证系统的稀疏性。这一部分跟后端非线性优化是一起进行的，这一部分对应的非线性优化的损失函数的先验部分。

void Estimator::slideWindow()
{
    TicToc t_margin;
    if (marginalization_flag == MARGIN_OLD)
    {
        double t_0 = Headers[0].stamp.toSec();
        back_R0 = Rs[0];
        back_P0 = Ps[0];
        if (frame_count == WINDOW_SIZE)
        {            
            for (int i = 0; i < WINDOW_SIZE; i++) // Swap equals to remove the oldest one.
            {
                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]);
            }
            // Manually change the newest position as same as the real newest one.
            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();

            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);

            }
            slideWindowOld();
        }
    }
    else // MARGIN_NEW
    {
        if (frame_count == WINDOW_SIZE)
        {
            for (unsigned int i = 0; i < dt_buf[WINDOW_SIZE].size(); i++)
            {
                double tmp_dt = dt_buf[WINDOW_SIZE][i];
                Vector3d tmp_linear_acceleration = linear_acceleration_buf[WINDOW_SIZE][i];
                Vector3d tmp_angular_velocity = angular_velocity_buf[WINDOW_SIZE][i];

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

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

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

            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();

            slideWindowNew();
        }
    }
}

// real marginalization is removed in optimization()
void Estimator::slideWindowNew()
{
    sum_of_front++;
    f_manager.removeFront(frame_count);

    if (USE_UWB)
    {
        uwb_keymeas.pop_back();
        //printf("slide window new-> size of uwb_keymeas %d \n", uwb_keymeas.size()); 
        if (KNOWN_ANCHOR == 0 && solver_flag == NON_LINEAR && uwbMeas4AnchorEst.size()>0)
        {
            uwbMeas4AnchorEst.pop_back();
        }

    }
}
// real marginalization is removed in optimization()
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];
        R1 = Rs[0] * ric[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();

    if (USE_UWB && uwb_keymeas.size() > WINDOW_SIZE_LONG+WINDOW_SIZE)
    {
        uwb_keymeas.pop_front();
    }

    if (USE_UWB && solver_flag == NON_LINEAR)
    {
        if (KNOWN_ANCHOR == 1)
        {
            PsLong.push_back(back_P0);
            RsLong.push_back(back_R0);
            //printf(" slide window old-> size of uwb_keymeas %d \n", uwb_keymeas.size());

            if (PsLong.size() > (WINDOW_SIZE_LONG))
            {
                PsLong.pop_front();
                RsLong.pop_front();
                //cout<< "SlideWindow Old, cur first Pos"<< back_P0[0]<<" "<< back_P0[1]<<" "<< back_P0[2]<< " uwb meas front "<< uwb_keymeas.front() <<" uwb meas end "<< uwb_keymeas.back() <<" wubmeas size " << uwb_keymeas.size()<<" ;"<<endl;
            }
            
        }
        else if(KNOWN_ANCHOR == 0)
        {
            pose4AnchorEst.push_back(Ps[WINDOW_SIZE - 1]);
        }       
    }        
}

锚点估计函数，同样在imageprocess函数中调用的。

void Estimator::estimateAnchorPos()
{
    if(USE_UWB && KNOWN_ANCHOR == 0 && pose4AnchorEst.size() > POS_SIZE_4_ANCHOR_EST && uwbMeas4AnchorEst.size()>POS_SIZE_4_ANCHOR_EST)
    {

        cout<<"------------NOW can do Anchor Estimation pose size "<<  pose4AnchorEst.size() <<"; uwb meas size  "<< uwbMeas4AnchorEst.size()<<" ;"<<endl; 
        
        ceres::Problem problem;
        ceres::Solver::Options options;
        options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
        //options.minimizer_progress_to_stdout = true;
        options.max_solver_time_in_seconds = 3;
        options.max_num_iterations = 50*3;
        ceres::Solver::Summary summary;
        // Add residual blocks, which are uwb factors.
        double uwbResidual;

        for(int idx = POS_SIZE_4_ANCHOR_EST; idx>0; idx--){
            double temprange = uwbMeas4AnchorEst.back();
            Eigen::Vector3d tempPose(pose4AnchorEst.back()[0],pose4AnchorEst.back()[1],pose4AnchorEst.back()[2]);

            uwbMeas4AnchorEst.pop_back();
            pose4AnchorEst.pop_back();

            if (temprange>0.5)
            {
                double res = tempPose.norm()-temprange;
                UWBAnchorFactor* anchor_factor = new UWBAnchorFactor(temprange, RANGE_W, tempPose);
                //ceres::LossFunction *loss_function
                //loss_function = new ceres::CauchyLoss(1.0) ;//new ceres::HuberLoss(0.1);

                problem.AddResidualBlock(anchor_factor, NULL, anchor_pos); 
                uwbResidual+=abs(res);
                cout << "range " << temprange<<" pos ("<<tempPose[0]<<", "<< tempPose[1]<<", "<<tempPose[2]<<" )"<<endl;
            }       
        }

        ceres::Solve(options, &problem, &summary);
        std::cout << summary.BriefReport()<< "\n";
        cout<< " Anchor "<< anchor_pos[0] << " "<< anchor_pos[1] << " "<< anchor_pos[2] << " " <<endl;

        KNOWN_ANCHOR = 1;

        Eigen::Vector3d eigen_anchor(anchor_pos[0],anchor_pos[1],anchor_pos[2]);
        ANCHOR_POS = eigen_anchor;
    }
}

小结

optimization函数是基于滑动窗口的优化方法最直接的体现，通过processImage函数在process线程中调用。要想理解optimization函数需要对于滑动窗口BA优化的原理搞清楚，对着公式原理，对着VINS的论文来看会好一些。VIR-SLAM是在VINS-mono的基础之上改来的，主要就是添加了uwb传感器进行优化限制vio的漂移，下一步希望根据这个代码进行一些论文的复现工作。