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}elseROS_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 windowfor (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 windowfor (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;elseoptions.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 measurementsif ( 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，//! 构造边缘化的的FactorMarginalizationFactor *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的Featuresint 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;//只选择被边缘化的帧的Featuresif (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_factorMarginalizationFactor *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);}elsef_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的漂移，下一步希望根据这个代码进行一些论文的复现工作。