后端优化1（SLAM十四讲ch10）-BA

概述   

 视觉里程计可以给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，如果时间长了这个地图是不准确的。所以我们希望构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。实际当中考虑到精度与性能的平衡，有许多不同的做法。

     视觉里程计只有短暂的记忆，在后端优化中，我们通常考虑一段更长时间内（或所有时间内）的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新自己，这种处理方式不妨称为“批量的”（Batch）。否则，如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定，那不妨称为“渐进的”（Incremental）。所以这是一个假设检验的过程。先验和后验。先由以前的位姿和观测方程预测下一步，然后利用下一步的内容预测这一步的最大可能。

线性系统 KF

非线性系统 EKF

BA与图优化

所谓的Bundle Adjustment，是指从视觉重建中提炼出最优的3D模型和相机参数（内参数和外参数）。从每一个特征点反射出来的几束光线（bundles of light rays），在我们把相机位姿和特征点空间位置做出最优的调整（adjustment）之后，最后收束到相机光心的这个过程，简称BA。

Bundle Adjustment简述

在SFM（structure from motion）的计算中BA（Bundle Adjustment）作为最后一步优化具有很重要的作用，在近几年兴起的基于图的SLAM（simultaneous localization and mapping）算法里面使用了图优化替代了原来的滤波器，这里所谓的图优化其实也是指BA。其实很多经典的文献对于BA都有深深浅浅的介绍，如果想对BA的全过程做一个全面的更深层次的了解，推荐阅读 Bundle Adjustment —A Modern Synthesis。

Bundle Adjustment用BA代替中文译为光束法平差，大概大家看到更多的翻译可能为束调整、捆集调整或者捆绑调整等等。所谓bundle，来源于bundle of light，其本意就是指的光束，这些光束指的是三维空间中的点投影到像平面上的光束，而重投影误差，正是利用这些光束来构建的，因此称为光束法强调光束也正是描述其优化模型是如何建立的。剩下的就是平差，那什么是平差呢

由于测量仪器的精度不完善和人为因素及外界条件的影响，测量误差总是不可避免的。为了提高成果的质量，处理好这些测量中存在的误差问题，观测值的个数往往要多于确定未知量所必须观测的个数，也就是要进行多余观测。有了多余观测，势必在观测结果之间产生矛盾，测量平差的目的就在于消除这些矛盾而求得观测量的最可靠结果并评定测量成果的精度。测量平差采用的原理就是“最小二乘法”。

 BA的本质是一个优化模型，其目的是最小化重投影误差——》对PnP给出的结果进行优化

看！这些五颜六色的线就是我们讲的光束啦！那现在就该说下什么叫重投影误差了，重投影也就是指的第二次投影，那到底是怎么投影的呢？我们来整理一下吧：

• 其实第一次投影指的就是相机在拍照的时候三维空间点投影到图像上（像素）
• 然后我们利用这些图像对一些特征点进行三角定位（triangulation，很多地方翻译为三角化或者三角剖分等等，当然笔者最喜欢的还是三角定位，显然是利用几何信息构建三角形来确定三维空间点的位置嘛，相关内容请参考对极几何）
• 最后利用我们计算得到的三维点的坐标（注意不是真实的，像素三角化得到）和我们计算得到的相机矩阵（当然也不是真实的，PnP得到）进行第二次投影，也就是重投影

重投影误差是指的真实三维空间点在图像平面上的投影（也就是图像上的像素点）和重投影（其实是用我们的计算的3D点得到的虚拟的像素点）的差值，因为种种原因计算得到的值和实际情况不会完全相符，也就是这个差值不可能恰好为0，此时也就需要将这些差值的和最小化获取最优的相机参数及三维空间点的坐标。

BA求解方法（参考 Bundle Adjustment简述）

BA现在基本都是利用LM（Levenbrg-Marquardt）算法并在此基础上利用BA模型的稀疏性质来进行计算的，LM算法是最速下降法（梯度下降法）和Gauss-Newton的结合体，至于是怎么结合的接下来就来慢慢介绍了。

——>非线性最小二乘

• 最速下降发
• 牛顿法
• 高斯牛顿法
• 列文伯格-马夸尔特方法（LM）

——>解方程

• 稠密矩阵的最小二乘解
• 稀疏矩阵的Cholesky分解

H矩阵的洗稀疏性

schur消元（marginalization边缘化）

ceres实践

cd build 

./ceres_customBundle -input ../data/problem-16-22106-pre.txt
#see more detail settings by :
./ceres_customBundle -help

安装meshlab

1. sudo add-apt-repository ppa:zarquon42/meshlab  
2. sudo apt-get update  
3. sudo apt-get install meshlab  

使用meshLab打开initial.ply

使用meshLab打开final.ply

第九章 project中实现视觉里程计。

视觉里程计能够估算局部时间内的相机运动及特征点位置。（但是也有缺点：容易丢失，轨迹漂移）

default.yaml中为配置文件，包括数据位置

./bin/run_vo ./config/default.yaml