slam解决的问题
SLAM是Simultaneous Localization and Maping,即“同时定位与地图构建”。具体指的是搭载特定传感器的主体,在没有环境先验信息的情况下,于运动过程中建立环境的模型,同时估计自己的运动。
一些基础知识
- 有线性方程
A
x
=
b
Ax=b
Ax=b, 若已知
A
,
b
A,b
A,b,需要求解
x
x
x,该如何求解?这对
A
A
A和
b
b
b有那些要求?
当 r ( A ) < r ( A b ) r(A)<r(Ab) r(A)<r(Ab)时,方程组无解;
当 r ( A ) = r ( A b ) = n r(A)=r(Ab)=n r(A)=r(Ab)=n时,方程组有唯一解;
当 r ( A ) = r ( A b ) < n r(A)=r(Ab)<n r(A)=r(Ab)<n时,方程组无穷解;
r ( A ) > r ( A b ) r(A)>r(Ab) r(A)>r(Ab)不可能,因为增广矩阵的秩小于等于系数矩阵的秩。 - 高斯分布是什么?它的一维形式是什么样子?它的高维形式是什么样子?
一维高斯分布密度函数(probability density function):
f ( x ) = 1 2 π σ e x p ( − ( x − μ ) 2 2 σ 2 ) {f(x)=\frac{1}{\sqrt{2\pi}\sigma}exp(-{\frac{(x-\mu)^2}{2{\sigma}^2}}) } f(x)=2πσ1exp(−2σ2(x−μ)2)
不同的 μ \mu μ值和 σ \sigma σ值对对曲线的影响
多维高斯分布公式:
p ( x ; μ , ∑ ) = 1 ( 2 π ) n 2 ∣ ∑ ∣ 1 2 e x p ( − 1 2 ( x − μ ) T ∑ − 1 ( x − μ ) ) {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))} p(x;μ,∑)=(2π)2n∣∑∣211exp(−21(x−μ)T∑−1(x−μ))
二维高斯分布图:
经典视觉SLAM框架
整个视觉SLAM流程包括以下步骤:
- 传感器信息读取:
主要是相机图像信息的读取和预处理。如果是机器人还可能有码盘、惯性传感器等信息的读取和同步 - 前端视觉里程计:主要任务是估算相邻图像间相机的运动以及局部地图的样子
- 后端(非线性)优化:后端接受不同时刻视觉里程计测量的相机位姿,以及回环检测的信息,对它们进行优化,得到全局一致的轨迹和地图
- 回环检测:回环检测判断机器人是否到达过先前的位置。如果检测到回环,它会把信息提供给后端进行处理
- 建图:它根据估计的轨迹,建立与任务要求对应的地图
SLAM问题的数学表述
假设带有传感器的设备在未知环境里运动,根据slam的定义这里有两件事情我们要关心,一个是设备的位置,一个是所处环境建图。我们将连续的时间运动变成离散时间
t
=
1
,
.
.
.
.
.
K
t=1,.....K
t=1,.....K所处位置
x
x
x,即各个时刻的位置记为
x
1
,
x
2
.
.
.
.
x
k
x_1,x_2....x_k
x1,x2....xk,这些构成了轨迹。地图方面我们假设地图由许多路标组成,而每个时刻,传感器会测量到一部分路标点。得到它们的观测数据。假设路标点共有
N
N
N个,用
y
1
,
y
2
.
.
.
.
.
.
y
N
y_1,y_2......y_N
y1,y2......yN表示它们。
在这样的假设条件下,有如下两件事情描述:
a. 什么是运动?我们要考察从
k
−
1
k-1
k−1时刻到
k
k
k时刻,位置
x
x
x如何变化?
我们用以下运动方程来描述:
x
k
=
f
(
x
k
−
1
,
μ
k
,
ω
k
)
x_k=f(x_{k-1},\mu_k,\omega_k)
xk=f(xk−1,μk,ωk)
μ
k
\mu_k
μk为运动传感器的读数或者输入,
ω
k
\omega_k
ωk为该过程加入的噪声
b. 什么是观测?在
k
k
k时刻于
x
k
x_k
xk处探测到了某一个路标
y
j
y_j
yj
我们用观测方程来描述,即描述的为在
x
k
x_k
xk位置上看到某个路标点
y
j
y_j
yj时,产生一个观测数据
z
k
,
j
z_{k,j}
zk,j
z
k
,
j
=
h
(
y
j
,
x
k
,
v
k
,
j
)
z_{k,j}=h(y_j,x_k,v_{k,j})
zk,j=h(yj,xk,vk,j)
这里
v
k
,
j
v_{k,j}
vk,j表示的是观测里的噪声
综上SLAM的过程可以总结为两个基本方程:
{
x
k
=
f
(
x
k
−
1
,
μ
k
,
ω
k
)
,
k
=
1
,
.
.
.
.
k
z
k
,
j
=
h
(
y
j
,
x
k
,
v
k
,
j
)
,
(
k
,
j
)
∈
o
\left\{ \begin{aligned} 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{aligned} \right.
{xkzk,j=f(xk−1,μk,ωk),k=1,....k=h(yj,xk,vk,j),(k,j)∈o
其中o是一个集合,记录着在哪个时刻观察到了哪个路标,这两个方程描述了最基本的SLAM问题:当知道运动测量的读数
μ
\mu
μ,以及传感器的读数
z
z
z时,如何求解定位问题(估计
x
x
x)和建图问题(估计
y
y
y),这时我们就把SLAM问题建模成了一个状态估计问题:如何通过带有噪声的测量数据,估计内部的、隐藏着的状态变量?
状态估计问题的求解,与两个方程的具体形式,以及噪声服从哪种分布有关,按照运动和观测方程是否线性,噪声是否服从高斯分布进行分类,分为线性/非线性和高斯/非高斯系统。其中线性高斯系统是最简单的,它的无偏的最优估计可以由卡尔曼滤波器给出。而在复杂的非线性非高斯系统中,我们会使用以扩展卡尔曼滤波器和非线性优化两大类方法求解。
在三维空间中,位置由6个自由度来描述,即三个轴的平移和三个轴的旋转
实践
- 安装linux系统,请参考博文:ubuntu8.04双系统安装
- 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!
- 使用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!
- 在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