首页 Slam Vslam十四讲笔记 2~8讲
文章
取消

Slam Vslam十四讲笔记 2~8讲

发表于 2022/10/17 更新于 2023/03/02
作者 Song Zhou
103 分钟阅读

第2讲 初识slam

各种相机

  1. 单目相机：无法通过单张图片判断举距离，需要运动（近处物体移动快）来形成视差来获取距离。但只知道的是相对值，无法获取真实尺度（比如特摄片）

  2. 双目相机：两相机之间距离确定（基线baseline），通过基线来估计每个像素的空间位置（基线越长，分辨率越高，越准确），计算量是主要问题

  3. 深度相机（RGB-D相机）：物理测量手段（光返回，单双目的相机是通过软件计算得到的），但噪声大，视野小，易受日光干扰，无法测量投射材质等问题，一般室内用

经典框架

前端 视觉里程计

VO visual odometry（又称 前端 front end）

最简单情况下，只估计两个图像间的运动，每次估计误差都会累计（累积漂移 accumulating drift），使用后端优化和回环检测来解决漂移问题

后端

前端和计算机视觉领域更为相关（如特征提取和匹配等）， 后端优化主要是处理噪声问题，主要分为滤波和非线性优化算法

回环检测

通过图像的相似性来识别到过的场景，如果回环检测成功，可以显著减小累计误差

建图

一般分为

  • 度量地图：精确地表示地图中物体的位置关系

    • 稀疏：用路标landmark标注（比如定位时，稀疏即可）

    • 稠密：通常用方格或者方块来划分（二维三维）

  • 拓扑地图：节点和边组成

数学表述

  1. 运动方程

    \[x_k=f(x_{k-1},u_k,w_k)\]

    $x_k$表示k时刻位置，$u_k$表示k时刻运动传感器的读数或者输入，$w_k$表示噪声

  2. 观测方程

    \[z_{k,j}=h(y_j,x_k,v_{k,j})\]

    $z_{k,j}$表示在$x_k$位置观测到路标$y_j$，$v_{k,j}$表示噪声

SLAM问题建模成了状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量。

求解与方程的具体形式和噪声服从的哪种分布有关，一般分为线性/非线性和高斯/非高斯系统，最简单的是线性高斯系统，无偏最优估计由卡尔曼滤波器KF给出，复杂的非线性非高斯系统，一般是扩展卡尔曼滤波器EKF和非线性优化两种求解方法，但EKF有明显的缺点（线性化误差和噪声高斯分布假设），现如今，主流世界SLAM使用以图优化为代表的优化技术进行状态估计

使用cmake

当程序规模越来越大时，一个工程可能包括许多个文件夹和源文件，编译命令可能越来越长，g++要输入很多内容，使用cmake可以生成makefike文件，用make命令根据该文件编译整个工程

CMakeLists.txt：

1
2
3
4
5
6
7

cmake_minimum_required(VERSION 2.8)

# define a cmake_project
project(HelloSLAM)

# program_name cpp_file
add_executable(helloSLAM hello.cpp)

cmake会创建很多临时文件，可以在工程下创建build文件夹放置这些文件

使用库

c++工程中，只有带有main函数的文件才会生成可执行程序，而供其他程序调用的，称为库Library

比如写个提供打印函数的库文件 libHello.cpp

1
2
3
4
5
6

#include<iostream>
using namespace std;

void printHello(){
    cout<<"Hello"<<endl;
}

CMakeLists中 通过 add_libraray(名字 源文件) 来自动生成.a的（静态）库文件

add_library(hellolib libHello.cpp)

共享库.so需要 添加 add_libraray(名字 SHARED 源文件) 静态库每次被调用都会生成一个副本，共享库只有一个副本，更省空间。

库文件是一个压缩包，里面是编译好的二进制函数，调用者不知道有什么，也不知道如何调用，此时需要头文件来进行说明。

1
2
3
4

#ifndef LIBHELLO_H_
#define LIBHELLO_H_
void printHello();
#endif

第一句可以防止重复引用头文件而引起重定义的错误，再简单写一个调用库函数的cpp文件：

1
2
3
4
5
6

#include "libHello.h"

int main(){
    printHello();
    return 0;
}

1
2
3

add_library(hellolib libHello.cpp)
add_executable(useHello useHello.cpp)
target_link_libraries(useHello hellolib)

第3讲 三维空间刚体运动

旋转矩阵

坐标系间的欧式变换

考虑运动时，设定一个固定不动的惯性坐标系（世界坐标系），同时，相机或者机器人是一个移动坐标系，相机坐标系中的某个向量p，根据机器人的位姿变换到世界坐标系。

对于刚体运动（两个坐标系之间的运动由一个旋转加上一个平移组成），同一个向量在各个坐标系中的长度和夹角不会发生变化，比如抛手机，手机坐标系到世界坐标之间相差了一个欧式变换

向量本身没变，根据坐标定义：

\[\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right]\] \[\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{lll} \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \stackrel{\text { def }}{=} \boldsymbol{R} \boldsymbol{a}^{\prime}\]

R被称为旋转矩阵（又被称为方向余弦矩阵，因为基向量的长度为1），该矩阵是行列式为1的正交矩阵（逆矩阵为本身的转置）

$\boldsymbol{a} \cdot \boldsymbol{b}=\boldsymbol{a}^{\mathrm{T}} \boldsymbol{b}=\sum_{i=1}^{3} a_{i} b_{i}= \boldsymbol{a}   \boldsymbol{b} \cos \langle\boldsymbol{a}, \boldsymbol{b}\rangle .$ 内积定义

再加上平移向量t

\[\pmb{a_1}=\pmb{R_{12}}\pmb{a_2}+\pmb{t_{12}}\]

其中，R12 表示把坐标系2中的向量变换到坐标系1（由于向量乘在右边，所以是从右到左读的），平移t12表示坐标系1原点指向坐标系2原点的向量

$t_{12}\neq -t_{21}$ 因为要考虑两个系的旋转关系

变换矩阵与齐次坐标

实践 Eigen

sudo apt install libeigen3-dev安装eigen库，通过sudo updatedb sudo locate eigen3确定安装位置，updatedb报错，需要通过sudo apt install mlocate，默认位置在/usr/include/eigen3

Eigen 常用示例 https://zhuanlan.zhihu.com/p/462494086

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

#include <iostream>

using namespace std;

#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

using namespace Eigen;

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/

int main(int argc, char **argv) {
  // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
  // 声明一个2*3的float矩阵
  Matrix<float, 2, 3> matrix_23;

  // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
  // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
  Vector3d v_3d;
  // 这是一样的
  Matrix<float, 3, 1> vd_3d;

  // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
  Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零
  // 如果不确定矩阵大小，可以使用动态大小的矩阵
  Matrix<double, Dynamic, Dynamic> matrix_dynamic;
  // 更简单的
  MatrixXd matrix_x;
  // 这种类型还有很多，我们不一一列举

  // 下面是对Eigen阵的操作
  // 输入数据（初始化）
  matrix_23 << 1, 2, 3, 4, 5, 6;
  // 输出
  cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

  // 用()访问矩阵中的元素
  cout << "print matrix 2x3: " << endl;
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";
    cout << endl;
  }

  // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
  v_3d << 3, 2, 1;
  vd_3d << 4, 5, 6;

  // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
  // Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
  // 应该显式转换
  Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
  cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;

  Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
  cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

  // 同样你不能搞错矩阵的维度
  // 试着取消下面的注释，看看Eigen会报什么错
  // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

  // 一些矩阵运算
  // 四则运算就不演示了，直接用+-*/即可。
  matrix_33 = Matrix3d::Random();      // 随机数矩阵
  cout << "random matrix: \n" << matrix_33 << endl;
  cout << "transpose: \n" << matrix_33.transpose() << endl;      // 转置
  cout << "sum: " << matrix_33.sum() << endl;            // 各元素和
  cout << "trace: " << matrix_33.trace() << endl;          // 迹
  cout << "times 10: \n" << 10 * matrix_33 << endl;               // 数乘
  cout << "inverse: \n" << matrix_33.inverse() << endl;        // 逆
  cout << "det: " << matrix_33.determinant() << endl;    // 行列式

  // 特征值
  // 实对称矩阵可以保证对角化成功
  SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
  cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
  cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

  // 解方程
  // 我们求解 matrix_NN * x = v_Nd 这个方程
  // N的大小在前边的宏里定义，它由随机数生成
  // 直接求逆自然是最直接的，但是求逆运算量大

  Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
      = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
  Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

  clock_t time_stt = clock(); // 计时
  // 直接求逆
  Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
  cout << "time of normal inverse is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // 通常用矩阵分解来求，例如QR分解，速度会快很多
  time_stt = clock();
  x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
  cout << "time of Qr decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // 对于正定矩阵，还可以用cholesky分解来解方程
  time_stt = clock();
  x = matrix_NN.ldlt().solve(v_Nd);
  cout << "time of ldlt decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  return 0;
}

【定义1】给定一个大小为 n×n 的实对称矩阵 A ，若对于任意长度为 n 的非零向量 x ，有 $x^TAx>0$恒成立，则矩阵 A 是一个正定矩阵。

【定义2】给定一个大小为 n×n 的实对称矩阵 A ，若对于任意长度为 n 的向量 x ，有$x^TAx>=0$ 恒成立，则矩阵 A 是一个半正定矩阵。

QR分解前提，是列满秩矩阵

Eigen库只有头文件，不需要 target_link _libraries

旋转向量

任意旋转都可以用一个旋转轴和一个旋转角来刻画，旋转向量定义如下：向量的方向为旋转轴，模为绕轴逆时针旋转的角度。

旋转向量到旋转矩阵的转换：

两边取迹，得到

对于转轴n，转轴上的向量旋转后也不会发生改变，即 $Rn=n$ ，所以转轴n是矩阵R特征值1对应的特征向量

欧拉角

3个分离的转角

重大缺点是 碰到著名的万向锁问题（不适合插值和迭代，存在奇异性问题，通常只用于人机交互），常用的是飞机模型那类的 等价于ZYX轴旋转顺序，绕Z轴 偏航角yaw，绕旋转之后的Y轴旋转 俯仰角 pitch，绕旋转之后的X轴 滚转角 roll

三个实数来表达三维旋转，都会遇到奇异性问题，有点类似于经纬度表示地球上的点，纬度为正负90°时，经度无意义

四元数

\[\pmb{q}=q_0+q_1i+q_2j+q_3k\]

ijk为三个虚部，满足

单位四元数可以表示三维空间中任意一个旋转

姿态表示

四元数紧凑没有奇异性，但缺点在于不够直观，运算比较复杂

四元数、旋转矩阵、轴角都可以用来描述旋转，之间的转换都有便利的库函数

实践 Eigen几何模块

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

#include <iostream>
#include <cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

// 本程序演示了 Eigen 几何模块的使用方法

int main(int argc, char **argv) {

  // Eigen/Geometry 模块提供了各种旋转和平移的表示
  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
  cout.precision(3);
  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵
  // 也可以直接赋值
  rotation_matrix = rotation_vector.toRotationMatrix();
  // 用 AngleAxis 可以进行坐标变换
  Vector3d v(1, 0, 0);
  Vector3d v_rotated = rotation_vector * v;
  cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
  // 或者用旋转矩阵
  v_rotated = rotation_matrix * v;
  cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

  // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

  // 欧氏变换矩阵使用 Eigen::Isometry
  Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
  T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
  T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
  cout << "Transform matrix = \n" << T.matrix() << endl;

  // 用变换矩阵进行坐标变换
  Vector3d v_transformed = T * v;                              // 相当于R*v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

  // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

  // 四元数
  // 可以直接把AngleAxis赋值给四元数，反之亦然
  Quaterniond q = Quaterniond(rotation_vector);
  cout << "quaternion from rotation vector = " << q.coeffs().transpose()
       << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
  // 也可以把旋转矩阵赋给它
  q = Quaterniond(rotation_matrix);
  cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
  // 使用四元数旋转一个向量，使用重载的乘法即可
  v_rotated = q * v; // 注意数学上是qvq^{-1}
  cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
  // 用常规向量乘法表示，则应该如下计算
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

  return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

rotation matrix =
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
(1,0,0) after rotation (by angle axis) = 0.707 0.707     0
(1,0,0) after rotation (by matrix) = 0.707 0.707     0
yaw pitch roll = 0.785    -0     0
Transform matrix = 
 0.707 -0.707      0      1
 0.707  0.707      0      3
     0      0      1      4
     0      0      0      1
v tranformed = 1.71 3.71    4
quaternion from rotation vector =     0     0 0.383 0.924
quaternion from rotation matrix =     0     0 0.383 0.924

  • 旋转矩阵 3x3 Eigen::Matrix3d

  • 旋转向量 3x1Eigen::AngleAxisd

  • 欧拉角 3x1 Eigen::Vector3d

  • 四元数 4x1 Eigen::Quaterniond

  • 欧式变换矩阵 4x4 Eigen::Isometry3d

  • 仿射变换 4x4 Eigen::Affine3d

  • 射影变换 4x4 Eigen::Projective3d

可视化演示

安装Pangolin先

  1. git clone https://github.com/stevenlovegrove/Pangolin.git 慢的话直接去下载解压

  2. 安装依赖 ./scripts/install_prerequisites.sh recommended

  3. mkdir build && cd build cmake .. sudo make -j4 4表示cpu线程，可自行调整

  4. sudo make install

  5. 测试 比如 ./examples/HelloPangolin cmake make 一番后

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

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "../trajectory.txt";

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {

  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  ifstream fin(trajectory_file);
  if (!fin) {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    Twr.pretranslate(Vector3d(tx, ty, tz));
    poses.push_back(Twr);
  }
  cout << "read total " << poses.size() << " pose entries" << endl;

  // draw trajectory in pangolin
  DrawTrajectory(poses);
  return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}

第4讲 李群与李代数

4.1 李群和李代数基础

三维旋转矩阵构成了特殊正交群SO(3) 变换矩阵构成了 特殊欧式群SE(3) \(\begin{array}{l} \mathrm{SO}(3)=\left\{\boldsymbol{R} \in \mathbb{R}^{3 \times 3} \mid \boldsymbol{R} \boldsymbol{R}^{\mathrm{T}}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}, \\ \mathrm{SE}(3)=\left\{\boldsymbol{T}=\left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\} . \end{array}\)

4.1.1 李群

群：比如旋转矩阵，它对加法是不封闭的(两个旋转矩阵相加不再是旋转矩阵)，但对乘法是封闭的，只有一个（良好的）运算的集合，称之为群（一个集合加上一个运算的代数结构）

群满足“封结幺逆”四个条件

李群：拥有连续（光滑）性质的群，比如SO(N) SE(N)在实数空间是连续的（刚体能够在空间中连续运动）

每一个李群对应着一个李代数

4.1.2 李代数的引出

R是某个相机的旋转，会随时间变化 满足 \(\boldsymbol{R}(t) \boldsymbol{R}(t)^{\mathrm{T}}=\boldsymbol{I}\) 对时间求导，再整理得 \(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}=-\left(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}\right)^{\mathrm{T}}\) 可见$\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}$是一个反对称矩阵（主对角线元素为0），第三章有涉及到^符号，即将一个向量变成反对称矩阵，同理，对于任意一个反对称矩阵，可以找到唯一一个与之对应的向量，该符号用反过来的^来表示： \(\boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a}\) 假设找到一个三维向量与该反对称矩阵相对应: \(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}=\boldsymbol{\phi}(t)^{\wedge}\) 右乘以R(t)，可以得到： \(\dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}(t)^{\wedge} \boldsymbol{R}(t)=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \boldsymbol{R}(t)\) 即求导，等于左乘一个矩阵即可。考虑$t_0$时，设此时$R(0)=I$按照导数定义，在t=0处一阶泰勒展开 \(\begin{aligned} \boldsymbol{R}(t) & \approx \boldsymbol{R}\left(t_{0}\right)+\dot{\boldsymbol{R}}\left(t_{0}\right)\left(t-t_{0}\right) \\ &=\boldsymbol{I}+\boldsymbol{\phi}\left(t_{0}\right)^{\wedge}(t) . \end{aligned}\) $\phi$反应了R的导数性质，称其在SO(3)原点附近的正切空间上，现在考虑$t_0$附近，设 $\phi$保持为常数，则： \(\dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\boldsymbol{\phi}_{0}^{\wedge} \boldsymbol{R}(t)\)

正切空间：以不太严谨的例子来说明的话，李群就像一个无法定义加运算的曲面，对于曲面上的两点，相加后不一定还在这个曲面上了。而李代数就像这个曲面对应的切面，在切线附近具有和原曲面相近的性质。就像我们可以用某个曲线的切线，来近似代替切点附近的曲线，进行一些操作。于是通过李代数，我们终于可以进行求导等操作了。

有初值，是一个微分方程，解得： \(\boldsymbol{R}(t)=\exp \left(\boldsymbol{\phi}_{0}^{\wedge} t\right)\) 给定时刻R，就能求得$\phi$ ,$\phi$对应到SO(3)上的李代数so(3)，其次，求解矩阵指数，即 李群和李代数间的指数/对数映射

4.1.3 李代数的定义

每个李群都有与之对应的李代数，李代数描述了单位元附近的正切空间

李代数由一个集合$\mathbb{V}$，一个数域$\mathbb{F}$和一个二元运算[,]构成,李代数满足

image-20221109112714475

二元运算又被称为 李括号，要求元素和自己做李括号之后为0，比如叉积就是一种李括号

4.1.4 李代数 $\mathfrak{s o}(3)$

上文提到的 $\phi$是SO(3)的李代数，是定义在$\mathbb{R^3}$上的向量，每个$\phi$都可以生成一个反对称矩阵 \(\Phi=\phi^{\wedge}=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \in \mathbb{R}^{3 \times 3}\) 记作： \(\mathfrak{s o}(3)=\left\{\phi \in \mathbb{R}^{3}, \boldsymbol{\Phi}=\phi^{\wedge} \in \mathbb{R}^{3 \times 3}\right\}\)

4.1.5 李代数$\mathfrak{s e}(3)$

\[\mathfrak{s e}(3)=\left\{\boldsymbol{\xi}=\left[\begin{array}{l} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{array}\right] \in \mathbb{R}^{6}, \boldsymbol{\rho} \in \mathbb{R}^{3}, \boldsymbol{\phi} \in \mathfrak{s o}(3), \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^{\mathrm{T}} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}\right\}\]

六维， 可以简单理解成一个平移加上一个旋转

4.2 指数和对数映射

image-20221109115249670

4.3 李代数求导与扰动模型

4.3.1 BCH公式和近似形式

在SO(3)中完成两个矩阵乘法时，两个李代数指数映射乘积的完整形式由BCH公式给出，形式较复杂： \(\ln (\exp (\boldsymbol{A}) \exp (\boldsymbol{B}))=\boldsymbol{A}+\boldsymbol{B}+\frac{1}{2}[\boldsymbol{A}, \boldsymbol{B}]+\frac{1}{12}[\boldsymbol{A},[\boldsymbol{A}, \boldsymbol{B}]]-\frac{1}{12}[\boldsymbol{B},[\boldsymbol{A}, \boldsymbol{B}]]+\cdots\) AB是矩阵，[]为李括号，考虑$\ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee}$, 当 $\phi_1$或者$\phi_2$为小量时，小量二次以上的项可以被忽略，此时，BCH拥有线性近似表达： \(\ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{ll} J_{l}\left(\phi_{2}\right)^{-1} \phi_{1}+\phi_{2} & \text { 当 } \phi_{1} \text { 为小量, } \\ J_{r}\left(\phi_{1}\right)^{-1} \phi_{2}+\phi_{1} & \text { 当 } \phi_{2} \text { 为小量. } \end{array}\right.\) 第一个近似为例，当R2左乘一个微小旋转R1，可以近似看出李代数phi2加上了…

于是，李代数在BCH近似下，分为左乘近似和右乘近似两种模型，以左乘为例，雅可比$J_l$就是之前4.2中的J \(\boldsymbol{J}_{l}=\boldsymbol{J}=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a a}^{\mathrm{T}}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} .\) 逆为： \(\boldsymbol{J}_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} \boldsymbol{I}+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) \boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}-\frac{\theta}{2} \boldsymbol{a}^{\prime}\) 右乘的： \(\boldsymbol{J}_{r}(\boldsymbol{\phi})=\boldsymbol{J}_{l}(-\boldsymbol{\phi})\) 对于SE(3)也有类似的BCH近似，暂略

4.3.2 SO(3)上的李代数求导

slam中，需要估计相机的位姿，涉及到李代数求导问题，来调整当前的估计值，但李群没有良好定义的加法。

  • 用李代数表示姿态，然后根据李代数加法对李代数求导
  • 对李群左乘或右乘微小扰动，然会对扰动求导

4.3.3 李代数求导

比较复杂 暂略, 一般不直接对李代数求导

4.3.4 扰动模型（左乘）

对R进行一次扰动$\Delta R$，看结果相对于扰动的变化率，记$\varphi$为左扰动的李代数 \(\begin{aligned} \frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{\varphi}} &=\lim _{\boldsymbol{\varphi} \rightarrow \mathbf{0}} \frac{\exp \left(\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\boldsymbol{\varphi}} \\ &=\lim _{\boldsymbol{\varphi} \rightarrow \mathbf{0}} \frac{\left(\boldsymbol{I}+\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\boldsymbol{\varphi}} \\ &=\lim _{\boldsymbol{\varphi} \rightarrow \mathbf{0}} \frac{\boldsymbol{\varphi}^{\wedge} \boldsymbol{R} \boldsymbol{p}}{\boldsymbol{\varphi}}=\lim _{\boldsymbol{\varphi} \rightarrow \mathbf{0}} \frac{-(\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{\varphi}}{\boldsymbol{\varphi}}=-(\boldsymbol{R} \boldsymbol{p})^{\wedge} . \end{aligned}\) 第二行用了BCH近似和泰勒展开舍去小项

4.3.5 SE(3)上的李代数求导

暂略

4.4 实践：Sophus

4.4.1 Sophus的基本用法

第5讲 相机与图像

相机讲三维世界坐标点映射到二维图像平面的过程可以用几何模型进行描述，其中最简单的称为针孔模型；由于镜头上有透镜，光线投影到成像平面的过程中会产生畸变

5.1 相机模型

5.1.1 针孔相机模型

针孔相机模型

根据相似：

\[\frac{Z}{f}=-\frac{X}{X^{'}}=-\frac{Y}{Y^{'}}\]

负号表示倒像，实际上相机成像是正的（相机会自动翻转），可以将符号去掉（相当于是移动到相机前方）

定义像素坐标系（假设成像平面上固定这一个像素平面o-u-v）：原点位于图像的坐上角，u轴与x轴同向右，v轴与y轴同向下。像素坐标系与成像平面之间相差一个缩放和一个原点的平移。

\[\begin{cases} u=\alpha X^{'}+c_x \\ v=\beta Y^{'}+c_y \end{cases}\]

将$\alpha f$合并为$f_x$ $\beta f$合并为$f_y$再整理后：

\[\begin{cases} u=f_x\frac{X}{Z} +c_x \\ v=f_y\frac{Y}{Z}+c_y \end{cases}\]

写成矩阵形式：

\(Z\left(\begin{array}{l} u \\ v \\ 1 \end{array}\right)=\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right) \stackrel{\text { def }}{=} \boldsymbol{K} \boldsymbol{P} .\) 矩阵K称为相机的内参数 Intrinsics，一般来说 相机的内参在出场之后是固定的（有时候可能需要自己去确定内参，称为标定）

相机的位姿R,t称为相机的外参 Extrinsics,外参会随着相机运动发生改变（机器人的轨迹） \(Z \boldsymbol{P}_{u v}=Z\left[\begin{array}{c} u \\ v \\ 1 \end{array}\right]=\boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{P}_{\mathrm{w}}+\boldsymbol{t}\right)=\boldsymbol{K} \boldsymbol{T} \boldsymbol{P}_{\mathrm{w}}\)

该式子运用了 非齐次坐标到齐次坐标的转换https://blog.csdn.net/hangl_ciom/article/details/104441075

该式子反过来理解，一个世界坐标点通过R和t先转化到相机坐标系，再通过归一化（相当于是去除了最后一维的数值，即该点距离成像平面的深度），得到该点在归一化平面（Z=1）上的坐标，再左乘内参，即可得到像素坐标 \(\left(\boldsymbol{R} \boldsymbol{P}_{\mathrm{w}}+\boldsymbol{t}\right)=\underbrace{[X, Y, Z]^{\mathrm{T}}}_{\text {相机坐标 }} \rightarrow \underbrace{[X / Z, Y / Z, 1]^{\mathrm{T}}}_{\text {归一化坐标 }}\) 该模型，相机坐标乘任意非零参数，归一化结果都一样，相当于单目相机，点的深度在投影过程中被丢失了。

5.1.2 畸变模型

添加透镜，透镜导致光线传播发生改变，产生畸变（distortion 失真），透镜形状本身引起的畸变称为径向畸变，透镜本身和成像平面没有严格平行，产生的畸变称为切向畸变

图像通常需要进行径向畸变和切向畸变的校正或者处理，通常的做法是先去畸变，得到去畸变后的图像

考虑归一化平面上的一点p，坐标为 $[x,y]^T$，极坐标形式为$[r,\theta]^T$，径向畸变可以看作是坐标点沿着长度方向发生了变化，切向畸变可以看作是坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。假设径向畸变呈多项式关系 \(\begin{array}{l} x_{\text {distorted }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \\ y_{\text {distorted }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \end{array}\) 对于切向畸变，另外引入p1,p2来纠正 \(\begin{array}{l} x_{\text {distorted }}=x+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right) \\ y_{\text {distorted }}=y+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y \end{array}\) 联合得到： \(\left\{\begin{array}{l} x_{\text {distorted }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right) \\ y_{\text {distorted }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y \end{array}\right.\) 实际使用时，可能为了简化模型，会舍去k2 k3 \(世界坐标\stackrel{\text{R,t}}{\rightarrow}相机坐标\stackrel{\text{归一化}}{\rightarrow}归一化坐标\\\stackrel{\text{畸变参数}}{\rightarrow}畸变后坐标\stackrel{\text{内参}}{\rightarrow}像素坐标\)

5.1.3 双目相机模型

单目相机丢失深度，双目相机与人眼类似，通过左右眼看到的景物差异（视差）来判断距离

image-20221028110111753 \(\frac{z-f}{z}=\frac{b-u_{\mathrm{L}}+u_{\mathrm{R}}}{b}\) 这里$U_R$ 是成像平面的坐标，按照定义，该值是负的 \(z=\frac{f b}{d}, \quad d \stackrel{\text { def }}{=} u_{\mathrm{L}}-u_{\mathrm{R}}\) 根据该关系，可以得到 b越大 能测的距离越远，同时，视差最小为1个像素，d本身的测量是比较困难的

5.1.4 RGB-D相机

主要分为两类：

  • 红外结构光：发送红外光，根据返回的结构光图案进行计算
  • 飞行时间 （Time-of-Flight TOF）：发射脉冲，计算返回的时间，确定距离（和激光传感器类似，但是激光传感器是逐点扫描的，ToF可以获取整个图像的像素深度）

容易受到日光影响

5.2 图像

image-20221028113257034

访问图像像素

1

unsigned char pixel = image[y][x]

注意xy的顺序，y是行，x是列

图中A表示的是透明度

5.3 实践：计算机中的图像

5.3.1 OpenCV的基本使用方法

源代码安装opencv

这里用的是ubuntu进行安装，流程如下：

  1. 先下载相关依赖：

    1

    $ sudo apt-get install libavcodec-dev libavformat-dev libswscale-dev libv4l-dev libxvidcore-dev libx264-dev libatlas-base-dev gfortran libgtk2.0-dev libjpeg-dev libpng-dev

  2. 官网地址 https://opencv.org/releases/ 这里下载4.6.0 sources

  3. 下载解压到/opt/opencv-4.6.0

  4. 创建build文件夹 命令行执行 cmake ..

  5. 再执行 sudo make -j8 // 8是8线程 可以 nproc查看最大支持的数量

  6. 最后 sudo make install 将opencv安装到机器上，而不仅仅是编译它，opencv默认安装在 /usr/local/opencv

前提：安装了cmake

简单说明一下 cmake 和 make：一个源文件假设通过gcc编译生成.o的可执行文件，但一个工程可能会有大量的源文件需要编译，或者有许多依赖需要处理，此时自己编译可能会报错，所有有了make工具，make工具会调用makefile中的命令进行编译，makefile中就包含了许多gcc编译源代码命令。但是快平台后，makefile可能就不适应了，需要重新修改创建，所以有了cmake命令来快速生成适应不同平台的makefile

一些基本操作：

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

#include<iostream>
#include<chrono>

using namespace std;

#include<opencv4/opencv2/core.hpp>
#include<opencv4/opencv2/highgui/highgui.hpp>

int main(int argc, char **argv){
    // read targeted path image
    cv::Mat image;
    image = cv::imread(argv[1]);

    if(image.data==nullptr){
        cerr<<"file "<<argv[1]<<"not exist!"<<endl;
        return 0;
    }

    cout<<"width: "<<image.cols<<", height: "<<image.rows<<", channels: "<<image.channels()<<endl;

    cv::imshow("image",image);
    cv::waitKey(0);
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for(size_t y=0;y<image.rows;y++){
        unsigned char *row_ptr = image.ptr<unsigned char>(y);
        for(size_t x=0;x<image.cols;x++){
            unsigned char *data_ptr = &row_ptr[x*image.channels()];
            for(int c=0;c!=image.channels();c++){
                unsigned char data = data_ptr[c];
                // cout<<data<<endl;
            }
        }
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
    cout << "遍历图像用时：" << time_used.count() << " 秒。" << endl;
    // 关于 cv::Mat 的拷贝
    // 直接赋值并不会拷贝数据
    cv::Mat image_another = image;
    // 修改 image_another 会导致 image 发生变化
    image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 将左上角100*100的块置零
    cv::imshow("image", image);
    cv::waitKey(0);

    // 使用clone函数来拷贝数据
    cv::Mat image_clone = image.clone();
    image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);
    cv::imshow("image", image);
    cv::imshow("image_clone", image_clone);
    cv::waitKey(0);

    // 对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看OpenCV官方文档查询每个函数的调用方法.
    cv::destroyAllWindows();
    return 0;
}

注意：直接赋值图像，不会进行复制，相当于多了个别名，需要调用clone函数

CMakeLists.txt

1
2
3
4

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(imageBasics imageBasics.cpp)
target_link_libraries(imageBasics ${OpenCV_LIBS})

5.3.2 图像去畸变

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

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "../distorted.png";   // 请确保路径正确

int main(int argc, char **argv) {

  // 本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
  // 畸变参数
  double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
  // 内参
  double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

  cv::Mat image = cv::imread(image_file, 0);   // 图像是灰度图，CV_8UC1
  int rows = image.rows, cols = image.cols;
  cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

  // 计算去畸变后图像的内容
  for (int v = 0; v < rows; v++) {
    for (int u = 0; u < cols; u++) {
      // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
      double x = (u - cx) / fx, y = (v - cy) / fy;
      double r = sqrt(x * x + y * y);
      double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
      double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
      double u_distorted = fx * x_distorted + cx;
      double v_distorted = fy * y_distorted + cy;

      // 赋值 (最近邻插值)
      if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
        image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
      } else {
        image_undistort.at<uchar>(v, u) = 0;
      }
    }
  }

  // 画图去畸变后图像
  cv::imshow("distorted", image);
  cv::imshow("undistorted", image_undistort);
  cv::waitKey();
  return 0;
}

实现原理，首先固定畸变参数和内参，获得图像的高和宽，遍历每一个去畸变后的像素：

  1. 先根据正常图像的像素坐标和内参得到，相机坐标
  2. 根据畸变参数得到畸变后的相机坐标
  3. 根据畸变后的相机坐标和内参得到畸变后的像素坐标
  4. 将指定畸变后的像素坐标的像素值赋值给正常的图片的像素坐标的像素值

5.4 实践：3D视觉

5.4.1 双目视觉

略

5.4.2 RGB-D视觉

略

第6讲 非线性优化

6.1 状态估计问题

6.1.1 批量状态估计与最大后验估计

第二讲的SLAM经典模型，一个运动方程，一个观测方程： \(\left\{\begin{array}{l} \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}\right)+\boldsymbol{v}_{k, j} \end{array}\right.\) 假设在xk处对路标yj(世界坐标)进行了一次观测，对应到图像上的像素位置为zk,j,则观测方程可以表示为： \(s \boldsymbol{z}_{k, j}=\boldsymbol{K}\left(\boldsymbol{R}_{k} \boldsymbol{y}_{j}+\boldsymbol{t}_{k}\right)\) K为内参，s为像素点的距离（也是 $\boldsymbol{R}{k} \boldsymbol{y}{j}+\boldsymbol{t}{k}$的第三个分量），假设存在两个噪声项 w和v满足零均值的高斯分布，$\boldsymbol{w}{k} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{R}{k}\right), \boldsymbol{v}{k} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{Q}_{k, j}\right)$，在存在噪声的情况下，推断位姿x和地图y，这构成了状态估计问题。

通常有两种处理方式：

  • 增量/渐进（incremental），或者称为滤波器：用新数据来更新当前时刻持有的估计状态
  • 批量（batch）：可以在更大的范围达到最优化，极端情况下可以收集所有时刻的数据，再统一处理，即SfM Structure from Motion的主流做法（非实时），通常是固定一些历史轨迹，对当前时刻附近的轨迹进行优化，这是后面提到的滑动窗口估计法

先介绍批量：

假设1到N的所有时刻，并假设有M个路标点，定义位姿和路标点： \(\boldsymbol{x}=\left\{\boldsymbol{x}_{1}, \ldots, \boldsymbol{x}_{N}\right\}, \quad \boldsymbol{y}=\left\{\boldsymbol{y}_{1}, \ldots, \boldsymbol{y}_{M}\right\}\) 用不带下标的u和z表示所有时刻的输入和所有时刻的观测数据，即$P(\boldsymbol{x}, \boldsymbol{y} \mid \boldsymbol{z}, \boldsymbol{u})$，利用贝叶斯法则来估计状态变量的条件分布 \(P(\boldsymbol{x}, \boldsymbol{y} \mid \boldsymbol{z}, \boldsymbol{u})=\frac{P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y}) P(\boldsymbol{x}, \boldsymbol{y})}{P(\boldsymbol{z}, \boldsymbol{u})} \propto \underbrace{P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y})}_{\text {似然 }} \underbrace{P(\boldsymbol{x}, \boldsymbol{y})}_{\text {先验 }} .\)

右边那个有点像无穷符号的是正比符号

最左侧的是后验，直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化是可行的 \((\boldsymbol{x}, \boldsymbol{y})^{*}{ }_{\text {MAP }}=\arg \max P(\boldsymbol{x}, \boldsymbol{y} \mid \boldsymbol{z}, \boldsymbol{u})=\arg \max P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y}) P(\boldsymbol{x}, \boldsymbol{y})\)

MAP 指的是 maximum a posteriori 最大后验估计

求解最大化概率等价于最大化似然和先验的乘积（分母与待估计的x和y无关，可以忽略），如果不知道先验，可以求解最大似然估计 MLE maximum likelihood estimation： \((\boldsymbol{x}, \boldsymbol{y})^{*}{ }_{\mathrm{MLE}}=\arg \max P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y})\) 最大似然可以理解为在什么样的状态下，最可能产生现在观测到的数据

6.1.2 最小二乘的引出

对于某一次观测： \(\boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}\right)+\boldsymbol{v}_{k, j}\) 假设噪声项$\boldsymbol{v}{k} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{Q}{k, j}\right)$，则观测数据的条件概率为： \(P\left(\boldsymbol{z}_{j, k} \mid \boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)=N\left(h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}\right), \boldsymbol{Q}_{k, j}\right) .\)

依然是一个高斯分布（正态分布），可以使用最小化负对数来求一个高斯分布的最大似然，任意高维高斯分布 $\boldsymbol{x} \sim \mathcal{N}(\boldsymbol{\mu}, \boldsymbol{\Sigma})$，它的概率密度函数展开形式为： \(P(\boldsymbol{x})=\frac{1}{\sqrt{(2 \pi)^{N} \operatorname{det}(\boldsymbol{\Sigma})}} \exp \left(-\frac{1}{2}(\boldsymbol{x}-\boldsymbol{\mu})^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}(\boldsymbol{x}-\boldsymbol{\mu})\right)\)

det是行列式值

取负对数： \(-\ln (P(\boldsymbol{x}))=\frac{1}{2} \ln \left((2 \pi)^{N} \operatorname{det}(\boldsymbol{\Sigma})\right)+\frac{1}{2}(\boldsymbol{x}-\boldsymbol{\mu})^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}(\boldsymbol{x}-\boldsymbol{\mu})\) 对数函数单调增，原函数求最大化，相当于对负对数求最小化，第一项于x无关，可以略去，即最小化右侧的二次型项，带入slam的观测模型，即

二次型： $\mathbb{R}^{n}$ 上的一个二次型是一个定义在上的函数，表达式为$ Q(x)=x^TAx$ ，其中 A 是一个 n×n 对称矩阵（一定要注意 A 是对称矩阵，不是一个随便的矩阵），矩阵 A 称为关于二次型的矩阵（二次型矩阵）

\[\begin{aligned} \left(\boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)^{*} &=\arg \max \mathcal{N}\left(h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}\right), \boldsymbol{Q}_{k, j}\right) \\ &=\arg \min \left(\left(\boldsymbol{z}_{k, j}-h\left(\boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)\right)^{\mathrm{T}} \boldsymbol{Q}_{k, j}^{-1}\left(\boldsymbol{z}_{k, j}-h\left(\boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)\right)\right) \end{aligned}\]

这个二次型称为马哈拉诺比斯距离，马氏距离。

考虑批量数据，假设各个时刻的输入和观测是相互独立的，意味着各个输入之间和各个观测之间是独立的，输入和观测也是独立的，可以对联合分布进行因式分解(??)： \(P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y})=\prod_{k} P\left(\boldsymbol{u}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{k}\right) \prod_{k, j} P\left(\boldsymbol{z}_{k, j} \mid \boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)\) 可以独立地处理各时刻的运动和观测，定义各次输入和观测数据与模型之间的误差； \(\begin{aligned} \boldsymbol{e}_{\boldsymbol{u}, k} &=\boldsymbol{x}_{k}-f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right) \\ \boldsymbol{e}_{\boldsymbol{z}, j, k} &=\boldsymbol{z}_{k, j}-h\left(\boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right) \end{aligned}\) 利用负对数： \(\min J(\boldsymbol{x}, \boldsymbol{y})=\sum_{k} e_{\boldsymbol{u}, k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{e}_{\boldsymbol{u}, k}+\sum_{k} \sum_{j} \boldsymbol{e}_{\boldsymbol{z}, k, j}^{\mathrm{T}} \boldsymbol{Q}_{k, j}^{-1} e_{\boldsymbol{z}, k, j}\) 得到一个最小二乘问题，由于噪声存在，估计的数据代入运动，不会完美成立，通常对状态的估计值进行微调，使整体的误差下降到一个极小值（典型的非线性优化的过程）

6.1.3 例子：批量状态估计

考虑一个离散时间系统： \(\begin{array}{l} \boldsymbol{x}_{k}=\boldsymbol{x}_{k-1}+\boldsymbol{u}_{k}+\boldsymbol{w}_{k}, \quad \boldsymbol{w}_{k} \sim \mathcal{N}\left(0, \boldsymbol{Q}_{k}\right) \\ \boldsymbol{z}_{k}=\boldsymbol{x}_{k}+\boldsymbol{n}_{k}, \quad \quad \quad \quad \boldsymbol{n}_{k} \sim \mathcal{N}\left(0, \boldsymbol{R}_{k}\right) \\ \end{array}\) 假设第一个方程为汽车运动方程，u为输入，w为噪声，第二个方程为观测方程，z为对汽车位置的测量，取时间k=1,2,3，设初始状态x0已知，根据先前的推导： \(\begin{aligned} \boldsymbol{x}_{\mathrm{map}}^{*} &=\arg \max P(\boldsymbol{x} \mid \boldsymbol{u}, \boldsymbol{z})=\arg \max P(\boldsymbol{u}, \boldsymbol{z} \mid \boldsymbol{x}) \\ &=\prod_{k=1}^{3} P\left(\boldsymbol{u}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{k}\right) \prod_{k=1}^{3} P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right), \end{aligned}\) 满足： \(P\left(\boldsymbol{u}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{k}\right)=\mathcal{N}\left(\boldsymbol{x}_{k}-\boldsymbol{x}_{k-1}, \boldsymbol{Q}_{k}\right),\\ P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right)=\mathcal{N}\left(\boldsymbol{x}_{k}, \boldsymbol{R}_{k}\right)\) 构建误差变量 \(\boldsymbol{e}_{\boldsymbol{u}, k}=\boldsymbol{x}_{k}-\boldsymbol{x}_{k-1}-\boldsymbol{u}_{k}, \quad \boldsymbol{e}_{z, k}=\boldsymbol{z}_{k}-\boldsymbol{x}_{k}\) 得到最小二乘的目标函数： \(\min \sum_{k=1}^{3} \boldsymbol{e}_{\boldsymbol{u}, k}^{\mathrm{T}} \boldsymbol{Q}_{k}^{-1} \boldsymbol{e}_{\boldsymbol{u}, k}+\sum_{k=1}^{3} \boldsymbol{e}_{\boldsymbol{z}, k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{e}_{z, k}\)

6.2 非线性最小二乘

\[\min _{\boldsymbol{x}} F(\boldsymbol{x})=\frac{1}{2}\|f(\boldsymbol{x})\|_{2}^{2}\]

该问题，如果f形式简单，可以直接令目标导数为0来求解，如果形式复杂，则使用迭代的方式来求解

image-20221116110219833

关键在于寻找增量

6.2.1 一阶和二阶梯度法

泰勒展开 \(F\left(\boldsymbol{x}_{k}+\Delta \boldsymbol{x}_{k}\right) \approx F\left(\boldsymbol{x}_{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}_{k}\right)^{\mathrm{T}} \Delta \boldsymbol{x}_{k}+\frac{1}{2} \Delta \boldsymbol{x}_{k}^{\mathrm{T}} \boldsymbol{H}\left(\boldsymbol{x}_{k}\right) \Delta \boldsymbol{x}_{k}\) 其中J为F(x)关于x的一阶导数（也叫梯度，雅可比矩阵），H则是二阶导数（海塞矩阵），如果只保留一阶，增量取负梯度，即可保证函数下降（通常还要指定一个步长，该方法称为最速下降法）；

如果保留二阶信息，此时增量方程为： \(\Delta \boldsymbol{x}^{*}=\arg \min \left(F(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}+\frac{1}{2} \Delta \boldsymbol{x}^{\mathrm{T}} \boldsymbol{H} \Delta \boldsymbol{x}\right)\) 令右侧导数为0： \(J+H \Delta x=0 \Rightarrow H \Delta x=-J .\) 该方法称为 牛顿法

最速下降法过于贪心，迭代次数有时会很多，牛顿法要算H，规模较大时比较困难

6.2.2 高斯牛顿法

思想：将f(x)进行一阶的泰勒展开 \(f(\boldsymbol{x}+\Delta \boldsymbol{x}) \approx f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x} .\) 当前目标是，寻得增量$\Delta{x}$，使得$|f(\boldsymbol{x}+\Delta \boldsymbol{x})|^{2}$最小，需要解一个线性的最小二乘问题： \(\Delta \boldsymbol{x}^{*}=\arg \min _{\Delta \boldsymbol{x}} \frac{1}{2}\left\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right\|^{2} .\) 展开： \(\begin{aligned} \frac{1}{2}\left\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right\|^{2} &=\frac{1}{2}\left(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right)^{\mathrm{T}}\left(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right) \\ &=\frac{1}{2}\left(\|f(\boldsymbol{x})\|_{2}^{2}+2 f(\boldsymbol{x}) \boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}+\Delta \boldsymbol{x}^{\mathrm{T}} \boldsymbol{J}(\boldsymbol{x}) \boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right) . \end{aligned}\) 求$\Delta{x}$的导数，并令其为0： \(\boldsymbol{J}(\boldsymbol{x}) f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \boldsymbol{J}^{\mathrm{T}}(\boldsymbol{x}) \Delta \boldsymbol{x}=\mathbf{0} .\) 得到 \(\underbrace{\boldsymbol{J}(\boldsymbol{x}) \boldsymbol{J}^{\mathrm{T}}}_{\boldsymbol{H}(\boldsymbol{x})}(\boldsymbol{x}) \Delta \boldsymbol{x}=\underbrace{-\boldsymbol{J}(\boldsymbol{x}) f(\boldsymbol{x})}_{\boldsymbol{g}(\boldsymbol{x})} .\) 该方程称为增量方程（高斯牛顿方程，正规方程），记作： \(\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g} .\) 对比牛顿法，高斯牛顿法将$JJ^T$作为海塞矩阵的近似，从而忽略了H计算的过程，由此，算法步骤可以写成：

image-20221116112837805

高斯牛顿法的缺陷：虽然速度比较快，但是可能会不收敛，因为H矩阵需要可逆，但$JJ^T$可能是奇异矩阵或病态矩阵

6.2.3 列文伯格-马夸尔特方法

比高斯牛顿法更健壮，但收敛更慢，也被称为阻尼牛顿法

问题性质较好时，用高斯牛顿，问题接近病态，则用该方法

后续 暂略…

6.3 实践：曲线拟合问题

6.3.1 手写高斯牛顿法

假设满足曲线方程： \(y=\exp \left(a x^{2}+b x+c\right)+w\) 求解最小二乘： \(\min _{a, b, c} \frac{1}{2} \sum_{i=1}^{N}\left\|y_{i}-\exp \left(a x_{i}^{2}+b x_{i}+c\right)\right\|^{2}\) 定义误差ei，求出误差项关于状态变量的导数: \(\begin{array}{l} \frac{\partial e_{i}}{\partial a}=-x_{i}^{2} \exp \left(a x_{i}^{2}+b x_{i}+c\right) \\ \frac{\partial e_{i}}{\partial b}=-x_{i} \exp \left(a x_{i}^{2}+b x_{i}+c\right) \\ \frac{\partial e_{i}}{\partial c}=-\exp \left(a x_{i}^{2}+b x_{i}+c\right) \end{array}\) 于是 $\boldsymbol{J}{i}=\left[\frac{\partial e{i}}{\partial a}, \frac{\partial e_{i}}{\partial b}, \frac{\partial e_{i}}{\partial c}\right]^{\mathrm{T}}$, 增量方程(??)： \(\left(\sum_{i=1}^{100} \boldsymbol{J}_{i}\left(\sigma^{2}\right)^{-1} \boldsymbol{J}_{i}^{\mathrm{T}}\right) \Delta \boldsymbol{x}_{k}=\sum_{i=1}^{100}-\boldsymbol{J}_{i}\left(\sigma^{2}\right)^{-1} e_{i}\) 代码：

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

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 开始Gauss-Newton迭代
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {

    Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;

    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - exp(ae * xi * xi + be * xi + ce);
      Vector3d J; // 雅可比矩阵
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
      J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

      H += inv_sigma * inv_sigma * J * J.transpose();
      b += -inv_sigma * inv_sigma * error * J;

      cost += error * error;
    }

    // 求解线性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      break;
    }

    ae += dx[0];
    be += dx[1];
    ce += dx[2];

    lastCost = cost;

    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

6.3.2 使用Ceres进行曲线拟合

安装ceres 需要 git clone https://github.com/ceres-solver/ceres-solver.git

相关依赖，不装编译会失败：

1

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-dev

使用流程： \(\begin{aligned} \min _{x} & \frac{1}{2} \sum_{i} \rho_{i}\left(\left\|f_{i}\left(x_{i_{1}}, \cdots, x_{i_{n}}\right)\right\|^{2}\right) \\ \text { s.t. } & l_{j} \leqslant x_{j} \leqslant u_{j} \end{aligned}\) xi为优化变量，又称为参数块，f为代价函数，也被称为残差块 residual blocks，在slam中可以理解为误差项，l和u为优化变量的下限和上限，简单可以设置为无穷，目标函数由许多平方项经过一个核函数(第9讲会讲到)$\rho()$之后求和组成

  1. 参数块通常是平凡的向量，在slam中可以定义成四元数，李代数这种特殊的结构
  2. 雅可比可以定义，也可以用ceres“自动求导”，如果需要自动求导，残差块需要按照特定的写法书写
  3. 将参数块和残差块加入到Ceres定义的Problem对象中调用Solve求解

代码：

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

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

struct CURVE_FITTING_COST{
    CURVE_FITTING_COST(double x, double y): _x(x), _y(y){}
    template<typename T>
    bool operator()(const T *const abc,T * residual) const{
        residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
        return true;
    }
    const double _x,_y;
};

int main(int argc, char ** argv){
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
    int N = 100;                                 // 数据点
    double w_sigma = 1.0;                        // 噪声Sigma值
    double inv_sigma = 1.0 / w_sigma;
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
    }
    double abc[3] = {ae, be, ce};
    ceres::Problem problem;
    for(int i=0;i<N;i++){
        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(
                new CURVE_FITTING_COST(x_data[i],y_data[i])
            ),
            nullptr,
            abc
        );
    }
    // 配置求解器
    ceres::Solver::Options options;     // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 增量方程如何求解
    options.minimizer_progress_to_stdout = true;   // 输出到cout

    ceres::Solver::Summary summary;                // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);  // 开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

    // 输出结果
    cout << summary.BriefReport() << endl;
    cout << "estimated a,b,c = ";
    for (auto a:abc) cout << a << " ";
    cout << endl;

    return 0;

}

第7讲 视觉里程计

slam前端又称为 视觉里程计，根据相邻图像的信息估计相机的运动，主要分为特征点法和直接法

7.1 特征点法

7.1.1 特征点法

经典slam模型中 具有代表性的点，称为路标，视觉slam中称为图像特征Feature

特征是图像信息的另一种数字表达方式，希望特征点在相机运动之后保持稳定，所以直接一个像素是不合适的，灰度值受光照等因素影响严重。

特征点是图像里一些特别的地方，角点、边缘、区块都是具有代表性的地方，一种直观的特征点提取方式是在不同图像间辨认角点并确定对应关系 。

单纯的角点（例如 FAST角点）也不太稳定，比如相机靠近，相机旋转等，后续，研究人员设计了更加稳定的局部图像特征（SIFT SURF ORB等）

特征点由关键点key-point和描述子descriptor 两部分组成

  • 关键点：该特征点在图像里的位置

  • 描述子：通常是一个向量，按照人为设计的方式，描述了该关键点周围像素的信息

    原则“外观相似的特征应该有相似的描述子”

SIFT scale-invariant feature transform 尺度不变特征变换，充分考虑了光照、尺度、旋转等变化，但计算量过大（引入GPU加速可以满足实时性要求，但成本很高）

ORB oriented FAST and Rotated BRIEF 采用改进的Fast关键点以及BRIEF二进制描述子，实时性极好

7.1.2 ORB特征

FAST关键点

思想：如果一个像素与领域的像素差别较大，那么他更可能是角点

过程：

  1. 选取像素p，假设亮度为$I_p$
  2. 设置阈值T比如亮度的20%
  3. 以像素p为中心，选取半径为3上的16个像素点
  4. 如果圆上有连续N个点亮度不在 $I_p-T, I_p+T$之间，p可以被认为是特征点（N通常取12 被称为FAST-12 其他常用的可以是9或者11）
  5. 对每一个像素进行相同的操作

image-20221028155153501

预测试操作：第1、5、9、13个像素的亮度，至少有3个不在区间内，才有可能是一个角点（就是连续的点超过一半嘛）

原始的FAST角点会出现“扎堆”现象，一般需要进行 非极大值抑制 在一定区域内只保留响应极大值的角点

FAST改进：

重复性不强，分布不均匀 不具备方向信息，因为固定取半径为3的点，存在尺度问题，（远看是，近看不是），ORB添加了尺度和旋转的描述，分别是通过图像金字塔（对图像进行不同层次的降采样）和灰度质心法

图像金字塔：底层是原始图像，每往上一层，就对图像进行一个固定倍率的缩放

image-20221028160624366

如果相机后退，应该能在上一个图像的金字塔的上层和下一个图像的金字塔的下层找到匹配。意味着：特征匹配需要针对不同图像进行不同层级的特征点匹配。

图像灰度质心：质心是以图像块灰度值作为权重的中心，一般图像块是圆形区域，圆心指向质心作为关键点的主方向，因为圆形才能确保前后参与计算的像素点不变

具体步骤如下：

  1. 在一个小的图像块B中，定义图形块的矩为： \(m_{p q}=\sum_{x, y \in B} x^{p} y^{q} I(x, y), \quad p, q=\{0,1\}\) x,y是图像坐标，I表示灰度值，m10为沿x轴方向的矩，m01为沿y轴方向的矩

  2. 通过矩找到图像块的质心： \(C=\left(\frac{m_{10}}{m_{00}}, \frac{m_{01}}{m_{00}}\right)\)

  3. 连接图像块的中心O和质心C，可以得到方向向量$\overrightarrow{O C}$ ，于是特征点方向定义为： \(\theta=\arctan \left(m_{01} / m_{10}\right)\) sin_theta = m01 / m_sqrt cos_theta = m10 / m_sqrt

BRIEF描述子改进

算法原理：BRIEF是一种二进制描述子，描述向量由0和1组成，关键点周围随机两点像素比较大小，比如p和q，如果p比q大，取1，否则取0，最后得到一个多维的0，1向量，由于FAST特征点提取阶段计算了关键点的方向，利用方向信息，旋转之后的 Steer BRIEF 使ORB描述子具有良好的旋转不变性

7.1.3 特征匹配

解决数据关联问题（描述子进行准确匹配）

误匹配较多：场景重复纹理较多

正确匹配的情况：

两张图 得到两个特征点集合，暴力匹配是根据描述子距离（两个特征之间的相似程度）排序进行匹配（浮点类型的一般用欧氏距离，二进制类型的如BRIEF一般用汉明距离（不同位数的个数））；如果特征点量很大时，比如某个帧和一个地图的时候，通常使用快速近似最近邻（FLANN）算法，该算法已经成熟且集成到OpenCV了，不多赘述

7.2 实践：特征提取和匹配

主流的特征匹配 OpenCV已经集成了

7.2.1 OpenCV的ORB特征

调用opencv来提取和匹配orb

源码：

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

#include<iostream>
#include<opencv2/core/core.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgcodecs/legacy/constants_c.h>
#include<chrono>

using namespace std;
using namespace cv;


int main(int argc, char **argv){
    if(argc!=3){
        cout<<"usage: feature_extraction img1 img2"<<endl;
        return 1;
    }
    // read image
    Mat img_1 = imread(argv[1],CV_LOAD_IMAGE_COLOR); // opencv4 need import this constant(euqal to 1)
    Mat img_2 = imread(argv[2],CV_LOAD_IMAGE_COLOR);
    assert(img_1.data!=nullptr&&img_2.data!=nullptr);

    // init
    vector<KeyPoint> keypoints_1,keypoints_2;
    Mat descriptor_1,descriptor_2;
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");

    // 1. detect oriented fast angular point
    chrono::steady_clock::time_point t1= chrono::steady_clock::now();
    detector->detect(img_1,keypoints_1);
    detector->detect(img_2, keypoints_2);
    
    // 2. compute brief descriptor
    descriptor->compute(img_1,keypoints_1,descriptor_1);
    descriptor->compute(img_2,keypoints_2,descriptor_2);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;

    Mat outimg1;
    drawKeypoints(img_1,keypoints_1,outimg1,Scalar::all(-1),DrawMatchesFlags::DEFAULT);
    imshow("ORB features",outimg1);

    // 3. match BRIEF descriptor with hamming distance
    vector<DMatch> matches;
    t1=chrono::steady_clock::now();
    matcher->match(descriptor_1,descriptor_2,matches);
    t2=chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;

    // 4. filter the matches
    auto min_max= minmax_element(matches.begin(),matches.end(),
        [](const DMatch &m1, const DMatch &m2){return m1.distance<m2.distance;});
    double min_dist = min_max.first->distance;
    double max_dist = min_max.second->distance;
    printf("--Max dist: %f\n",max_dist);
    printf("--Min dist: %f",min_dist);

    // when the distance between the descriptors is smaller than the double min_dist, it should be thought wrong match;
    // sometimes the min_dist would be very small, usually an empirical value 30 will be set as the lower bound
    vector<DMatch> good_matches;
    for(int i=0;i<descriptor_1.rows;i++){
        if(matches[i].distance<=max(2*min_dist,30.0)){
            good_matches.push_back(matches[i]);
        }
    }

    // plot the match result
    Mat img_match;
    Mat img_goodmatch;
    drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, img_match);
    drawMatches(img_1,keypoints_1,img_2,keypoints_2,good_matches,img_goodmatch);
    imshow("all matches",img_match);
    imshow("good matches", img_goodmatch);
    waitKey(0);
    
    return 0;

}

代码说明：

  • CV_LOAD_IMAGE_COLOR 一个常量定义 值就是1 用来加载彩图
  • 用KeyPoint来存储关键点信息，关键点包括位置、大小、角度等信息
  • 根据关键点信息进行描述子计算
  • 用汉明距离进行描述子的匹配
  • 过滤匹配，一般认为距离 大于两倍的最小距离 时 认定为错误匹，但有时候最小距离会很小，会设一个经验值30作为下限

CMakeLists.txt

1
2
3

find_package(OpenCV 4 REQUIRED)
add_executable(orb_cv orb_cv.cpp)
target_link_libraries(orb_cv ${OpenCV_LIBS})

效果：

image-20221102111438437

image-20221102111508490

7.2.2 手写ORB特征

暂略，后续补充

7.3 2D-2D 对极几何

7.3.1 对极约束

通过两个图像之间若干对匹配点，可以恢复出两帧之间的相机运动。

image-20221102120014693

相关术语：$\overrightarrow{O_{1} p_{1}}$ 与$ \overrightarrow{O_{2} p_{2}}$ 在空间中会相交于点$P$. $I_1,I_2$是像平面

  • 极平面 Epipolar plane: $O_1, O_2,P$ 三点构成的平面
  • 极点 Epipole: $e_1,e_2$
  • 基线: $O_1O_2$
  • 极线 Epipolar line: 极平面和像平面之间的相交线$l_1,l_2$

代数角度分析:

在第一帧的坐标系下, 设P的空间位置为: \(\boldsymbol{P}=[X,Y,Z]^T\) 像素点的位置可以描述为: \(s_{1} \boldsymbol{p}_{1}=\boldsymbol{K} \boldsymbol{P}, \quad s_{2} \boldsymbol{p}_{2}=\boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t})\) 有时会使用齐次坐标来表示像素点, $s_{1} \boldsymbol{p}_{1}$和$\boldsymbol{p_1}$在齐次坐标的意义下是相等的, 两者是投影关系, 称之为尺度意义上的相等, 记作: \(s \boldsymbol{p} \simeq \boldsymbol{p}\) 于是, 上述投影关系可写为: \(\boldsymbol{p}_{1} \simeq \boldsymbol{K} \boldsymbol{P}, \quad \boldsymbol{p}_{2} \simeq \boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t})\) 现在, 取: \(\boldsymbol{x}_{1}=\boldsymbol{K}^{-1} \boldsymbol{p}_{1}, \quad \boldsymbol{x}_{2}=\boldsymbol{K}^{-1} \boldsymbol{p}_{2}\) x1,x2 是像素点在归一化平面上的坐标, 代入上式得: \(\begin{aligned} x_{2} &\simeq \boldsymbol{R} \boldsymbol{x}_{1}+t\\\rightarrow \boldsymbol{t}^{\wedge} \boldsymbol{x}_{2} &\simeq \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}\\\rightarrow \boldsymbol{x}_{2}^{\mathrm{T}} \boldsymbol{t}^{\wedge} \boldsymbol{x}_{2} &\simeq \boldsymbol{x}_{2}^{\mathrm{T}} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1} \end{aligned}\)

第二部 左乘$\boldsymbol{t}^{\wedge}$ t和本身外积为0

$t^{\wedge}x_2$是和t和x2都垂直的向量 和 $x_2^T$内积 等于0, 可以转化为通常的相等符号 \(\boldsymbol{x}_{2}^{\top} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}=0\) 重新带入p1,p2: \(\boldsymbol{p}_{2}^{\mathrm{T}} \boldsymbol{K}^{-\mathrm{T}} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{K}^{-1} \boldsymbol{p}_{1}=0\) 上述两个式子都称为 对极约束, 中间部分记作两个矩阵: 基础矩阵(Fundamental Matrix) F和本质矩阵(Essential Matrix) E: \(\boldsymbol{E}=\boldsymbol{t}^{\wedge} \boldsymbol{R}, \quad \boldsymbol{F}=\boldsymbol{K}^{-\mathrm{T}} \boldsymbol{E} \boldsymbol{K}^{-1}, \quad \boldsymbol{x}_{2}^{\mathrm{T}} \boldsymbol{E} \boldsymbol{x}_{1}=\boldsymbol{p}_{2}^{\mathrm{T}} \boldsymbol{F} \boldsymbol{p}_{1}=0\) 根据对极约束, 相机位置问题变成了:

  • 根据配对点的像素位置计算E或者F
  • 根据E或者F求出R,t

对极约束的几何意义是$O_1,O_2,P$三点共面

7.3.2 本质矩阵

  • 本质矩阵E是由对极约束定义的, E乘以任意非零常数后, 约束仍然满足, 称E在不同尺度下是等价的.
  • 本质矩阵的内在性质: 奇异值必定是 $[\sigma, \sigma, 0]^{\mathrm{T}}$的形式
  • 平移和旋转各有3个自由度,所以本质矩阵有6个自由度, 由于尺度等价性, 只有5个自由度( 所以理论上5对点可以求解)

5个自由度的解释: https://zhuanlan.zhihu.com/p/500798616

image-20221103103845625

五点求解比较费劲, 通常使用8点法

考虑一对匹配点 $\boldsymbol{x}{1}=\left[u{1}, v_{1}, 1\right]^{\mathrm{T}}, \boldsymbol{x}{2}=\left[u{2}, v_{2}, 1\right]^{\mathrm{T}}$, 根据对极约束: \(\left(u_{2}, v_{2}, 1\right)\left(\begin{array}{lll} e_{1} & e_{2} & e_{3} \\ e_{4} & e_{5} & e_{6} \\ e_{7} & e_{8} & e_{9} \end{array}\right)\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right)=0\) 8个点构成线性方程组($u^i,v^i$)表示第i个特征点 \(\left(\begin{array}{ccccccccc} u_{2}^{1} u_{1}^{1} & u_{2}^{1} v_{1}^{1} & u_{2}^{1} & v_{2}^{1} u_{1}^{1} & v_{2}^{1} v_{1}^{1} & v_{2}^{1} & u_{1}^{1} & v_{1}^{1} & 1 \\ u_{2}^{2} u_{1}^{2} & u_{2}^{2} v_{1}^{2} & u_{2}^{2} & v_{2}^{2} u_{1}^{2} & v_{2}^{2} v_{1}^{2} & v_{2}^{2} & u_{1}^{2} & v_{1}^{2} & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ u_{2}^{8} u_{1}^{8} & u_{2}^{8} v_{1}^{8} & u_{2}^{8} & v_{2}^{8} u_{1}^{8} & v_{2}^{8} v_{1}^{8} & v_{2}^{8} & u_{1}^{8} & v_{1}^{8} & 1 \end{array}\right)\left(\begin{array}{c} e_{1} \\ e_{2} \\ e_{3} \\ e_{4} \\ e_{5} \\ e_{6} \\ e_{7} \\ e_{8} \\ e_{9} \end{array}\right)=0\) $\boldsymbol{e}$位于矩阵的零空间中(矩阵A的零空间就是Ax=0的解的集合)

通过奇异值分解来得到 R和t \(\boldsymbol{E}=\boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^{\mathrm{T}}\) $\sum=diag(\sigma,\sigma,0)$ 对于任意一个E, 存在两个可能的t,R与它对应: \(\begin{array}{l} \boldsymbol{t}_{1}^{\wedge}=\boldsymbol{U} \boldsymbol{R}_{Z}\left(\frac{\pi}{2}\right) \boldsymbol{\Sigma} \boldsymbol{U}^{\mathrm{T}}, \quad \boldsymbol{R}_{1}=\boldsymbol{U} \boldsymbol{R}_{Z}^{\mathrm{T}}\left(\frac{\pi}{2}\right) \boldsymbol{V}^{\mathrm{T}} \\ \boldsymbol{t}_{2}^{\wedge}=\boldsymbol{U} \boldsymbol{R}_{Z}\left(-\frac{\pi}{2}\right) \boldsymbol{\Sigma} \boldsymbol{U}^{\mathrm{T}}, \quad \boldsymbol{R}_{2}=\boldsymbol{U} \boldsymbol{R}_{Z}^{\mathrm{T}}\left(-\frac{\pi}{2}\right) \boldsymbol{V}^{\mathrm{T}} \end{array}\) $\boldsymbol{R}_{Z}^{\mathrm{T}}\left(\frac{\pi}{2}\right) $表示沿Z轴宣旋转90°, 由于E与-E是等价的, 所以 任意t去负号, 也会得到同样的结果, 所以一共4个可能解(保持投影点不变的情况下)

img

不过只有第一种解,P在两个相机中都有正的深度

小结:

本质矩阵5个自由度, 通常使用8点法方便求解, 8点 需要满足 稀疏矩阵满秩, 奇异值分解, 有四个可能解, 但只有一解正确

7.3.3 单应矩阵

单应矩阵通常描述处于共同平面(比如地面 墙壁)上的一些点在两张图像之间的变化关系。

推导：假设有匹配的特征点$p_1,p_2$ 特征点落在平面P上，则平面满足方程 \(\boldsymbol{n}^{\mathrm{T}} \boldsymbol{P}+d=0\) 其中，n是法向量，变形后： \(-\frac{\boldsymbol{n}^{\mathrm{T}} \boldsymbol{P}}{d}=1\) 于是可以得到： \(\begin{aligned} \boldsymbol{p}_{2} & \simeq \boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t}) \\ & \simeq \boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t} \cdot\left(-\frac{\boldsymbol{n}^{\mathrm{T}} \boldsymbol{P}}{d}\right)\right) \\ & \simeq \boldsymbol{K}\left(\boldsymbol{R}-\frac{\boldsymbol{t} \boldsymbol{n}^{\mathrm{T}}}{d}\right) \boldsymbol{P} \\ & \simeq \boldsymbol{K}\left(\boldsymbol{R}-\frac{\boldsymbol{t n}^{\mathrm{T}}}{d}\right) \boldsymbol{K}^{-1} \boldsymbol{p}_{1} . \end{aligned}\) 将中间部分记为H \(\boldsymbol{p}_{2} \simeq \boldsymbol{H} \boldsymbol{p}_{1}\) 该矩阵就是所谓的单应矩阵，与旋转、平移、平面的参数有关，展开上式： \(\left(\begin{array}{c} u_{2} \\ v_{2} \\ 1 \end{array}\right) \simeq\left(\begin{array}{lll} h_{1} & h_{2} & h_{3} \\ h_{4} & h_{5} & h_{6} \\ h_{7} & h_{8} & h_{9} \end{array}\right)\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right)\)

\[\begin{aligned} u_{2} &=\frac{h_{1} u_{1}+h_{2} v_{1}+h_{3}}{h_{7} u_{1}+h_{8} v_{1}+h_{9}} \\ v_{2} &=\frac{h_{4} u_{1}+h_{5} v_{1}+h_{6}}{h_{7} u_{1}+h_{8} v_{1}+h_{9}} \end{aligned}\]

说明：原本不是普通的等号，第三行 可以看作是$1= k(h_7u_1+h_8v_1+h_9)$，同样第一行和第二行也设一个k倍，带回第一行和第二行，就可以得到普通等号的。

由于k可以是任意非零常数，所以实际处理中可以使$h_9=1$，整理得： \(\begin{array}{l} h_{1} u_{1}+h_{2} v_{1}+h_{3}-h_{7} u_{1} u_{2}-h_{8} v_{1} u_{2}=u_{2} \\ h_{4} u_{1}+h_{5} v_{1}+h_{6}-h_{7} u_{1} v_{2}-h_{8} v_{1} v_{2}=v_{2} \end{array}\) 得到两项约束，8自由度，所以4对匹配点（不能三点共线）可以算出H，再进行分解，可以得到R和t

当特征点共面或者相机发生纯旋转时，基础矩阵的自由度会下降，即出现了所谓的退化 degenerate， 现实数据总包含一些噪声，如果用8点法，多余出来的自由度会主要由噪声决定，一般会同时估计基础矩阵和单应矩阵，选用重投影误差比较小的那个作为最终的运动估计矩阵

7.4 实践：对极约束求解相机运动

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

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include<opencv2/imgcodecs/legacy/constants_c.h>
// #include "extra.h" // use this if in OpenCV2

using namespace std;
using namespace cv;

/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);

void pose_estimation_2d2d(
  std::vector<KeyPoint> keypoints_1,
  std::vector<KeyPoint> keypoints_2,
  std::vector<DMatch> matches,
  Mat &R, Mat &t);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);

int main(int argc, char **argv) {
  if (argc != 3) {
    cout << "usage: pose_estimation_2d2d img1 img2" << endl;
    return 1;
  }
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  //-- 估计两张图像间运动
  Mat R, t;
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

  //-- 验证E=t^R*scale
  Mat t_x =
    (Mat_<double>(3, 3) << 0, -t.at<double>(2, 0), t.at<double>(1, 0),
      t.at<double>(2, 0), 0, -t.at<double>(0, 0),
      -t.at<double>(1, 0), t.at<double>(0, 0), 0);

  cout << "t^R=" << endl << t_x * R << endl;

  //-- 验证对极约束
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  for (DMatch m: matches) {
    Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);
    Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);
    Mat d = y2.t() * t_x * R * y1;
    cout << "epipolar constraint = " << d << endl;
  }
  return 0;
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
                          std::vector<KeyPoint> keypoints_2,
                          std::vector<DMatch> matches,
                          Mat &R, Mat &t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

  //-- 把匹配点转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;

  for (int i = 0; i < (int) matches.size(); i++) {
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);
  }

  //-- 计算基础矩阵
  Mat fundamental_matrix;
  fundamental_matrix = findFundamentalMat(points1, points2, cv::FM_8POINT);
  cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;

  //-- 计算本质矩阵
  Point2d principal_point(325.1, 249.7);  //相机光心, TUM dataset标定值
  double focal_length = 521;      //相机焦距, TUM dataset标定值
  Mat essential_matrix;
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
  cout << "essential_matrix is " << endl << essential_matrix << endl;

  //-- 计算单应矩阵
  //-- 但是本例中场景不是平面，单应矩阵意义不大
  Mat homography_matrix;
  homography_matrix = findHomography(points1, points2, RANSAC, 3);
  cout << "homography_matrix is " << endl << homography_matrix << endl;

  //-- 从本质矩阵中恢复旋转和平移信息.
  // 此函数仅在Opencv3中提供
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
  cout << "R is " << endl << R << endl;
  cout << "t is " << endl << t << endl;

}

特征点提取和匹配参考7.2 不再赘述

当提供的点超过8对时，会构成超定方程，不一定会得到解，但可以计算最小二乘解,但一般会有错误匹配，通常是使用随机采样一致性（Random Sample Concensus RANSAC）

7.5 三角测量

单目仅通过单张图像无法获取像素的深度信息，通过三角测量（或三角化）来获取深度

image-20221107182158533

理论上同一个路标点而言，两条直线会相交，但是，存在一定的噪声，所以同行是不会相交的 \(s_{2} \boldsymbol{x}_{2}=s_{1} \boldsymbol{R} \boldsymbol{x}_{1}+\boldsymbol{t}\) 可以左乘$x_2^\wedge$ \(s_{2} \boldsymbol{x}_{2}^{\wedge} \boldsymbol{x}_{2}=0=s_{1} \boldsymbol{x}_{2}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}+\boldsymbol{x}_{2}^{\wedge} \boldsymbol{t}\) 根据R和t，即可求得s1，再求s2，由于噪声存在，不能精确为0，常见做法是求最小二乘解

三角化的矛盾：平移小，像素上的不确定性将导致较大的深度不确定性，如果平移大，图像中的物体外观可能发生较大的变化，特征提取和匹配将会变得困难

7.6 实践：三角测量

主要步骤：

  • 计算特征点和匹配点
  • 用2D-2D估计R和t
  • 调用opencv的triangulatePoints

image-20221108120035092

已知运动情况以及配对的特征点，求得 特征点的位置

这里T1可以设置为：

1
2
3
4

 Mat T1 = (Mat_<float>(3, 4) <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0);
即将相机1的坐标系作为世界坐标系，则第二个相机的投影矩阵即为R t的增广矩阵，第三个参数是第一个相机对应的相机坐标的特征点，第四个就是第二个相机对应的相机坐标的特征点。

7.7 3D-2D：PnP

PnP Perspective-n-Points 是求解3D到2D点对运动的方法，描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿

常见的求解方法：

  • P3P：用3对点估计位姿
  • DLT：直接线性变换
  • EPnP
  • UPnP
  • 非线性优化：光束法平差 Bundle Adjustment BA

7.7.1 直接线性变换

考虑某个空间点P，齐次坐标 $(X,Y,Z,1)^T$ ，特征点$(u_1,v_1,1)^T$，定义增广矩阵 R|t \(s\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right)=\left(\begin{array}{cccc} t_{1} & t_{2} & t_{3} & t_{4} \\ t_{5} & t_{6} & t_{7} & t_{8} \\ t_{9} & t_{10} & t_{11} & t_{12} \end{array}\right)\left(\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array}\right)\) 利用最后一行，消去s，得到两个约束 \(u_{1}=\frac{t_{1} X+t_{2} Y+t_{3} Z+t_{4}}{t_{9} X+t_{10} Y+t_{11} Z+t_{12}}, \quad v_{1}=\frac{t_{5} X+t_{6} Y+t_{7} Z+t_{8}}{t_{9} X+t_{10} Y+t_{11} Z+t_{12}}\) 简化表示，定义行向量 t1 t2 t3 \(\boldsymbol{t}_{1}^{\mathrm{T}} \boldsymbol{P}-\boldsymbol{t}_{3}^{\mathrm{T}} \boldsymbol{P} u_{1}=0,\quad \boldsymbol{t}_{2}^{\mathrm{T}} \boldsymbol{P}-\boldsymbol{t}_{3}^{\mathrm{T}} \boldsymbol{P} v_{1}=0\) 每个特征点提供了2个关于t的约束，t一共12维，所以最少6对匹配点可实现求解

7.7.2 P3P

image-20221108143131608

三个相似三角形， 来考虑Oab和OAB的关系，余弦定理： \(O A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle=A B^{2}\) 其他两个三角形类似…后续求解略

7.7.3 最小化重投影误差求解PnP

线性方法：先求相机位姿，再求空间点位置；

非线性：都看作优化变量，放在一起优化，该类问题称为 Bundle Adjustment BA问题： \(\boldsymbol{T}^{*}=\arg \min _{\boldsymbol{T}} \frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{u}_{i}-\frac{1}{s_{i}} \boldsymbol{K} \boldsymbol{T} \boldsymbol{P}_{i}\right\|_{2}^{2}\) u是像素坐标，P是世界坐标，该问题的误差项，是将3D点的投影位置和观测位置做差，所以被称为重投影误差。假设有两个相机的成像，通过特征匹配知道了特征点P的成像p1和p2，设立初始值，P和p2’，一开始与p2距离较远，不断缩小误差（需要同时考虑多个点）

在使用优化算法求解之前，需要知道误差项关于优化变量的导数，也就是线性化： \(\boldsymbol{e}(\boldsymbol{x}+\Delta \boldsymbol{x}) \approx \boldsymbol{e}(\boldsymbol{x})+\boldsymbol{J}^{\mathrm{T}} \Delta \boldsymbol{x}\) 推导JT如下：

记 $\boldsymbol{P}^{\prime}=(\boldsymbol{T} \boldsymbol{P})_{1: 3}=\left[X^{\prime}, Y^{\prime}, Z^{\prime}\right]^{\mathrm{T}}$，相机投影模型：$s \boldsymbol{u}=\boldsymbol{K} \boldsymbol{P}^{\prime}$，展开： \(\left[\begin{array}{c} s u \\ s v \\ s \end{array}\right]=\left[\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{l} X^{\prime} \\ Y^{\prime} \\ Z^{\prime} \end{array}\right]\) 得到： \(u=f_{x} \frac{X^{\prime}}{Z^{\prime}}+c_{x}, \quad v=f_{y} \frac{Y^{\prime}}{Z^{\prime}}+c_{y}\) 与第5讲的相机模型一致，求误差时，可以把u和v与实际的测量值比较，求差。对T左乘扰动量，考虑e的变化关于扰动量的导数，利用链式法则： \(\frac{\partial \boldsymbol{e}}{\partial \delta \boldsymbol{\xi}}=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\boldsymbol{e}(\delta \boldsymbol{\xi} \oplus \boldsymbol{\xi})-\boldsymbol{e}(\boldsymbol{\xi})}{\delta \boldsymbol{\xi}}=\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}^{\prime}} \frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \boldsymbol{\xi}}\) $\oplus$指李代数上的左乘扰动，第一项是关于投影点的导数， \(\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}^{\prime}}=-\left[\begin{array}{ccc} \frac{\partial u}{\partial X^{\prime}} & \frac{\partial u}{\partial Y^{\prime}} & \frac{\partial u}{\partial Z^{\prime}} \\ \frac{\partial v}{\partial X^{\prime}} & \frac{\partial v}{\partial Y^{\prime}} & \frac{\partial v}{\partial Z^{\prime}} \end{array}\right]=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right]\) 第二项是变换后的点关于李代数的导数，根据4.3.5的推导（额额..没推）： \(\frac{\partial(\boldsymbol{T} \boldsymbol{P})}{\partial \delta \boldsymbol{\xi}}=(\boldsymbol{T} \boldsymbol{P})^{\odot}=\left[\begin{array}{cc} \boldsymbol{I} & -\boldsymbol{P}^{\prime \wedge} \\ \mathbf{0}^{\mathrm{T}} & \mathbf{0}^{\mathrm{T}} \end{array}\right]\) 这里的P’是只取了前三维，于是得： \(\frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \boldsymbol{\xi}}=\left[\boldsymbol{I},-\boldsymbol{P}^{\prime \wedge}\right]\) 两项相乘，就得到了2x6的雅可比矩阵： \(\frac{\partial \boldsymbol{e}}{\partial \delta \boldsymbol{\xi}}=-\left[\begin{array}{cccccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} & -\frac{f_{x} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & f_{x}+\frac{f_{x} X^{\prime 2}}{Z^{\prime 2}} & -\frac{f_{x} Y^{\prime}}{Z^{\prime}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} & -f_{y}-\frac{f_{y} Y^{\prime 2}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime}}{Z^{\prime}} \end{array}\right]\) e是由观测值减预测值定义的，除了优化位姿，还需要优化特征点的空间位置，所有需要讨论e关于P的导数 \(\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}}=\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}^{\prime}} \frac{\partial \boldsymbol{P}^{\prime}}{\partial \boldsymbol{P}}\) 第二项求导剩下R,于是： \(\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}}=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right] \boldsymbol{R}\) 于是推导出了观测相机方程关于相机位姿与特征点的两个导数矩阵，能够在优化过程中提供重要的梯度方向，指导优化的迭代。

7.8 实践：求解PnP

7.8.1 使用EPnP求解位姿

这里调用OpenCV的solvePnP求解PnP问题

前提安装g2o和sophus，github下代码，build下cmake .., make, make install

sophus依赖于 eigen和fmt

1

void solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags = CV_ITERATIVE);
  • objectPoints - 世界坐标系下的控制点的坐标，vector的数据类型在这里可以使用
  • imagePoints - 在图像坐标系下对应的控制点的坐标。vector在这里可以使用
  • cameraMatrix - 相机的内参矩阵
  • distCoeffs - 相机的畸变系数。以上两个参数通过相机标定可以得到。
  • rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系
  • tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系
  • flags - 默认使用CV_ITERATIV迭代法。可以选 CV_P3P、CV_EPNP、CV_ITERATIVE（opencv3里多了DLS和UPnP解法）。

7.8.2 手写位姿估计

未完待续…

7.8.3 使用g20进行BA优化

第8讲 视觉里程计2

8.1 直接法的引出

间接法缺点：特征点和描述子计算耗时，只用特征点导致信息丢失，纹理弱的环境导致没有足够的匹配点。

解决办法：

  • 不计算匹配描述子，使用光流法来跟踪特征点的运动（仍然要求关键点具有可区别性，即要求为角点）
  • 不计算描述子，使用直接法计算特征点下一时刻在图像中的位置

间接法是将特征点看作固定在三维空间的不动点，根据在相机中的投影位置，通过最小化重投影误差优化相机运动；但直接法不要知道点和点之间的对应关系，通过最小化光度误差来求得。

8.2 2D光流

随时间变化，像素会在图像中运动（前提 灰度不变假设），计算部分像素的称为稀疏光流，所有像素的称为稠密光流。稀疏光流以Lucas-Kanade光流(LK光流)为代表，稠密光流以Horn-Schunck光流为代表。

假设t时刻像素位于(x,y)，t+dt时刻运动到了（x+dx,y+dy），由于灰度不变： \(\boldsymbol{I}(x+\mathrm{d} x, y+\mathrm{d} y, t+\mathrm{d} t)=\boldsymbol{I}(x, y, t)\) 进行泰勒展开，保留一阶项： \(\boldsymbol{I}(x+\mathrm{d} x, y+\mathrm{d} y, t+\mathrm{d} t) \approx \boldsymbol{I}(x, y, t)+\frac{\partial \boldsymbol{I}}{\partial x} \mathrm{~d} x+\frac{\partial \boldsymbol{I}}{\partial y} \mathrm{~d} y+\frac{\partial \boldsymbol{I}}{\partial t} \mathrm{~d} t\) 由于假设了灰度不变，则： \(\frac{\partial \boldsymbol{I}}{\partial x} \mathrm{~d} x+\frac{\partial \boldsymbol{I}}{\partial y} \mathrm{~d} y+\frac{\partial \boldsymbol{I}}{\partial t} \mathrm{~d} t=0 \\) 进一步整理： \(\frac{\partial \boldsymbol{I}}{\partial x} \frac{\mathrm{d} x}{\mathrm{d} t}+\frac{\partial \boldsymbol{I}}{\partial y} \frac{\mathrm{d} y}{\mathrm{d} t}=-\frac{\partial \boldsymbol{I}}{\partial t}\) 而 $\frac{\mathrm{d} x}{\mathrm{d} t}$是像素在x轴上运动速度，$\frac{\mathrm{d} y}{\mathrm{d} t}$是像素在y轴上的运动速度，记为u和v，$\frac{\partial \boldsymbol{I}}{\partial x}$是图像在该点处x方向的梯度，另一项则是y方向的梯度，记为 $\boldsymbol{I}_x,\boldsymbol{I}_x$。图像灰度对时间的变量量记为 $\boldsymbol{I}_t$

假设图像w*w像素，则： \(\left[\begin{array}{ll} \boldsymbol{I}_{x} & \boldsymbol{I}_{y} \end{array}\right]_{k}\left[\begin{array}{l} u \\ v \end{array}\right]=-\boldsymbol{I}_{t k}, \quad k=1, \ldots, w^{2}\) 记： \(\boldsymbol{A}=\left[\begin{array}{c} {\left[\boldsymbol{I}_{x}, \boldsymbol{I}_{y}\right]_{1}} \\ \vdots \\ {\left[\boldsymbol{I}_{x}, \boldsymbol{I}_{y}\right]_{k}} \end{array}\right], \boldsymbol{b}=\left[\begin{array}{c} \boldsymbol{I}_{t 1} \\ \vdots \\ \boldsymbol{I}_{t k} \end{array}\right]\) 使用最小二乘法求解： \(\left[\begin{array}{l} u \\ v \end{array}\right]^{*}=-\left(\boldsymbol{A}^{\mathrm{T}} \boldsymbol{A}\right)^{-1} \boldsymbol{A}^{\mathrm{T}} \boldsymbol{b} .\) LK光流常被用来跟踪角点的运动

8.3 实践：LK光流

8.3.1 使用LK光流

1
2
3
4
5
6

//GFTTDetector三个参数从左到右依次为
//maxCorners表示最大角点数目。在此处为500。
//qualityLevel表示角点可以接受的最小特征值，一般0.1或者0.01，不超过1。在此处为0.01。
//minDistance表示角点之间的最小距离。在此处为20。
Ptr<GFTTDetector> detector = GFTTDetector::create(500,0.01,20);
detector->detect(img1,kp1);

用GFTTDetector来获取图像的角点（使用Harris角点作为LK光流法的跟踪输入点）

调用opencv的calcOpticalFlowPyrLK代码：

1
2
3
4
5

vector<Point2f> pt1, pt2;
for (auto &kp: kp1) pt1.push_back(kp.pt);
vector<uchar> status;
vector<float> error;
cv::calcOpticalFlowPyrLK(img1, img2, pt1, pt2, status, error);

输入图片，角点对应坐标

8.3.2 用高斯牛顿法实现光流

单层光流

光流可以看作是一个优化问题，通过最小化灰度误差估计最优的像素偏移。

后续暂略

8.4 直接法

光流中，首先跟踪特征点的位置，再根据这些位置确定相机的运动，两步走很难确保全局的最优性，直接法遵循着后一步调整前一步的思路得到。

设某空间点P在两帧图像中的位置为p1,p2，在特征点法中，通过匹配描述子知道p1和p2的位置，所以可以计算重投影的位置，在直接法中，是根据当前相机的位姿寻找p2的位置，若位姿不够好，p2和p1相差就大，为减小区别，不断优化位姿，寻找和p1更相似的p2，最小化的是光度误差

推导暂略…

误差相对于李代数的雅可比矩阵 \(\boldsymbol{J}=-\frac{\partial \boldsymbol{I}_{2}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}\)

8.4.2 直接法的讨论

P的来源：

  1. P来自于稀疏关键点，称之为稀疏直接法
  2. P来自于部分像素，选取带有梯度的像素，舍弃梯度不明显的像素，称之为半稠密的直接法
  3. P为所有像素，稠密直接法，需要GPU加速，可以建立完整地图

8.5 实践：直接法

8.5.1 单层直接法

后续暂略

SLAM, SLAM书籍
slam学习
本文由作者按照 CC BY 4.0 进行授权
最近更新
文章内容

Django Svg sprite引入问题

Ros 通信机制