《视觉 SLAM 十四讲》V2 第 9 讲 后端优化1 【扩展卡尔曼滤波器 EKF && BA+非线性优化(Ceres、g2o)】
文章目录
- 第9讲 后端1
-
-
- 9.1.2 线性系统和 KF
- 9.1.4 扩展卡尔曼滤波器 EKF 不足
- 9.2 BA 与 图优化
-
- 9.2.1 投影模型和 BA 代价函数
- 9.2.2 BA 的求解
- 9.2.3 稀疏性 和 边缘化
- 9.2.4 鲁棒核函数
- 9.3 实践： Ceres BA 【Code】
-
-
-
- 本讲 CMakeLists.txt
-
-
- 9.4 实践：g2o 求解 BA 【Code】
- 习题
-
第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
本讲 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)
mkdir build && cd build
cmake ..
make
./bundle_adjustment_ceres ../problem-16-22106-pre.txt
byzanz-record -x 72 -y 64 -w 1848 -h 893 -d 20 --delay=5 -c /home/xixi/myGIF/test.gif
byzanz-record -x 72 -y 64 -w 1848 -h 893 -d 30 --delay=5 -c /home/xixi/myGIF/test.gif
bundle_adjustment_ceres.cpp
#include 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"
| ^~~~~~~~~~~~~~~~
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>>>());
改动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));
其它问题： 链接库的问题， 看 CMakeLists.txt
cd build
cmake ..
make
./bundle_adjustment_g2o ../problem-16-22106-pre.txt
byzanz-record -x 72 -y 64 -w 1848 -h 893 -d 30 --delay=5 -c /home/xixi/myGIF/test.gif
byzanz-record -x 72 -y 64 -w 1848 -h 893 -d 20 --delay=5 -c /home/xixi/myGIF/test.gif
bundle_adjustment_g2o.cpp
#include > BlockSolverType;
typedef g2o::LinearSolverCSparse LinearSolverType;
// use LM
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique(g2o::make_unique()));*/
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));
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];
}
}
习题
