SLAM初探

SLAM初探

1.视觉SLAM框架

整个视觉SLAM包括以下流程

• 传感器信息读取，主要是相机图像信息的读取和处理
• 前端视觉里程计，它的任务是估算相邻图像之间相机的运动和局部的地图
• 后端优化，接受不同时刻视觉里程计输出的相机位姿以及回环检测的信息，对他们进行规模更大的优化，得到全局一致的轨迹和地图
• 回环检测，判断机器人是否到达先前的位置
• 建图，根据估计的轨迹，建立环境地图

2.数学描述

第一行式子为运动方程，第二行式子为观测方程

$$\begin{gathered} x_k=f(x_{k-1},u_k,w_k) \\ {z}_{k,j}=h(y_j,x_k,v_{k,j}) \end{gathered}$$

x代表机器人自身的姿态信息，涉及到SLAM中的定位问题。y代表传感器观测的路标点信息，涉及到SLAM中建图问题。SLAM解决的问题是已知控制输入u和观测值z，推断最大可能性的x和y。

本质上是个状态估计问题：如何通过带有噪声的测量数据估计内部的状态变量

状态估计问题的求解，依据方程形式和噪声分布可以分为线性/非线性和高斯/非高斯系统。其中先行高斯系统的无偏最优估计可以由卡尔曼滤波给出。在复杂的非线性非高斯系统中可以使用扩展卡尔曼滤波（EKF）、粒子滤波（PF）以及非线性优化解决。目前广泛使用非线性优化方案来进行状态估计。

3.实践部分

3.1 HelloSLAM（使用Cmake）

源程序main.cpp

#include <iostream>

int main(){
    cout<<"hello SLAM!"<<endl;
    return 0;
}

CMakeLists.txt

project(helloSLAM)

add_executable(sayhello main.cpp)

最终利用cmake指令会编译出sayhello.a可执行文件

3.2 HelloSLAM（使用库）

源程序main.cpp

#include <hello.h>

int main(){
    sayHello();
    return 0;
}

调用库hello.cpp

#include <hello.h>
#include <iostream>

using namespace std;

void sayHello(){
    cout<<"hello SLAM!"<<endl;
}

头文件hello.h

#ifndef __HELLO_H__
#define __HELLO_H__

void sayHello();

#endif

CMakeLists.txt

project(helloSLAM)

include_directories("include")               //提供头文件的声明
add_library(libhello src/hello.cpp)          //将src目录下的hello.cpp编译成一个库
add_executable(sayhello main.cpp)            //将源程序编译成一个可执行文件sayhello.a
target_link_libraries(sayhello libhello)     //把编译好的库链接到可执行文件

4.作业

4.1 阅读综述论文(个人认为很有必要读一读)

• 《Visual simultaneous localization and mapping: a survey》
• 《Past，Present，and Future of Simultaneous Localization And Mapping: Towards the Robust-Perception Age》
• 《基于单目视觉的同时定位与地图构建方法综述》

4.2 CMake实践

书写⼀个由 cmake 组织的 C++ ⼯程，要求如下：

• include/hello.h 和 src/hello.c 构成了 libhello.so 库。hello.c 中提供⼀个函数 sayHello()，调⽤此函数时往屏幕输出⼀⾏“Hello SLAM”。我们已经为你准备了 hello.h 和 hello.c 这两个⽂件，见“code/”⽬录下。

• ⽂件 useHello.c 中含有⼀个 main 函数，它可以编译成⼀个可执⾏⽂件，名为“sayhello”。

• 默认⽤ Release 模式编译这个⼯程。

• 如果⽤户使⽤ sudo make install，那么将 hello.h 放⾄/usr/local/include/下，将 libhello.so 放⾄/usr/local/lib/下。

请按照上述要求组织源代码⽂件，并书写 CMakeLists.txt。

本工程的目录结构如下：

CMakeList.txt文件代码如下：

//指定cmake版本
cmake_minimum_required(VERSION 3.0)
//命名工程
project(sayhello)
//设置编译模式为Release
set(CMAKE_BUILD_TYPE "Release")
//添加可执行文件sayhello
add_executable(sayhello useHello.cpp)
//引入头文件
include_directories(${PROJECT_SOURCE_DIR}/include)
//生成hello.cpp库
add_library(hello SHARED ${PROJECT_SOURCE_DIR}/src/hello.cpp)
//链接动态库
target_link_libraries(sayhello hello)
//将hello.h安装到/usr/local/include目录下
install(FILES ${PROJECT_SOURCE_DIR}/include/hello.h DESTINATION /usr/local/include)
//将动态链接库libhello.so安装到/usr/local/lib目录下
install(TARGETS hello LIBRARY DESTINATION /usr/local/lib)

4.3 运行ORB-SLAM2

编译ORB-SLAM2工程这部分就不细说了，主要注意三点

• Pangolin不能安装最新版本 需要安装旧版本
• 对于处理器核数小的用户，比如使用虚拟机的，建议将build.sh里的make -j 都改成make 以防止编译时死机
• src目录下的部分源代码文件缺少头文件<unistd.h>
• include目录下LoopClosing.h中第49行的代码应改成如下代码：

typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
        Eigen::aligned_allocator<std::pair<KeyFrame* const, g2o::Sim3> > > KeyFrameAndPose;

4.3.1 测试RGB-D数据集

这里需要到TUM官网下载数据集，本人下载的数据集是rgbd_dataset_freiburg1_desk，在associations文件夹下已经有了它的关联文件fr1_desk.txt。执行下面命令，跑RGB-D的demo：

./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml ./Examples/RGB-D/rgbd_dataset_freiburg1_desk ./Examples/RGB-D/associations/fr1_desk.txt 

效果如下图：

---

4.3.2 用自己的摄像头实时跑ORB-SLAM2

首先需要调用OpenCV来打开摄像头并读取帧送入ORB-SLAM2系统中进行追踪，代码如下：

//
// Created by xiang on 11/29/17.
//

// 该文件将打开你电脑的摄像头，并将图像传递给ORB-SLAM2进行定位
// 需要opencv
#include <opencv2/opencv.hpp>

// ORB-SLAM的系统接口
#include "System.h"

#include <string>
#include <chrono>   // for time stamp
#include <iostream>

using namespace std;

// 参数文件与字典文件
// 如果你系统上的路径不同，请修改它
string parameterFile = "./myslam.yaml";
string vocFile = "./ORBvoc.txt";

int main(int argc, char **argv) {

    // 声明 ORB-SLAM2 系统
    ORB_SLAM2::System SLAM(vocFile, parameterFile, ORB_SLAM2::System::MONOCULAR, true);

    // 获取相机图像代码
    cv::VideoCapture cap(0);    // change to 1 if you want to use USB camera.

    // 分辨率设为640x480
    cap.set(CV_CAP_PROP_FRAME_WIDTH, 640);
    cap.set(CV_CAP_PROP_FRAME_HEIGHT, 480);

    // 记录系统时间
    auto start = chrono::system_clock::now();

    while (1) {
        cv::Mat frame;
        cap >> frame;   // 读取相机数据
        auto now = chrono::system_clock::now();
        auto timestamp = chrono::duration_cast<chrono::milliseconds>(now - start);
        SLAM.TrackMonocular(frame, double(timestamp.count())/1000.0);
    }

    return 0;
}

还需要对自己的相机进行标定，标定文件如下：

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 500.0
Camera.fy: 500.0
Camera.cx: 320.0
Camera.cy: 240.0

Camera.k1: 0
Camera.k2: 0
Camera.p1: 0
Camera.p2: 0
Camera.k3: 0

# Camera frames per second 
Camera.fps: 30.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid   
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid  
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast      
ORBextractor.iniThFAST: 10
ORBextractor.minThFAST: 5

将上述两个文件都放到Example文件夹下，注意这里也将Vocabulary文件夹下的ORBvoc.txt文件放进来了

为了执行myslam.cpp内的内容 ，我们还需要在整个工程CMakeLists.txt中添加为myslam生成可执行内容的代码

add_executable(myslam Examples/Monocular/myslam.cpp)
target_link_libraries(myslam ${PROJECT_NAME})

编译运行效果如下：

对桌子上的物体进行实时环境建模

---