边缘化(marginalization )和稀疏化(sparsification)---ceres-solver
我拿港科技沈老师的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]);}elseROS_DEBUG("estimate extinsic param");}TicToc t_whole, t_prepare;
//把要优化的变量转成数组的形式vector2double();
//添加上一次边缘化的parameter blocksif (last_marginalization_info){// construct new marginlization_factorMarginalizationFactor *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;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());// 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_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){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_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];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());
}
边缘化(marginalization )和稀疏化(sparsification)---ceres-solver相关推荐
- 还在用全部token训练ViT?清华UCLA提出token的动态稀疏化采样,降低inference时的计算量...
关注公众号,发现CV技术之美 本文分享一篇由清华& UCLA联合研究的论文『DynamicViT: Effificient Vision Transformers with Dynamic T ...
- Ceres Solver: 高效的非线性优化库(二)实战篇
Ceres Solver: 高效的非线性优化库(二)实战篇 接上篇: Ceres Solver: 高效的非线性优化库(一) 如何求导 Ceres Solver提供了一种自动求导的方案,上一篇我们已经看 ...
- Ceres Solver从零开始手把手教学使用
目录 一 .简介 二.安装 三.介绍 四.Hello Word! 五.导数 1 数值导数 2解析求导 六.实践 Powell函数 一 .简介 笔者已经半年没有更新新的内容了,最近学习视觉SLAM的过程 ...
- 少即是多:视觉SLAM的点稀疏化(IROS 2022)
论文名称:Keeping Less is More: Point Sparsification for Visual SLAM 出处:IROS 2022 ,作者:Yeonsoo Park和Soohyu ...
- 不用GPU,稀疏化也能加速你的YOLOv3深度学习模型
水木番 发自 凹非寺 来自|量子位 你还在为神经网络模型里的冗余信息烦恼吗? 或者手上只有CPU,对一些只能用昂贵的GPU建立的深度学习模型"望眼欲穿"吗? 最近,创业公司Neur ...
- 清华鲁继文团队提出DynamicViT:一种高效的动态稀疏化Token的ViT
[导读] 由于随着ViT中的token数量的增长,会导致计算成本呈平方级急剧增加!近期,清华黄高团队提出了自适应序列长度的DVT方案,本篇文章,我们将介绍另一种方法.6月3日,清华鲁继文教授团队提出了 ...
- Google AI与Deepmind强强联合,加速神经网络稀疏化进程
来源:Google AI Blog 编辑:keyu [导读]优化神经网络的一个方法是稀疏化,然而,受到支持不足和工具缺乏的限制,该技术在生产中的使用仍然受限.为了解决这一问题,近日,Google联合D ...
- 深度学习- Dropout 稀疏化原理解析
搬运原文链接:https://zhuanlan.zhihu.com/p/38200980 深度学习中 Dropout 原理解析 文章目录 深度学习中 Dropout 原理解析 1. Dropout 简 ...
- Ceres Solver Document学习笔记
Ceres Solver Document学习笔记 Ceres Solver Document学习笔记 1. 基本概念 2. 基本方法 2.1 CostFunction 2.2 AutoDiffCos ...
- 【ArcGIS风暴】ArcGIS点云抽稀(稀疏化LAS点)详解案例教程
考虑对过采样 LAS 数据(例如通过摄影测量方式获得的点云以及多个叠加激光雷达扫描的返回值)使用稀疏化LAS点工具,以优化显示性能和加速分析操作. 文章目录 1. 创建las数据集 2. 稀疏化LAS ...
最新文章
- C语言解析pcap文件得到HTTP信息实例(原创,附源码)
- 从“创业输家”到“创智赢家”
- linux怎么关闭iptables linux如何关闭防火墙
- jquery 选择器大全的详细说明和实例
- 2019牛客暑期多校训练营(第七场)J A+B problem
- python怎么运行_程序员大牛讲解,Python程序的执行原理
- 【高校宿舍管理系统】第四章 创建前端项目以及完成登录页面
- Linux:Vim的安装与配置
- steam授权_听歌、看番、学习甚至开车...steam好像忘了自己是个游戏平台
- RPM打包技术与典型SPEC文件分析(转)
- 能源消耗总量计算公式_七、能源统计(21)
- plc输入/输出模块的选择
- 最新免费纯净版PE制作工具V2.1【更新说明】
- python中3个单引号,Pyhton3中单引号、双引号、三个引号的用法和区别
- Getting Real ——把握现实
- opencv_dnn模型部署学习记录
- Devexpress PdfViewer预览pdf,禁止下载,打印,复制
- 比较const ,readonly, stitac readonly
- 文字转语音离线html,web端文字转语音的几种方案
- 微信公众账号与网站信息对接
热门文章
- 使用vue-router却导致页面空白无法呈现-报错?
- java计算机毕业设计基于安卓/微信小程序的健身房健身管理系统
- DM8达梦数据库学习总结(上)
- Java网络爬虫(一)--使用HttpClient请求资源并抓取响应
- APP性能测试——内存测试
- ceph rbd mysql_Ceph 实战之 RBD
- 计算机最近被访问的文件夹,电脑复制文件夹提示“目标文件夹访问被拒绝”怎么办?[多图]...
- 11月总结#nobody
- 用GNS3制作路由交换网络拓扑图
- JS快速获取本周、本月时间区间的方法