VINS-Mono和VINS-Mobile是香港科技大學沈劭劼團隊開源的單目視覺慣導SLAM方案。是基於優化和滑動窗口的VIO,使用IMU預積分構建緊耦合框架。並且具備自動初始化,在線外參標定,重定位,閉環檢測,以及全局位姿圖優化功能。
方案最大的貢獻是構建了效果很好的融合算法,視覺閉環等模塊倒是使用了較爲常見的算法。
系列博客將結合課題組發表的paper,從代碼層面,逐步剖析系統的各個模塊,達到對單目VIO整體的把握,幫助自己理解各類算法,並開發出針對應用場景的視覺慣導SLAM系統。最終目標是使用在AR應用中(Android)。
系統pipeline
主要分爲五部分
1. 傳感器數據處理:
- 單目相機Monocular Camera: Feature detection and Tracking
- IMU: Pre-integration
2. 初始化:
- 僅使用視覺構建SfM
- 將SfM結果和IMU預積分結果對齊
3. 基於滑動窗口的非線性優化:
4. 閉環檢測:
5. 4自由度全局位姿圖優化:
主要依賴的庫只有OpenCV, Eigen和Ceres Solver,代碼目錄如下
核心算法都在feature_tracker和vins_estimator包中。
按照REDEME步驟跑EuRoC/MH_05_difficult.bag錄好的數據結果如下:
使用rqt_graph得到系統的node和topic關係:
rosbag將記錄好的imu數據和單目相機獲取的圖像數據分別發佈到/imu0和/cam0/image_raw話題;/feature_tracker節點通過訂閱/cam0/image_raw話題獲取圖像數據,/vins_estimator節點通過訂閱/imu0話題獲取imu數據,同時/feature_tracker節點將提取出的圖像特徵發佈到/feature_tracker/feature話題,由/vins_estimator訂閱獲取。
因此,/feature_tracker節點負責視覺提取和跟蹤,/vins_estimator則是融合系統的主要部分。
爲了方便看代碼,整理了一下各個部分架構圖(更新中):
processImage():
系統入口是feature_tracker_node.cpp文件中的main函數
1. 首先創建feature_tracker節點,從配置文件中讀取信息(parameters.cpp),包括:
- ROS中發佈訂閱的話題名稱;
- 圖像尺寸;
- 特徵跟蹤參數;
- 是否需要加上魚眼mask來去除邊緣噪點;
%YAML:1.0
#common parameters
imu_topic: "/imu0"
image_topic: "/cam0/image_raw"
#camera calibration
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: -2.917e-01
k2: 8.228e-02
p1: 5.333e-05
p2: -1.578e-04
projection_parameters:
fx: 4.616e+02
fy: 4.603e+02
cx: 3.630e+02
cy: 2.481e+02
# 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.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
ex_calib_result_path: "/config/euroc/ex_calib_result.yaml" # If you choose 1 or 2, the extrinsic calibration result will be written vins_folder_path + ex_calib_result_path.
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0, -1, 0,
1, 0, 0,
0, 0, 1]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
rows: 3
cols: 1
dt: d
data: [-0.02,-0.06, 0.01]
#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
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
fisheye: 0 # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points
#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.2 # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.02 # gyroscope measurement noise standard deviation. #0.05
acc_w: 0.0002 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 2.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.81007 # gravity magnitude
#loop closure parameters
loop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly;
#also give the camera calibration file same as feature_tracker node
pattern_file: "/support_files/brief_pattern.yml"
voc_file: "/support_files/brief_k10L6.bin"
min_loop_num: 25
該config.yaml文件中的其他參數在vins_estimator_node中被讀取,屬於融合算法的參數。
- 優化參數(最大求解時間以保證實時性,不卡頓;最大迭代次數,避免冗餘計算;視差閾值,用於選取sliding window中的關鍵幀);
- imu參數,包括加速度計陀螺儀的測量噪聲標準差、零偏隨機遊走噪聲標準差,重力值(imu放火星上需要改變);
- imu和camera之間的外參R,t;可選(0)已知精確的外參,運行中無需改變,(1)已知外參初值,運行中優化,(2)什麼都不知道,在線初始化中標定
- 閉環參數,包括brief描述子的pattern文件(前端視覺使用光流跟蹤,不需要計算描述子),針對場景訓練好的DBow二進制字典文件;
2. 監聽IMAGE_TOPIC, 有圖像信息發佈到IMAGE_TOPIC上時,執行回調函數:
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
3. img_callback()
前端視覺的算法基本在這個回調函數中,步驟爲:
1. 頻率控制,保證每秒鐘處理的image不多於FREQ;
2. 對於單目:
1). readImage;
2). showUndistortion(可選);
3). 將特徵點矯正(相機模型camodocal)後歸一化平面的3D點(此時沒有尺度信息,3D點p.z=1),像素2D點,以及特徵的id,封裝成ros的sensor_msgs::PointCloud消息類型;
3. 將處理完的圖像信息用PointCloud和Image的消息類型,發佈到"feature"和"feature_img"的topic:
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
4. 包含的視覺算法:
1. CLAHE(Contrast Limited Adaptive Histogram Equalization)
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
2. Optical Flow(光流追蹤)
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
3. 根據匹配點計算Fundamental Matrix, 然後用Ransac剔除不符合Fundamental Matrix的外點
cv::findFundamentalMat(un_prev_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
4. 特徵點檢測:goodFeaturesToTrack, 使用Shi-Tomasi的改進版Harris corner
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask);
特徵點之間保證了最小距離30個像素,跟蹤成功的特徵點需要經過rotation-compensated旋轉補償的視差計算,視差在30個像素以上的特徵點纔會去參與三角化和後續的優化,保證了所有的特徵點質量都是比較高的,同時降低了計算量。