ORB-SLAM3算法复现及遇到的问题解决 T265

GitHub - zhaozhongch/orbslam3_ros

https://github.com/UZ-SLAMLab/ORB_SLAM3

安装ORB-SLAM3教程_Smile_HT的博客-CSDN博客

参考以上作者

环境：ubuntu18.04 ROS melodic 原作者ORB-SLAM3

一、编译源码

将CMakelists文件中opencv4.4改为3.2

chmod +x build.sh
./build.sh

二、非ROS运行KITTI 双目数据集

/ORB_SLAM3/Examples/Stereo路径下

./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence
./stereo_kitti ../../Vocabulary/ORBvoc.txt ./KITTI1.yaml /home/flycar/dataset/kitti/stereo

KITTI1.yaml如下

%YAML:1.0

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

#针孔相机
Camera.type: "PinHole"
#鱼眼相机
#Camera.type: "KannalaBrandt8"

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 718.856
Camera.fy: 718.856
Camera.cx: 607.1928
Camera.cy: 185.2157
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

Camera.width: 1241
Camera.height: 376

#基线长度
Camera.bf: 0.53716

#深度阈值
ThDepth: 35.0
# Camera frames per second 
Camera.fps: 10.0

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

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

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

# 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		
#检测fast角点阈值	
#没有检测到角点的前提下降低阈值
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#-----------------------------------------------------------------
# Viewer Parameters
#-----------------------------------------------------------------
#可视化参数
Viewer.KeyFrameSize: 0.1
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 1
Viewer.PointSize: 2
Viewer.CameraSize: 0.15
Viewer.CameraLineWidth: 2
Viewer.ViewpointX: 0
Viewer.ViewpointY: -100
Viewer.ViewpointZ: -0.1
Viewer.ViewpointF: 2000

三、非ROS运行TUM RGB-D数据集

/ORB_SLAM3/Examples/RGB-D路径下

./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association

注意在使用TUM数据集前需要以下操作对齐数据，在数据集文件夹目录下

python 1.py rgb.txt depth.txt > accelerometer.txt 
1.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])))

./rgbd_tum ../../Vocabulary/ORBvoc.txt ./TUM1.yaml /home/flycar/dataset/TUM/rgbd_dataset_freiburg2_pioneer_slam/ /home/flycar/dataset/TUM/rgbd_dataset_freiburg2_pioneer_slam/accelerometer.txt

四、ROS下运行KITTI数据集

下载所需包:链接: https://pan.baidu.com/s/1HPaD0h6D8lVmsy1_Qus8aw 提取码: qikq 

安装第三方库
cd orbslam3_ros
./build_thrid_party.sh

安装Pangolin0.6
cd Pangolin0.6
mkdir build
cd build
cmake ..
sudo make -j16
sudo make install

编译ORB-SLAM3
cd ../..
catkin_make

在bag包运行ORB-SLAM3
roscore

cd orbslam3/
source devel/setup.bash
rosrun orbslam3 ros_stereo_inertial src/orbslam3_ros/Vocabulary/ORBvoc.txt src/orbslam3_ros/Examples/Stereo-Inertial/EuRoC.yaml true

rosbag play /home/flycar/bag/MH_03_medium.bag /cam0/image_raw:=/gray_image0 /cam1/image_raw:=/gray_image1 /imu0:=/gx5/imu/data

五、usb摄像头 实时运行ORBSLAM3

修改../orbslam3/src/orbslam3_ros/Examples/ROS/src 路径下ros_mono.cc文件如图所示位置。

重新编译工程

cd orbslam3/

catkin_make

 安装 运行摄像头
 sudo apt-get install ros-melodic-usb-cam
 roslaunch usb_cam usb_cam-test.launch

 roscore
 
 cd orbslam3/
 source devel/setup.bash
 rosrun orbslam3 ros_mono src/orbslam3_ros/Vocabulary/ORBvoc.txt 
 src/orbslam3_ros/Examples/Monocular/EuRoC.yaml 

六、T265 运行 ORB-SLAM3

Intel Realsense T265使用教程_熊猫飞天的博客-CSDN博客_t265

修改 ../orbslam3/src/orbslam3_ros/Examples/ROS/src/下ros_stereo.cc

 重新编译

cd orbslam3

catkin_make

roscore 

roslaunch realsense2_camera demo_t265.launch

cd orbslam3
source devel/setup.bash 
rosrun orbslam3 ros_stereo src/orbslam3_ros/Vocabulary/ORBvoc.txt src/orbslam3_ros/Examples/Stereo/EuRoC.yaml true