视觉SLAM十四讲笔记-3-01
文章目录
第三讲-三维空间刚体运动
参考链接： link，
高翔,张涛,等. 视觉 SLAM 十四讲:从理论到实践[M]. 电子工业出版社, 2019.
主要目标：
1.理解三维空间的刚体运动描述方式：旋转矩阵、变换矩阵、四元数和欧拉角
2.掌握Eigen库的矩阵、几何模块的使用方法
3.1 旋转矩阵和变换矩阵
3.1.1点、向量和坐标系
一个空间点的位置可以由3个坐标指定。但是，对于刚体，不仅有位置，还要有姿态。例如：相机可以看成三维空间的刚体，位置是指相机在空间中的哪个地方，姿态是指相机的朝向。怎么用数学语言描述位置和姿态？
三维空间中的某个点的坐标可以用
R
3
R^3
R3来表示。设在线性空间内，找到了一组基(e1,e2,e3)，则任意向量a在这个基下就有了一个坐标：
a
=
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
a
1
e
1
+
a
2
e
2
+
a
3
e
3
a = \left [ e1, e2, e3 \right ] \begin{bmatrix} a1 \\ a2 \\ a3 \end{bmatrix} = a1e1+a2e2+a3e3
a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3
(
a
1
,
a
2
,
a
3
)
T
(a1,a2,a3)^T
(a1,a2,a3)T称为a在此基下的坐标。
一般坐标系是由3个正交的坐标轴组成。例如，当给定x和y轴时，z轴就可以通过右手(左手)法则由x*y定义出来。根据定义方式的不同，坐标系又分为左手系和右手系。常见的右手系(OpenGL、3D Max等)，部分库用左手系(Unity,Direct3D等)。
向量的运算：数乘、加法、减法、内积、外积等。
内积：
a
.
b
=
a
T
b
=
∑
i
=
1
3
a
i
b
i
=
∣
a
∣
∣
b
∣
c
o
s
<
a
,
b
>
a.b = a^Tb=\sum_{i=1}^{3} a_{i}b_{i} = \left | a \right | \left | b\right | cos<a,b>
a.b=aTb=∑i=13aibi=∣a∣∣b∣cos<a,b>
外积：
a
×
b
=
∥
e
1
e
2
e
3
a
1
a
2
a
3
b
1
b
2
b
3
∥
=
[
a
2
b
3
−
a
3
b
2
a
3
b
1
−
a
1
b
3
a
1
b
2
−
a
2
b
1
]
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
b
=
a
∧
b
a \times b = \left \| \begin{matrix} e1 & e2 & e3\\ a1 & a2 & a3\\ b1 & b2 & b3 \end{matrix} \right \| = \begin{bmatrix} a2b3 - a3b2\\ a3b1 - a1b3\\ a1b2-a2b1 \end{bmatrix} = \begin{bmatrix} 0 & -a3& a2\\ a3 & 0& -a1\\ -a2 & a1& 0 \end{bmatrix}b = a\wedge b
a×b=∥∥∥∥∥∥e1a1b1e2a2b2e3a3b3∥∥∥∥∥∥=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=a∧b
外积的结果是一个向量，它的方向垂直于这两个向量，大小为
∣
a
∣
∣
b
∣
sin
<
a
,
b
>
\left | a \right | \left | b \right | \sin <a,b>
∣a∣∣b∣sin<a,b>，是两个向量张成的四边形的有向面积。
对于外积，引入
∧
\wedge
∧，把a写成一个矩阵，这样就可以看做是矩阵与向量的点乘的点乘。事实上是一个反对称矩阵，
∧
\wedge
∧为反对称符号，记：
a
∧
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
a\wedge = \begin{bmatrix} 0 & -a3& a2\\ a3 & 0 & -a1\\ -a2 & a1 &0 \end{bmatrix}
a∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤
3.1.2 坐标系间的欧氏变换
欧氏变换由旋转和平移组成
1.旋转
两个坐标系之间的运动由一个旋转加一个平移组成，这种运动称为刚体运动。
下图为同一个向量 p，它是世界坐标系下的坐标
p
w
p_w
pw 和在相机坐标系的坐标系下的坐标
p
c
p_c
pc 是不同的。这个变换关系由变换矩阵 T 来表示。
设某个单位正交基(e1,e2,e3)经过一次旋转变为(e1’,e2’,e3’)。对于同一个向量a，在两个坐标系下的坐标为
[
a
1
,
a
2
,
a
3
]
T
[a1,a2,a3]^T
[a1,a2,a3]T和
[
a
1
′
,
a
2
′
,
a
3
′
]
T
[a1',a2',a3']^T
[a1′,a2′,a3′]T。因为向量本身没变，因此根据坐标系的定义，有：
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
[
e
1
′
,
e
2
′
,
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
[e1, e2,e3]\begin{bmatrix} a1\\ a2\\ a3 \end{bmatrix} = [e1', e2', e3']\begin{bmatrix} a_{1}' \\ a_2'\\ a_3' \end{bmatrix}
[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
对于上式，左右两边左乘
[
a
1
T
,
e
T
,
e
T
]
[a1^T, e^T, e^T]
[a1T,eT,eT]，那么左边的系数就变成了单位矩阵，所以：
[
a
1
a
2
a
3
]
=
[
e
1
T
e
1
′
e
1
T
e
2
′
e
1
T
e
3
′
e
2
T
e
1
′
e
2
T
e
2
′
e
2
T
e
3
′
e
3
T
e
1
′
e
3
T
e
2
′
e
3
T
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
=
R
a
′
\begin{bmatrix} a1\\ a2\\ a3 \end{bmatrix} = \begin{bmatrix} e_1^Te_1' & e_1^Te_2' & e_1^Te_3'\\ e_2^Te_1' & e_2^Te_2' & e_2^Te_3'\\ e_3^Te_1' & e_3^Te_2' &e_3^Te_3' \end{bmatrix}\begin{bmatrix} a_1' \\ a_2' \\ a_3' \end{bmatrix} = Ra'
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤=Ra′
中间的矩阵定义为R,这个矩阵由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标旋转关系。只要旋转是一样的，这个矩阵就是一样的。 ==矩阵 R 描述了旋转本身，称为旋转矩阵。==该矩阵的各分量是两个坐标系基的内积。由于基向量的长度为1，所以实际上是各基向量夹角的余弦值。
旋转矩阵的性质：旋转矩阵为行列式为1的正交矩阵。
S
O
(
n
)
=
R
∈
R
n
∗
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
SO(n) = {R \in R^{n * n} | RR^T = I, det(R) = 1}
SO(n)=R∈Rn∗n∣RRT=I,det(R)=1
S
O
(
n
)
SO(n)
SO(n)是特殊正交群
R
−
1
=
R
T
R^{-1} = R^T
R−1=RT，旋转矩阵的逆(即转置)描述了一个相反的旋转，因此
a
′
=
R
−
1
a
=
R
T
a
a' = R^{-1}a = R^Ta
a′=R−1a=RTa。
2.平移
考虑世界坐标系中的向量a,经过一次旋转 ® 和一次平移 t (t为 平移向量)后，得到了 a’，把旋转和平移合在一起，有：
a
′
=
R
a
+
t
a' = Ra + t
a′=Ra+t
3.1.3 变换矩阵和齐次坐标
a
′
=
R
a
+
t
a' = Ra + t
a′=Ra+t
上式很好地描述了欧氏空间的旋转和平移。假设进行了两次变换：
b
=
R
1
a
+
t
1
,
c
=
R
2
b
+
t
2
b = R_1a + t_1, c = R_2b + t_2
b=R1a+t1,c=R2b+t2
那么，从a到c的变换为
c
=
R
2
(
R
1
a
+
t
1
)
+
t
2
c = R_2(R_1a+t_1)+t_2
c=R2(R1a+t1)+t2,这样的形式在变换多次后会显得很繁琐。因此，引入齐次坐标和变换矩阵(在三维向量末尾添加1变为四维向量，称为齐次坐标。对于这个四维向量，把旋转和平移写在一个矩阵里，使整个关系变为线性关系，T称为变换矩阵)
[
a
′
1
]
=
[
R
t
0
T
1
]
[
a
1
]
=
T
[
a
1
]
\begin{bmatrix} a' \\ 1 \end{bmatrix} = \begin{bmatrix} R & t\\ 0^T &1 \end{bmatrix} \begin{bmatrix} a\\ 1 \end{bmatrix} = T\begin{bmatrix} a \\ 1 \end{bmatrix}
[a′1]=[R0Tt1][a1]=T[a1]
关于变换矩阵 T，有着特殊的结构：左上角为旋转矩阵，右侧为平移向量，左下角为0，右下角为1。这种矩阵又称为特殊欧氏群。
S
E
(
3
)
=
{
T
=
[
R
t
0
1
]
∈
R
4
∗
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
SE(3) = \left \{ T = \begin{bmatrix} R & t\\ 0 &1 \end{bmatrix} \in R^{4*4} | R \in SO(3), t \in R^3\right \}
SE(3)={T=[R0t1]∈R4∗4∣R∈SO(3),t∈R3}
3.2 实践：Eigen
主要目的：使用 Eigen 表示矩阵和向量
Eigen 是一个 C++ 开源线性代数库。
通过
sudo apt-get install libeigen3-dev
来进行安装，Eigen 头文件默认位置是在 “/usr/include/eigen3” 中。
Eigen 的特殊之处：是一个纯用头文件搭建起来的库，没有类似.so或者.a的二进制文件。在使用时，只需要引入 Eigen 的头文件即可，不需要链接库文件(因为Eigen没有库文件)。
首先，新建文件夹，并在该文件夹下打开 VS Code：
mkdir chap03
cd chap03
mkdir useEigen
cd useEigen
code .
然后在该文件夹下创建两个文件：eigenMatrix.cpp 和 CMakeLists.txt，再建立一个 build 文件夹用于 cmake 编译。
然后在该文件夹下新建 launch.json 文件和 tasks.json 文件。
直接参考这篇博客 link 中的步骤，进行创建。
//launch.json
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "g++ - 生成和调试活动文件",
"type": "cppdbg",
"request":"launch",
"program":"${workspaceFolder}/build/eigenMatrix",
"args": [],
"stopAtEntry": false,
"cwd": "${workspaceFolder}",
"environment": [],
"externalConsole": false,
"MIMode": "gdb",
"setupCommands": [
{
"description": "为 gdb 启动整齐打印",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
],
"preLaunchTask": "Build",
"miDebuggerPath": "/usr/bin/gdb"
}
]
}
//tasks.json
{
"version": "2.0.0",
"options":{
"cwd": "${workspaceFolder}/build" //指明在哪个文件夹下做下面这些指令
},
"tasks": [
{
"type": "shell",
"label": "cmake", //label就是这个task的名字,这个task的名字叫cmake
"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
"args":[
".."
]
},
{
"label": "make", //这个task的名字叫make
"group": {
"kind": "build",
"isDefault": true
},
"command": "make", //这个task要执行的任务是make
"args": [
]
},
{
"label": "Build",
"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
"dependsOn":[ //这个label依赖于上面两个label
"cmake",
"make"
]
}
]
}
然后先用该链接中模板编写 CMakeLists.txt，看是否可以正常调试
cmake_minimum_required(VERSION 3.0)
project(EIGENMATRIX)
#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)
#添加头文件
include_directories(include)
#或者通过绝对路径来进行添加，include_directories(${CMAKE_SOURCE_DIR}/include)
#其中${CMAKE_SOURCE_DIR}就是VSCODE_C_LEARNING_3这个文件夹的绝对路径
add_executable(eigenMatrix eigenMatrix.cpp)
然后保存所有文件，按下 F5，发现可以正常编译调试。
接下来开始 eigen 代码的学习。
由于要使用 eigen 库，因此需要在 CMakeLists.txt 中将 eigen 的头文件添加进去，这里直接使用绝对路径
cmake_minimum_required(VERSION 3.0)
project(EIGENMATRIX)
#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)
#添加头文件
include_directories(include)
include_directories("/usr/include/eigen3")
#或者通过绝对路径来进行添加，include_directories(${CMAKE_SOURCE_DIR}/include)
#其中${CMAKE_SOURCE_DIR}就是VSCODE_C_LEARNING_3这个文件夹的绝对路径
add_executable(eigenMatrix eigenMatrix.cpp)
保存，按下 F5 发现可以正常编译。
但是，在eigenMatrix.cpp文件中添加头文件显示找不到，如下图：
但是直接在终端用 cmake 和 make 编译却正常。
解决办法：可以参考这个链接：link，
这篇博客中提到了两种办法：第一种：建立软链接；第二种：更改 include 中的目录。
第一种：
cd /usr/include
sudo ln -sf eigen3/Eigen Eigen //就是eigen3/Eigen指向Eigen
sudo ln -sf eigen3/unsupported unsupported
然后不用更改 eigenMatrix.cpp 文件中的头文件
第二种：更改 include 中的目录，改为
#include<eigen3/Eigen/Core>
尝试了一下，两种方法都可以。这里我就用第一种吧，建立软链接。
然后编写程序，写完后eigenMatrix.cpp 如下：
#include <iostream>
using namespace std;
#include <ctime>
#include <Eigen/Core>
//稠密矩阵的代数计算(逆，特征值等)
#include <Eigen/Dense>
using namespace Eigen;
#define MATRIX_SIZE 50 //宏定义 矩阵大小
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 2*3 from 1 to 6: \n" << matrix_23 << 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_23为Matrix<float, 2, 3>，v_3d为Vector3d,Vector3d中是double类型
//Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
//因此应该显示转换
Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
//transpose为转置
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;
//同样，不能搞错矩阵的维度
//试着取消下面的注释，看看会报什么错
//error: ‘YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES’ is not a member of ‘Eigen::internal::static_assertion<false>’
//Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;
Eigen::Matrix<double, 2, 1> result_right_dimension = matrix_23.cast<double>() * v_3d;
cout << "result_wrong_dimension:" << result_right_dimension << endl;
//一些矩阵运算
//四则运算就不演示了，直接用+-*/即可
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;
}
按下 F5 输出：
matrix 2*3 from 1 to 6:
1 2 3
4 5 6
1 2 3
4 5 6
[1,2,3,4,5,6] * [3,2,1]=10 28
[1,2,3,4,5,6] * [4,5,6]=32 77
result_wrong_dimension:10
28
random Matrix:
0.680375 0.59688 -0.329554
-0.211234 0.823295 0.536459
0.566198 -0.604897 -0.444451
transpose:
0.680375 -0.211234 0.566198
0.59688 0.823295 -0.604897
-0.329554 0.536459 -0.444451
sum:1.61307
trace:1.05922
times 10:
6.80375 5.9688 -3.29554
-2.11234 8.23295 5.36459
5.66198 -6.04897 -4.44451
inverse:
-0.198521 2.22739 2.8357
1.00605 -0.555135 -1.41603
-1.62213 3.59308 3.28973
det:0.208598
Eigen values =
0.0242899
0.992154
1.80558
Eigen vectors =
-0.549013 -0.735943 0.396198
0.253452 -0.598296 -0.760134
-0.796459 0.316906 -0.514998
time of normal inverse is: 107.482ms
x =-55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734
time of Qr decomposition is: 4.925ms
x =-55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734
time of ldlt decomposition is: 1.525ms
x =-55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734
注：在本代码工程中， 由于 Eigen 库只有头文件， CMakeLists.txt中直接使用
include_directories(“/usr/include/eigen3”) 来添加头文件目录。不需要再用 target_link_libraries 语句将程序链接到库上。不过，对于其他大部分库，大多数情况需要用到链接命令。
而且，==在本工程代码中，CMakeLists.txt添加库的写法也不好，==因为每个人的 Eigen 安装在了不同的位置，每次都得手动修改头文件目录。在之后的工作中，会使用 find_package 命令搜索库，不过这节先这样。
这里只列举了 Eigen 的基本语法操作，可以阅读 Eigen 的官网教程: link 学习更多的 EIgen 知识。
扫一扫
1391
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
