边缘化（marginalization ）和稀疏化(sparsification)---ceres-solver
来源：互联网 发布：饥荒mac版mod 编辑：程序博客网 时间：2024/06/05 19:37
我拿港科技沈老师的VINS中的BA优化来阐述ceres-solver怎么做边缘化和稀疏化.
代码如下：
void Estimator::optimization(){ ceres::Problem problem; ceres::LossFunction *loss_function; //loss_function = new ceres::HuberLoss(1.0); loss_function = new ceres::CauchyLoss(1.0);//添加需要优化的变量（camera的pose，imu的biases） for (int i = 0; i < WINDOW_SIZE + 1; i++) { ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization); problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS); }//添加camera和IMU的坐标变换的变量 for (int i = 0; i < NUM_OF_CAM; i++) { ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization); if (!ESTIMATE_EXTRINSIC) { ROS_DEBUG("fix extinsic param"); problem.SetParameterBlockConstant(para_Ex_Pose[i]); } else ROS_DEBUG("estimate extinsic param"); } TicToc t_whole, t_prepare;//把要优化的变量转成数组的形式 vector2double();//添加上一次边缘化的parameter blocks if (last_marginalization_info) { // construct new marginlization_factor MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info); problem.AddResidualBlock(marginalization_factor, NULL, last_marginalization_parameter_blocks); }//添加当前sliding window中的优化变量 for (int i = 0; i < WINDOW_SIZE; i++) { int j = i + 1; if (pre_integrations[j]->sum_dt > 10.0) continue; IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]); problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]); } int f_m_cnt = 0; int feature_index = -1; for (auto &it_per_id : f_manager.feature)//遍历sliding window的帧数 { it_per_id.used_num = it_per_id.feature_per_frame.size();//每一帧图像feature的个数 if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; ++feature_index; int imu_i = it_per_id.start_frame, imu_j = imu_i - 1; Vector3d pts_i = it_per_id.feature_per_frame[0].point; for (auto &it_per_frame : it_per_id.feature_per_frame)//遍历每一帧图像所有features { imu_j++; if (imu_i == imu_j) { continue; } Vector3d pts_j = it_per_frame.point; 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++; } } relocalize = false; //loop close factor//闭环检测，闭环这里就不说了 if(LOOP_CLOSURE) { int loop_constraint_num = 0; for (int k = 0; k < (int)retrive_data_vector.size(); k++) { for(int i = 0; i < WINDOW_SIZE; i++) { if(retrive_data_vector[k].header == Headers[i].stamp.toSec()) { relocalize = true; ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); problem.AddParameterBlock(retrive_data_vector[k].loop_pose, SIZE_POSE, local_parameterization); loop_window_index = i; loop_constraint_num++; 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 <= i) { while(retrive_data_vector[k].features_ids[retrive_feature_index] < it_per_id.feature_id) { retrive_feature_index++; } if(retrive_data_vector[k].features_ids[retrive_feature_index] == it_per_id.feature_id) { Vector3d pts_j = Vector3d(retrive_data_vector[k].measurements[retrive_feature_index].x, retrive_data_vector[k].measurements[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], retrive_data_vector[k].loop_pose, para_Ex_Pose[0], para_Feature[feature_index]); retrive_feature_index++; } } } } } } ROS_DEBUG("loop constraint num: %d", loop_constraint_num); } ROS_DEBUG("visual measurement count: %d", f_m_cnt); ROS_DEBUG("prepare for ceres: %f", t_prepare.toc()); ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_SCHUR;//线性求解类型 为什么是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;//选择schur //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()); // relative info between two loop frame if(LOOP_CLOSURE && relocalize)//计算闭环后的相对位姿 { for (int k = 0; k < (int)retrive_data_vector.size(); k++) { for(int i = 0; i< WINDOW_SIZE; i++) { if(retrive_data_vector[k].header == Headers[i].stamp.toSec()) { retrive_data_vector[k].relative_pose = true; Matrix3d Rs_i = Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix(); Vector3d Ps_i = Vector3d(para_Pose[i][0], para_Pose[i][1], para_Pose[i][2]); Quaterniond Qs_loop; Qs_loop = Quaterniond(retrive_data_vector[k].loop_pose[6], retrive_data_vector[k].loop_pose[3], retrive_data_vector[k].loop_pose[4], retrive_data_vector[k].loop_pose[5]).normalized().toRotationMatrix(); Matrix3d Rs_loop = Qs_loop.toRotationMatrix(); Vector3d Ps_loop = Vector3d( retrive_data_vector[k].loop_pose[0], retrive_data_vector[k].loop_pose[1], retrive_data_vector[k].loop_pose[2]); retrive_data_vector[k].relative_t = Rs_loop.transpose() * (Ps_i - Ps_loop); retrive_data_vector[k].relative_q = Rs_loop.transpose() * Rs_i; retrive_data_vector[k].relative_yaw = Utility::normalizeAngle(Utility::R2ypr(Rs_i).x() - Utility::R2ypr(Rs_loop).x()); if (abs(retrive_data_vector[k].relative_yaw) > 30.0 || retrive_data_vector[k].relative_t.norm() > 20.0) retrive_data_vector[k].relative_pose = false; } } } } double2vector();//边缘化操作 TicToc t_whole_marginalization;//沈老师这里边缘化有两个策略，如果在sliding window中第二近的frame是关键帧则丢弃sliding window中最老的帧、否则丢弃该帧。无论丢弃哪一帧，都需要边缘化。 if (marginalization_flag == MARGIN_OLD)//丢弃老的一帧。 { 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++) { 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 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) { 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);//添加优化变量 } } { int feature_index = -1; for (auto &it_per_id : f_manager.feature) { it_per_id.used_num = it_per_id.feature_per_frame.size(); if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue; ++feature_index; int imu_i = it_per_id.start_frame, imu_j = imu_i - 1; 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; 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);//添加优化变量 } } } 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]; 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 { 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]; 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());}阅读全文
1 0
- 边缘化（marginalization ）和稀疏化(sparsification)---ceres-solver
- Ceres solver
- Ceres-Solver库使用（二）--HelloWorld
- Ceres-Solver库入门
- Ceres solver tutorial
- Windows 配置 Ceres-solver
- VS2013 ceres-solver编译
- ceres-solver拟合椭球
- ceres solver 学习笔记
- ceres-solver使用
- ceres solver使用
- Ceres Solver for android
- Ceres Solver使用
- Ceres(5): Solver
- Ceres-Solver库使用（一）--windows下安装配置
- Ceres-Solver库使用（三）--导数(Derivatives)
- Ceres-Solver库使用（四）-- 一些例子
- ceres-solver库编译说明
- Android 定位2
- C语言中sprintf()函数的用法
- SVN版本管理平台(Subversion+Apache+Jsvnadmin)安装配置和使用
- 使用qt和cmakelists进行debug调试
- （四） tensorflow笔记：常用函数说明
- 边缘化（marginalization ）和稀疏化(sparsification)---ceres-solver
- Struts2的运行原理及简要剖析
- 自定义上传组件样式
- ContenType类型大全(包括Office2007文件等问题的解决办法)
- Hive编写UDF函数
- C语言二级指针做函数参数改变该指针的指向
- Android 中的Looper如何实现阻塞与唤醒的
- memcmp的问题
- Spring Boot访问mysql(JPA方式)最简单配置
