杭州网站快速备案,网站需求分析文档,潍坊网站建设方案书,安徽建设工程信息网文件前言
续接上一篇#xff0c;本本篇接着介绍VIR-SLAM中estimator.cpp/.h文件的函数#xff0c;尤其是和UWB相关的相比于VINS改动过的函数#xff0c;仍然以具体功能情况代码注释的形式进行介绍。
重点函数介绍
优化函数#xff0c;代码是先优化#xff0c;后边缘化。
…前言
续接上一篇本本篇接着介绍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 (neibLinkPsLong.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_SIZEWINDOW_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::dequedouble temp_keymeas uwb_keymeas;for (int k 0; kavgWindow/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;//coutavg 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;uwbResidualabs(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); // uwbResidualabs(res);// }}}}//coutBefore 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_castint(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_SIZEWINDOW_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);uwbResidualabs(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);uwbResidualabs(res);}}}}//coutafter optimization: Total uwb residuals uwbResidual anchor pos anchor_pos[0] anchor_pos[1] anchor_pos[2] ;endl; }//Step3marg部分//1.把之前存的残差部分加进来//2.把与当前要marg掉的帧的所有相关残差项加进来,IMU,vision.//3.preMarginalize- 调用Evaluate计算所有ResidualBlock的残差和雅克比,parameter_block_data是margniliazation中存参数块的容器//4.多线程构造Hxb的结构,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){vectorint drop_set;for (int i 0; i static_castint(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,vectordouble *{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},vectorint{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,vectordouble *{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},vectorint{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,vectordouble *{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},vectorint{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_maplong, double * addr_shift;for (int i 1; i WINDOW_SIZE; i){addr_shift[reinterpret_castlong(para_Pose[i])] para_Pose[i - 1];addr_shift[reinterpret_castlong(para_SpeedBias[i])] para_SpeedBias[i - 1];}for (int i 0; i NUM_OF_CAM; i)addr_shift[reinterpret_castlong(para_Ex_Pose[i])] para_Ex_Pose[i];if (ESTIMATE_TD){addr_shift[reinterpret_castlong(para_Td[0])] para_Td[0];}vectordouble * 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){vectorint drop_set;for (int i 0; i static_castint(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_maplong, 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_castlong(para_Pose[i])] para_Pose[i - 1];addr_shift[reinterpret_castlong(para_SpeedBias[i])] para_SpeedBias[i - 1];}else{addr_shift[reinterpret_castlong(para_Pose[i])] para_Pose[i];addr_shift[reinterpret_castlong(para_SpeedBias[i])] para_SpeedBias[i];}}for (int i 0; i NUM_OF_CAM; i)addr_shift[reinterpret_castlong(para_Ex_Pose[i])] para_Ex_Pose[i];if (ESTIMATE_TD){addr_shift[reinterpret_castlong(para_Td[0])] para_Td[0];}vectordouble * 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){mapdouble, 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 (mapdouble, 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_LONGWINDOW_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; idx0; idx--){double temprange uwbMeas4AnchorEst.back();Eigen::Vector3d tempPose(pose4AnchorEst.back()[0],pose4AnchorEst.back()[1],pose4AnchorEst.back()[2]);uwbMeas4AnchorEst.pop_back();pose4AnchorEst.pop_back();if (temprange0.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); uwbResidualabs(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的漂移下一步希望根据这个代码进行一些论文的复现工作。
