slam1~2初识SLAM

slam解决的问题

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

一些基础知识

  1. 有线性方程 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)不可能,因为增广矩阵的秩小于等于系数矩阵的秩。
  2. 高斯分布是什么?它的一维形式是什么样子?它的高维形式是什么样子?
    一维高斯分布密度函数(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π)2n211exp(21(xμ)T1(xμ))
    二维高斯分布图:
    在这里插入图片描述

经典视觉SLAM框架

在这里插入图片描述
整个视觉SLAM流程包括以下步骤:

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

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 k1时刻到 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(xk1,μ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(xk1,μ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个自由度来描述,即三个轴的平移和三个轴的旋转

实践

  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!
  1. 使用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!
  1. 在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

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值