Overview

歡迎訪問 https://cgabc.xyz/posts/1df60b35/，持續更新

• Alg. PDF: Formula Derivation and Code Analysis of S-MSCKF

• Code: 本文

1. ImageProcessor

initialize

• loadParameters
• create FastFeatureDetector

imuCallback

• imu_msg_buffer.push_back(*msg) after the first image

stereoCallback

• get stereo mono images and timestamps

• createImagePyramids

  • buildOpticalFlowPyramid for cam0_img and cam1_img

• initializeFirstFrame

  • detect new_features on the frist cam0_img --> cam0_points
  • stereoMatch
    • cam0_points --> cam1_points and inlier_markers
  • get cam0_inliers, cam1_inliers and response_inliers
  • Group the features into grids: grid_new_features
  • sort by ImageProcessor::featureCompareByResponse
  • Collect new features within each grid with high response --> curr_features_ptr

• trackFeatures

  • integrateImuData: compute a rough relative rotation cam0_R_p_c and cam1_R_p_c
    • 用兩幀圖像之間的IMU數據，通過積分計算兩幀圖像的相對旋轉矩陣cam0_R_p_c，cam1_R_p_c，直接對角速度進行積分獲得旋轉矢量，然后用羅德里格斯公式轉化成旋轉矩陣
  • Organize the features in the previous image: prev_cam0_points and prev_cam1_points
  • predictFeatureTracking
    • prev_cam0_points --> curr_cam0_points
    • H = K * cam0_R_p_c * K.inv(), p2 = H * p1
  • calcOpticalFlowPyrLK
    • prev_cam0_points --> curr_cam0_points and track_inliers
  • stereoMatch
    • curr_tracked_cam0_points --> curr_cam1_points and match_inliers
  • twoPointRansac: remove outliers
    • prev_matched_cam0_points and curr_matched_cam0_points --> cam0_ransac_inliers
    • prev_matched_cam1_points and curr_matched_cam1_points --> cam0_ransac_inliers
  • curr_matched_cam0_points and curr_matched_cam1_points --> curr_features_ptr

• addNewFeatures

  • Create a mask to avoid redetecting existing features
  • Detect new_features with mask
  • Collect the new detected features based on the grid new_feature_sieve
  • Select the ones with top response within each grid afterwards and add to new_features --> cam0_points
  • stereoMatch
    • cam0_points --> cam1_points and inlier_markers
  • get cam0_inliers, cam1_inliers and response_inliers
  • Group the features into grids grid_new_features
  • sort by ImageProcessor::featureCompareByResponse
  • Collect new features within each grid with high response --> curr_features_ptr

• pruneGridFeatures

  • sort by ImageProcessor::featureCompareByLifetime
  • Remove some of the features of a grid by processor_config.grid_max_feature_num

• publish

  • curr_cam0_points_undistorted[x,y]
  • curr_cam1_points_undistorted[x,y]

sub func/algms

• stereoMatch: cam0_points --> cam1_points and inlier_markers
  • Initialize cam1_points by projecting cam0_points to cam1 using the rotation from stereo extrinsics R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu
  • calcOpticalFlowPyrLK: cam0_points --> cam1_points and inlier_markers
  • remove outliers: compute the essential matrix E --> inlier_markers

2. MsckfVio

initialize

• loadParameters
• Initialize state server
  • state_server.continuous_noise_cov
• Initialize the chi squared test table with confidence level 0.95
  • chi_squared_test_table

imuCallback

• initializeGravityAndBias
  • 將前200幀加速度和角速度求平均
  • IMUState::gravity: 平均加速度的模值g作為重力加速度
  • state_server.imu_state.gyro_bias: 平均角速度作為陀螺儀的bias
  • state_server.imu_state.orientation: 計算重力向量(0,0,-g)和平均加速度之間的夾角(旋轉四元數), 標定初始時刻IMU系與world系之間的夾角
  • 因此MSCKF要求前200幀IMU是靜止不動的

featureCallback

• 第一幀時間戳給 state_server.imu_state.time

• batchImuProcessing（傳播IMU狀態，處理兩幀圖像之間的所有IMU觀測數據）

  • processModel（每幀IMU數據）
    • 狀態向量預測 predictNewState：姿態預測、速度和位置預測
    • 狀態協方差預測
  • state_server.imu_state.id = IMUState::next_id++;
  • Remove all used IMU msgs

• stateAugmentation

  • 狀態向量擴增
  • 協方差擴增

• addFeatureObservations

  • 將特征添加到map_server, 將特征添加到對應feature.id的observations(std::map)中
  • 計算跟蹤已有特征的比例，get tracking_rate

• removeLostFeatures（特征跟丟了需要移除特征）

  • Remove the features that lost track from map_server and get jacobian_row_size
    • checkMotion
    • initializePosition
  • featureJacobian
  • gatingTest why
  • measurementUpdate
  • Remove all processed features from the map

• pruneCamStateBuffer（相機狀態數量達到最大限制max_cam_state_size需要剔除掉相機狀態）

  • Find two camera states to be removed findRedundantCamStates
    • rotation_threshold, translation_threshold, tracking_rate_threshold
    • sort(rm_cam_state_ids.begin(), rm_cam_state_ids.end());
  • remove feature.observations and get jacobian_row_size
    • checkMotion
    • initializePosition
  • featureJacobian
  • gatingTest why
  • measurementUpdate
  • remove
    • Remove the corresponding rows and columns in the state covariance matrix
    • Remove this camera state in the state vector

• publish

  • odom: Eigen::Isometry3d T_b_w = IMUState::T_imu_body * T_i_w * IMUState::T_imu_body.inverse();
  • 3D points: feature_msg_ptr->points

sub func/algms

• initializePosition

  • trianglation: generateInitialGuess
  • L-M --> position

• featureJacobian

  • Check how many camera states in the provided camera id camera has actually seen this feature
  • measurementJacobian
    • (why) Modifty the measurement Jacobian to ensure observability constrain
  • Project the residual and Jacobians onto the nullspace of H_fj

總結

以上是生活随笔為你收集整理的S-MSCKF 论文公式推导与代码解析的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。