SLAM学习记录（一）

最新推荐文章于 2022-11-19 18:11:32 发布

_月_夜_

最新推荐文章于 2022-11-19 18:11:32 发布

阅读量364

点赞数 3

分类专栏： SLAM

本文链接：https://blog.csdn.net/yue__ye/article/details/108527219

版权

SLAM 专栏收录该内容

1 篇文章 1 订阅

订阅专栏

ROS的学习结束。可以开始SLAM的学习了，开心。
说到SLAM的学习，就不得不提到高翔大大的《视觉 SLAM 十四讲》，准备跟着《视觉 SLAM 十四讲》把基础的东西学一遍。

1、初识SLAM

pdf版本下载地址在B站上看到一个UP大大给的地址，搬运过来
SLAM(simultaneous localization and mapping):即时定位于地图构建
各种相机做SLAM的特点：
单目相机（单目SLAM）:有个很大的问题尺度不确定性。
双目相机：深度量程和精度受双目的基线与分辨率限制，而且视差的计算非常消耗计算资源
深度相机：测量范围窄、噪声大、视野小、易受日光干扰、无法测量透射材质（主要用于室内SLAM，室外SLAM干扰太多）

1.1SLAM模块

SLAM框架：
在这里插入图片描述
视觉里程计：通过前后两帧图像估计相机运动，并恢复场景的空间结构。把相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。
但是会出现累计漂移(因为估计的相机位置并不是没有误差，视觉里程计把相邻时刻的运动“串”起来，误差就会累计)，通过后端优化跟回环检测校正。
后端优化：主要指处理 SLAM 过程中噪声的问题。
回环检测：主要解决位置估计随时间漂移的问题。设置标志物让机器人具备识别曾到达过的场景的能力。或者判断图像间的相似性，来完成回环检测。
建图：构建地图的过程。可以分为度量地图与拓扑地图两种。
度量地图强调精确地表示地图中物体的位置关系。这种地图需要存储每一个格点的状态，耗费大量的存储空间，很小的一点转向误差，可能会导致两间屋子的墙出现重叠，使得地图失效。
拓扑地图强调地图元素之间的关系。不擅长表达具有复杂结构的地图。

1.2、SLAM数学表述

用一个数学模型表示相机k-1时间到k时间的位置变化关系:
在这里插入图片描述
xk表示相机在k时刻的位置，xk-1表示相机在k-1时刻的位置,wk表示噪声，uk表示传感器输入数据。字面意思就是通过相机k-1时刻的位置，相机k时刻的输入数据，以及噪声，得到相机在k时刻的位置。这就是运动方程。
观测方程：在这里插入图片描述
当小萝卜在 xk 位置上看到某个路标点 yj，产生了一个观测数据 zk,j，vk,j是噪声。

1.3、实践：编程基础

安装 Linux 操作系统（这个，已用Virtualbox+ubuntu，具体安装看前面博客）
建立并进入文件夹：
在这里插入图片描述
建立helleslam.cpp写入代码并保存。

输出helloslam，表示程序正常运行。
新建 CMakeLists.txt文件。
输入：

# 声明要求的 cmake 最低版本
cmake_minimum_required( VERSION 2.8 )
# 声明一个 cmake 工程
project( HelloSLAM )
# 添加一个可执行程序
# 语法：add_executable( 程序名 源代码文件 ）
add_executable( helloSLAM helloSLAM.cpp )

在当前目录运行cmake .
在这里插入图片描述
用meke进行编译并运行./helloSLAM

但是cmake会生成中间文件。可以建立一个build文件夹，进入build文件下，用cmake …运行上一层目录的CMakeLists.txt
用刚才的办法实践了下。

实践写一个库文件：
新建一个文件libHelloSLAM.cpp输入：

# include<iostream>
using namespace std;
void printHello(){
    cout<<"helloslam"<<endl;
}

在CMakeLists.txt中加入：add_library( hello libHelloSLAM.cpp )
cmake ,make 得到文件libhello.a
在这里插入图片描述
在 Linux 中，库文件分成静态库和共享库两种①。静态库以.a 作为后缀名，共享库以.so
结尾。所有库都是一些函数打包后的集合，差别在于静态库每次被调用都会生成一个副本，
而共享库则只有一个副本，更省空间。如果我们想生成共享库而不是静态库，只需用：
add_library( hello_shared SHARED libHelloSLAM.cpp )
在这里插入图片描述
得到libhello_share.so文件
实践写一个头文件：
创建文件libHelloSLAM.h输入：

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

实践下头文件+库文件是否可以调用。
新建文件useHello.cpp，输入：

#include "libHelloSLAM.h"
int main(int argc,char **argv){
    printHello();
    return 0;
}

在CMakeLists.txt中加上

add_executable( useHello useHello.cpp )
target_link_libraries( useHello hello_shared )

接着cmake和make一下
在这里插入图片描述

2、三维空间的刚体运动

2.1、旋转矩阵

只有当我们指定这个三维空间中的某个坐标系时，才可以谈论该向量在此坐标系下的坐标。
如果我们确定一个坐标系，也就是一个线性空间的基 (e1, e2, e3)，那就可以谈论向量 a 在这组基下的坐标：
在这里插入图片描述
对于 a, b ∈ R3，内积可以写成：

外积为：

外积的方向垂直于这两个向量，大小为 |a| |b|sin ⟨a, b⟩，是两个向量张成的四边形的有向面积。
我们还能用外积表示向量的旋转。
设定一个惯性坐标系，（或者叫世界坐标系），可以认为它是固定不动的，同时，相机或机器人则是一个移动坐标系，机器人位姿转换到世界坐标系中，这个转换关系由一个矩阵 T 来描述，由一个旋转和一个平移两部分组成。设某个单位正交基(e1, e2, e3) 经过一次旋转，变成了 (e′1, e′2, e′3)。对于同一个向量 a，它在两个坐标系下的坐标为 [a1, a2, a3]T 和 [a′1, a′2, a′3]T。有：
在这里插入图片描述
们对上面等式左右同时左乘
有：
R就是旋转矩阵。这个矩阵由两组基之间的内积组成。旋转矩阵有一些特别的性质。事实上，它是一个行列式为 1 的正交矩阵。反之，行列式为 1 的正交矩阵也是一个旋转矩阵。所以，我们可以把旋转矩阵的集合定义如下：在这里插入图片描述
SO(n) 是特殊正交群（Special Orthogonal Group）的意思。
由于旋转矩阵为正交阵，它的逆（即转置）描述了一个相反的旋转。按照上面的定义
方式，有：

除了旋转之外还有一个平移。考虑世界坐标系中的向量 a，经过一次旋转（用 R 描述）和一次平移 t 后，得到了 a′，那么把旋转和平移合到一起，有：
在这里插入图片描述
这里的变换关系不是一个线性关系。假设我们进行了两次变换：R1, t1 和 R2, t2，满足：

但是从 a 到 c 的变换为：

引入齐次坐标和变换矩阵重写式

我们把一个三维向量的末尾添加 1，变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成了线性关系。
在齐次坐标中，某个点 x 的每个分量同乘一个非零常数 k 后，仍然表示的是同一个点。可以把所有坐标除以最后一项，得到一个点唯一的坐标表示。
在这里插入图片描述
依靠齐次坐标和变换矩阵，两次变换的累加就可以有很好的形式：

变换矩阵 T，它具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为 0 向量，右下角为 1。这种矩阵又称为特殊欧氏群（Special Euclidean Group）：
在这里插入图片描述
与 SO(3) 一样，求解该矩阵的逆表示一个反向的变换：

2.2、实践：Eigen

安装Eigen库:sudo apt-get install libeigen3-dev
在ch3/useEigen/目录下新建文件eigenMatrix.cpp，输入：

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

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
 ****************************/
using namespace std;

int main(int argc, char **argv)
{
    // Eigen 以矩阵为基本数据单元。它是一个模板类。它的前三个参数为：数据类型，行，列
    // 声明一个 2*3 的 float 矩阵
    Eigen::Matrix<float, 2, 3> matrix_23;
    // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是 Eigen::Matrix
    // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>
    Eigen::Vector3d v_3d;
    // 还有 Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
    // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
    // 更简单的
    Eigen::MatrixXd matrix_x;
    // 这种类型还有很多，我们不一一列举

    // 下面是对矩阵的操作
    // 输入数据
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 输出
    cout << matrix_23 << endl;
    // 用()访问矩阵中的元素
    for (int i = 0; i < 1; i++)
        for (int j = 0; j < 2; j++)
            cout << matrix_23(i, j) << endl;

    v_3d << 3, 2, 1;
    // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
    // 但是在这里你不能混合两种不同类型的矩阵，像这样是错的
    // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;

    // 应该显式转换
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

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

    // 一些矩阵运算
    // 四则运算就不演示了，直接用对应的运算符即可。
    matrix_33 = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl
         << endl;

    cout << matrix_33.transpose() << endl;   //转置
    cout << matrix_33.sum() << endl;         //各元素和
    cout << matrix_33.trace() << endl;       //迹
    cout << 10 * matrix_33 << endl;          //数乘
    cout << matrix_33.inverse() << endl;     //逆
    cout << matrix_33.determinant() << endl; //行列式

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

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

    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);

    clock_t time_stt = clock(); // 计时
    // 直接求逆
    Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout << "time use in normal invers is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms"
         << endl;
    // 通常用矩阵分解来求，例如 QR 分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time use in Qr compsition is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms"
         << endl;

    return 0;
}

如果是用的vscode，按F1,输入C/C++，选中C/C++:编辑配置（JSON），在c_cpp_properties.json文件中includePath部分加入Eigen库的路径。
在这里插入图片描述
点左边调试图标，选择g++ -生成和调试活动文件
在tasks.json里面添加“-I”,“Eigen库路径”

调试生成eigenMatrix

运行eigenMatrix，得到

2.3、旋转向量和欧拉角

变换矩阵描述一个六自由度的三维刚体运动太冗余。
任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量。对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。
假设有一个旋转轴为 n，角度为 θ 的旋转，显然，它对应的旋转向量为 θn。由旋转向量到旋转矩阵的过程由罗德里格斯公式（Rodrigues’s Formula ）表明，由于推导过程比较复杂，我们不作描述，只给出转换的结果：
在这里插入图片描述
欧拉角使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。
1.绕物体的 Z 轴旋转，得到偏航角 yaw；
2. 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；
3. 绕旋转之后的 X 轴旋转，得到滚转角 roll。
可以使用 [r, p, y]T 这样一个三维的向量描述任意旋转。
在俯仰角为±90◦ 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由三次旋转变成了两次旋转）。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。

2.4、四元数

一个四元数 q 拥有一个实部和三个虚部。
在这里插入图片描述
其中 i, j, k 为四元数的三个虚部。这三个虚部满足关系式：

由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数：

这里，s 称为四元数的实部，而 v 称为它的虚部。如果一个四元数虚部为 0，称之为实四元数。反之，若它的实部为 0，称之为虚四元数。
假设某个旋转是绕单位向量 n = [nx, ny, nz]T进行了角度为 θ 的旋转，那么这个旋转的四元数形式为：
在这里插入图片描述
反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：

在四元数中，任意的旋转都可以由两个互为相反数的四元数表示
有两个四元数 qa, qb，它们的向量表示为 [sa, va], [sb, vb]。

四元数 qa, qb 的加减运算为：
在这里插入图片描述
乘法是把 qa 的每一项与 qb 每项相乘，最后相加

写成向量形式

四元数的共轭是把虚部取成相反数：

四元数共轭与自己本身相乘，会得到一个实四元数，其实部为模长的平方:

四元数的模长定义为：
在这里插入图片描述
一个四元数的逆为：

四元数可以与数相乘：

点乘是指两个四元数每个位置上的数值分别相乘：

假设一个空间三维点 p = [x, y, z] ∈ R3，以及一个由轴角 n, θ 指定的旋转。三维点 p 经过旋转之后变成为 p′。如果使用矩阵描述，那么有 p′ = Rp。
三维空间点用一个虚四元数来描述：在这里插入图片描述
用四元数 q 表示这个旋转：

那么，旋转后的点 p′ 即可表示为这样的乘积：

设四元数 q = q0 + q1i + q2j + q3k，对应的旋转矩阵 R 为：

反之，由旋转矩阵到四元数的转换如下。假设矩阵为 R = {mij}, i, j ∈ [1, 2, 3]，其对应的四元数 q 由下式给出：
在这里插入图片描述
相似变换比欧氏变换多了一个自由度，它允许物体进行均匀的缩放，其矩阵表示为：

仿射变换的矩阵形式如下：

仿射变换只要求 A 是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫正交投影。
射影变换是最一般的变换，它的矩阵形式为:
在这里插入图片描述
它左上角为可逆矩阵 A，右上为平移 t，左下缩放 aT。
由于采用齐坐标，当 v = 0时，我们可以对整个矩阵除以 v 得到一个右下角为 1 的矩阵；否则，则得到右下角为 0 的矩阵。因此，2D 的射影变换一共有 8 个自由度，3D 则共有 15 个自由度。

2.5、实践：Eigen 几何模块

在ch3/useGeometry/目录下创建文件useGeometry.cpp。
输入以下代码：
cout .precision(3)表述保留三位小数。

#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

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

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

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

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

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

// 四元数
// 可以直接把 AngleAxis 赋值给四元数，反之亦然
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
cout<<"quaternion = \n"<<q.coeffs() <<endl; // 请注意 coeffs 的顺序是 (x,y,z,w), w 为实部，前三者为虚部
// 也可以把旋转矩阵赋给它
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<<q.coeffs() <<endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q*v; // 注意数学上是 qvq^{-1}
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

return 0;
}

在这里插入图片描述
同样生成一个useGeometry文件，可以在终端输入就能查看。

2.6、可视化演示

书中并没有相关代码，代码在高翔大大的github中，github网址
打开网址，进入ch3中的visualizeGeometry。复制粘贴visualizeGeometry.cpp中的内容。
新建CMakeLists.txt.复制粘贴CMakeLists中的内容。
新建build文件夹，进入build文件夹，cmake …然后make
在这里插入图片描述
需要安装pangolin.

2.6.1安装pangolin

新建一个文件夹放pangolin的github代码。
进入文件夹，并克隆代码：git clone https://github.com/stevenlovegrove/Pangolin.git
在这里插入图片描述
安装pangolin的依赖包sudo apt-get install libglew-dev
进入pangolin文件夹下，新建build文件夹。
进入build 文件夹cmake …然后make再sudo make install
进入ch3/visualizeGeometry/build文件夹下，再次cmake …和make

看到生成visualizeGeometry文件。运行这个文件。
在这里插入图片描述