Ubuntu20.04用D435i运行VINS-Fusion
Ubuntu20.04用D435i运行VINS-Fusion
- 一、安装VINS-Fusion
-
- 1.安装ceres solver，进VINS-Fusion给的链接：
- 2.安装VINS-Fusion
- 二、安装realsense
-
- 1.安装相机驱动
- 2.安装realsense-ros
- 三、用D435i运行VINS-Fusion
记录一下用D435i运行VINS-Fusion过程
电脑配置：Intel NUC 12WSKi5（8+512）
系统：Ubuntu20.04
ros版本：noetic
参考：Ubuntu20.04下运行VINS系列：VINS-Mono、VINS-Fusion和GVINS
VINS-Mono可以参考我之前的这篇：Ubuntu20.04用D435i运行VINS-Mono
前置要求：
1.已经安装完成Ubuntu20.04系统
2.已经安装完成ROS-noetic
3.更换过国内源，并且有梯子（有些步骤如果下载缓慢就需要梯子）
一、安装VINS-Fusion
进入VINS-Fusion在Github的开源地址：
https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
根据里面的步骤：
第一步是安装ros，这个已经安装过了
1.安装ceres solver，进VINS-Fusion给的链接：
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)
2.安装VINS-Fusion
VINS-Fusion的github官方页面：
https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
主目录下新建一个工作空间，拷下VINS-Fusion的github代码
mkdir -p ~/vinsfusion_ws/src
cd ~/vinsfusion_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
由于源代码不是在ros noetic下写的，所以需要修改代码：
修改1：将所有文件夹下的CMakeLists.txt文件由C++11全部修改为C++14编译，具体有camera_models，global_fusion，loop_fusion，vins_estimator这些文件夹
#set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS "-std=c++14")
修改2：camera_model/include/chessboard/Chessboard.h中添加
#include <opencv2/imgproc/types_c.h>
#include <opencv2/calib3d/calib3d_c.h>
修改3：camera_models/include/calib/CameraCalibration.h 和 loop_fusion/src/pose_graph.h 和 vins_estimator/src/featureTracker/feature_tracker.h 和 loop_fusion/src/ThirdParty/DVision/BRIEF.h中添加
#include <opencv2/imgproc/types_c.h>
#include <opencv2/imgproc/imgproc_c.h>
修改4：在KITTIGPSTest.cpp和KITTIOdomTest.cpp中将CV_LOAD_IMAGE_GRAYSCALE修改为cv::IMREAD_GRAYSCALE
更改完成，编译文件：
cd ~/vinsfusion_ws
catkin_make
echo "source ~/vinsfusion_ws/devel/setup.bash" >> ~/.bashrc
可跑数据集验证安装，Euroc数据集可前往官网下载，从官网给的下载地址中下载，下载其中的bag文件，比如V1_01_easy.bag。将其放在目录~/dataset/下，然后分别打开三个终端：运行Stereo cameras + IMU
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/vinsfusion_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
(optional) rosrun loop_fusion loop_fusion_node ~/vinsfusion_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
rosbag play ~/dataset/V1_01_easy.bag
运行结果如下：
二、安装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-Fusion
需要修改一下rs_camera.launch，IMU开启，相机分辨率640×480，重命名为rs_camera_vins.launch
<launch>
<arg name="serial_no" default=""/>
<arg name="usb_port_id" default=""/>
<arg name="device_type" default=""/>
<arg name="json_file_path" default=""/>
<arg name="camera" default="camera"/>
<arg name="tf_prefix" default="$(arg camera)"/>
<arg name="external_manager" default="false"/>
<arg name="manager" default="realsense2_camera_manager"/>
<arg name="fisheye_width" default="640"/>
<arg name="fisheye_height" default="480"/>
<arg name="enable_fisheye" default="false"/>
<arg name="depth_width" default="640"/>
<arg name="depth_height" default="480"/>
<arg name="enable_depth" default="false"/>
<arg name="infra_width" default="640"/>
<arg name="infra_height" default="480"/>
<arg name="enable_infra1" default="true"/>
<arg name="enable_infra2" default="true"/>
<arg name="color_width" default="640"/>
<arg name="color_height" default="480"/>
<arg name="enable_color" default="true"/>
<arg name="fisheye_fps" default="30"/>
<arg name="depth_fps" default="30"/>
<arg name="infra_fps" default="30"/>
<arg name="color_fps" default="30"/>
<arg name="gyro_fps" default="200"/>
<arg name="accel_fps" default="250"/>
<arg name="enable_gyro" default="true"/>
<arg name="enable_accel" default="true"/>
<arg name="enable_pointcloud" default="false"/>
<arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
<arg name="pointcloud_texture_index" default="0"/>
<arg name="enable_sync" default="true"/>
<arg name="align_depth" default="true"/>
<arg name="publish_tf" default="true"/>
<arg name="tf_publish_rate" default="0"/>
<arg name="filters" default=""/>
<arg name="clip_distance" default="-2"/>
<arg name="linear_accel_cov" default="0.01"/>
<arg name="initial_reset" default="false"/>
<arg name="unite_imu_method" default="linear_interpolation"/>
<arg name="topic_odom_in" default="odom_in"/>
<arg name="calib_odom_file" default=""/>
<arg name="publish_odom_tf" default="true"/>
<arg name="allow_no_texture_points" default="false"/>
<arg name="emitter_enable" default="false"/>
<group ns="$(arg camera)">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="external_manager" value="$(arg external_manager)"/>
<arg name="manager" value="$(arg manager)"/>
<arg name="serial_no" value="$(arg serial_no)"/>
<arg name="usb_port_id" value="$(arg usb_port_id)"/>
<arg name="device_type" value="$(arg device_type)"/>
<arg name="json_file_path" value="$(arg json_file_path)"/>
<arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/>
<arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
<arg name="pointcloud_texture_index" value="$(arg pointcloud_texture_index)"/>
<arg name="enable_sync" value="$(arg enable_sync)"/>
<arg name="align_depth" value="$(arg align_depth)"/>
<arg name="fisheye_width" value="$(arg fisheye_width)"/>
<arg name="fisheye_height" value="$(arg fisheye_height)"/>
<arg name="enable_fisheye" value="$(arg enable_fisheye)"/>
<arg name="depth_width" value="$(arg depth_width)"/>
<arg name="depth_height" value="$(arg depth_height)"/>
<arg name="enable_depth" value="$(arg enable_depth)"/>
<arg name="color_width" value="$(arg color_width)"/>
<arg name="color_height" value="$(arg color_height)"/>
<arg name="enable_color" value="$(arg enable_color)"/>
<arg name="infra_width" value="$(arg infra_width)"/>
<arg name="infra_height" value="$(arg infra_height)"/>
<arg name="enable_infra1" value="$(arg enable_infra1)"/>
<arg name="enable_infra2" value="$(arg enable_infra2)"/>
<arg name="fisheye_fps" value="$(arg fisheye_fps)"/>
<arg name="depth_fps" value="$(arg depth_fps)"/>
<arg name="infra_fps" value="$(arg infra_fps)"/>
<arg name="color_fps" value="$(arg color_fps)"/>
<arg name="gyro_fps" value="$(arg gyro_fps)"/>
<arg name="accel_fps" value="$(arg accel_fps)"/>
<arg name="enable_gyro" value="$(arg enable_gyro)"/>
<arg name="enable_accel" value="$(arg enable_accel)"/>
<arg name="publish_tf" value="$(arg publish_tf)"/>
<arg name="tf_publish_rate" value="$(arg tf_publish_rate)"/>
<arg name="filters" value="$(arg filters)"/>
<arg name="clip_distance" value="$(arg clip_distance)"/>
<arg name="linear_accel_cov" value="$(arg linear_accel_cov)"/>
<arg name="initial_reset" value="$(arg initial_reset)"/>
<arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
<arg name="topic_odom_in" value="$(arg topic_odom_in)"/>
<arg name="calib_odom_file" value="$(arg calib_odom_file)"/>
<arg name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
<arg name="allow_no_texture_points" value="$(arg allow_no_texture_points)"/>
</include>
</group>
</launch>
config/realsense_d435i/realsense_stereo_imu_config.yaml复制以下代码
%YAML:1.0
#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;
imu: 1
num_of_cam: 2
imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "~/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1 # 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.
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ 1, 0, 0, -0.00552,
0, 1, 0, 0.0051,
0, 0, 1, 0.01174,
0, 0, 0, 1 ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ 1, 0, 0, 0.0446571,
0, 1, 0, 0.0051,
0, 0, 1, 0.01174,
0, 0, 0, 1 ]
#Multiple thread support
multiple_thread: 1
#feature traker paprameters
max_cnt: 150 # max feature number in feature tracking
min_dist: 30 # min distance between two features
freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
F_threshold: 1.0 # ransac threshold (pixel)
show_track: 1 # publish tracking image as topic
flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy
#optimization parameters
max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.04 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.004 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.002 # accelerometer bias random work noise standard deviation. #0.002
gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.805 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
config/realsense_d435i/left.yaml和config/realsense_d435i/right.yaml的内参修改为相机内置参数
projection_parameters:
fx: 388.81756591796875
fy: 388.81756591796875
cx: 319.6447448730469
cy: 237.4071502685547
开四个终端，分别执行以下命令
roslaunch realsense2_camera rs_camera_vins.launch
rosrun vins vins_node ~/vinsfusion_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
rosrun loop_fusion loop_fusion_node ~/vinsfusion_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
roslaunch vins vins_rviz.launch
结果如下：
