使用D435i实时运行ORB-SLAM2（一）

ubuntu20.04

一、安装第三方库

1.1安装依赖

sudo apt-get update
sudo apt-get install git gcc g++ vim make cmake

1.2安装Pangolin

Pangolin压缩包

（1）安装依赖

sudo apt-get install libglew-dev libboost-dev libboost-thread-dev libboost-filesystem-dev
sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev libpng-dev

（2）配置并编译
cd 时记得更改下载的文件名

cd Pangolin 
mkdir build && cd build
cmake -DCPP11_NO_BOOST=1 ..
make
sudo make install

（3）执行命令查看是否安装成功

cd ../examples/HelloPangolin
mkdir build && cd build
cmake ..
make
./HelloPangolin

若安装成功会出弹出下面画面

1.3安装Eigen3

Eigen3下载地址
（1）克隆源码

git clone git@github.com:eigenteam/eigen-git-mirror.git

运行指令

cd eigen3
mkdir build && cd build
cmake ..
make
sudo make install

（2）将eigen的头文件复制到根目录下的调用文件夹中

sudo cp -r /usr/local/include/eigen3 /usr/include 

二、 安装ORB-SLAM2

ORB-SLAM2下载地址
（1）克隆源码

git clone git@gitcode.com:mirrors/raulmur/ORB_SLAM2.git

运行指令

cd ORB_SLAM2
chmod +x build.sh
./build.sh

chmod +x 赋予shell文件运行权限，注意系统若硬件不行（线程少），将build.sh里的make -j（默认启用最大线程）修改为make，防止卡死，电脑性能好可增加数字（线程）：如make -j4/j8/jx

（2）编译时候出现错误

error: ’usleep’ was not declared in this scope

在错误对应的头文件前面加上：

#include <unistd.h>

出现错误：

error: static assertion failed: std::map must have the same value_type as its allocator

把ORB-SLAM2源码目录中include/LoopClosing.h文件中的

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

修改为

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

（3）下载官方TMU数据集
下载地址
单目示例运行TUM数据集的rgbd_dataset_freiburg1_xyz

将下载好后的rgbd_dataset_freiburg1_xyz文件夹放置在ORB-SLAM2的目录下

（4）运行单目

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM2.yaml rgbd_dataset_freiburg1_xyz/

三、安装D435i驱动

3.1安装IntelRealSense SDK2.0

（1）注册服务器的publish key

sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE

（2）把服务器添加到仓库列表

sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u

（3）安装库

sudo apt update
sudo apt install librealsense2-dkms
sudo apt install librealsense2-utils
sudo apt install librealsense2-dev
sudo apt install librealsense2-dbg

（4）查看是否安装成功

realsense-Viewer

3.2 realsense-ros的安装

（1）创建一个工作空间

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src/

（2）克隆源码 realsense-ros

git clone https://github.com/IntelRealSense/realsense-ros.git
cd realsense-ros/
git checkout `git tag | sort -V | grep -P "^2.\d+\.\d+" | tail -1`
cd ..

（3）安装ros包ddynamic_reconfigure

sudo apt install ros-noetic-ddynamic-reconfigure

（4）编译

catkin_init_workspace
cd ..
catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install

（5）下载功能包源码 到src目录下

git clone https://gitcode.com/mirrors/ros-drivers/usb_cam/tree/develop/src

（6）重新编译工作空间并配置环境

cd ~/catkin_ws/
catkin_make
source ~/catkin_ws/devel/setup.bash

（7）测试摄像头

roslaunch usb_cam usb_cam-test.launch

使用rqt_graph查看消息

（8）如果没有显示图像，可能是如下原因
a.报错

RLException: [usb_cam-test.launch] is neither a launch file in package [usb_cam] nor is [usb_cam] a launch file name
The traceback for the exception was written to the log file

原因：未配置环境

source ~/catkin_ws/devel/setup.bash

如果不想每次都输一遍的话，可以将这句添加到 ~/.bashrc 文件中

sudo gedit ~/.bashrc

保存退出，执行：

source ~/.bashrc

b.摄像头显示花屏
解决：摄像头的默认像素格式是mjpeg格式的，launch文件中定义的像素格式是yuyv格式的（第六行），将其修改为mjpeg

但是此时终端中会提示警告

eprecated pixel format used, make sure you did set range correctly

然后在usb_cam-develop源码src文件中找到usb_cam.cpp文件447行，video_sws_ = sws_getContext…之前添加：

{
    AVPixelFormat pixFormat;
    switch (avcodec_context_->pix_fmt) {
    case AV_PIX_FMT_YUVJ420P :
        pixFormat = AV_PIX_FMT_YUV420P;
        break;
    case AV_PIX_FMT_YUVJ422P  :
        pixFormat = AV_PIX_FMT_YUV422P;
        break;
    case AV_PIX_FMT_YUVJ444P   :
        pixFormat = AV_PIX_FMT_YUV444P;
        break;
    case AV_PIX_FMT_YUVJ440P :
        pixFormat = AV_PIX_FMT_YUV440P;
        break;
    default:
        pixFormat = avcodec_context_->pix_fmt;
        break;
    }
    avcodec_context_->pix_fmt = pixFormat;
  }

重新编译工作空间并配置环境

c.摄像头黑屏 / 识别到的不是我们连接的摄像头
解决：电脑中有很多虚拟的摄像头，我们要把usb_cam-test.launch中的第三行video*换成我们usb连接的摄像头
1、打印电脑的摄像头

ls /dev/video*

2、但是我打印出来的是一堆video,实际上我电脑上只有一个自带摄像头和一个外接的usb摄像头

/dev/video0  /dev/video2  /dev/video4  /dev/video6
/dev/video1  /dev/video3  /dev/video5  /dev/video7

3、那我们如何知道上边打印出来的一堆摄像头到底那个才是真的摄像头呢？那就利用v4l2-ctl命令：

sudo apt install v4l-utils

4、下边我们如下指令就一个一个的进行实验：

v4l2-ctl -d  /dev/video0 --all

5、打印了一大堆信息，我们不用管，看重点打印了camera ok：有camera ok那就说明这个摄像头的确是真的，反之亦然，至于是电脑还是外接，在usb_cam-test.launch中的第三行video*这里更改对应的数字，然后进行测试就可以了

四、ROS实时运行ORB-SLAM2

4.1 相机标定

1、标定板
2、noetic摄像头标定工具安装

sudo apt-get install ros-noetic-camera-calibration

3、#启动摄像头

roslaunch usb_cam usb_cam-test.launch

4、新开一个终端，开始标定
注意参数设置(根据实际情况修改):–size 9x6 --square 0.016

rosrun camera_calibration cameracalibrator.py --size 9x6 --square 0.016 image:=/usb_cam/image_raw camera:=/usb_cam --k-coefficients=4

按照x（左右）、y（上下）、size（前后）、skew（倾斜）等方式移动棋盘，直到x,y,size,skew的进度条都变成绿色位置。

都变成绿色位置了就按下CALIBRATE按钮，等一段时间就可以完成标定。
完成后点击SAVE按钮就会把数据保存到默认的目录下：
终端会显示以下信息：

('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')

此文件夹tmp在根目录（其他位置，计算机）中显示，双击进入会看到calibrationdata.tar.gz压缩包，打开会发现有图片和文件数据信息：

注意：要及时的把标定结果压缩包从默认路径tep文件夹中移出，要不然隔段时间再次打开可能随时就没有标定的结果了。
点击commit按钮，提交数据并退出程序，可以看到提示写入文件head_camera.yaml。
（5）找到标定结果文件后，按照其数据修改Examples/ROS/ORB_SLAM2目录下Asus.yaml。

4.2编译ORB-SLAM2

将ORB_SLAM2源码文件夹复制到~/catkin_ws/src文件夹下，在ORB_SLAM2文件夹下打开终端
（1）修改ros示例源文件
为Examples/ROS/ORB_SLAM2/src路径下的所有.cc文件添加头文件

#include <unistd.h>

将单目ros_mono.cc和ros_mono_ar.cc文件的订阅:

ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);

替换为：

ros::Subscriber sub = nodeHandler.subscribe("/usb_cam/image_raw", 1, &ImageGrabber::GrabImage,&igb);

（2）编译

cd ~/catkin_ws/src/ORB-SLAM2
chmod +x build.sh build_ros.sh
./build.sh

4.3实时运行ORB-SLAM2

roslaunch usb_cam usb_cam-test.launch
rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/Asus.yaml