bmi055 标定_Ubuntu16.04+RealsenseT265跑通VINS-Fusion

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了。

参考文章：