Robot SLAM and autonomous navigation (3)-SLAM function package
table of Contents
One, gmapping
1. Gmapping function package
Input: 1. Depth information 2. IMU information 3. Odometer information
Output: Raster map
$ rosmsg show nav_msgs/OccupancyGrid
2. The principle of raster map value
3. gmapping installation
$ sudo apt-get install ros-kinetic-gmapping
4. Configure the gmapping node
For parameter description, please refer to: http://wiki.ros.org/gmapping
5. Start gmapping demo (depth information is based on lidar)
Open three terminals and run the following commands:
$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch # 启动仿真环境
$ roslaunch mbot_navigation gmapping_demo.launch # 启动建图节点，灰色地图建成，黑色障碍物
$ roslaunch mbot_teleop mbot_teleop.launch #启动键盘控制节点
After creating the map, save the map
$ rosrun map_server map_saver -f cloister_gmapping
# cloister_gmapping是文件名的意思，是自己保存文件名的意思
# 保存的路径在当前/home文件夹下,有两个文件.pgm和.yaml
View the generated map file
6. Start gmapping (kinect)
Poor mapping effect (not recommended)
$ roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch
$ roslaunch mbot_navigation gmapping_demo.launch
$ roslaunch mbot_teleop mbot_teleop.launch
二、hector_slam
1. hector_slam function package
Input: depth information
Output: raster map
2. Install hector_slam
$ sudo apt-get install ros-kinetic-hector-slam
3. Configure hector_mapping node
For parameter description, please refer to: http://wiki.ros.org/hector_slam
4. Start hector_slam demo (depth information is based on lidar)
The first node starts the gazebo simulation, which uses the simulated radar to give a scan scan data; then the nodes below subscribe to this data and process it, and finally get a two-dimensional grid map.
5. Slippage phenomenon
When using the hector_slam function package, the robot must not move too fast, otherwise the calculation will not keep up, and slippage will occur. The
hector algorithm has a big problem, which lies in its self-positioning. It uses its own lidar data for positioning. If the scan data of one scan is basically the same as the scan data of the next, it is difficult to determine whether it is moving. For example, when I give a large enough space, there is a long corridor without any feature points, which is much longer than the laser. When scanning the maximum distance, hector basically loses the ability to create images.
三、cartographer
1. Cartographer function package
2. Cartographer installation
Method one (new method):
sudo apt-get update
sudo apt-get install ros-<your ros version>-cartographer* # 安装全部关于cartographer的包
Method 2: The
cartographer function package is not integrated into the ROS software source, so it needs to be installed by source code compilation. In order not to conflict with other installation packages, it is best to create a special workspace for cartographer. Here we create a workspace catkin_google_ws:
possible problems with the cartographer function package
3. Configure the cartographer node
4. Start the 2D slam demo demo
5. Start the 3D slam demo demonstration
6. Parameter configuration
7. Start cartographer simulation
Four, Karto_slam
1. Overview of Karto_slam
Evaluation:
Karto_SLAM is based on the idea of graph optimization. It uses highly optimization and non-iterative cholesky decomposition for sparse system decoupling as the solution. The graph optimization method uses the average value of the graph to represent the map, and each node represents a position point of the robot trajectory and the sensor measurement data set, and each new node is added, it will be calculated and updated.
The ROS version of Karto_SLAM, in which the Spare Pose Adjustment (SPA) is related to scan matching and closed-loop detection. The more landmarks, the greater the memory requirements. However, the map optimization method has a greater advantage than other methods in drawing in a large environment, because it only contains a point map (robot pose), and the map is calculated after the pose is obtained.
2. Karto_slam specific introduction
The overall program framework
As can be seen from the above figure, the process is actually quite simple. Slam's traditional soft real-time operating mechanism, each time a frame of data is entered, it is processed and then returned.
1) Scan matching
Keywords:
Region of interest: The
matching method is scanTomap, and the region of interest is a rectangular submap, which can also be understood as a reference model.
Search area:
a rectangular area centered on the position estimated by the odometer to indicate the possible range of the final position. When matching, traverse the search area to obtain the position with the highest response value.
Lookup table:
For the data information obtained by the laser, project with a certain angular resolution and angular offset value to obtain a lookup table for matching.
Running-scans:
Real-time maintenance of the local laser data link. The first and last two frames are within a certain distance and meet a certain data scale. Otherwise, the end data frame needs to be deleted. Maintain the current local data link.
Generate submap (because the matching method is ScanToMap).
Generate lookup table:
the meaning of lookup table is that compared with violent matching, do not recalculate each laser data information every time. The laser data information at the same angle and different positions only needs to be indexed once.
The current estimated attitude angle can be obtained by the odometer prediction, the real attitude angle must be nearby;
the laser information represented in the original robot coordinate system (local coordinate system) is mapped to different angles with a certain resolution and offset value , Get a lookup table containing n angles.
Among them:
lookup table -> submap (matching)
The state of the mobile robot itself includes (x, y, θ). The angle problem is effectively solved by the look-up table, and the position problem still needs to be solved at the moment. The discretized search area is used for positioning and multi-resolution is used to increase the search speed. .
The calculation of the response value is described as follows: cast the lookup table to the submap at a certain level. At this time, the submap has been generated by running-scans, and Gaussian is used for blurring. Assuming that there are a total of n points hit by the lookup table, hit Each point in the score is different (the effect of Gaussian blur), the score is accumulated and divided by the highest score that can be achieved.
Lookup table -> submap (rough matching -> fine matching) In
order to improve the search efficiency, a multi-resolution method is adopted, that is, the candidate area is searched with a lower resolution during coarse matching, and then the candidate area is divided to obtain an accurate solution.
Calculating the mean value of the node
In each matching process (coarse matching and fine matching), select several pose states with optimal response values, and take the average value as the matching result.
The optimal response value is one, and a 10^-6 is set as the tolerance. Once the response value of other pose states and the optimal response value are within the tolerance, it is equal; the final node is their average value.
Iteration:
rough matching (obtained mean) -> (as the initial value) precise matching (to give a final average)
computing node covariance - covariance position (rough matching)
computing the covariance nodes - the angle of covariance (coarse matching)
above Association There is a situation where the variance is contrary to the theory, and the component calculated every time needs to be multiplied by a response value.
2) Add vertices and edges
Add vertices: the pose of the key frame.
Add edges: The current frame is connected with the previous frame, running-scans, and near-chains.
There are three main steps to add an edge:
(1) Link to previsous scan
(2) Link to Running scans
RunningScan chain: a certain number of laser data links within a certain distance from the current.
(3) Link to other near chains
NearChain: traverse all nodes within a certain distance from the graph in a breadth-first manner from the current node, and search for chains within a certain range from the sensorManager according to the current id. nearLinkScans.
The first two steps are easy to understand, mainly the third one, with the current node as the search center, perform a breadth search in a certain range (for example, 4m), get all the associated nodes, and then use the nodes to generate a data link (data link needs Contains about 5 consecutive key frames with ID) (excluding the current node, otherwise the data link is discarded), the current node is used for matching, if the response value reaches a certain threshold, an edge is generated. One end of this edge is the current node. One end is the node whose centroid is closest to the centroid of the current node in the data chain.
3) Loop detection
The operation of loop detection is similar to adding adjacent edges, and the steps are more cumbersome:
(1) According to the current node, find all nodes adjacent to it (within a certain distance) from the Graph.
(2) Adopt the breadth-first search method and add adjacent (next) and connected (adjacentVertices) to nearLinkedScans.
(3) Take the front to back from the sensorManager, select candidateScans that are within a certain distance from the current and not in the nearLinkedScans according to the id serial number, and return when the number reaches a certain size.
(4) loopScanMatcher performs scanTomap matching. When the matching response and covariance meet certain requirements, it is considered that the closed loop is detected. Get the adjusted correct pose.
(5) Add link to loop: adjust the edge (global closed loop).
(6) Trigger correctPose: The
first step of spa optimization , first remove those nodes that are adjacent to the current node in time, generate a data link for those nodes that are within the search range and are not adjacent in time, and then proceed Match, if the response value is greater than the threshold, add loopback and perform global optimization.
4) Optimized solution The
entire optimization algorithm uses the LM optimization algorithm. In the original text, the author has a large amount of information on how to obtain the Hessian matrix and optimize the solution efficiently. The LM system is as follows:
The sparseness of the Hessian matrix:
3. karto_slam summary
1) From the perspective of the paper alone, the paper does not introduce the entire framework, most of the content is about optimization methods, the construction of the Hessian matrix, etc. There is very little mention about scan matching, vertices and edges, loop detection, etc.
2) The source code contains two parts, the main function and the library function, with a clear framework and strong readability; except for eigen, no additional dependent libraries are required.
3) There are not as many introductions about karto_slam as hector and gmapping, it may be that the mainstream map update mechanism (probability map) is not adopted. Only the pose node and laser data are saved as a pose-graph.
Five, ORB_SLAM
1. ORB_SLAM function package
基于特征点的实时单目SLAM系统
实时解算摄像机的移动轨迹
构建三维点云地图
不仅适用于手持设备获取的一组连续图像，也可以应用于汽车行驶过程中获取的连续图像
Raul Mur-Artal，J.M.M.Montiel和Juan D.Tardos于2015年发表在IEEE Transactions on Robotics上
2. ORB_SLAM2 installation
1), installation tool & download source code:
sudo apt-get install libboost-all-dev libblas-dev liblapack-dev
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2 # 克隆到home下
2), install eigen3.2.10
去官网下载：http://eigen.tuxfamily.org/index.php?title=Main_Page
解压源码包，并进入目录：
mkdir build
cd build
cmake ..
make
sudo make install
3), compile g2o
cd ~/ORB_SLAM2/Thirdparty/g2o/
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make
4), compile DBoW2
cd ~/ORB_SLAM2/Thirdparty/DBoW2/
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make
5), compile Pangolin
sudo apt-get install libglew-dev
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build && cd build
cmake ..
cmake --build .
6), compile ORM_SLAM
cd ~/ORB_SLAM2
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make
7), compile the function package (configure ROS environment)
# 首先修改.bashrc文件
$ cd ~
$ gedit .bashrc
# 打开.bashrc文件在最后一行加入
source ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/devel/setup.bash
$ source ~/.bashrc
Note: The seventh step, the problem of compiling the function package:
solution:
修改 ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt,添加 -lboost_system
3. Start monocular SLAM example (based on data package)
1) First download the data package TUM: this is mainly indoor, monocular and RGB-D
https://vision.in.tum.de/data/datasets/rgbd-dataset/download#freiburg1_desk
在ORB-SLAM2下新建文件夹Data，把测试的数据解压在这里。
TUM数据集分为相机fr1，fr2，fr3，对应TUM1-3.yaml；
一般第一次测试用fr1/xyz这个数据集，这个就是x，y，z方向来回动，用来检测一下系统出没出什么问题。
其他的数据看名字就知道，比如desk就是在桌子附近来回转，room就是在房间里面扫来扫去。
值得注意的是，运行其他数据集的时候，单目不一定能追踪成功，在台式机上能成功的在虚拟机上也不一定能成功，这就需要我们进行一些调整，比如调整初始化需求数量等，这个关系到对SLAM系统的理解。
2) Open three terminals and run the following three commands:
$ roscore
# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml
# 进入数据包所在目录下，运行命令
# /camera/image_raw表示播放的数据要发布的话题名称，此时可以在界面中看到建图的效果
$ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw
PATH_TO_VOCABULARY：算法参数文件，在ORB_SLAM2/Vocabulary中，将其中的压缩包解压即可；
PATH_TO_SETTINGS_FILE：相机参数设置文件，需要对camera进行标定产生，也可以使用ORB_SLAM2/Examples/ROS/ORB_SLAM2中已有的设置文件Asus.yaml。
4. Start AR example (based on data package)
$ roscore
# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml
# 进入数据包所在目录下，运行命令
# /camera/image_raw表示播放的数据要发布的话题名称，此时可以在界面中看到建图的效果
$ rosbag play rgbd_dataset_freiburg1_desk.bag /camera/rgb/image_color:=/camera/image_raw
Click the interface Insert Cube, see the square inserted in, show the AR effect
5. Start the ORB_SLAM example (real camera)
$ roslaunch mbot_navigation usb_cam_remap.launch
# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml
6. Start AR example (real camera)
The camera needs to be moved to complete the map construction
$ roslaunch mbot_vision usb_cam_remap.launch
# 进入ORB_SLAM2目录下启动Mono功能节点。
# 看到开启的等待数据的可视化建图界面
$ rosrun ORB_SLAM2 MonoAR Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml