ORB-SLAM辅助Cartographer重定位建图效果改进系列—第二篇

任务动机：根和据暴露的问题，视觉重定位模块更新了第二个版本。与之前相比主要有如下改进：1. 使用两种定位模式(基于地图和基于BOW)实现稳定定位； 2. 定位频率保持5Hz左右； 3. 只需要激光板回传优化后的低频轨迹即可。

任务描述：整理开发产出包括功能描述、接口描述、配置文档、重要参数描述到文档。

1. 模块介绍

        本模块功能是基于雷达SLAM输出轨迹真值的视觉全局定位。定位采用两种模式：基于特征地图定位和基于关键帧定位。优先基于特征地图定位，在特征地图稀疏或没有成功建立特征地图的区域使用基于关键帧定位。其中基于特征地图的定位结果更准确。目前算法足够鲁棒，在严苛环境下能够连续给出全局定位的pose值。

1.1 功能

系统整体框架如下图。

视觉板输入输出

Input：

1. 保存视觉帧的开始和结束信号

2. 激光雷达高频的tf话题

3. 激光雷达低频的优化后的轨迹

Output：

1. 如果基于特征地图定位，则给出唯一的定位结果pose值

2. 如果基于关键帧定位，则给出BOW查询结果中相似的前几个pose值，其中第一个pose为最好的pose

1.2 视觉板与制图板通信过程

保存帧

        使用SaveFrames.srv服务，服务定义如下, 服务名字“save_frames”。步骤：制图板发送保存帧请求(save_flag=true)，视觉板将建图标志位置位后返回完成标志（finish_flag=true）；制图板完成数据采集后，发送停止保存帧请求(save_flag=false)通知视觉板停止采集图像，视觉板将建图标志位复位后返回完成标志（finish_flag=true）。另外，在保存帧时，视觉板会订阅制图板发出的话题名为“/tf”的话题，并将激光雷达的位姿以TUM格式保存为本地文件。

SaveFrames.srv:

bool save_flag # true-发送保存帧请求；false-发送停止保存帧请求
---
bool finish_flag # 接收到开启或停止保存信号

注：

1. 每次开启保存帧自动删除上一次保存的帧。

2. “/tf”话题为“tf2_msgs/TFMessage.msg”，定义见：https://github.com/ros/geometry2/blob/noetic-devel/tf2_msgs/msg/TFMessage.msg

3. 建图，使用CartoToVisionMap.srv服务，服务定义如下。

• 制图板为client，视觉板为server,服务名字“carto_to_vision_map”

制图板完成位姿优化后向视觉板发送建图请求（map_flag=true），并且将所有scan帧的时间戳和pose（poses）发送给视觉板，视觉板返回接收到信号标志（finish_flag=true）,然后视觉板开始建图。

• 制图板为server，视觉板为client, 服务名字“carto_to_vision_map_rep”

视觉板建完图后发送建图完成标志（map_flag=true）

CartoToVisionMap.srv:

bool map_flag # true-发送建图请求；
geometry_msgs/PoseStamped[] poses # 激光雷达所有scan帧的时间戳和位姿(雷达到世界坐标系)
---
bool finish_flag # true-接收到建图信号

注：geometry_msgs/PoseStamped定义见：https://github.com/ros/common_msgs/blob/noetic-devel/geometry_msgs/msg/PoseStamped.msg

4. 重定位

使用Relocalization.srv,服务定义如下, 服务名字“relocalization”。步骤：制图板向视觉板发送重定位请求（reloc_flag=true），视觉板返回接收到信号标志（finish_flag=true）。另外，若存储中已有地图，返回map_built_flag=true，开启重定位，定位成功结果后以话题形式发出；若存储中没有地图，返回map_built_flag=false，重定位未开启，需要保存好地图后再次发送重定位请求。

Relocalization.srv:

bool reloc_flag # true-发送重定位请求;
---
bool finish_flag # true-接收到重定位信号
bool map_built_flag # true-已有地图 false-没有地图

注：定位成功后发送的话题为“geometry_msgs/ PoseArray”，名字为“reloc_pose”。topic定义见：https://github.com/ros/common_msgs/blob/noetic-devel/geometry_msgs/msg/PoseArray.msg

话题详解

1. 时间戳为图像帧的时间戳，pose为雷达到世界坐标系（T_wl）。

2. pose数组的第一个pose为最佳结果，其余为次优结果，次优结果随机排序。

3. 话题的“frame_id”用于指示哪种定位结果，若其为“map_reloc”则表示为特征地图定位结果；若其为“kf_reloc”则表示为关键帧定位结果。

2. 部署方法

2.1 安装Ubuntu

官网参考链接：http://wiki.t-firefly.com/zh_CN/Firefly-RK3399/01-bootmode.html （主要是“更新固件”内容）

1. 方法概述：使用一台安装ubuntu（我的是ubuntu 16.04）的电脑通过Type-C data cable数据线与Firefly-RK3399 开发板相连，在电脑上运行命令通过官方“Linux_Upgrade_Tool”工具烧录系统。硬件连接如下图

   

    

2. 硬件环境

• Firefly-RK3399 开发板
• Type-C数据线

• HDMI转VGA转接头（安装系统时不用连，安装完系统显示用）。

安装步骤

1. 通过官网链接下载Ubuntu 16.04镜像文件压缩包（https://pan.baidu.com/s/17xsxTXmVig4t6U3qlvFVBQ#list/path=%2F&parentPath=%2Fsharelink1414141670-882445172942659，提取码：byl8），然后下载“Ubuntu->Ubuntu16.04->Public-> FIREFLY-RK3399-UBUNTU16.04-GPT-20190910-1001.img.7z”。另外，也可以从firefly官网链接（http://www.t-firefly.com/doc/download/page/id/3.html）中的“固件->Ubuntu(GPT)”进入下载链接

2. 在安装ubuntu的电脑解压“FIREFLY-RK3399-UBUNTU16.04-GPT-20190910-1001.img.7z”，文件中包含升级工具“Linux_Upgrade_Tool”文件和镜像文件（pack(2)中文件，.img文件大小2.3G）。

3. 设备先断开电源适配器和 Type-C 数据线的连接

• USB 一端连接主机，Type-C 一端连接开发板 Type-C 母口
• 按住设备上的 RECOVERY （恢复）键并保持
• 接上电源（蓝色指示灯不亮）
• 大约两秒钟后，松开 RECOVERY 键
• 在“Linux_Upgrade_Tool”文件夹下打开终端，然后运行如下命令

sudo chmod a+x upgrade_tool

sudo upgrade_tool uf FIREFLY-RK3399-UBUNTU16.04-GPT-20190910-1001.img

# 等待安装，安装需要几分钟，安装完成会显示“Upgrade fireware ok.”，等待1分钟左右系统会自动重启，蓝色指示灯亮起，可连接显示器。若安装失败参见官网参考链接寻找解决办法

# 更新

sudo apt-get update

sudo apt-get upgrade

2.2 环境安装

参考链接：

https://blog.csdn.net/nannan7777/article/details/107161720

https://blog.csdn.net/weixin_44436677/article/details/105587986

https://blog.csdn.net/weixin_45137708/article/details/105565800

1. 安装cmake

sudo apt-get install cmake

2. 安装gcc、g++、git

sudo apt-get install gcc g++ git

3. 安装opencv 3.2.0

参考链接：

http://dev.t-firefly.com/forum.php?mod=viewthread&tid=12143

https://blog.csdn.net/radiantjeral/article/details/82193370

4. 安装依赖

sudo apt-get install build-essential

# 下一条中libgtk2.0-dev的依赖
sudo apt-get install libpng12-dev
sudo apt-get install libgdk-pixbuf2.0-dev
sudo apt-get install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
sudo apt-get install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev

5. 下载源码

• opencv官网手动下载3.2.0版本：https://opencv.org/releases/page/5/

git clone https://github.com/opencv/opencv.git
cd opencv/
git checkout 3.2.0

6. 编译与安装

#首先进入安装包的根目录
mkdir build
cd build
cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local ..
make
sudo make install

7. 安装pangolin

• 安装依赖

sudo apt-get install libglew-dev libpython2.7-dev libboost-dev libboost-thread-dev libboost-filesystem-dev

• 下载源码

git clone https://github.com/stevenlovegrove/Pangolin

• 编译与安装

# 进入Pangolin根目录

mkdir build
cd build
cmake ..
make -j4
sudo make install

8. 安装eigen3

• 下载源码

wget https://bitbucket.org/eigen/eigen/get/3.2.10.tar.bz2
tar -xjf 3.2.10.tar.bz2
# 重命名
mv eigen-eigen-b9cd8366d4e8/ eigen-3.2.10

• 编译与安装

进入eigen-3.2.10根目录

mkdir build
cd build
cmake ..
make
sudo make install

9. 安装ROS Kinetic，参照官网链接

# 使用清华镜像

sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt update
sudo apt-get install ros-kinetic-desktop-full

#如果报错unable to …,就再更新一次，再安装一下

sudo apt update
sudo apt-get install ros-kinetic-desktop-full
apt-cache search ros-kinetic
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential
sudo apt install python-rosdep
sudo rosdep init
rosdep update

# 开启主节点验证是否安装完成
roscore

10. 安装ROS的uvc_camera包

sudo apt-get install ros-kinetic-uvc-camera

11. 下载、编译与运行ORB_SLAM2_standlone

• 新建ROS工作空间

mkdir -p ~/orb_slam2_ws/src
cd ~/orb_slam2_ws/src
catkin_init_workspace
cd ~/orb_slam2_ws/
catkin_make
source devel/setup.bash

• 下载ORB_SLAM2_standlone源码

cd ~/orb_slam2_ws/src
git clone https://gitee.com/yiluzhang/orb_slam2_standalone.git

• 编译

cd orb_slam2_standalone
mkdir -p data/Frames
chmod +x build.sh
./build.sh
cd ../..
catkin_make

• 更改配置文件（orb_slam2_standalone/Examples/Monocular/ iisg_carto.yaml）里设置参数

# 是否开启可视化界面
# 0-close viewer 1-open viewer
bUseViewer: 0

 

#相机参数
# 相机topic
Camera_topic: “/camera/image_raw”

# 相机参数
Camera.width:640
Camera.height:480

# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 644.148203
Camera.fy: 641.932757
Camera.cx: 454.125051
Camera.cy: 263.812763
Camera.k1: -0.390540
Camera.k2: 0.147062
Camera.p1: 0.000460
Camera.p2: -0.001930

# 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: 1

# tf话题参数

# 话题名
TF_topic: "/tf"

# 与激光雷达pose对应的tf话题中的“frmae_id”名
Odom_frame: "odom"

# 与激光雷达pose对应的tf话题中的“child_frame_id”名
Tracking_frame: "laser_link"

 

相机和激光雷达的外参

# 相机坐标系到激光雷达坐标系的相对位姿
T_laser_cam:

 

# 文件夹路径
# 程序运行中需要保存和加载文件的data文件夹路径，更换data文件夹的路径，相对路径和绝对路径都可以
data_dir: "src/orb_slam2_standalone/data/"

• 启动主程序

# 打开新终端，启动ROS主节点

roscore

# 启动相机，用bag测试时不用启动相机, /dev/video*为相机设备名

roslaunch orb_slam2_ros uvc_cam.launch device:=/dev/video*

# 打开新终端，启动ORB_SLAM2_standlone节点

cd ~/orb_slam2_ws
source devel/setup.bash

# 第一个参数为ORB词袋文件，第二个参数为配置参数文件

rosrun orb_slam2_ros Mono src/orb_slam2_standalone/Vocabulary/ORBvoc.txt src/orb_slam2_standalone/Examples/Monocular/iisg_carto.yaml

• 启动测试节点：在没有lidar制图板时，使用bag包单独测试视觉板

# 打开新终端，启动接收topic和service测试节点

功能：

1.接收主程序建图完成发出的service

2.显示主程序重定位成功发出的topic

cd ~/orb_slam2_ws
source devel/setup.bash
rosrun orb_slam2_ros test_sub_srver

# 打开新终端，启动发送topic和service测试节点，path为lidar pose的TUM格式.txt文件，该节点可用于向主程序发送topic和service，键盘输入数字‘x’，然后按下“enter”键发送命令，‘x’对应命令如下：

1）发送保存帧请求，然后打开新终端使用rosbag play [包路径]回放

2）发送停止保存帧请求，待回放完bag包后发送

3）发送建图请求，从下面启动测试节点[path]中读取lidar poses发送

4）发送停止建图请求，建图完成后会自动停止，可以不用手动发送

5）发送重定位请求，然后打开新终端使用rosbag play [包路径]回放

6）发送停止重定位请求，重定位可一直运行，无需停止

注：

      1）目前主程序只支持顺序切换，往回切需重启主程序，帧和地图会自动保存到存储，无需重复保存帧和建图

      2）保存帧时使用的bag包需要包含相机和tf话题

cd ~/orb_slam2_ws
source devel/setup.bash
rosrun orb_slam2_ros test_node [path]

• 关键参数说明

调节以下参数可以改善建图结果。

     1）初始化参数

# 选择单应矩阵H或基础矩阵F的阈值，取值0.4-0.45，越大越偏向H
init_RH: 0.4

# 三角化时剔除异常点的重投影误差阈值, 取值4.0-10.0
init_th2: 8.0

# 初始化成功所需最小点数占经过随机采样一致性筛选后点数的比例，取值# 0.7-0.9
nMinGood_ratio: 0.9

# 初始化成功所需的最小3D点数，取值80-100
init_tracked_mappoints: 100

     2）跟踪参数

# 关键帧跟踪成功所需当前帧与参考关键帧最小匹配点数，取值0-15
refer_nmatches: 5

# 关键帧跟踪成功所需当前帧与参考关键帧共视的最小3D点数, 取值0-10
refer_nmatchesMap: 3

# 普通帧跟踪成功所需当前帧与参考帧最小匹配点数，取值0-20
motion_nmatches: 5

# 普通帧跟踪成功所需当前帧与参考帧共视的最小3D点数, 取值0-10
motion_nmatchesMap: 3

# 重定位后跟踪局部地图成功所需当前帧与局部地图共视的最小3D点数，#_取值0-50
local_map_reloc_nmatches: 10

# 正常跟踪局部地图成功所需当前帧与局部地图共视的最小3D点数, 取值# 0-30
local_map_nmatches: 5

# 3D点跟踪丢失后插入关键帧的距离间隔，取值0.2-1.0，单位：米
KF_delta_distance: 0.2

# 3D点跟踪丢失后插入关键帧的航向角间隔，取值2.0-10.0，单位：度
KF_delta_yaw: 2.0