环境:Ubuntu18.04+ros-melodic
一、编译VINS-Fusion
1.1 安装Ceres Solver
//安装依赖
sudo apt-get install libgoogle-glog-dev libgflags-dev
sudo apt-get install libatlas-base-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-dev
//安装cere solver
git clone https://ceres-solver.googlesource.com/ceres-solver
cd ceres-solver
mkdir build
cd build
cmake ..
make
sudo make install
1.2 安装VINS-Fusion
mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ..
catkin_make
二、运行数据集
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
rosbag play /你自己的路径/MH_01_easy.bag
效果如图, 如果想要点云不消失,把rviz左侧的HistoryPointCloud里的Decay Time改大一点就行
![](https://i-blog.csdnimg.cn/blog_migrate/dcdff48ef90bca01c86448543c06ec69.png)
三、D455相机实测
安装步骤跳过,和D435i相机一样,具体可以看参考这篇D435i安装,注意两个版本号对应上。
然后我们开始配置D455相机的参数文件,将config下的realsense_d435i文件夹复制粘贴重命名为realsense_d455,将里面的left.yaml、right.yaml进行修改,然后配置realsense_stereo_imu_config.yaml
根据标定结果,修改上述三个文件,标定过程可参考双目imu标定1,2,3
这里贴出我的配置文件:
%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: "/home/tzy/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
# Extrinsic parameter between IMU and Camera.
# IMU和Camera之间的外部参数
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. 有一个准确的外在参数。我们将信任以下 imu^R_cam,imu^T_cam,不要更改它
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.初步猜测外在参数。我们将围绕您最初的猜测进行优化。
#cam0 to body
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ 0.99994541, 0.00421397, 0.00956181, -0.02966641,
-0.00427543, 0.99997028, 0.00641563, 0.00135581,
-0.00953449, -0.00645616, 0.9999337, 0.01513884,
0, 0, 0, 1]
#cam1 to body
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ 0.99996667, 0.00423679, 0.00697879, 0.06545514,
-0.00428232, 0.99996956, 0.00652227, 0.00101751,
-0.00695095, -0.00655194, 0.99995438, 0.01459833,
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)ransac 阈值(像素)
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.01229199829486073 # accelerometer measurement noise standard deviation. #0.2 0.04 0.1
gyr_n: 0.00282608342737 # gyroscope measurement noise standard deviation. #0.05 0.004 0.01
acc_w: 0.000294331088336 # accelerometer bias random work noise standard deviation. #0.002 0.001
gyr_w: 0.000105243922055 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.73473346 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: 0.0018085345916211656 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
#loop closure parameters
#回环检测
load_previous_pose_graph: 1 # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/tzy/out" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
开始实际运行,打开vins-fusion自带显示rviz:
roslaunch realsense2_camera rs_camera_vins.launch
打开相机,stereo-imu.launch文件需要修改的地方可参考这里:(建议infra_fps为15)
roslaunch realsense2_camera stereo-imu.launch
关闭结构光:
rosrun rqt_reconfigure rqt_reconfigure
开启跟踪节点:
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
开启闭环
rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
实测效果如下:
下一篇开始使用surfelmapping 进行稠密建图