slam1~2初识SLAM

slam解决的问题

SLAM是Simultaneous Localization and Maping，即“同时定位与地图构建”。具体指的是搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。

一些基础知识

1. 有线性方程 $Ax=b$, 若已知$A,b$，需要求解$x$，该如何求解？这对$A$和$b$有那些要求？
   当$r(A)<r(Ab)$时，方程组无解；
   当$r(A)=r(Ab)=n$时，方程组有唯一解；
   当$r(A)=r(Ab)<n$时，方程组无穷解；
   $r(A)>r(Ab)$不可能，因为增广矩阵的秩小于等于系数矩阵的秩。
2. 高斯分布是什么？它的一维形式是什么样子？它的高维形式是什么样子？
   一维高斯分布密度函数(probability density function):
   $$f(x)=\frac{1}{\sqrt{2\pi }\sigma }exp(-\frac{(x-\mu {)}^2}{2{\sigma }^2})$$
   不同的$\mu$值和$\sigma$值对对曲线的影响
   
   多维高斯分布公式：
   $$p(x;\mu ,\sum )=\frac{1}{(2\pi {)}^{\frac{n}{2}}|\sum {|}^{\frac{1}{2}}}exp(-\frac{1}{2}(x-\mu {)}^T{\sum }^{-1}(x-\mu ))$$
   二维高斯分布图：
   

经典视觉SLAM框架

整个视觉SLAM流程包括以下步骤：

1. 传感器信息读取：
   主要是相机图像信息的读取和预处理。如果是机器人还可能有码盘、惯性传感器等信息的读取和同步
2. 前端视觉里程计：主要任务是估算相邻图像间相机的运动以及局部地图的样子
3. 后端（非线性）优化：后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图
4. 回环检测：回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理
5. 建图：它根据估计的轨迹，建立与任务要求对应的地图

SLAM问题的数学表述

假设带有传感器的设备在未知环境里运动，根据slam的定义这里有两件事情我们要关心，一个是设备的位置，一个是所处环境建图。我们将连续的时间运动变成离散时间$t=1,.....K$所处位置$x$,即各个时刻的位置记为$x_1,x_2....x_k$，这些构成了轨迹。地图方面我们假设地图由许多路标组成，而每个时刻，传感器会测量到一部分路标点。得到它们的观测数据。假设路标点共有$N$个，用$y_1,y_2......y_N$表示它们。
在这样的假设条件下，有如下两件事情描述：
a. 什么是运动？我们要考察从$k-1$时刻到$k$时刻，位置$x$如何变化？
我们用以下运动方程来描述：
$$x_k=f(x_{k-1},{\mu }_k,{\omega }_k)$$
${\mu }_k$为运动传感器的读数或者输入，${\omega }_k$为该过程加入的噪声

b. 什么是观测？在$k$时刻于$x_k$处探测到了某一个路标$y_j$
我们用观测方程来描述，即描述的为在$x_k$位置上看到某个路标点$y_j$时，产生一个观测数据$z_{k,j}$
$$z_{k,j}=h(y_j,x_k,v_{k,j})$$
这里$v_{k,j}$表示的是观测里的噪声
综上SLAM的过程可以总结为两个基本方程：
$$\{\begin{array}{rl}{x}_k& =f(x_{k-1},{\mu }_k,{\omega }_k),k=1,....k\\ z_{k,j}& =h(y_j,x_k,v_{k,j}),(k,j)\in o\end{array}$$
其中o是一个集合，记录着在哪个时刻观察到了哪个路标，这两个方程描述了最基本的SLAM问题：当知道运动测量的读数$\mu$，以及传感器的读数$z$时，如何求解定位问题(估计$x$)和建图问题(估计$y$)，这时我们就把SLAM问题建模成了一个状态估计问题:如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？
状态估计问题的求解，与两个方程的具体形式，以及噪声服从哪种分布有关，按照运动和观测方程是否线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。其中线性高斯系统是最简单的，它的无偏的最优估计可以由卡尔曼滤波器给出。而在复杂的非线性非高斯系统中，我们会使用以扩展卡尔曼滤波器和非线性优化两大类方法求解。
在三维空间中，位置由6个自由度来描述，即三个轴的平移和三个轴的旋转

实践

1. 安装linux系统，请参考博文：ubuntu8.04双系统安装
2. linux编写C++代码并编译运行测试
   编写源代码

//slambook2/ch2/helloSLAM.cpp
#include<iostream>
using namespace std;

int main(int argc, char **argv){
	cout <<"Hello SLAM!"<<endl;
	return 0;
} 

终端编译输出

g++ helloSLAM.cpp
./a.out
Hello SLAM!

3. 使用cmake
   为什么要用cmake？理论上任何一个c++程序都可以用g++来编译，但是当工程大的时候用g++编译是不现实的，因此我们采用cmake来管理源代码，在一个cmake工程中，我们会用cmake命令生成一个makefile文件，然后用make命令根据整个makefile文件的内容编译整个工程。
   首先我们新建一个CMakeList.txt文件，它用于告诉cmake要对目录下的文件做什么事情。CMakeLists.txt文件的内容需要遵守cmake的语法，内容如下：

//slambook2/ch2/CMakeLists.txt
# 声明要求的cmake最低版本
cmake_minimum_required(VERSION 2.8)

#声明一个cmake工程
project(HelloSLAM)

#添加一个可执行程序
#语法：add_executable(程序名 源代码文件)
add_executable(helloSLAM helloSlam.cpp)

同时为了让cmake生成的中间文件与源代码分离，我们通常会创建一个build文件夹终端输入

mkdir build
cmake ..
make
./helloSLAM
Hello SLAM!

4. 在cmake中使用库
   在C++工程中，并非所有的代码都会编译成可执行文件，只有main函数文件才会生成可执行文件，而另一些代码我们希望打包成一个东西供其他程序调用，这个东西称为库，如Eigen库提供了很多矩阵代数运算。

编辑一个库文件

//slambook2/ch2/libHelloSLAM.cpp
#include<iostream>
using namespace std;

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

对库文件进行编译，生成libhello_shared.so

add_library(hello_shared SHARED libHelloSLAM.cpp)

此时有个问题是.so文件是一个二进制文件包，并不知道里面包含有那些函数，因为需要提供一个库文件对应的头文件，说明库里包含的函数，拿到头文件就可以调用这个库，对应的头文件为：

#slambook2/ch2/libHelloSLAM.h
#ifndef LIBHELLOSLAM_H_
#define LIBHELLOSLAM_H_
//宏定义为了防止重复引用
void printHello();

#endif

根据上面编译好的.so库文件和上面的头文件，就可以在main函数中调用了，调用方式为

#include "libHelloSLAM.h"

int main(int argc, char **argv){
	printHello();
	return 0;
}

对应的在CMakeList.txt中，讲main函数链接到调用的库上

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

完整的CMakeLists.txt

//slambook2/ch2/CMakeLists.txt
# 声明要求的cmake最低版本
cmake_minimum_required(VERSION 2.8)

#声明一个cmake工程
project(HelloSLAM)

#添加一个可执行程序
#语法：add_executable(程序名 源代码文件)
add_executable(helloSLAM helloSlam.cpp)

#对库文件进行编译
add_library(hello_shared SHARED libHelloSLAM.cpp)
#添加调用库的main函数可执行文件
add_executable(useHello useHell0.cpp)
#将main函数链接到库上
target_link_libraries(useHello hello_shared)

reference

https://www.cnblogs.com/rhyswang/p/6798973.html