VINS紧耦合优化公式及代码解析
1.首先确定待优化的状态变量
对应代码,优化参数为:
Vector3d Ps[(WINDOW_SIZE + )];(平移向量)
Vector3d Vs[(WINDOW_SIZE + )];(速度)
Matrix3d Rs[(WINDOW_SIZE + )];(旋转矩阵)
Vector3d Bas[(WINDOW_SIZE + )];(加速度计偏置)
Vector3d Bgs[(WINDOW_SIZE + )];(陀螺仪重力偏置)
Matrix3d ric[NUM_OF_CAM]; (camera->IMU)
Vector3d tic[NUM_OF_CAM];
还需要转换成ceres可以接受的参数数组,转换如下(在函数 Estimator::Vector2double()中)
for (int i = ; i <= WINDOW_SIZE; i++)
{
para_Pose[i][] = Ps[i].x();
para_Pose[i][] = Ps[i].y();
para_Pose[i][] = Ps[i].z();
Quaterniond q{Rs[i]};
para_Pose[i][] = q.x();
para_Pose[i][] = q.y();
para_Pose[i][] = q.z();
para_Pose[i][] = q.w(); para_SpeedBias[i][] = Vs[i].x();
para_SpeedBias[i][] = Vs[i].y();
para_SpeedBias[i][] = Vs[i].z(); para_SpeedBias[i][] = Bas[i].x();
para_SpeedBias[i][] = Bas[i].y();
para_SpeedBias[i][] = Bas[i].z(); para_SpeedBias[i][] = Bgs[i].x();
para_SpeedBias[i][] = Bgs[i].y();
para_SpeedBias[i][] = Bgs[i].z();
}
for (int i = ; i < NUM_OF_CAM; i++)
{
para_Ex_Pose[i][] = tic[i].x();
para_Ex_Pose[i][] = tic[i].y();
para_Ex_Pose[i][] = tic[i].z();
Quaterniond q{ric[i]};
para_Ex_Pose[i][] = q.x();
para_Ex_Pose[i][] = q.y();
para_Ex_Pose[i][] = q.z();
para_Ex_Pose[i][] = q.w();
}
double para_Pose[WINDOW_SIZE + ][SIZE_POSE];
double para_SpeedBias[WINDOW_SIZE + ][SIZE_SPEEDBIAS];
double para_Ex_Pose[NUM_OF_CAM][SIZE_POSE];
2. 向ceres中添加优化变量
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
3. 将优化量存入数组,代码如下,依次加入margin项,IMU项和视觉feature项. 每一项都是一个factor, 这是ceres的使用方法, 创建一个类继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式. 分别对应marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三个类.
a) 添加边缘化的残差项
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
b)添加IMU的residual
for (int i = ; i < WINDOW_SIZE; i++)
{
int j = i + ;
if (pre_integrations[j]->sum_dt > 10.0)
continue;
//!添加代价函数
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
//!注意在添加残差的组成部分,由前后两帧的[p,q,v,b]组成,在计算雅克比的时候[p,q](7),[v,b](9)分开计算
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
重点介绍IMUFactor类重写的Evaluate(),该函数定义了通过输入parameter计算residual。关键代码:
Eigen::Map<Eigen::Matrix<double, , >> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
主要计算在pre_integration->evaluate()函数中进行
Eigen::Matrix<double, , > residuals;
//! 对应参考文献[1]中的公式(12),求取α,β,θ的一阶近似
//! (3,9)
Eigen::Matrix3d dp_dba = jacobian.block<, >(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<, >(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<, >(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<, >(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<, >(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
//! 求取近似之后的残差,对应参考文献[1]中的公式(22),IMU Model
residuals.block<, >(O_P, ) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
residuals.block<, >(O_R, ) = * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<, >(O_V, ) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<, >(O_BA, ) = Baj - Bai;
residuals.block<, >(O_BG, ) = Bgj - Bgi;
return residuals;
对应公式如下:
c)添加视觉的residual
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);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[], para_Feature[feature_index]);
f_m_cnt++;
}
三个误差项的特点:
1)边缘化的residual:1个.
2)IMU的residual:WINDOW_SIZE个(总长度WINDOW_SIZE+1), 每相邻两个Pose之间一个IMU residual项.
3)视觉的residual:观测数大于2的特征, 首次观测与后面的每次观测之间各一个residual项.
VINS紧耦合优化公式及代码解析的更多相关文章
- JavaScript “跑马灯”抽奖活动代码解析与优化(二)
既然是要编写插件.那么叫做"插件"的东西肯定是具有的某些特征能够满足我们平时开发的需求或者是提高我们的开发效率.那么叫做插件的东西应该具有哪些基本特征呢?让我们来总结一下: 1.J ...
- 优化C/C++代码的小技巧
说明: 无意看到一篇小短文,猜测作者应该是一个图形学领域的程序员或专家,介绍了在光线(射线)追踪程序中是如何优化C/C++代码的.倒也有一些参考意义,当然有的地方我并不赞同或者说我也不完全理解,原文在 ...
- Kakfa揭秘 Day8 DirectKafkaStream代码解析
Kakfa揭秘 Day8 DirectKafkaStream代码解析 今天让我们进入SparkStreaming,看一下其中重要的Kafka模块DirectStream的具体实现. 构造Stream ...
- MYSQL常见出错mysql_errno()代码解析
如题,今天遇到怎么一个问题, 在理论上代码是不会有问题的,但是还是报了如上的错误,把sql打印出來放到DB中却可以正常执行.真是郁闷,在百度里面 渡 了很久没有相关的解释,到时找到几个没有人回复的 & ...
- 优化C/C++代码的小技巧(转)
源:http://www.cnblogs.com/lizhenghn/p/3969531.html 说明: 无意看到一篇小短文,猜测作者应该是一个图形学领域的程序员或专家,介绍了在光线(射线)追踪程序 ...
- [代码]解析nodejs的require,吃豆人的故事
最近在项目中需要对nodejs的require关键字做解析,并且替换require里的路径.一开始我希望nodejs既然作为脚本语言,内核提供一个官方的parser库应该是一个稳定可靠又灵活的渠道,然 ...
- 解析数学表达式 代码解析AST语法树
2019年2月20日09:18:22 AST语法树自己写代码解析的话就比较麻烦,有现成的库可以解析PHP,就像webpack就是自己解析js的语法代码,编译成各种版本的可用代码 github http ...
- java代码解析二维码
java代码解析二维码一般步骤 本文采用的是google的zxing技术进行解析二维码技术,解析二维码的一般步骤如下: 一.下载zxing-core的jar包: 二.创建一个BufferedImage ...
- Java生鲜电商平台-SpringCloud微服务架构中网络请求性能优化与源码解析
Java生鲜电商平台-SpringCloud微服务架构中网络请求性能优化与源码解析 说明:Java生鲜电商平台中,由于服务进行了拆分,很多的业务服务导致了请求的网络延迟与性能消耗,对应的这些问题,我们 ...
随机推荐
- 网络体系结构的概念 - 网络协议TCP - 红黑联盟
https://i.cnblogs.com/EditPosts.aspx?opt=1 网络体系结构的概念 计算机网络就是一组通过一定形式连接起来的计算机系统,它需要四个要素的支持,即通信线路和通信设 ...
- 郑州Day6
今天考了毕姥爷的一套题,差点保龄 题目 挺良心的一套题,至少我不用再搬一遍题面了 T1.B君的第一题 我为什么当时去写了一个树形\(dp\)还妄图\(A\)掉啊 这题保龄感觉舒爽 首先如果我们要求的是 ...
- linux shell——zsh的安装与使用
Shell是在程序员与服务器间建立一个桥梁,它对外提供一系列命令,让我们得以控制服务器.常用的Bash就是Shell的一种,也是Linux下默认Shell程序.这里介绍一种更强大的.更人性化的Shel ...
- 用keytool制作证书并在tomcat配置https服务(四)
用keytool制作证书并在tomcat配置https服务(一) 用keytool制作证书并在tomcat配置https服务(二) 用keytool制作证书并在tomcat配置https服务(三) 上 ...
- 关于eclipse中引入项目报错或者没有JRE System Library问题(jre报错)或者jre1.7(8)改为jre1.8(7)等问题
解决方法: 右键项目工程-->>properties->>java bulid path -->>>libraries -->>add libra ...
- 【luoguP1238】【NOIP2014】生活大爆炸版剪刀石头布
生活大爆炸版剪刀石头布 ——[传送门] 这道题可以原原本本地说得上是一道水题了,通过判断两人的出拳不同给分然后统计输出.就是对于游戏得分 ...
- 重写viewWillAppear 和 viewWillDisAppear时[super viewWillAppear] 和 [super viewWillDisappear]的调用位置
参考网址:https://stackoverflow.com/questions/3906704/when-should-i-call-super 在写代码的过程中如果重写了viewWillAppea ...
- vue进阶语法及生命周期函数
1.calss和style绑定 操作元素的 class 列表和内联样式style是数据绑定的一个常见需求,它两都是属性,所以可以通过v-bind来绑定 1.1绑定HTML class 可以给v-bin ...
- HTML5--应用网页模板
因为刚开始写博客,只想着把知识点记录在这,也想给你们一些参考,在布局上有些没有思考太多;回过头来看,实在是不忍直视,对不住之前阅读的100+,既然昨天的事无法挽回,那就从现在开始从新整改吧!也希望大家 ...
- 全文搜索引擎 Elasticsearch 安装踩坑记录
一.安装 Elastic 需要 Java 8 环境.如果你的机器还没安装 Java 安装完 Java,就可以跟着官方文档安装 Elastic.直接下载压缩包比较简单. $ wget https://a ...