ROS工作空间内利用rgbd_dataset_freiburg2_pioneer_360数据集配置ORB_SLAM2

ORB-SLAM2官方教程
一、准备工作：安装第三方依赖库
本人的依赖库均存放在文档目录里

1、安装Eigen

sudo apt install libeigen3-dev

2、安装Pangolin

git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
sudo apt install libgl1-mesa-dev libglew-dev cmake
sudo apt install libpython2.7-dev python-pip
git submodule init && git submodule update
sudo python -mpip install numpy pyopengl Pillow pybind11
sudo apt install pkg-config
sudo apt install libegl1-mesa-dev libwayland-dev libxkbcommon-dev wayland-protocols
mkdir build
cd build
cmake ..
cmake --build .
sudo make install

3、安装Sophus

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build
cd build
cmake ..
make

报错问题：
在make过程中会出现如下的错误，需要修改Sophus/sophus中的so2.cpp文件，之后重新make即可

4、安装OpenCV

git clone https://github.com/opencv/opencv.git
cd opencv
git checkout 3.4.3
sudo apt install build-essential
sudo apt install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
sudo apt install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev

报错问题：
在安装相关依赖时可能会出现如下错误，执行下面的命令：

sudo add-apt-repository "deb http://security.ubuntu.com/ubuntu xenial-security main"
sudo apt update
sudo apt install libjasper1 libjasper-dev

接前面的步骤→

mkdir build
cd build
cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local ..
make -j8
sudo make install

报错问题：
在make -j8过程中会出现如下的错误，需要修改gen_java.py中的文件，之后重新make即可

修改内容：assert path[-3:]!=’.in’,path修改为assert path[-4:]!=’.in’,path
重新编译后如果还报错，将f.wrtite(buf)修改为f.write(buf.encode(‘utf-8’))

5、安装PCL

sudo apt install libpcl-dev
sudo apt install pcl-tools

6、安装Ceres Solver

git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver
sudo apt install cmake libeigen3-dev
sudo apt install libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev
mkdir build
cd build
cmake ..
make -j4
sudo make install

7、安装g2o

git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
sudo apt install cmake libeigen3-dev
sudo apt install libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-dev

报错问题：
在安装相关依赖时可能会出现E:软件包libqglviewer-dev没有可安装候选的错误，执行下面的命令：

apt-cache search libqglviewer-dev
sudo apt-get install libqglviewer-dev-qt5

接前面的步骤→

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

二、构建ORB-SLAM2库和示例
因为后期会将ORB_SLAM2和ROS关联运行，所以下述代码均存放在ros工作空间catkin_ws/src目录下进行相关测试，测试内容并没有涉及ROS相关功能，可以看成一个普通的文件夹
1、构建ORB-SLAM2库和示例

git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2 && chmod +x build.sh && ./build.sh

在编译过程中可能会出现的问题及解决方案：
（1）
问题：error: ‘usleep’ was not declared in this scope usleep(3000)

解决：在如下的文件中的头文件上分别加上#include<unistd.h>
ORB_SLAM2/src/LocalMapping.cc
ORB_SLAM2/src/LoopClosing.cc
ORB_SLAM2/src/System.cc
ORB_SLAM2/src/Tracking.cc
ORB_SLAM2/src/Viewer.cc
ORB_SLAM2/Examples/Monocular/mono_euroc.cc
ORB_SLAM2/Examples/Monocular/mono_kitti.cc
ORB_SLAM2/Examples/Monocular/mono_tum.cc
ORB_SLAM2/Examples/RGB-D/rgbd_tum.cc
ORB_SLAM2/Examples/Stereo/stereo_euroc.cc
ORB_SLAM2/Examples/Stereo/stereo_kitti.cc


（2）
问题：CMakeFiles/Stereo.dir/build.make:227:recipe for target ‘…/Stereo’ failed
CMakeFiles/RGBD.dir/build.make:197: recipe for target ‘…/RGBD’ failed
CMakeFiles/Makefile2:67: recipe for target ‘CMakeFiles/RGBD.dir/all’ failed
CMakeFiles/Makefile2:104:recipe for target ‘CMakeFiles/Stereo.dir/all’ failed
解决：把ORB_SLAM2/Examples/ROS/ORB_SLAM2/文件夹下的CMakeLists.txt文件进行修改，在set（LIBS的最后加上-lboost_system

（3）
问题：ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc:233:9: error: ‘usleep’ was not declared in this scope usleep(mT*1000)
解决：在文件ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc开始处加上#include<unistd.h>

2、下载公开数据集
以rgbd_dataset_freiburg2_pioneer_360为例，从TUM下载压缩包，后解压到ORB_SLAM2/data文件夹中

cd ORB_SLAM2
mkdir data
cd data
tar zxvf rgbd_dataset_freiburg2_pioneer_360.tgz

3、下载associate.py测试工具放在orb_slam2/Examples/RGB-D/目录下面

cd ..
cd Example/RGB-D

associate.py中的详细内容如下：

#!/usr/bin/python
# Software License Agreement (BSD License)
#
# Copyright (c) 2013, Juergen Sturm, TUM
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of TUM nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Requirements: 
# sudo apt-get install python-argparse

"""
The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images.

For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches.
"""

import argparse
import sys
import os
import numpy


def read_file_list(filename):
    """
    Reads a trajectory from a text file. 
    
    File format:
    The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
    and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 
    
    Input:
    filename -- File name
    
    Output:
    dict -- dictionary of (stamp,data) tuples
    
    """
    file = open(filename)
    data = file.read()
    lines = data.replace(","," ").replace("\t"," ").split("\n") 
    list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
    list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
    return dict(list)

def associate(first_list, second_list,offset,max_difference):
    """
    Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 
    to find the closest match for every input tuple.
    
    Input:
    first_list -- first dictionary of (stamp,data) tuples
    second_list -- second dictionary of (stamp,data) tuples
    offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
    max_difference -- search radius for candidate generation

    Output:
    matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
    
    """
    first_keys = first_list.keys()
    second_keys = second_list.keys()
    potential_matches = [(abs(a - (b + offset)), a, b) 
                         for a in first_keys 
                         for b in second_keys 
                         if abs(a - (b + offset)) < max_difference]
    potential_matches.sort()
    matches = []
    for diff, a, b in potential_matches:
        if a in first_keys and b in second_keys:
            first_keys.remove(a)
            second_keys.remove(b)
            matches.append((a, b))
    
    matches.sort()
    return matches

if __name__ == '__main__':
    
    # parse command line
    parser = argparse.ArgumentParser(description='''
    This script takes two data files with timestamps and associates them   
    ''')
    parser.add_argument('first_file', help='first text file (format: timestamp data)')
    parser.add_argument('second_file', help='second text file (format: timestamp data)')
    parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
    parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
    parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
    args = parser.parse_args()

    first_list = read_file_list(args.first_file)
    second_list = read_file_list(args.second_file)

    matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))    

    if args.first_only:
        for a,b in matches:
            print("%f %s"%(a," ".join(first_list[a])))
    else:
        for a,b in matches:
            print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))           

4、运行时间戳关联函数associate.py
目的是将rgb 图像序列和depth深度图的序列，进行时间上的关联。运行结束后，orb_slam2/Examples/RGB-D/目录下面会生成associations.txt文件。

python associate.py ../../data/rgbd_dataset_freiburg2_pioneer_360/rgb.txt ../../data/rgbd_dataset_freiburg2_pioneer_360/depth.txt > associations.txt

5、运行ORB_SLAM2
在ORB_SLAM2主目录上运行执行指令

cd ..
cd ..
./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM2.yaml data/rgbd_dataset_freiburg2_pioneer_360 Examples/RGB-D/associations.txt