-
《视觉 SLAM 十四讲》V2 第 9 讲 后端优化1 【扩展卡尔曼滤波器 EKF && BA+非线性优化(Ceres、g2o)】
第9讲 后端1
滤波器 EKF
前端视觉里程计： 短时间内的轨迹和地图。
后端优化： 长时间内的最优轨迹和地图9.1 概述
9.1.1 状态估计的概率解释渐进的(Incremental)： 当前的状态 只由 过去的时刻决定，甚至只由前一个决定。
批量的(Batch)：不仅使用过去的信息更新自己的状态，也会用未来的信息来更新。
SfM: Structure from Motion.
马尔可夫性： k k k 时刻状态仅与 k − 1 k-1 k−1 时刻状态 有关。 【扩展卡尔曼滤波(EKF)】
考虑 k k k 时刻状态 与 之前所有状态 的关系。 【非线性优化】- 视觉 SLAM 主流
9.1.2 线性系统和 KF
经典卡尔曼滤波：从概率角度出发的最大后验概率估计方式。
在线性高斯系统中，卡尔曼滤波器 构成了 该系统中的最大后验概率估计。
卡尔曼滤波器构成了线性系统的最优无偏估计。
9.1.3 非线性系统 和 EKF
把卡尔曼滤波器 的结果 扩展到 非线性系统中，
扩展卡尔曼滤波器。
9.1.4 扩展卡尔曼滤波器 EKF 不足
同等计算量的情况下， 非线性优化能取得更好的效果。
精度和鲁棒性更好。9.2 BA 与 图优化
视觉三维重建
Bundle Adjustment (BA)：从视觉图像中提炼出最优的3D模型和相机参数(内参和外差)。
考虑从任意特征点 发射出来的几束光线(bundles of light rays)， 它们会在几个相机的成像平面上变成像素或是检测到的特征点，如果我们调整(adjustment) 各相机姿态和各自特征点的空间位置，使得这些光纤最终收束到 相机的光心，称为BA其它译法： 光束法平差、捆集调整
9.2.1 投影模型和 BA 代价函数
9.2.2 BA 的求解
9.2.3 稀疏性 和 边缘化
Schur消元 Marginalization(边缘化)
9.2.4 鲁棒核函数
1、构造 BA 问题
2、设置 Schur 消元
3、调用 稠密或稀疏 矩阵求解器对变量进行优化9.3 实践： Ceres BA 【Code】
定义 投影误差模型
sudo apt-get install meshlab- 1
本讲 CMakeLists.txt
cmake_minimum_required(VERSION 2.8) set(CMAKE_CXX_STANDARD 17) project(bundle_adjustment) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-O3 -std=c++14") LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) Find_Package(G2O REQUIRED) Find_Package(Eigen3 REQUIRED) Find_Package(Ceres REQUIRED) Find_Package(Sophus REQUIRED) Find_Package(CSparse REQUIRED) SET(G2O_LIBS g2o_csparse_extension g2o_stuff g2o_core cxsparse) include_directories(${PROJECT_SOURCE_DIR} ${EIGEN3_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR}) add_library(bal_common common.cpp) add_executable(bundle_adjustment_ceres bundle_adjustment_ceres.cpp) target_link_libraries(bundle_adjustment_ceres ${CERES_LIBRARIES} bal_common) add_executable(bundle_adjustment_g2o bundle_adjustment_g2o.cpp) target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} g2o_solver_csparse g2o_csparse_extension ${Sophus_LIBRARIES} bal_common)- 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
mkdir build && cd build cmake .. make ./bundle_adjustment_ceres ../problem-16-22106-pre.txt- 1
- 2
- 3
- 4
byzanz-record -x 72 -y 64 -w 1848 -h 893 -d 20 --delay=5 -c /home/xixi/myGIF/test.gif- 1
byzanz-record -x 72 -y 64 -w 1848 -h 893 -d 30 --delay=5 -c /home/xixi/myGIF/test.gif- 1
bundle_adjustment_ceres.cpp#include#include #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"; } - 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
9.4 实践：g2o 求解 BA 【Code】
报错：/home/xixi/Downloads/slambook2-master/ch9/bundle_adjustment_g2o.cpp:10:10: fatal error: sophus/se3.hpp: No such file or directory 10 | #include "sophus/se3.hpp" | ^~~~~~~~~~~~~~~~- 1
- 2
- 3
- 4
SO3d 去掉 d 。 共3个
报错2：/usr/local/include/g2o/stuff/tuple_tools.h:41:46: error: ‘tuple_size_v’ is not a member of ‘std’; did you mean ‘tuple_size’? 41 | f, t, i, std::make_index_sequence<std::tuple_size_v<std::decay_t<T>>>());- 1
- 2
- 3
改动3：
第143-147行 更换成下述代码std::unique_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver (new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>()); std::unique_ptr<g2o::BlockSolverX> solver_ptr (new g2o::BlockSolverX(std::move(linearSolver))); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));- 1
- 2
- 3
- 4
其它问题： 链接库的问题， 看 CMakeLists.txt
cd build cmake .. make ./bundle_adjustment_g2o ../problem-16-22106-pre.txt- 1
- 2
- 3
- 4
byzanz-record -x 72 -y 64 -w 1848 -h 893 -d 30 --delay=5 -c /home/xixi/myGIF/test.gif- 1
byzanz-record -x 72 -y 64 -w 1848 -h 893 -d 20 --delay=5 -c /home/xixi/myGIF/test.gif- 1
bundle_adjustment_g2o.cpp#include#include #include #include #include #include #include #include "common.h" #include "sophus/se3.h" using namespace Sophus; using namespace Eigen; using namespace std; /// 姿态和内参的结构 struct PoseAndIntrinsics { PoseAndIntrinsics() {} /// set from given data address explicit PoseAndIntrinsics(double *data_addr) { rotation = SO3::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; } SO3 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 = SO3::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("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(); // pose dimension 9, landmark is 3 /*typedef g2o::BlockSolver (g2o::make_unique ()));*/ - 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
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 208
- 209
习题
√ 题 1
证明式 (9.25) 成立。提示：你可能会用到 SMW 公式，参考文献 [6, 76].
根据 SMW(Sherman-Morrison-Woodbury) 恒等式 证明 K \bm{K} K 的求解 可以不依靠 P ^ k \bm{\hat{P}}_k P^k。
即 用 别的已知量替换 K \bm{K} K 定义中的 P ^ k \bm{\hat{P}}_k P^k
State estimation for robotics: A matrix lie group approach.
来自上述书籍的公式：
——————————
证明:
K = P ^ k C k T Q − 1 由式 ( 9.16 ) = ( C k T Q − 1 C k + P ˇ k − 1 ) − 1 C k T Q − 1 由式 ( 2.75 c ) 由右到左 其中： A ⇒ P ˇ k , B ⇒ C k T , C ⇒ C k , D ⇒ Q = P ˇ k C k T ( Q + C k P ˇ k C k T ) − 1 \begin{align*} \bm{K} & = \bm{\hat{P}}_k\bm{C}_k^T\bm{Q}^{-1} \\ & 由 式 (9.16) \\ &= (\bm{C}_k^T\bm{Q}^{-1}\bm{C}_k+\bm{\check{P}}_k^{-1})^{-1}\bm{C}_k^T\bm{Q}^{-1} \\ & 由 式 (2.75c) 由右到左 \\ & 其中： A \Rightarrow \bm{\check{P}}_k, B \Rightarrow \bm{C}_k^T, C \Rightarrow \bm{C}_k, D \Rightarrow \bm{Q}\\ & = \bm{\check{P}}_k\bm{C}_k^T(\bm{Q} + \bm{C}_k\bm{\check{P}}_k\bm{C}_k^T)^{-1} \end{align*} K=P^kCkTQ−1由式(9.16)=(CkTQ−1Ck+Pˇk−1)−1CkTQ−1由式(2.75c)由右到左其中：A⇒Pˇk,B⇒CkT,C⇒Ck,D⇒Q=PˇkCkT(Q+CkPˇkCkT)−1证毕。
————————————————————$\check{A}$- 1
A ˇ \check{A} Aˇ
关于 Q 的补充：
感觉上面这个 记法 比较清楚， 十四讲里有点混乱。
题 4
证明 S \bm{S} S 矩阵为 半正定矩阵。- 待 证
S = B − E C − 1 E T \bm{S} = \bm{B}-\bm{EC}^{-1}\bm{E}^T S=B−EC−1ET
-
相关阅读:
日本人之间交谈的一个方式了解下
Git：Git的一些基本操作
【校招VIP】前端专业课考点之死锁的特点
Qt5.14.2 大文件处理的Qt多线程黑科技
学习vue一些小记
Memcache概念和安装方法
运营干货：虾皮店铺装修怎么做？
深入解析Linux Bridge：原理、架构、操作与持久化配置
A-Level化学例题解析及练习
mybaits入门
- 原文地址：https://blog.csdn.net/weixin_46034116/article/details/133226114
