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的漂移，下一步希望根据这个代码进行一些论文的复现工作。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/news/187252.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

mysql 日志分析

mysql 日志分析

程序启动标志 可以直接全局搜索&#xff0c;查看启动了几次 可以看到总共11次&#xff0c;当前是第2次 如何判断mysql是正常关闭&#xff0c;手动启动的 下图中启动之前出现 Shutdown complete打印说明启动之前是正常关闭的
阅读更多...
Serilog .net下的新兴的日志框架

Serilog .net下的新兴的日志框架

Serilog .net下的新兴的日志框架 1.Serilog简介 Serilog 是针对 .NET 应用程序的流行日志记录框架。它以其灵活性、易用性和可扩展性而闻名。借助 Serilog&#xff0c;开发人员可以轻松记录应用程序中的事件、错误和消息。它支持结构化日志记录&#xff0c;能够以结构化格式存…
阅读更多...
贪心 55. 跳跃游戏 45.跳跃游戏 II

贪心 55. 跳跃游戏 45.跳跃游戏 II

55. 跳跃游戏 题目&#xff1a; 给定非负数组&#xff0c;初始位置在数组第一格&#xff0c;数组值是可以选择的最大跳跃步数&#xff0c;判断能不能达到数组末尾。 示例 1: * 输入: [2,3,1,1,4] * 输出: true * 解释: 我们可以先跳 1 步&#xff0c;从位置 0 到达 位置 1,…
阅读更多...
AWS EC2 如何 使用 SSM会话管理器登陆

AWS EC2 如何 使用 SSM会话管理器登陆

首先只有特定版本的OS会默认附带SSM Agent。 预安装了 SSM Agent 的 Amazon Machine Images&#xff08;AMIs&#xff09; - AWS Systems Manager 其次EC的instance role必须有一个叫“AmazonSSMManagedInstanceCore”的策略 如何给IAM User赋权&#xff0c;让他们可以使用SSM…
阅读更多...
appium ：输入框控件为android.view.View 时输入内容（如：验证码、密码输入框）

appium ：输入框控件为android.view.View 时输入内容（如：验证码、密码输入框）

问题背景 输入密码的组件信息为&#xff1a;<android.view.View resource-id“com.qq.ac.android:id/pwd_input”> 由于输入框控件是android.view.View&#xff0c;不是android.widget.EditText&#xff0c;所以只能点击&#xff0c;而启动appium后&#xff0c;会将输入…
阅读更多...
Vue语音播报，不用安装任何包和插件，直接调用。

Vue语音播报，不用安装任何包和插件，直接调用。

Vue语音播报功能可以通过使用浏览器提供的Web Speech API来实现。这个API允许你的应用程序通过浏览器朗读文本&#xff0c;不用安装任何包和插件&#xff0c;直接调用。以下是一个简单的介绍&#xff0c;演示如何在Vue中使用语音提示功能&#xff1a; 一、JS版本 <template…
阅读更多...
XTU OJ 1339 Interprime 学习笔记

XTU OJ 1339 Interprime 学习笔记

链接 传送门 代码 #include<bits/stdc.h> using namespace std;const int N1e610; //78498 我计算了一下&#xff0c;6个0的范围内有这么多个素数&#xff0c;所以开这么大的数组存素数 //计算的代码是一个循环 int prime[80000]; int a[N],s[N];//s数组是前缀和数组b…
阅读更多...
高等数学上岸宝典笔记

高等数学上岸宝典笔记

①不单调的函数也可能有反函数 ②注意反函数与函数转换时的定义域与值域 ③收敛数列不一定有最值 收敛数列必有上界和下界&#xff0c;但不一定有最值&#xff0c;比如{An}1/n&#xff0c;下界为0&#xff0c;但永远取不到0 ④数列与其子数列的关系 例题&#xff1a; ⑤带根号…
阅读更多...
Linux常用命令——cd命令

Linux常用命令——cd命令

文章目录 1. 简介2. 命令参数3. 常见用法与实例3.1 基本用法3.2 使用绝对路径或相对路径3.3 使用特殊字符3.4 使用参数 4. 总结 1. 简介 cd命令是Linux系统中最基础且频繁使用的命令之一&#xff0c;用于改变当前工作目录。它是“change directory”的缩写&#xff0c;对于任何…
阅读更多...
网狐类源码游戏配置数据库数据（一键配置网狐数据库）

网狐类源码游戏配置数据库数据（一键配置网狐数据库）

网狐类源码游戏配置数据库数据&#xff08;一键配置网狐数据库&#xff09; 一般拿到网狐的源码或组件&#xff0c;需要先附加或配置数据库&#xff0c;以下为全部需要更改数据的地方&#xff0c;这里以荣耀系列版本数据库为例&#xff1a; 1. 数据库设置 [RYPlatformDB].…
阅读更多...
Redis7--基础篇5（管道、发布订阅）

Redis7--基础篇5（管道、发布订阅）

管道是什么 管道(pipeline)可以一次性发送多条命令给服务端&#xff0c;服务端依次处理完完毕后&#xff0c;通过一条响应一次性将结果返回&#xff0c;通过减少客户端与redis的通信次数来实现降低往返延时时间。pipeline实现的原理是队列&#xff0c;先进先出特性就保证数据的…
阅读更多...
【驱动】SPI驱动分析(二)-SPI驱动框架

【驱动】SPI驱动分析(二)-SPI驱动框架

SPI驱动框架 SPI驱动属于总线-设备-驱动模型的&#xff0c;与I2C总线设备驱动模型相比&#xff0c;大体框架是一样&#xff0c;他们都是实际的总线。总体框架如下图所示&#xff1a; 从上到下&#xff0c;分为三层&#xff0c;用户空间&#xff0c;内核空间&#xff0c;和硬件…
阅读更多...
【驱动】串口驱动分析（一）-软件架构

【驱动】串口驱动分析（一）-软件架构

区分不同的终端类型 串行端口终端&#xff08;/dev/ttySn&#xff09; 串行端口终端&#xff08;Serial Port Terminal&#xff09;是使用计算机串行端口连接的终端设备。计算机把每个串行端口都看作是一个字符设备。 有段时间这些串行端口设备通常被称为终端设备&#xff0…
阅读更多...
vue项目下npm或yarn下安装echarts多个版本

vue项目下npm或yarn下安装echarts多个版本

最近在大屏展示的时候&#xff0c;用到了百度的echarts图表库&#xff0c;看完效果图后&#xff0c;又浏览了一下echarts官网案例&#xff0c;大同小异。但是搬砖过程中发现实际效果和demo相差甚远&#xff0c;一番折腾发现&#xff0c;项目中安装的是echarts4.x版本&#xff0…
阅读更多...
二进制求和

二进制求和

这篇文章会收录到 : 算法通关村第十三关-白银挑战数字与数学高频问题-CSDN博客 二进制求和 描述 : 给你两个二进制字符串 a 和 b &#xff0c;以二进制字符串的形式返回它们的和。 题目 : LeetCode 67.二进制求和 : 67. 二进制求和 分析 : 这个题也是用字符串来表示数据的…
阅读更多...
深入理解Zookeeper系列-1.初识Zoookeeper

深入理解Zookeeper系列-1.初识Zoookeeper

&#x1f44f;作者简介&#xff1a;大家好&#xff0c;我是爱吃芝士的土豆倪&#xff0c;24届校招生Java选手&#xff0c;很高兴认识大家&#x1f4d5;系列专栏&#xff1a;Spring源码、JUC源码、Kafka原理、分布式技术原理&#x1f525;如果感觉博主的文章还不错的话&#xff…
阅读更多...
设计模式-创建型模式之原型、建造者设计模式

设计模式-创建型模式之原型、建造者设计模式

文章目录 七、原型模式八、建造者模式 七、原型模式 原型模式&#xff08;Prototype Pattern&#xff09;是用于创建重复的对象&#xff0c;同时又能保证性能。它提供了一种创建对象的最佳方式。 这种模式是实现了一个原型接口&#xff0c;该接口用于创建当前对象的克隆。当直…
阅读更多...
Python实现FA萤火虫优化算法优化循环神经网络回归模型(LSTM回归算法)项目实战

Python实现FA萤火虫优化算法优化循环神经网络回归模型(LSTM回归算法)项目实战

说明&#xff1a;这是一个机器学习实战项目&#xff08;附带数据代码文档视频讲解&#xff09;&#xff0c;如需数据代码文档视频讲解可以直接到文章最后获取。 1.项目背景 萤火虫算法&#xff08;Fire-fly algorithm&#xff0c;FA&#xff09;由剑桥大学Yang于2009年提出 , …
阅读更多...
[操作系统] 文件管理

[操作系统] 文件管理

文章目录 5.1 磁盘调度算法1. 先来先服务算法( First Come First Served, FCFS) 算法2. 最短寻道时间优先算法( Shortest Seek Time First, SSTF) 算法3. 扫描算法( SCAN ) 算法4. 循环扫描算法( Circular Scan, CSCAN ) 算法5. LOOK 与 CLOOK 算法 5.2 进程写文件时&#xff0…
阅读更多...
禁奥义·SQL秘籍

禁奥义·SQL秘籍

sql secret scripts sql 语法顺序、执行顺序、执行过程、要点解析、优化技巧。 1、语法顺序 如上图所示&#xff0c;为 sql 语法顺序与执行顺序对照图。其具体含义如下&#xff1a; 0、select&#xff1a; 用于从数据库中选取数据&#xff0c;即表示从数据库中查询到的数据的…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023