devbox
算法研究专家
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

Ubuntu16.04+RealsenseT265跑通VINS-Fusion_t265运行vins fusion

作者：算法研究专家 | 2024-02-04 18:45:41

赞

踩

t265运行vins fusion

摘自：https://blog.csdn.net/weixin_44631150/article/details/104495156?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-4.compare&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-4.compare

 

Ubuntu16.04+RealsenseT265跑通VINS-Fusion

 

IATBOMSW 2020-02-25 21:04:06 780 收藏 4

最后发布:2020-02-25 21:04:06首发:2020-02-25 21:04:06

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文链接：https://blog.csdn.net/weixin_44631150/article/details/104495156

版权

一、提前条件

系统版本：ubuntu16.04+ROS（kinetic）
默认已经掌握了ubuntu系统下的基本命令以及ROS的基本操作

二、realsenseT265的SDK测试

官方网站https://www.intelrealsense.com/get-started-tracking-camera/
照着其中https://github.com/IntelRealSense/librealsense/blob/development/doc/distribution_linux.md安装过程进行安装即可，保证运行realsense-viewer后能有相关界面。

三、realsenseT265的标定

1.准备工作（需要注意以下文件编译过程中，可能出现依赖库缺失的报错，这很正常，按照提示的错误信息安装对应依赖库即可）

1.1 下载并编译ceres

  1. git clone https://github.com/ceres-solver/ceres-solver
  2. cd ceres
  3. mkdir build
  4. cd build
  5. cmake ..
  6. make
  7. sudo make install

1.2 下载并编译code_utils

首先，安装依赖库

sudo apt-get install libdw-dev

之后，安装code_utils

  1. cd ~/catkin_ws/src
  2. git clone https://github.com/gaowenliang/code_utils
  3. cd ..
  4. catkin_make
  5. source ~/catkin_ws/devel/setup.bash

如果有报错fatal error: backward.hpp: 没有那个文件或目录。此时在code_utils下面找到sumpixel_test.cpp，修改#include "backward.hpp"为#include “code_utils/backward.hpp”后再catkin_make。
我也遇见过no module named “XXX”的错误，是因为没安装对应的依赖库，安装后再catkin_make即可。

1.3 下载并编译imu_utils

  1. cd ~/catkin_ws/src
  2. git clone https://github.com/gaowenliang/imu_utils.git
  3. cd ..
  4. catkin_make
  5. source ~/catkin_ws/devel/setup.bash

注意：先编译code_utils，再编译imu_utils，不能放在一起编译。

1.4 下载并编译kalibr

首先安装依赖库

  1. sudo apt-get install python-setuptools
  2. sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen
  3. sudo apt-get install ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev

接下来安装kalibr

  1. cd ~/catkin_ws/src
  2. git clone https://github.com/ethz-asl/Kalibr.git
  3. cd ..
  4. catkin_make
  5. source ~/catkin_ws/devel/setup.bash

在看其他博客时发现可能会出现python相关的问题，我用的是ubuntu16.04自带的Python 2.7.12版本，在安装过程中没有出现问题。

1.5 下载并安装realsense-ros
首先安装依赖库

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

接下来安装realsense-ros

  1. cd ~/catkin_ws/src
  2. git clone https://github.com/IntelRealSense/realsense-ros
  3. cd ~/catkin_ws
  4. catkin_make
  5. source ~/catkin_ws/devel/setup.bash

至此，我们的准备工作就做好了。

2.标定流程

2.1修改工作空间下文件的读写权限，不然后面在运行时会报错

  1.  cd ~/catkin_ws
  2.  sudo chmod 777 src/ -R

2.2 相关文件的修改
打开位于realsense-ros/realsense2_camera/launch目录下的rs_t265.launch文件，将原本的代码<arg name="unite_imu_method" default=""/>修改为<arg name="unite_imu_method" default="linear_interpolation"/>。
在~/catkin_ws/src/imu_utils/launch中新建t265_imu.launch如下

  1. <launch>
  2.     <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
  3.         <param name="imu_topic" type="string" value= "/camera/imu"/> 
  4.         <param name="imu_name" type="string" value= "BMI055"/>
  5.         <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
  6.         <param name="max_time_min" type="int" value= "60"/> 
  7.         <param name="max_cluster" type="int" value= "200"/> 
  8.     </node>
  9. </launch>

2.3 IMU的校准
将realsenseT265插上电脑后，建议在你喜欢的地方新建一个文件夹，比如我是在桌面上新建了一个文件夹，然后在其中打开终端，输入以下命令

  1. roslaunch realsense2_camera rs_t265.launch
  2. roslaunch imu_utils t265_imu.launch

注意过程中显示wait for imu data是正常情况，等待大约60分钟即可出结果，在你新建的文件夹内生成了BMI055_imu_param.yaml文件，该文件给出了加速度计和陀螺仪三轴的noise_density(后缀n)和random_walk(后缀w)，同时计算出了平均值，后面IMU+摄像头联合标定的时候需要这些均值。

2.4 相机的标定
下载官方给的april_6x6_80x80cm_A0.pdf或者其它标定文件。打印或者在屏幕显示，测量实际的尺寸后，在你之前新建的文件夹中新建apriltags.yaml，我的文件内容如下：

  1. target_type: 'aprilgrid' #gridtype
  2. tagCols: 6               #number of apriltags
  3. tagRows: 6               #number of apriltags
  4. tagSize: 0.16           #size of apriltag, edge to edge [m]
  5. tagSpacing: 0.3125         #ratio of space between tags to tagSize
  6.                          #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

之后，在你新建的文件夹中打开终端，开启realsenseT265

roslaunch realsense2_camera rs_t265.launch

修改话题发布频率

  1. rosrun topic_tools throttle messages /camera/fisheye1/image_raw 10.0 /fisheye1
  2. rosrun topic_tools throttle messages /camera/fisheye2/image_raw 10.0 /fisheye2

录制文件，注意录制过程中要缓慢移动相机，使其能看到完整清晰的标定文件（可以先在录制前打开rviz，调用image的话题进行观察，判断移动的位置）

rosbag record -O cameras_calibration /fisheye1 /fisheye2

调用kalibr的算法计算各个摄像头的内参和外参

kalibr_calibrate_cameras --target ./apriltags.yaml --bag ./cameras_calibration.bag --bag-from-to 2 35 --models omni-radtan omni-radtan --topics /fisheye1 /fisheye2

因为并不一定要用整个录制视频，2和35是你想要的起始和截止时间，可以修改。
如果在过程中出现Using the default setup in the initial run leads to an error of Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.相机不同步的报错，可以通过修改话题发布的频率，或者在kalibr命令的末尾加上–approx-sync 0.04来解决。
最终会生成camchain-.cameras_calibration.yaml、results-cam-.cameras_calibration.txt和report-cam-.cameras_calibration.pdf。
2.5. Camera-IMU联合标定
在你之前新建的文件夹中，新建imu.yaml文件如下（根据你之前的BMI055_imu_param.yaml填写参数）：

  1. #Accelerometers
  2. accelerometer_noise_density: 8.003e-02   #Noise density (continuous-time)
  3. accelerometer_random_walk:   5.382e-03   #Bias random walk
  4.  
  5. #Gyroscopes
  6. gyroscope_noise_density:     8.163e-03   #Noise density (continuous-time)
  7. gyroscope_random_walk:       3.470e-05   #Bias random walk
  8.  
  9. rostopic:                    /imu      #the IMU ROS topic
  10. update_rate:                 200.0      #Hz (for discretization of the values above)

修改rs_t265.launch其中的两行代码如下：

  1. <arg name="enable_sync"         default="true"/> 
  2. <arg name="unite_imu_method"    default="copy"/>

，开启T265

roslaunch realsense2_camera rs_t265.launch

修改发布频率

  1. rosrun topic_tools throttle messages /camera/fisheye1/image_raw 20.0 /fisheye1
  2. rosrun topic_tools throttle messages /camera/fisheye2/image_raw 20.0 /fisheye2
  3. rosrun topic_tools throttle messages /camera/imu 200.0 /imu

录制文件

rosbag record -O imu_cameras_calibration /fisheye1 /fisheye2 /imu

调用kalibr的算法计算IMU和camera外参

kalibr_calibrate_imu_camera --target ./apriltags.yaml --cam ./camchain-.cameras_calibration.yaml --imu ./imu.yaml --bag ./imu_cameras_calibration.bag --bag-from-to 2 35 --max-iter 30 --show-extraction

最终会输出camchain-imucam-.imu_cameras_calibration.yaml、imu-.imu_cameras_calibration.yaml、results-imucam-.imu_cameras_calibration.txt、report-imucam-.imu_cameras_calibration.pdf四个文件，你可以通过pdf文件查看你标定的准确性。
至此，我们完成了相机和IMU的标定。

四、运行VINS-FUSION

先下载并编译VINS-Fusion

  1. cd ~/catkin_ws/src
  2. git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
  3. cd ..
  4. catkin_make
  5. source ~/catkin_ws/devel/setup.bash

在VINS-Fusion/config文件夹中，新建文件夹名为realsense_t265，并在其中新建fisheye1.yaml,fisheye2.yaml,stereo_imu.yaml三个文档，内容如下（注意相关参数需要参考BMI055_imu_param.yaml、results-cam-.cameras_calibration.txt和results-imucam-.imu_cameras_calibration.txt自行修改）：

fisheye1.yaml

  1. %YAML:1.0
  2. ---
  3. model_type: MEI
  4. camera_name: camera
  5. image_width: 848
  6. image_height: 800
  7. mirror_parameters:
  8.    xi: 1.75347951
  9. distortion_parameters:
  10.    k1: 0.03509054
  11.    k2: -0.3590536
  12.    p1: 0.00250588
  13.    p2: 0.00058101
  14. projection_parameters:
  15.    gamma1: 788.10829678
  16.    gamma2: 788.19861186
  17.    u0: 416.33019648
  18.    v0: 404.21771229

fisheye2.yaml

  1. %YAML:1.0
  2. ---
  3. model_type: MEI
  4. camera_name: camera
  5. image_width: 848
  6. image_height: 800
  7. mirror_parameters:
  8.    xi: 1.95209328
  9. distortion_parameters:
  10.    k1: 0.18993538
  11.    k2: -0.8234989
  12.    p1: 0.00337246
  13.    p2: 0.00013959
  14. projection_parameters:
  15.    gamma1: 843.77832442
  16.    gamma2: 843.19711184
  17.    u0: 419.54774026
  18.    v0: 406.36245572

stereo_imu.yaml

  1. %YAML:1.0
  2.  
  3. #common parameters
  4. #support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
  5. imu: 1         
  6. num_of_cam: 2  
  7.  
  8. imu_topic: "/camera/imu"
  9. image0_topic: "/camera/fisheye1/image_raw"
  10. image1_topic: "/camera/fisheye2/image_raw"
  11. output_path: "~/output"   
  12.  
  13. cam0_calib: "fisheye1.yaml"
  14. cam1_calib: "fisheye2.yaml"
  15. image_width: 848
  16. image_height: 800
  17.    
  18.  
  19. # Extrinsic parameter between IMU and Camera.
  20. estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
  21.                         # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
  22.  
  23. body_T_cam0: !!opencv-matrix #cam0 coordinate under body coordinate
  24.    rows: 4
  25.    cols: 4
  26.    dt: d
  27.    data: [-0.9999306582373404, 0.001093290620038494, 0.01172533294445286, 0.5201790241280906,
  28.                 -0.0011878229478030982, -0.9999668259020238, -0.008058298336212454, 0.04796159845159734,
  29.                 0.011716133905124508, -0.008071667179143842, 0.9998987850754022, -0.05434762530417168,
  30.                 0., 0., 0., 1.]
  31.  
  32. body_T_cam1: !!opencv-matrix
  33.    rows: 4
  34.    cols: 4
  35.    dt: d
  36.    data: [-0.9999985320918137, 0.0011840662046389947, -0.0012384673758261621, -0.10570692194161202,
  37.                 -0.001173065649638605, -0.999960187777609, -0.008845720815767944, 0.048863128512499784,
  38.                 -0.0012488919887611945, -0.008844255027525165, 0.9999601089152549, -0.04892047362600982,
  39.                 0., 0., 0., 1. ]
  40.  
  41. #Multiple thread support
  42. multiple_thread: 1
  43.  
  44. #feature traker paprameters
  45. max_cnt: 150            # max feature number in feature tracking
  46. min_dist: 30            # min distance between two features 
  47. 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 
  48. F_threshold: 1.0        # ransac threshold (pixel)
  49. show_track: 1           # publish tracking image as topic
  50. flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy
  51.  
  52. #optimization parameters
  53. max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
  54. max_num_iterations: 8   # max solver itrations, to guarantee real time
  55. keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
  56.  
  57. #imu parameters       The more accurate parameters you provide, the better performance
  58. acc_n: 8.00274e-02          # accelerometer measurement noise standard deviation. #0.2   0.04
  59. gyr_n: 8.16326e-03         # gyroscope measurement noise standard deviation.     #0.05  0.004
  60. acc_w: 5.38231e-03         # accelerometer bias random work noise standard deviation.  #0.002
  61. gyr_w: 3.470378e-05       # gyroscope bias random work noise standard deviation.     #4.0e-5
  62. g_norm: 9.805         # gravity magnitude
  63.  
  64. #unsynchronization parameters
  65. estimate_td: 0                      # online estimate time offset between camera and imu
  66. td: 0.0                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
  67.  
  68. #loop closure parameters
  69. load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
  70. pose_graph_save_path: "~/output" # save and load path
  71. save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0

之后打开终端，运行

  1. roslaunch realsense2_camera rs_t265.launch
  2. rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_t265/stereo_imu.yaml 
  3. roslaunch vins vins_rviz.launch

就可以开始愉快的跑VINS-Fusion了。
在这里插入图片描述

参考文章：

Realsense T265标定及运行VINS–kalibr和imu_utils
如何用Realsense D435i运行VINS-Mono等VIO算法 获取IMU同步数据
使用imu_utils工具生成IMU的Allan方差标定曲线
VINS-Fusion

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/article/detail/59622
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号