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.hloop_fusion/src/pose_graph.hvins_estimator/src/featureTracker/feature_tracker.hloop_fusion/src/ThirdParty/DVision/BRIEF.h中添加

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

修改4:在KITTIGPSTest.cppKITTIOdomTest.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

运行结果如下:
Ubuntu20.04用D435i运行VINS-Fusion_第1张图片

二、安装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传输速率不足会很卡。
Ubuntu20.04用D435i运行VINS-Fusion_第2张图片

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

Ubuntu20.04用D435i运行VINS-Fusion_第3张图片
运行以下代码可查看rgb图和深度图:

rosrun rqt_image_view rqt_image_view

Ubuntu20.04用D435i运行VINS-Fusion_第4张图片
运行如下代码可以查看点云图:

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.yamlconfig/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 

结果如下:
Ubuntu20.04用D435i运行VINS-Fusion_第5张图片

你可能感兴趣的:(自动驾驶,深度学习,机器人)