vins-mono后端优化

预积分量约束第i，j帧的残差计算：

// 计算和给定相邻帧状态量的残差Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj){Eigen::Matrix<double, 15, 1> residuals;Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(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;residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;residuals.block<3, 1>(O_BA, 0) = Baj - Bai;residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;return residuals;}

由于更新bias会导致预积分量也发生变化，而重新积分非常复杂，所以对于预积分量直接在i时刻的bias附近用一阶泰勒展开来近似，而不用真的迭代计算。

vins-mono后端优化相关推荐

基于ceres的后端优化的代码实现
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达来源丨从零开始搭SLAM 作者丨李太白lx 由于g2o天然是进行位姿图优化的, 所以十分契合karto ...
VINS-Mono关键知识点总结——预积分和后端优化IMU部分
VINS-Mono关键知识点总结--预积分和后端优化IMU部分 VINS-Mono关键知识点总结--预积分和后端优化IMU部分 1. 预积分的理论推导 2. 预积分的代码分析 3. 后端优化IMU部分 ...
在 VSLAM 的后端优化中的重投影误差的雅可比计算详细推导
对于相机位姿的变换可以通过旋转矩阵或者四元数进行表示,对于旋转矩阵的定义满足: R{∣R∣=1RRT=IR \begin{cases} |R| = 1 \\ RR^T = I\\ \end{cases ...
【SLAM】LIO-SAM解析——后端优化mapOptimization(5)
系列文章链接: [SLAM]LIO-SAM解析--流程图(1) [SLAM]LIO-SAM解析--数据预处理imageProjection(2) [SLAM]LIO-SAM解析--特征提取featur ...
SLAM之小觅相机跑开源方案（ORB_SLAM2，VINS MONO，VINS FUSION，RTAB-Map）
传感器: 小觅相机标准版开源SLAM方案: ORB_SLAM2,VINS MONO,VINS FUSION,RTAB-Map 测试地点: 室内大厅(光线不均)/ 露天阳台实现形式: 小觅相机 / ...
SLAM后端优化之-核函数
1.核函数作用:保证每条边的误差不会大的没边,掩盖掉其他的边在SLAM后端优化中,BA优化了所有的相机姿态和所有路标点,使用的最小化误差项作的二范数平方和作为目标函数:当我们的误差来源特别大的时候: ...
SLAM Cartographer（18）后端优化问题求解器
SLAM Cartographer(18)后端优化问题求解器 1. 全局优化 2. 提供数据 3. 后端优化器 4. 求解过程 4.1. 定义优化问题 4.1.1. 子图全局位姿 4.1.2. 节点全 ...
视觉SLAM笔记（54） Ceres 操作后端优化
视觉SLAM笔记(54) Ceres 操作后端优化 1. Ceres 求解 BA 2. 求解 1. Ceres 求解 BA g2o 用 Edges 来保存每一个代价函数,但 Ceres 却是用 Pro ...
后端优化 | VINS-Mono 论文公式推导与代码解析分讲
3. 后端优化(紧耦合) VIO 紧耦合方案的主要思路就是通过将基于视觉构造的残差项和基于IMU构造的残差项放在一起构造成一个联合优化的问题,整个优化问题的最优解即可认为是比较准确的状态估计. 为了限 ...
FAST_LIO_SAM 融入后端优化的FASTLIO SLAM 系统前端：FAST_LIO2 后端：LIO_SAM
FAST_LIO_SAM Front_end : fastlio2 Back_end : lio_sam Videos : FAST-LIO-SAM Bilibili_link Source code ...

vins-mono后端优化

vins-mono后端优化相关推荐

最新文章

热门文章