BA理论解读
1. 基本原理
1.1 BA介绍
最小化重投影误差，获得最优的机器人位姿估计。bundle指的是光束，就是通过针孔相机模型获得的像素。重投影误差指的真实三维空间点在图像平面上的投影像素（真实值）和通过针孔相机模型计算得到的像素（估计值）差值。
这些东西归根结底就是Gauss“发明”的least squares method（最小二乘法）。当年天文学家Piazzi整天闲得没事看星星，在1801年1月1号早上发现了一个从来没观测到的星星，再接下来的42天里做了19次观测之后这个星星就消失了。当时的天文学家为了确定这玩意到底是什么绞尽了脑汁，这时候Gauss出现了，（最初）只用了3个观察数据，就用least squares算出了这个小行星的轨道，接下来天文学家根据Gauss的预测，也重新发现了这个小行星（虽然有小小的偏差），并将其命名为Ceres，也就是谷神星。Google的ceres-solver就是根据这个来命名的。
1.2 BA建模
相机投影模型如下，其中：$s_i$为比例参数；$K$为内参矩阵（形式如式2）；$\exp \left(\boldsymbol{\xi}^{\wedge}\right)$为李代数表示的外参，$\xi$是一个1×6的向量（为什么不直接用一个4×4的矩阵直接表示外参呢？）
其中：
令：
则
由第三行可得$s=Z^{‘}$,则重投影（估计的）像素坐标为
构建最小二乘问题，寻找最优相机位姿
使用李代数，我们构建了无约束的优化问题，很方便地通过高斯牛顿法、L-M方法等优化算法进行求解。不过，在使用高斯牛顿法和L-M方法之前，我们需要知道每个误差项关于优化变量的导数，也就是线性化：
令
则
构建最小二乘，对方程进行求导，得到下式（即我们常说的正规方程）
化简：
得到增量方程
1
2
3
4
| 给定初始值 x0。
对于第 k 次迭代，求出当前的雅可比矩阵 J(x_k) 和误差 f(x_k)。
求解增量方程： H∆x_k = g。
若 ∆x_k 足够小，则停止。否则，令 x_{k+1} = x_k + ∆x_k，返回第 2 步。
|
这里的 $J$ 的形式是值得讨论的，甚至可以说是关键所在。
其中
上面两项相乘
另一方面，除了优化位姿，我们还希望优化特征点的空间位置。因此，需要讨论 e 关于空间点
P 的导数。所幸这个导数矩阵相对来说容易一些。仍利用链式法则，有：
上述方法适用于前端小型BA的实时求解。
对于大型BA的求解就不得不了解$H$矩阵的稀疏性。
2. 非线性最小二乘与因子图之间的联系
Dellaert, F.和 Kaess, M的论文Square Root SAM中揭示了因子图与非线性最小二乘之间紧密的联系。因子图是一个概率图形模型，它表示所有因子的联合概率分布
如果考虑每个因子在$f_i(x_i)$上具有高斯分布且协方差$Σ_i$的情况，
那么MAP推断是
等式中的MAP推理问题被转换成上面提到的形式相同的非线性最小二乘优化问题。可以按照前一节中相同的步骤解决。
使用因子图对SLAM中的非线性最小二乘问题建模有几个优点。因子图对问题的概率性质进行编码，并且可以轻松地可视化大多数SLAM问题的潜在稀疏性，因为大多数（如果不是全部）因子$x_i$都是很小的集合。
3. 代码实践（使用ceres）
参考
假设：使用针孔相机模型；相机旋转$R$，平移$t$，焦距$f$和两个径向畸变参数$k1$和$k2$。将3D点$X$投影到摄像机为参数$R，t，f，k1，k2$表示u的公式为：
1
2
3
4
5
| P = R * X + t（从世界坐标转换为相机坐标）
p = -P / P.z（归一化除法）
p'= f * r（p）* p（转换为像素坐标）
|
其中P.z是P的第三个（z）坐标。在最后一个方程中，r（p）是一个消除径向畸变的函数：
1
| r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4.
|
这样就给出了以像素为单位的投影，其中图像的原点是图像的中心，x轴的正指向右，y轴的正指向上方（此外，在相机坐标系中，z轴为正轴指向后方，因此相机向下看Z轴的负方向。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
| #include <cmath>
#include <cstdio>
#include <iostream>
#include "ceres/ceres.h"
#include "ceres/rotation.h"
class BALProblem {
public:
~BALProblem()
{
delete[] point_index_;
delete[] camera_index_;
delete[] observations_;
delete[] parameters_;
}
int num_observations() const { return num_observations_; }
const double* observations() const { return observations_; }
double* mutable_cameras() { return parameters_; }
double* mutable_points() { return parameters_ + 9 * num_cameras_; }
double* mutable_camera_for_observation(int i)
{
return mutable_cameras() + camera_index_[i] * 9;
}
double* mutable_point_for_observation(int i) {
return mutable_points() + point_index_[i] * 3;
}
bool LoadFile(const char* filename) {
FILE* fptr = fopen(filename, "r");
if (fptr == nullptr) {
return false;
};
FscanfOrDie(fptr, "%d", &num_cameras_);
FscanfOrDie(fptr, "%d", &num_points_);
FscanfOrDie(fptr, "%d", &num_observations_);
point_index_ = new int[num_observations_];
camera_index_ = new int[num_observations_];
observations_ = new double[2 * num_observations_];
num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
parameters_ = new double[num_parameters_];
for (int i = 0; i < num_observations_; ++i) {
FscanfOrDie(fptr, "%d", camera_index_ + i);
FscanfOrDie(fptr, "%d", point_index_ + i);
for (int j = 0; j < 2; ++j) {
FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
}
}
for (int i = 0; i < num_parameters_; ++i) {
FscanfOrDie(fptr, "%lf", parameters_ + i);
}
return true;
}
private:
template<typename T>
void FscanfOrDie(FILE *fptr, const char *format, T *value) {
int num_scanned = fscanf(fptr, format, value);
if (num_scanned != 1) {
LOG(FATAL) << "Invalid UW data file.";
}
}
int num_cameras_;
int num_points_;
int num_observations_;
int num_parameters_;
int* point_index_;
int* camera_index_;
double* observations_;
double* parameters_;
};
struct SnavelyReprojectionError {
SnavelyReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
template <typename T>
bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
T xp = - p[0] / p[2];
T yp = - p[1] / p[2];
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = 1.0 + r2 * (l1 + l2 * r2);
const T& focal = camera[6];
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp;
residuals[0] = predicted_x - observed_x;
residuals[1] = predicted_y - observed_y;
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)));
}
double observed_x;
double observed_y;
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
if (argc != 2) {
std::cerr << "usage: simple_bundle_adjuster <bal_problem>\n";
return 1;
}
BALProblem bal_problem{};
if (!bal_problem.LoadFile(argv[1])) {
std::cerr << "ERROR: unable to open file " << argv[1] << "\n";
return 1;
}
const double* observations = bal_problem.observations();
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
ceres::CostFunction* cost_function =
SnavelyReprojectionError::Create(observations[2 * i + 0],
observations[2 * i + 1]);
problem.AddResidualBlock(cost_function,
nullptr ,
bal_problem.mutable_camera_for_observation(i),
bal_problem.mutable_point_for_observation(i));
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
return 0;
}
|
数据集：BAL数据集，来源于 Building Rome in a Day项目。使用到的数据中包含16台相机拍摄到的22106特征点
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
| 数据集部分数据，其按下面格式排列
<num_cameras> <num_points> <num_observations>
<camera_index_1> <point_index_1> <x_1> <y_1>
16 22106 83718
0 0 -3.859900e+02 3.871200e+02
1 0 -3.844000e+01 4.921200e+02
2 0 -6.679200e+02 1.231100e+02
7 0 -5.991800e+02 4.079300e+02
12 0 -7.204300e+02 3.143400e+02
13 0 -1.151300e+02 5.548999e+01
0 1 3.838800e+02 -1.529999e+01
1 1 5.597500e+02 -1.061500e+02
10 1 3.531899e+02 1.649500e+02
0 2 5.915500e+02 1.364400e+02
1 2 8.638600e+02 -2.346997e+01
2 2 4.947200e+02 1.125200e+02
6 2 4.087800e+02 2.846700e+02
7 2 4.246100e+02 3.101700e+02
9 2 2.848900e+02 1.928900e+02
10 2 5.826200e+02 3.637200e+02
12 2 4.940601e+02 2.939500e+02
13 2 7.968300e+02 -7.853003e+01
15 2 7.798900e+02 4.082500e+02
|
使用DENSE_SCHUR方法求解
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
| iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 4.185660e+06 0.00e+00 1.09e+08 0.00e+00 0.00e+00 1.00e+04 0 7.20e+00 7.90e+00
......
6 1.803390e+04 9.02e-02 6.35e+01 8.00e-01 1.00e+00 2.50e+06 1 3.28e+01 2.03e+02（约合3.4min）
Original Reduced
Parameter blocks 22122 22122
Parameters 66462 66462
Residual blocks 83718 83718
Residuals 167436 167436
Minimizer TRUST_REGION
Dense linear algebra library EIGEN
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver DENSE_SCHUR DENSE_SCHUR
Threads 1 1
Linear solver ordering AUTOMATIC 22106,16
Schur structure 2,3,9 2,3,9
Cost:
Initial 4.185660e+06
Final 1.803390e+04
Change 4.167626e+06
Minimizer iterations 7
Successful steps 7
Unsuccessful steps 0
Time (in seconds):
Preprocessor 0.700777
Residual only evaluation 0.667742 (7)
Jacobian & residual evaluation 39.360399 (7)
Linear solver 183.064203 (7)
Minimizer 229.831484
Postprocessor 0.014315
Total 230.546577
Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.769759e-09 <= 1.000000e-06)
|
使用SPARSE_SCHUR求解
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
| iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 4.185660e+06 0.00e+00 1.09e+08 0.00e+00 0.00e+00 1.00e+04 0 6.34e+00 6.86e+00
......
6 1.803390e+04 9.02e-02 6.35e+01 8.00e-01 1.00e+00 2.50e+06 1 3.30e+01 2.05e+02（约合3.4min）
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 4.185660e+06
Final 1.803390e+04
Change 4.167626e+06
Minimizer iterations 7
Successful steps 7
Unsuccessful steps 0
Time (in seconds):
Preprocessor 0.524276
Residual only evaluation 0.655267 (7)
Jacobian & residual evaluation 38.889743 (7)
Linear solver 185.728749 (7)
Minimizer 231.991205
Postprocessor 0.010115
Total 232.525597
Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.769757e-09 <= 1.000000e-06)
|
使用SPARSE_NORMAL_CHOLESKY方法求解
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
| iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 4.185660e+06 0.00e+00 1.09e+08 0.00e+00 0.00e+00 1.00e+04 0 6.46e+00 6.63e+00
......
6 1.803390e+04 9.02e-02 6.35e+01 8.00e-01 1.00e+00 2.50e+06 1 6.63e+00 4.68e+01（约合0.8min）
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_NORMAL_CHOLESKY SPARSE_NORMAL_CHOLESKY
Threads 1 1
Linear solver ordering AUTOMATIC 22122
Cost:
Initial 4.185660e+06
Final 1.803390e+04
Change 4.167626e+06
Minimizer iterations 7
Successful steps 7
Unsuccessful steps 0
Time (in seconds):
Preprocessor 0.173184
Residual only evaluation 0.591482 (7)
Jacobian & residual evaluation 38.593874 (7)
Linear solver 1.753153 (7)
Minimizer 47.594235
Postprocessor 0.009093
Total 47.776512
Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.769774e-09 <= 1.000000e-06)
|
使用DENSE_NORMAL_CHOLESKY求解
1
| terminate called after throwing an instance of 'std::bad_alloc'
|
提示超内存了……