1、提早条件
系统版本：ubuntu16.04+ROS(kinetic)
默认已经掌握了ubuntu系统下的基本命令以及ROS的基本操做node
2、realsenseT265的SDK测试
3、realsenseT265的标定
1.准备工做(须要注意如下文件编译过程当中，可能出现依赖库缺失的报错，这很正常，按照提示的错误信息安装对应依赖库便可)
1.1 下载并编译cereslinux
git clone https://github.com/ceres-solver/ceres-solver
cd ceres
mkdir build
cd build
cmake ..
make
sudo make install
1.2 下载并编译code_utilsgit
首先，安装依赖库github
sudo apt-get install libdw-dev
以后，安装code_utilsweb
cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/code_utils
cd ..
catkin_make
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_utilsubuntu
cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make
source ~/catkin_ws/devel/setup.bash
注意：先编译code_utils，再编译imu_utils，不能放在一块儿编译。bash
1.4 下载并编译kalibrapp
首先安装依赖库
sudo apt-get install python-setuptools
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen
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
cd ~/catkin_ws/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ..
catkin_make
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
cd ~/catkin_ws/src
git clone https://github.com/IntelRealSense/realsense-ros
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash
至此，咱们的准备工做就作好了。
2.标定流程
2.1修改工做空间下文件的读写权限，否则后面在运行时会报错
cd ~/catkin_ws
sudo chmod 777 src/ -R
2.2 相关文件的修改
打开位于realsense-ros/realsense2_camera/launch目录下的rs_t265.launch文件，将本来的代码修改成。
在~/catkin_ws/src/imu_utils/launch中新建t265_imu.launch以下
2.3 IMU的校准
将realsenseT265插上电脑后，建议在你喜欢的地方新建一个文件夹，好比我是在桌面上新建了一个文件夹，而后在其中打开终端，输入如下命令
roslaunch realsense2_camera rs_t265.launch
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，个人文件内容以下：
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.16 #size of apriltag, edge to edge [m]
tagSpacing: 0.3125 #ratio of space between tags to tagSize
#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
以后，在你新建的文件夹中打开终端，开启realsenseT265
roslaunch realsense2_camera rs_t265.launch
修改话题发布频率
rosrun topic_tools throttle messages /camera/fisheye1/image_raw 10.0 /fisheye1
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填写参数)：
#Accelerometers
accelerometer_noise_density: 8.003e-02 #Noise density (continuous-time)
accelerometer_random_walk: 5.382e-03 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 8.163e-03 #Noise density (continuous-time)
gyroscope_random_walk: 3.470e-05 #Bias random walk
rostopic: /imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)
修改rs_t265.launch其中的两行代码以下：
进入你以前新建的文件夹，打开终端，开启T265
roslaunch realsense2_camera rs_t265.launch
修改发布频率
rosrun topic_tools throttle messages /camera/fisheye1/image_raw 20.0 /fisheye1
rosrun topic_tools throttle messages /camera/fisheye2/image_raw 20.0 /fisheye2
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的标定。
4、运行VINS-FUSION
先下载并编译VINS-Fusion
cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
cd ..
catkin_make
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
%YAML:1.0
---
model_type: MEI
camera_name: camera
image_width: 848
image_height: 800
mirror_parameters:
xi: 1.75347951
distortion_parameters:
k1: 0.03509054
k2: -0.3590536
p1: 0.00250588
p2: 0.00058101
projection_parameters:
gamma1: 788.10829678
gamma2: 788.19861186
u0: 416.33019648
v0: 404.21771229
fisheye2.yaml
%YAML:1.0
---
model_type: MEI
camera_name: camera
image_width: 848
image_height: 800
mirror_parameters:
xi: 1.95209328
distortion_parameters:
k1: 0.18993538
k2: -0.8234989
p1: 0.00337246
p2: 0.00013959
projection_parameters:
gamma1: 843.77832442
gamma2: 843.19711184
u0: 419.54774026
v0: 406.36245572
stereo_imu.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/fisheye1/image_raw"
image1_topic: "/camera/fisheye2/image_raw"
output_path: "~/output"
cam0_calib: "fisheye1.yaml"
cam1_calib: "fisheye2.yaml"
image_width: 848
image_height: 800
# 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 #cam0 coordinate under body coordinate
rows: 4
cols: 4
dt: d
data: [-0.9999306582373404, 0.001093290620038494, 0.01172533294445286, 0.5201790241280906,
-0.0011878229478030982, -0.9999668259020238, -0.008058298336212454, 0.04796159845159734,
0.011716133905124508, -0.008071667179143842, 0.9998987850754022, -0.05434762530417168,
0., 0., 0., 1.]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [-0.9999985320918137, 0.0011840662046389947, -0.0012384673758261621, -0.10570692194161202,
-0.001173065649638605, -0.999960187777609, -0.008845720815767944, 0.048863128512499784,
-0.0012488919887611945, -0.008844255027525165, 0.9999601089152549, -0.04892047362600982,
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: 8.00274e-02 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 8.16326e-03 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 5.38231e-03 # accelerometer bias random work noise standard deviation. #0.002
gyr_w: 3.470378e-05 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.805 # gravity magnitude
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.0 # 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: "~/output" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
以后打开终端，运行
roslaunch realsense2_camera rs_t265.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_t265/stereo_imu.yaml
roslaunch vins vins_rviz.launch
就能够开始愉快的跑VINS-Fusion了。
参考文章：
