SLAM中状态估计优化退化问题(degraded optimization problem)浅析和源码实现

参考论文 On degeneracy of optimization-based state estimation problems发表在2016 ICRA上

原理非常直观，用线性代数的基础知识。用Guassian-Newton，增量方程的Hessian matrix包含了求解该优化问题的退化信息。必须要求其为正定矩阵。

求解增量方程时，可以将Hessian matrix的eigen value， eigen vector先解出来。较小的eigen vector对应的eigen value就代表类退化方向。这样Hessian不为正定也可以求解了，退化的方向可由其他传感器补充。从而构建一个松耦合串连式多传感器位姿估计框架。

论文的算法流程为：

下面给出我用eigen的算法实现，具体实施如下：

一般我们解为 x_delta = -(JT*J).inv() *JT*b, 将x_delta， JT*J放到modifysolution即可。

//input:
//    eigThreshlod: for eigen value
//    matAtA: Hessian matrix for the normal equation
//    matX: original solution
//output:
//    matX: modified solution
bool modifySolution(double eigThreshlod , Eigen::Matrix<double, 6, 6> &matAtA,
Eigen::Matrix<double, 6, 1> &matX)
{Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>> es(matAtA);Eigen::Matrix<double, 3, 1> matE;// = es.eigenvalues();Eigen::Matrix<double, 3, 3> matV;// = es.eigenvectors();Eigen::Matrix<double, 3, 3> matV2;// = es.eigenvectors();for (int i = 2; i >= 0; i--){matE.row(i) = es.eigenvalues().row(2-i);matV.row(i) = es.eigenvectors().row(2-i);matV2.row(i) = es.eigenvectors().row(2-i); }isDegenerate = false;double eignThre[3] = {eigThreshlod, eigThreshlod, eigThreshlod};for (int i = 2; i >= 0; i--){if (matE(i, 0) < eignThre[i]) {for (int j = 0; j < 3; j++){matV2(i, j) = 0;}m_isDegenerate = true;} else break;}matP = matV.inverse() * matV2;if (isDegenerate) {Eigen::Matrix<double, 3, 1> matX2 = matX;matX = matP * matX2;}return !isDegenerate;
}

SLAM中状态估计优化退化问题(degraded optimization problem)浅析和源码实现相关推荐

SLAM中的退化问题
SLAM中的退化问题简介相关论文论文小结 Paper Ⅰ Paper Ⅱ Paper Ⅲ 总结参考简介最近在研究定位的退化方向的判断和处理,看了几篇关于SLAM退化问题中的论文,并做个小结 ...
中石油训练赛 - Fermat‘s Optimization Problem(Java高精度运算+二分)
题目大意:给出误差函数 ,现在给出 z 和 n,要求求出 x 和 y ,使得函数 F 的取值最小题目分析:首先数据范围是幂次级别的,1e5 的 10 次方,也就是 1e50 次方,需要用到大数,我用 ...
从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达小白:师兄师兄,最近我在看SLAM的优化算法,有种方法叫" ...
SLAM | 视觉SLAM中的后端：后端优化算法与建图模板
点击上方"AI算法修炼营",选择加星标或"置顶" 标题以下,全是干货前面的话前面系列一中我们介绍了,VSLAM 是利用多视图几何理论,根据相机拍摄的图像信息 ...
论文精读 | slam中姿态估计的图优化方法比较
一. 摘要对于位置环境中的自主导航问题,同步定位与建图(Simultaneous localization and mapping, SLAM)是一个非常重要的工具框架.根据SLAM字面含义可以得知 ...
SLAM中姿态估计的图优化方法比较（g2o/Ceres/GTSAM/SE-Sync）
编辑 | 深蓝AI 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取后台回复[SLAM综述]获取视觉SLAM.激光SLAM.RGBD-SLAM等多篇综述! 本 ...
SLAM中的BA优化
SLAM中的BA优化 SLAM中的BA优化,先根据相机模型和A,B图像特征匹配好的像素坐标,求出A图像上的像素坐标对应的归一化的空间点坐标,然后根据该空间点的坐标计算重投影到B图像上的像素坐标,重投影 ...
深度学习中的优化问题(Optimization)
文章目录 1. 优化中的挑战 1.1. 优化问题 1.2. 挑战 1.3. 解决方法 2. 优化算法 2.1. 优化算法的分类 2.1.1. 批量大小的选择 2.1.2. 学习率调整 2.1.3. 梯 ...
视觉SLAM总结——LSD SLAM中关键知识点总结
视觉SLAM总结--LSD SLAM中关键知识点总结视觉SLAM总结--LSD SLAM中关键知识点总结 1. LSD SLAM的创新点/关键点是什么? 2. LSD SLAM的整体框架是怎样的? ...

SLAM中状态估计优化退化问题(degraded optimization problem)浅析和源码实现

SLAM中状态估计优化退化问题(degraded optimization problem)浅析和源码实现相关推荐

最新文章

热门文章