目标：
1.理解后端的概念；
2.理解以EKF为代表的滤波器后端的工作原理；
3.理解非线性优化的后端，明白稀疏性是如何利用的；4.使用g2o和Ceres实际操作后端优化。
9.1 概述
9.1.1 状态估计的概率解释
1.后端优化引出
前段视觉里程计仅能给出一个短时间内的轨迹和地图，会造成误差累积；
后端优化（批量、渐进）解决长时间最优轨迹和地图，“批量的”不仅使用过去的信息更新自己的状态，也会用未来的信息来更新；“渐进的”指当前的状态只由过去的时刻决定，甚至只由前一个时刻决定。
2.运动和观测方程
SLAM过程可以由运动方程和观测方程来描述。假设在t=0到t=N到的时间内，有位姿
到
，并且有路标
。运动和观测方程为
（1）观测方程中，只有当
看到了
时，才会产生观测数据。事实上，在一个位置通常只能看到一小部分路标。而且，由于视觉SLAM特征点数量众多、所以实际中观测方程的数量会远远大于运动方程。
（2）可能没有测量运动的装置，也可能没有运动方程。在这个情况下，有若干种处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种方式都是可行的。在没有运动方程的情况下，整个优化问题就只由许多个观测方程组成。这就非常类似于SfM问题，相当于通过一组图像来恢复运动和结构。不同的是，SLAM中的图像有时间上的先后顺序，而SfM中允许使用完全无关的图像。
3.问题转化
每个方程都受噪声影响，所以要把这里的位姿和路标看成服从某种概率分布的随机变量，而不是单独的一个数。
因此，我们关心的问题就变成：当我们拥有某些运动数据和观测数据时，如何确定状态量的分布？进而，如果得到了新时刻的数据，它们的分布又将发生怎样的变化？
在比较常见且合理的情况下，假设状态量和噪声项服从高斯分布——这意味着在程序中只需要存储它们的均值和协方差矩阵即可。均值可看作对变量最优值的估计，而协方差矩阵则度量了它的不确定性。
那么，问题就转变为：当存在一些运动数据和观测数据时，我们如何估计状态量的高斯分布？
只有运动方程时，相当于我们蒙着眼睛在一个未知的地方走路。尽管我们知道自己每一步走了多远，但是随着时间流逝，我们越来越不确定自己的位置——内心也就越不安。这说明当输人数据受噪声影响时，误差是逐渐累积的，我们对位置方差的估计将越来越大。但若能不断地观测到外部场景，就会使得位置估计的不确定性变小。如果用椭圆或椭球直观地表达协方差阵，那么这个过程有点像是在手机地图软件中走路的感觉。
以下图为例，当没有观测数据时，这个圆会随着运动越来越大；而如果有正确的观测数据，圆就会缩小至一定的大小，保持稳定。
4.运算推导
在第6讲中，介绍了最大似然估计，提到批量状态估计问题可以转化为最大似然估计问题，并使用最小二乘法进行求解。如何将该结论应用于渐进式问题？在视觉SLAM 里，最小二乘法又有何特殊的结构？
（1）方程转换
首先，由于位姿和路标点都是待估计的变量，令
同时，把k时刻的所有观测记作。这里不会出现y，但我们心里要明白这时心中已经包含了之前的y
（2）通过过去的状态估计当前状态现在考虑第k时刻的情况。我们希望用过去0到k中的数据来估计现在的状态分布
下标0：k表示从0时刻到k时刻的所有数据。同时，
下面看如何对状态进行估计。按照贝叶斯法则，把
第一项称为似然，第二项称为先验。似然由观测方程给定，而先验部分，我们要明白当前状态是基于过去所有的状态估计得来的。至少，它会受影响，于是以
如果考虑更久之前的状态，也可以继续对此式进行展开。（二阶先验、三阶先验..）但现在我们只关心k时刻和k-1时刻的情况。至此，我们给出了贝叶斯估计，因为上式还没有具体的概率分布形式，所以没法实际操作它。对这一步的后续处理，方法上产生了一些分歧。
（3）卡尔曼滤波器与扩展卡尔曼滤波器
一种方法是假设马尔可夫性，简单的一阶马氏性认为，k时刻状态只与k-1时刻状态有关，而与再之前的无关。如果做出这样的假设，我们就会得到以扩展卡尔曼滤波(EKF)为代表的滤波器方法。在滤波方法中，我们会从某时刻的状态估计，推导到下一个时刻。
另一种方法是依然考虑k时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。目前，视觉SLAM的主流为非线性优化方法。
9.1.2 线性系统和KF
根据马尔科夫性，式子
简化为
考虑到k时刻的输入量
与k-1时刻的状态无关，所以我们把
可以看到，这一项实际上是k-1时刻的状态分布。
于是，这一系列方程说明，我们实际在做的是“如何把k-1时刻的状态分布推导至时刻”这样一件事。
也就是说，在程序运行期间，我们只要维护一个状态量，对它不断地进行迭代和更新即可。如果假设状态量服从高斯分布，那么我们只需考虑维护状态量的均值和协方差即可。
假设小萝卜上的定位系统一直在向外输出两个定位信息：一是自己的位姿，二是自己的不确定性。实际中往往也是如此。
1.线性高斯到卡尔曼滤波器
线性高斯系统是指运动方程和观测方程可以由线性方程来描述
假设所有的状态和噪声均满足高斯分布，记这里的噪声服从零均值高斯分布
利用马尔可夫性，假设我们知道了k-1时刻的后验（在k-1时刻看来）状态估计
及其协方差
，现在要根据k时刻的输入和观测数据，确定
表示后验，以下帽子
表示先验分布。
（1）
卡尔曼滤波器的第一步，通过运动方程确定
这一步称为预测(Predict)。它显示了如何从上一个时刻的状态，根据输入信息（但是有噪声R)推断当前时刻的状态分布。这个分布也就是先验。记
k时刻似然表达式
为了得到后验概率，我们想要计算它们的乘积。然而，虽然我们知道最后会得到一个关于
，那么
既然我们已经知道等式两侧都是高斯分布，那就只需比较指数部分，无须理会高斯分布前面的因子部分。指数部分很像是一个二次型的配方，我们来推导一下。首先把指数部分展开，有
为了求左侧的
和
定义中间变量
左右乘以
比较一次性系数，有
取系数并转置得
两侧乘以
2.小结
总而言之，上面的两个步骤可以归纳为“预测”和“更新”(Update)两个个步骤：
（1）预测
（2）计算卡尔曼增益
更新后验概率的分布
至此，已经推导了经典的卡尔曼滤波器的整个过程。我们使用从概率角度出发的最大后验概率估计的方式。
在线性高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计。而且，由于高斯分布经过线性变换后仍服从高斯分布，所以整个过程中我们没有进行任何的近似。可以说，卡尔曼滤波器构成了线性系统的最优无偏估计。
9.1.3 非线性系统和EKF
在理解了卡尔曼滤波之后，我们必须要澄清一点：SLAM中的运动方程和观测方程通常是非线性函数，尤其是视觉SLAM中的相机模型，需要使用相机内参模型及李代数表示的位姿，更不可能是一个线性系统。
一个高斯分布，经过非线性变换后，往往不再是高斯分布。所以在非线性系统中，我们必须取一定的近似，将一个非高斯分布近似成高斯分布。
1.扩展卡尔曼滤波器（非线性）
把卡尔曼滤波器的结果拓展到非线性系统中，称为扩展卡尔曼滤波器。通常的做法是，在某个点附近考虑运动方程及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。
令k-1时刻的均值与协方差矩阵为
这里的偏导数为
同样，对于观测方程，亦有
记这里偏导数为
那么，在预测步骤中，根据运动方程有
这些推导和卡尔曼滤波是十分相似的。为方便表述，记这里的先验和协方差的均值为
然后，考虑在观测中我们有
最后，根据最开始的贝叶斯展开式，可以推导出
的后验概率形式。我们略去中间的推导过程，只介绍其结果。读者可以仿照卡尔曼滤波器的方式，推导EKF的预测与更新方程。简而言之，我们会先定义一个卡尔曼增益
在卡尔曼增益的基础上，后验概率的形式为
卡尔曼滤波器给出了在线性化之后状态变量分布的变化过程。在线性系统和高斯噪声下，卡尔曼滤波器给出了无偏最优估计。而在SLAM这种非线性的情况下，它给出了单次线性近似下的最大后验估计。
2.EKF讨论
EKF形式简洁、应用广泛著称。当想要在某段时间内估计某个不确定量时，我们首先想到的就是EKF。在早期的SLAM中，EKF占据了很长一段时间的主导地位，研究者们讨论了各种各样的滤波器在SLAM中的应用，如IF(信息滤波器)、IKF(Iterated KF)、UKF(Unscented KF)和粒子滤波器、SWF(Sliding Window Filter)等等，或者用分治法等思路改进EKF的效率。时至今日，尽管我们认识到非线性优化比滤波器占有明显的优势，但是在计算资源受限，或待估计量比较简单的场合，EKF仍不失为一种有效的方式。
EKF有哪些局限呢?
（1）滤波器方法在一定程度上假设了马尔可夫性，也就是时刻的状态只与时刻相关，而与之前的状态和观测都无关(或者和前几个有限时刻的状态相关)。这有点像是在视觉里程计中只考虑相邻两帧的关系。如果当前帧确实与很久之前的数据有关(例如回环)，那么滤波器会难以处理。而非线性优化方法则倾向于使用所有的历史数据。它不光考虑邻近时刻的特征点与轨迹关系，更会把很久之前的状态也考虑进来，称为全体时间上的SLAM(Full-SLAM)。在这种意义下，非线性优化方法使用了更多信息，当然也需要更多的计算。
（2）与第6讲介绍的优化方法相比，EKF滤波器仅在处做了一次线性化，就直接根据这次线性化的结果，把后验概率给算了出来。这相当于在说，我们认为该点处的线性化近似在后验概率处仍然是有效的。而实际上，当我们离工作点较远时，一阶泰勒展开并不一定能够近似整个函数，这取决于运动模型和观测模型的非线性情况。如果它们有强烈的非线性，那么线性近似就只在很小范围内成立，不能认为在很远的地方仍能用线性来近似。这就是EKF的非线性误差，也是它的主要问题所在。在优化问题中，尽管我们也做一阶(最速下降)或二阶(高斯牛顿法或列文伯格一马夸尔特方法)的近似，但每迭代一次，状态估计发生改变之后，我们会重新对新的估计点做泰勒展开，而不像EKF那样只在固定点上做一次泰勒展开。这就使得优化的方法适用范围更广，在状态变化较大时也能适用。所以大体来说，可以粗略地认为EKF仅是优化中的一次迭代R。（3）从程序实现上来说，EKF需要存储状态量的均值和万差，并对它们进行维护和更新。如
果把路标也放进状态，由于视觉SLAM 中路标数量很大，则这个存储重是相当可观的，且与状态量呈平方增长(因为要存储协方差矩阵)。因此，普遍认为EKF SLAM个适用于大型场景。
（4）EKF等滤波器方法没有异常检测机制，导致系统在存在异常值的时候很容易发散。而在视觉SLAM 中，异常值却是很常见的：无论特征匹配还是光流法，都容易追踪或匹配到错误的点。没有异常值检测机制会让系统在实用中非常不稳定由于EKF存在这些明显的缺点，我们通常认为，在同等计算量的情况下，非线性优化能取得更好的效果。这里“更好”是指精度和鲁棒性同时达到更好的意思。下面我们来讨论以非线性优化为主的后端。
9.2 BA与图优化
所谓的Bundle Adjustment(BA)，是指从视觉图像中提炼出最优的3D模型和相机参数(内参数和外参数)。考虑从任意特征点发射出来的几束光线(bundles of light rays)，它们会在几个相机的成像平面上变成像素或是检测到的特征点。如果我们调整(adjustment)各相机姿态和各特征点的空间位置，使得这些光线最终收束到相机的光心，就称为BA。
我们在第5讲和第7讲已经简单介绍过BA的原理，本节的重点是介绍它对应的图模型结构的特点，然后介绍一些通用的快速求解方法。
9.2.1 投影模型和BA代价函数
1.投影过程
从一个世界坐标系中的点p出发，把相机的内外参数和畸变都考虑进来，最后投影成像素坐标，需要如下步骤：
（1）把世界坐标转换到相机坐标，这里将用到相机外参数（R,t）
（2）将
投至归一化平面，得到归一化坐标
（3）考虑归一化坐标的畸变情况，得到去畸变前的原始像素坐标。这里暂时只考虑径向畸变
（4）根据内参模型，计算像素坐标
这一系列计算流程看似复杂。我们用流程图（如下）表示整个过程。读者应该能领会到，这个过程也就是前面讲的观测方程，之前我们把它抽象地记成
现在，我们给出了它的详细参数化过程。具体地说，这里的x指代此时相机的位姿，即外参R,t，它对应的李群为T，李代数为。路标y即这里的三维点p，而观测数据则是像素坐标
。以最小二乘的角度来考虑，那么可以列写关于此次观测的误差
然后，把其他时刻的观测量也考虑进来，我们可以给误差添加一个下标。设
为在位姿
处观察路标
产生的数据，那么整体的代价函数为
对这个最小二乘进行求解，相当于对位姿和路标同时做了调整，也就是所谓的BA。接下来，我们会根据该目标函数和第6讲介绍的非线性优化内容，逐步深入地探讨该模型的求解。
9.2.2 BA的求解
观察9.2.1节中的观测模型h(T,p)，很容易判断该函数不是线性函数。所以我们希望使用6.2节介绍的一些非线性优化手段来优化它。
根据非线性优化的思想，我们应该从某个初始值开始，不断地寻找下降方向
，来找到目标函数的最优解，即不断地求解增量方程中的增量
本节用高斯牛顿法进行优化，高斯牛顿法的核心就是求解J矩阵，即代价函数对每个待优化变量求偏导数。增量方程中的
其中，表示整个代价函数在当前状态下对相机姿态的偏导数，而
表示该函数对路标点位置的偏导。现在，把相机位姿变量放在一起
并把空间点的变量也放在一起
那么，上式可简化为
需要注意的是，该式从一个由很多个小型二次项之和，变成矩阵形式。这里的雅可比矩阵和必须是整体目标函数对整体变量的导数，它将是一个很大块的矩阵，而里头每个小分块，需要由每个误差项的导数和拼凑”起来。然后，无论我们使用高斯牛顿法还是列文伯格一马夸尔特方法，最后都将面对增量线性方程
根据第6讲的知识，我们知道高斯牛顿法和列文伯格—马夸尔特方法的主要差别在于，这里的H是取
还是
的形式。由于我们把变量归类成了位姿和空间点两种，所以雅可比矩阵可以分块为
那么，以高斯牛顿法为例，则H矩阵为
当然，在列文伯格一马夸尔特方法中我们也需要计算这个矩阵。不难发现，因为考虑了所有的优化变量，所以这个线性方程的维度将非常大，包含了所有的相机位姿和路标点。尤其是在视觉SLAM中，一幅图像会提出数百个特征点，大大增加了这个线性方程的规模。如果直接对H求逆来计算增量方程，由于矩阵求逆是复杂度为的操作，那么消耗的计算资源会非常多。但这里的矩阵是有一定的特殊结构的。利用这个特殊结构，我们可以加速求解过程。
9.2.3 稀疏性和边缘化
H矩阵的稀疏性是由雅可比矩阵
引起的。考虑这些代价函数当中的其中一个
。注意，这个误差项只描述了在
这个误差项的雅可比矩阵，除了这两处为非零块，其余地方都为零。这体现了该误差项与其他路标和轨迹无关的特性。从图优化角度来说，这条观测边只和两个顶点有关。那么，它对增量方程有何影响？H矩阵为什么会产生稀疏性呢？
9.3 实践：CeresBA
1.bundle_adjustment_ceres.cpp
#include <iostream>
#include <ceres/ceres.h>
#include "common.h"
#include "SnavelyReprojectionError.h"
using namespace std;
void SolveBA(BALProblem &bal_problem);
int main(int argc, char **argv) {
if (argc != 2) {
cout << "usage: bundle_adjustment_ceres bal_data.txt" << endl;
return 1;
}
BALProblem bal_problem(argv[1]);
bal_problem.Normalize();
// bal_problem.Perturb(0.1, 0.5, 0.5);
bal_problem.WriteToPLYFile("initial.ply");
SolveBA(bal_problem);
bal_problem.WriteToPLYFile("final.ply");
return 0;
}
void SolveBA(BALProblem &bal_problem) {
const int point_block_size = bal_problem.point_block_size();
const int camera_block_size = bal_problem.camera_block_size();
double *points = bal_problem.mutable_points();
double *cameras = bal_problem.mutable_cameras();
// Observations is 2 * num_observations long array observations
// [u_1, u_2, ... u_n], where each u_i is two dimensional, the x
// and y position of the observation.
const double *observations = bal_problem.observations();
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
ceres::CostFunction *cost_function;
// Each Residual block takes a point and a camera as input
// and outputs a 2 dimensional Residual
cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);
// If enabled use Huber's loss function.
ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
// Each observation corresponds to a pair of a camera and a point
// which are identified by camera_index()[i] and point_index()[i]
// respectively.
double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
double *point = points + point_block_size * bal_problem.point_index()[i];
problem.AddResidualBlock(cost_function, loss_function, camera, point);
}
// show some information here ...
std::cout << "bal problem file loaded..." << std::endl;
std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
<< bal_problem.num_points() << " points. " << std::endl;
std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;
std::cout << "Solving ceres BA ... " << endl;
ceres::Solver::Options options;
options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
}
2.SnavelyReprojectionError.h
#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H
#include <iostream>
#include "ceres/ceres.h"
#include "rotation.h"
class SnavelyReprojectionError {
public:
SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x),
observed_y(observation_y) {}
template<typename T>
bool operator()(const T *const camera,
const T *const point,
T *residuals) const {
// camera[0,1,2] are the angle-axis rotation
T predictions[2];
CamProjectionWithDistortion(camera, point, predictions);
residuals[0] = predictions[0] - T(observed_x);
residuals[1] = predictions[1] - T(observed_y);
return true;
}
// camera : 9 dims array
// [0-2] : angle-axis rotation
// [3-5] : translateion
// [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion
// point : 3D location.
// predictions : 2D predictions with center of the image plane.
template<typename T>
static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions) {
// Rodrigues' formula
T p[3];
AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center fo distortion
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
// Apply second and fourth order radial distortion
const T &l1 = camera[7];
const T &l2 = camera[8];
T r2 = xp * xp + yp * yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
const T &focal = camera[6];
predictions[0] = focal * distortion * xp;
predictions[1] = focal * distortion * yp;
return true;
}
static ceres::CostFunction *Create(const double observed_x, const double observed_y) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
new SnavelyReprojectionError(observed_x, observed_y)));
}
private:
double observed_x;
double observed_y;
};
#endif // SnavelyReprojection.h
3.CMakeLists.txt
#声明要求cmake的最低版本
cmake_minimum_required(VERSION 3.16)
#声明一个cmake工程
project(slam_09)
#启用最高优化级别，移除调试开销
set(CMAKE_BUILD_TYPE "Release")
#并行计算，加速运算
#add_definitions("-DENABLE_SSE")
# 启用C++11
set(CMAKE_CXX_FLAGS "-O3 -std=c++11")
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
# 用标准方式指定 C++ 标准，别再手写 -std=...
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# 其它优化旗标可保留，但不要再含 -std=...
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -march=native -g")
# Ceres
Find_Package(Ceres REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# Sophus
set(EIGEN3_INCLUDE_DIR "/usr/local/include/eigen3")
include_directories(${EIGEN3_INCLUDE_DIR})
# g2o
list( APPEND CMAKE_MODULE_PATH /home/caohao/g2o/cmake_modules )
find_package(G2O REQUIRED)
SET(G2O_LIBS g2o_csparse_extension g2o_stuff g2o_core cxsparse)
include_directories(${G2O_INCLUDE_DIRS})
#Eigen
Find_Package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})
#CSparse
Find_Package(CSparse REQUIRED)
include_directories(${CSPARSE_INCLUDE_DIRS} /usr/include/suitesparse)
# 添加一个可执行程序，可执行文件名 cpp文件名
add_library(bal_common common.cc)
add_executable(bundle_adjustment_g2o bundle_adjustment_g2o.cpp)
target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} bal_common)
add_executable(bundle_adjustment_ceres bundle_adjustment_ceres.cpp)
target_link_libraries(bundle_adjustment_ceres ${CERES_LIBRARIES} bal_common)4.运行结果
caohao@ubuntu:~/桌面/slam/slam_09/build$ ./bundle_adjustment_ceres ../problem-16-22106-pre.txt
Header: 16 22106 83718
bal problem file loaded...
bal problem have 16 cameras and 22106 points.
Forming 83718 observations.
Solving ceres BA ...
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 5.449147e+12 0.00e+00 3.24e+26 0.00e+00 0.00e+00 1.00e+04 0 9.38e-02 2.93e-01
1 7.421971e+16 -7.42e+16 0.00e+00 4.29e+04 -2.72e+04 5.00e+03 1 1.36e-01 4.29e-01
2 3.837108e+17 -3.84e+17 0.00e+00 3.47e+04 -1.41e+05 1.25e+03 1 1.13e-01 5.42e-01
3 1.202275e+14 -1.15e+14 0.00e+00 2.00e+04 -4.21e+01 1.56e+02 1 1.10e-01 6.52e-01
4 2.094139e+12 3.36e+12 1.45e+26 7.57e+03 1.23e+00 4.69e+02 1 1.90e-01 8.42e-01
5 1.715858e+15 -1.71e+15 0.00e+00 3.56e+04 -1.64e+03 2.34e+02 1 1.23e-01 9.66e-01
6 1.822053e+24 -1.82e+24 0.00e+00 2.53e+04 -1.74e+12 5.86e+01 1 1.34e-01 1.10e+00
7 1.586121e+13 -1.38e+13 0.00e+00 1.33e+04 -1.31e+01 7.32e+00 1 1.25e-01 1.22e+00
8 7.984580e+11 1.30e+12 6.40e+25 8.71e+03 1.24e+00 2.20e+01 1 1.92e-01 1.42e+00
9 3.717661e+11 4.27e+11 2.72e+25 3.49e+04 1.07e+00 6.59e+01 1 1.83e-01 1.60e+00
10 1.397932e+11 2.32e+11 1.12e+25 1.54e+05 1.25e+00 1.98e+02 1 1.91e-01 1.79e+00
11 7.927556e+10 6.05e+10 4.65e+24 4.29e+05 8.66e-01 3.25e+02 1 2.01e-01 1.99e+00
12 2.985965e+10 4.94e+10 1.95e+24 8.55e+05 1.25e+00 9.76e+02 1 2.45e-01 2.24e+00
13 1.467261e+13 -1.46e+13 0.00e+00 2.13e+06 -9.81e+02 4.88e+02 1 1.43e-01 2.38e+00
14 4.564222e+11 -4.27e+11 0.00e+00 1.95e+06 -2.86e+01 1.22e+02 1 1.30e-01 2.51e+00
15 8.197420e+10 -5.21e+10 0.00e+00 1.47e+06 -3.49e+00 1.52e+01 1 1.33e-01 2.64e+00
16 6.408802e+11 -6.11e+11 0.00e+00 7.34e+05 -4.10e+01 9.53e-01 1 1.14e-01 2.76e+00
17 1.305787e+10 1.68e+10 1.04e+24 1.85e+05 1.16e+00 2.86e+00 1 1.82e-01 2.94e+00
18 2.524155e+10 -1.22e+10 0.00e+00 3.25e+05 -1.88e+00 1.43e+00 1 1.13e-01 3.05e+00
19 5.170160e+10 -3.86e+10 0.00e+00 2.21e+05 -6.02e+00 3.57e-01 1 1.10e-01 3.16e+00
20 6.704971e+09 6.35e+09 6.69e+23 8.71e+04 1.10e+00 1.07e+00 1 1.79e-01 3.34e+00
21 2.840951e+09 3.86e+09 3.83e+23 1.64e+05 1.19e+00 3.22e+00 1 1.82e-01 3.52e+00
22 4.867017e+09 -2.03e+09 0.00e+00 2.61e+05 -1.44e+00 1.61e+00 1 1.14e-01 3.64e+00
23 4.306863e+09 -1.47e+09 0.00e+00 1.80e+05 -1.06e+00 4.02e-01 1 1.15e-01 3.75e+00
24 6.444281e+10 -6.16e+10 0.00e+00 7.67e+04 -4.93e+01 5.03e-02 1 1.16e-01 3.87e+00
25 2.292929e+09 5.48e+08 1.81e+23 1.37e+04 1.01e+00 1.51e-01 1 1.78e-01 4.05e+00
26 1.428202e+12 -1.43e+12 0.00e+00 3.57e+04 -1.87e+03 7.54e-02 1 1.13e-01 4.16e+00
27 1.737359e+09 5.56e+08 1.47e+23 1.95e+04 1.02e+00 2.26e-01 1 1.82e-01 4.34e+00
28 1.026685e+09 7.11e+08 9.36e+22 4.82e+04 1.08e+00 6.79e-01 1 1.86e-01 4.53e+00
29 5.007847e+08 5.26e+08 8.52e+22 1.01e+05 1.12e+00 2.04e+00 1 1.85e-01 4.71e+00
30 7.339717e+08 -2.33e+08 0.00e+00 1.73e+05 -9.92e-01 1.02e+00 1 1.19e-01 4.83e+00
31 2.216996e+08 2.79e+08 8.18e+22 1.15e+05 1.22e+00 3.05e+00 1 1.82e-01 5.02e+00
32 1.344091e+08 8.73e+07 7.28e+22 1.97e+05 8.93e-01 5.93e+00 1 1.89e-01 5.20e+00
33 1.119116e+09 -9.85e+08 0.00e+00 2.59e+05 -1.79e+01 2.96e+00 1 1.17e-01 5.32e+00
34 6.409134e+07 7.03e+07 3.86e+22 1.72e+05 1.29e+00 8.89e+00 1 1.81e-01 5.50e+00
35 1.704850e+08 -1.06e+08 0.00e+00 2.87e+05 -5.34e+00 4.45e+00 1 1.14e-01 5.62e+00
36 1.387159e+20 -1.39e+20 0.00e+00 1.89e+05 -7.04e+12 1.11e+00 1 1.08e-01 5.72e+00
37 4.409479e+07 2.00e+07 2.51e+22 8.27e+04 1.07e+00 3.33e+00 1 1.80e-01 5.90e+00
38 7.478206e+07 -3.07e+07 0.00e+00 1.48e+05 -3.18e+00 1.67e+00 1 1.17e-01 6.02e+00
39 3.260413e+07 1.15e+07 3.60e+22 9.54e+04 1.23e+00 5.00e+00 1 1.86e-01 6.21e+00
40 3.340178e+07 -7.98e+05 0.00e+00 1.66e+05 -1.98e-01 2.50e+00 1 1.18e-01 6.33e+00
41 2.788577e+07 4.72e+06 2.89e+22 1.05e+05 1.21e+00 7.50e+00 1 1.79e-01 6.50e+00
42 3.434983e+10 -3.43e+10 0.00e+00 1.99e+05 -1.93e+04 3.75e+00 1 1.14e-01 6.62e+00
43 5.636097e+07 -2.85e+07 0.00e+00 1.25e+05 -1.71e+01 9.38e-01 1 1.18e-01 6.74e+00
44 2.624261e+07 1.64e+06 2.40e+22 5.11e+04 1.13e+00 2.81e+00 1 1.80e-01 6.92e+00
45 2.530990e+07 9.33e+05 2.54e+22 9.99e+04 1.12e+00 8.44e+00 1 2.03e-01 7.12e+00
46 2.482545e+07 4.84e+05 1.78e+22 1.86e+05 9.15e-01 1.97e+01 1 1.85e-01 7.30e+00
47 3.309190e+07 -8.27e+06 0.00e+00 3.28e+05 -1.76e+01 9.83e+00 1 1.17e-01 7.42e+00
48 2.467838e+07 1.47e+05 5.06e+20 1.99e+05 3.99e-01 9.75e+00 1 1.77e-01 7.60e+00
49 4.326292e+08 -4.08e+08 0.00e+00 1.98e+05 -1.19e+03 4.88e+00 1 1.17e-01 7.72e+00
50 2.512612e+07 -4.48e+05 0.00e+00 1.27e+05 -1.56e+00 1.22e+00 1 1.07e-01 7.82e+00
Solver Summary (v 2.0.0-eigen-(3.4.0)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)
Original Reduced
Parameter blocks 22122 22122
Parameters 66462 66462
Residual blocks 83718 83718
Residuals 167436 167436
Minimizer TRUST_REGION
Sparse linear algebra library SUITE_SPARSE
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver SPARSE_SCHUR SPARSE_SCHUR
Threads 1 1
Linear solver ordering AUTOMATIC 22106,16
Schur structure 2,3,9 2,3,9
Cost:
Initial 5.449147e+12
Final 2.467838e+07
Change 5.449122e+12
Minimizer iterations 51
Successful steps 24
Unsuccessful steps 27
Time (in seconds):
Preprocessor 0.199083
Residual only evaluation 1.030133 (50)
Jacobian & residual evaluation 1.535440 (24)
Linear solver 4.412791 (50)
Minimizer 7.626142
Postprocessor 0.005908
Total 7.831134
Termination: NO_CONVERGENCE (Maximum number of iterations reached. Number of iterations: 50.)
9.4 实践：g2o求解BA
1.bundle_adjustment_g2o.cpp
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <iostream>
#include "common.h"
#include "sophus/se3.hpp"
using namespace Sophus;
using namespace Eigen;
using namespace std;
/// 姿态和内参的结构
struct PoseAndIntrinsics {
PoseAndIntrinsics() {}
/// set from given data address
explicit PoseAndIntrinsics(double *data_addr) {
rotation = SO3d::exp(Vector3d(data_addr[0], data_addr[1], data_addr[2]));
translation = Vector3d(data_addr[3], data_addr[4], data_addr[5]);
focal = data_addr[6];
k1 = data_addr[7];
k2 = data_addr[8];
}
/// 将估计值放入内存
void set_to(double *data_addr) {
auto r = rotation.log();
for (int i = 0; i < 3; ++i) data_addr[i] = r[i];
for (int i = 0; i < 3; ++i) data_addr[i + 3] = translation[i];
data_addr[6] = focal;
data_addr[7] = k1;
data_addr[8] = k2;
}
SO3d rotation;
Vector3d translation = Vector3d::Zero();
double focal = 0;
double k1 = 0, k2 = 0;
};
/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPoseAndIntrinsics() {}
virtual void setToOriginImpl() override {
_estimate = PoseAndIntrinsics();
}
virtual void oplusImpl(const double *update) override {
_estimate.rotation = SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;
_estimate.translation += Vector3d(update[3], update[4], update[5]);
_estimate.focal += update[6];
_estimate.k1 += update[7];
_estimate.k2 += update[8];
}
/// 根据估计值投影一个点
Vector2d project(const Vector3d &point) {
Vector3d pc = _estimate.rotation * point + _estimate.translation;
pc = -pc / pc[2];
double r2 = pc.squaredNorm();
double distortion = 1.0 + r2 * (_estimate.k1 + _estimate.k2 * r2);
return Vector2d(_estimate.focal * distortion * pc[0],
_estimate.focal * distortion * pc[1]);
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};
class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPoint() {}
virtual void setToOriginImpl() override {
_estimate = Vector3d(0, 0, 0);
}
virtual void oplusImpl(const double *update) override {
_estimate += Vector3d(update[0], update[1], update[2]);
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};
class EdgeProjection :
public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
virtual void computeError() override {
auto v0 = (VertexPoseAndIntrinsics *) _vertices[0];
auto v1 = (VertexPoint *) _vertices[1];
auto proj = v0->project(v1->estimate());
_error = proj - _measurement;
}
// use numeric derivatives
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};
void SolveBA(BALProblem &bal_problem);
int main(int argc, char **argv) {
if (argc != 2) {
cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
return 1;
}
BALProblem bal_problem(argv[1]);
bal_problem.Normalize();
bal_problem.Perturb(0.1, 0.5, 0.5);
bal_problem.WriteToPLYFile("initial2.ply");
SolveBA(bal_problem);
bal_problem.WriteToPLYFile("final2.ply");
return 0;
}
void SolveBA(BALProblem &bal_problem) {
const int point_block_size = bal_problem.point_block_size();
const int camera_block_size = bal_problem.camera_block_size();
double *points = bal_problem.mutable_points();
double *cameras = bal_problem.mutable_cameras();
// pose dimension 9, landmark is 3
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;
typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
// use LM
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);
/// build g2o problem
const double *observations = bal_problem.observations();
// vertex
vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics;
vector<VertexPoint *> vertex_points;
for (int i = 0; i < bal_problem.num_cameras(); ++i) {
VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();
double *camera = cameras + camera_block_size * i;
v->setId(i);
v->setEstimate(PoseAndIntrinsics(camera));
optimizer.addVertex(v);
vertex_pose_intrinsics.push_back(v);
}
for (int i = 0; i < bal_problem.num_points(); ++i) {
VertexPoint *v = new VertexPoint();
double *point = points + point_block_size * i;
v->setId(i + bal_problem.num_cameras());
v->setEstimate(Vector3d(point[0], point[1], point[2]));
// g2o在BA中需要手动设置待Marg的顶点
v->setMarginalized(true);
optimizer.addVertex(v);
vertex_points.push_back(v);
}
// edge
for (int i = 0; i < bal_problem.num_observations(); ++i) {
EdgeProjection *edge = new EdgeProjection;
edge->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]);
edge->setVertex(1, vertex_points[bal_problem.point_index()[i]]);
edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1]));
edge->setInformation(Matrix2d::Identity());
edge->setRobustKernel(new g2o::RobustKernelHuber());
optimizer.addEdge(edge);
}
optimizer.initializeOptimization();
optimizer.optimize(40);
// set to bal problem
for (int i = 0; i < bal_problem.num_cameras(); ++i) {
double *camera = cameras + camera_block_size * i;
auto vertex = vertex_pose_intrinsics[i];
auto estimate = vertex->estimate();
estimate.set_to(camera);
}
for (int i = 0; i < bal_problem.num_points(); ++i) {
double *point = points + point_block_size * i;
auto vertex = vertex_points[i];
for (int k = 0; k < 3; ++k) point[k] = vertex->estimate()[k];
}
}
2.运行结果
