Ubuntu20.04用D435i运行VINS-Mono

搜到的一些资料都是Ubuntu16.04下用D435i运行VINS-Mono，ros版本都是Kinetic，对于Ubuntu20.04下的noetic版本的ros的资料较少，走了一些坑，终于算是跑起来了，记录一下过程
电脑配置：Intel NUC 12WSKi5（8+512）
系统：Ubuntu20.04
ros版本：noetic

参考：https://blog.csdn.net/qq_41839222/article/details/86552367
https://blog.csdn.net/weixin_44580210/article/details/89789416
https://blog.csdn.net/Chasinglight/article/details/128328183
https://blog.csdn.net/zardforever123/article/details/129042974

前置要求：
1.已经安装完成Ubuntu20.04系统
2.已经安装完成ROS-noetic
3.更换过国内源，并且有梯子（有些步骤如果下载缓慢就需要梯子）

一、安装VINS-Mono

直接进入VINS-Mono在Github的开源地址：
https://github.com/HKUST-Aerial-Robotics/VINS-Mono
根据里面的步骤：

1.首先安装需要的ros包，如果安装的是完整ros，应该是都安装过的

sudo apt-get install ros-noetic-cv-bridge ros-noetic-tf ros-noetic-message-filters ros-noetic-image-transport

2.安装ceres solver，进VINS-Mono给的链接：

http://ceres-solver.org/installation.html
根据官网步骤：
首先安装依赖glog和eigen3：终端中逐行输入以下代码

# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev libgflags-dev
# Use ATLAS for BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# Eigen3
sudo apt-get install libeigen3-dev
# SuiteSparse (optional)
sudo apt-get install libsuitesparse-dev

将下载好的ceres-solver-2.1.0.tar.gz移到主目录下，在主目录下打开终端：逐行输入以下代码

tar zxf ceres-solver-2.1.0.tar.gz
mkdir ceres-bin
cd ceres-bin
cmake ../ceres-solver-2.1.0
make -j3
make test
# Optionally install Ceres, it can also be exported using CMake which
# allows Ceres to be used without requiring installation, see the documentation
# for the EXPORT_BUILD_DIR option for more information.
sudo make install

安装完成后验证安装：

bin/simple_bundle_adjuster ../ceres-solver-2.1.0/data/problem-16-22106-pre.txt

输出跟官网一致则ceres solver安装成功：

  iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
     0  4.185660e+06    0.00e+00    1.09e+08   0.00e+00   0.00e+00  1.00e+04       0    7.59e-02    3.37e-01
     1  1.062590e+05    4.08e+06    8.99e+06   5.36e+02   9.82e-01  3.00e+04       1    1.65e-01    5.03e-01
     2  4.992817e+04    5.63e+04    8.32e+06   3.19e+02   6.52e-01  3.09e+04       1    1.45e-01    6.48e-01
     3  1.899774e+04    3.09e+04    1.60e+06   1.24e+02   9.77e-01  9.26e+04       1    1.43e-01    7.92e-01
     4  1.808729e+04    9.10e+02    3.97e+05   6.39e+01   9.51e-01  2.78e+05       1    1.45e-01    9.36e-01
     5  1.803399e+04    5.33e+01    1.48e+04   1.23e+01   9.99e-01  8.33e+05       1    1.45e-01    1.08e+00
     6  1.803390e+04    9.02e-02    6.35e+01   8.00e-01   1.00e+00  2.50e+06       1    1.50e-01    1.23e+00

Solver Summary (v 2.1.0-eigen-(3.4.0)-lapack-suitesparse-(5.10.1)-acceleratesparse-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                        22122                    22122
Parameters                              66462                    66462
Residual blocks                         83718                    83718
Residuals                              167436                   167436

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                     DENSE_SCHUR              DENSE_SCHUR
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                 22106,16
Schur structure                         2,3,9                    2,3,9

Cost:
Initial                          4.185660e+06
Final                            1.803390e+04
Change                           4.167626e+06

Minimizer iterations                        7
Successful steps                            7
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.121654

  Residual only evaluation           0.065968 (7)
  Jacobian & residual evaluation     0.303356 (7)
  Linear solver                      0.436650 (7)
Minimizer                            0.890535

Postprocessor                        0.001684
Total                                1.013873

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.769756e-09 <= 1.000000e-06)

3.安装VINS-Mono

主目录下新建一个工作空间，拷下VINS-Mono的github代码

mkdir -p ~/vinsmono_ws/src
cd ~/vinsmono_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git

然后需要对拷下来的代码进行更改以适配Ubuntu20.04和ros noetic
更改1：将所有文件夹下的CMakeLists.txt文件由C++11全部修改为C++14编译，具体有ar_demo，benchmark_publisher，camera_model，feature_tracker，pose_graph，vins_estimator这些文件夹

#set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS "-std=c++14")

ROS noetic版本自带OpenCV4，而VINS-Mono中开发使用的是OpenCV3，修改VINS-Mono代码兼容OpenCV4。
更改2：在camera_model/include/chessboard下的头文件Chessboard.h中添加

#include <opencv2/imgproc/types_c.h>
#include <opencv2/calib3d/calib3d_c.h>

更改3：在camera_model/include/calib的头文件CameraCalibration.h、pose_graph/src下的头文件pose_graph.h和keyframe.h以及pose_graph/src/ThirdParty/DVision下的头文件BRIEF.h中添加

#include <opencv2/imgproc/types_c.h>
#include <opencv2/imgproc/imgproc_c.h>

更改完成，之后编译文件

cd ~/vinsmono_ws
catkin_make
echo "source ~/vinsmono_ws/devel/setup.bash" >> ~/.bashrc

运行之前在对应的配置文件***_config.yaml中修改结果输出目录，比如要运行Euroc数据集，则在euroc_config.yaml中更改

output_path: "your_path/output"

Euroc数据集可前往官网下载，从官网给的下载地址中下载，下载其中的bag文件，比如V1_01_easy.bag。将其放在目录~/dataset/下，然后分别打开三个终端

roslaunch vins_estimator euroc.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play ~/dataset/V1_01_easy.bag

运行结果如下图，如果运行起来了那就表示VINS-Mono安装成功

二、安装realsense

1.安装相机驱动

进入realsense官网的linux安装驱动链接，首先注册公钥：

sudo mkdir -p /etc/apt/keyrings
curl -sSf https://librealsense.intel.com/Debian/librealsense.pgp | sudo tee /etc/apt/keyrings/librealsense.pgp > /dev/null

确认https支持是否已经下载：

sudo apt-get install apt-transport-https

将服务器添加到存储库列表：

echo "deb [signed-by=/etc/apt/keyrings/librealsense.pgp] https://librealsense.intel.com/Debian/apt-repo `lsb_release -cs` main" | \
sudo tee /etc/apt/sources.list.d/librealsense.list
sudo apt-get update

安装库：

sudo apt-get install librealsense2-dkms
sudo apt-get install librealsense2-utils

可选择安装developer和debug包

sudo apt-get install librealsense2-dev
sudo apt-get install librealsense2-dbg

都安装完成后，插上相机，运行realsense，注意D435i需要连接USB3.0的口

realsense-viewer

运行结果如下图，注意左上方是否是USB3.*，USB2传输速率不足会很卡。

2.安装realsense-ros

还是进realsense-ros的github官网，跟着官网的步骤来：
https://github.com/IntelRealSense/realsense-ros/tree/ros1-legacy
根据步骤：首先创建工作空间

mkdir -p ~/realsense_ws/src
cd ~/realsense_ws/src/

拷下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 ..

确认ddynamic_reconfigure是否已经安装：

sudo apt-get install ros-noetic-ddynamic-reconfigure

编译文件：

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

添加.bashrc

echo "source ~/realsense_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

验证安装：
启动相机节点：

roslaunch realsense2_camera rs_camera.launch

查看话题列表，列表如下图所示：

rostopic list

运行以下代码可查看rgb图和深度图：

rosrun rqt_image_view rqt_image_view

运行如下代码可以查看点云图：

roslaunch realsense2_camera demo_pointcloud.launch

三、用D435i运行VINS-Mono

这里再贴一下参考：
从零开始使用Realsense D435i运行VINS-Mono
如何用Realsense D435i运行VINS-Mono等VIO算法 获取IMU同步数据

1.修改realsense包里的rs_camera.launch文件

第一处，修改unite_imu_method如下，这里是让IMU的角速度和加速度作为一个topic输出

<arg name="unite_imu_method"      default="copy"/>

第二处，修改enable_sync参数为true，这里是开机相机和IMU的同步

<arg name="enable_sync"           default="true"/>

然后是第三处，realsense默认图片流开启，但imu流却是关闭的，需要打开imu流：不然就会出现错误：执行roslaunch vins_estimator realsense_color.launch指令时提示：no previous pose graph，且打开rviz时无轨迹，fixed frame报错unknown frame world，这个上述博主都没讲到，不知是否是版本原因。

<arg name="enable_gyro"         default="true"/>
<arg name="enable_accel"        default="true"/>

2.修改VINS-Mono包里的realsense_color_config.yaml文件

第一处，修改订阅的topic

imu_topic: "/camera/imu"
image_topic: "/camera/color/image_raw"

第二处，修改相机内参，这里先再次打开运行realsesne

roslaunch realsense2_camera rs_camera.launch

然后可以通过如下命令获取相机内参

rostopic echo /camera/color/camera_info

图片的宽和高分别填入：

image_width: 1280
image_height: 720

K里面第1个是fx，第3个是cx，第5个是fy，第6个是cy，照读出来的对应填写即可。

projection_parameters:
   fx: 910.2757568359375
   fy: 909.9263305664062
   cx: 652.0943603515625
   cy: 379.8188781738281

第三处，IMU到相机的变换矩阵，这里因为没有去搞外参，所以选2自动修正

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.

第四处，IMU参数，这里全部修改注释给的参数

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.02         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.805       # gravity magnitude

第五处，是否需要在线估计同步时差，因为realsense D435i已经做好了硬件同步，这里选择不需要

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.000                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

第六处，相机曝光改成全局曝光

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0.033               # unit: s. rolling shutter read out time per frame (from data sheet). 

第七处，搜索output，把两个output_path改为自己的路径

output_path: "/home/zz/output/"
pose_graph_save_path: "/home/zz/output/pose_graph/"

3.打开相机，运行VINS-Mono

首先对realsense_ws和vinsmono_ws进行重新编译

cd ~/realsense_ws
catkin_make
cd ~/vinsmono_ws
catkin_make

打开相机，运行VINS-Mono，分别打开三个终端输入如下代码

roslaunch realsense2_camera rs_camera.launch 
roslaunch vins_estimator realsense_color.launch 
roslaunch vins_estimator vins_rviz.launch

拿着D435i在房间内走了一下，结果如下图所示

这样整个过程就走通了，最后如果rviz的Global Status一开始显示错误的话，就把相机拿起来动一动就可以了。
整个过程有些是必须从国外的源下载的，碰到有下载缓慢的情况，就把梯子开全局，速度就会上去。