anson2004110的文章 - 知乎  https://zhuanlan.zhihu.com/p/97287779

 

目录

1. run_subscribe_msckf 启动函数

1.1 new VioManager 创建 VIO系统，加载配置参数，初始化特征点、相机和imu状态

1.2 ROS设备同步机制 message_filters

1.3 回调函数

2. IMU 回调 callback_inertial

3.单目回调 callback_monocular

4.双目回调 callback_stereo

4.1 获取时间戳和图像数据

4.2 调用feed_measurement_stereo 启动MSCKF的各大核心模块

 

run_subscribe_msckf 初始化VIO系统和Visualizer显示系统，订阅相关的相机和IMU话题和绑定回调函数。

正文

1. run_subscribe_msckf 启动函数

1.1 new VioManager 创建 VIO系统，加载配置参数，初始化特征点、相机和imu状态

// Create our VIO system

sys = new VioManager(nh);

step 1 // Load our state options

StateOptions state_options;

step 2 // Create the state!! 把上述加载的参数传递到VIO状态中

state = new State(state_options); //参见state 文件中的定义

 

step 3 设置 相机和imu的时间偏差

// Timeoffset from camera to IMU

double calib_camimu_dt;

nh.param<double>("calib_camimu_dt", calib_camimu_dt, 0.0);

Eigen::VectorXd temp_camimu_dt;

temp_camimu_dt.resize(1);

temp_camimu_dt(0) = calib_camimu_dt;

state->calib_dt_CAMtoIMU()->set_value(temp_camimu_dt);

state->calib_dt_CAMtoIMU()->set_fej(temp_camimu_dt);

 

step 4 设置重力方向

// Global gravity

Eigen::Matrix<double,3,1> gravity;

std::vector<double> vec_gravity;

std::vector<double> vec_gravity_default = {0.0,0.0,9.81};

nh.param<std::vector<double>>("gravity", vec_gravity, vec_gravity_default);

gravity << vec_gravity.at(0),vec_gravity.at(1),vec_gravity.at(2);

 

step 5 设置相机模式

a. 是否是鱼眼模型

state->get_model_CAM(i) = is_fisheye;

b. 图像宽高

std::pair<int,int> wh(matrix_wh.at(0),matrix_wh.at(1));

c.相机内参数和畸变

cam_calib << matrix_k.at(0),matrix_k.at(1),matrix_k.at(2),matrix_k.at(3),matrix_d.at(0),matrix_d.at(1),matrix_d.at(2),matrix_d.at(3);

// Save this representation in our state

state->get_intrinsics_CAM(i)->set_value(cam_calib);

state->get_intrinsics_CAM(i)->set_fej(cam_calib);

d.设置IMU和相机的关系

// Load these into our state

Eigen::Matrix<double,7,1> cam_eigen;

cam_eigen.block(0,0,4,1) = rot_2_quat(T_CtoI.block(0,0,3,3).transpose());

cam_eigen.block(4,0,3,1) = -T_CtoI.block(0,0,3,3).transpose()*T_CtoI.block(0,3,3,1);

注意kalibr 标定的是相机到imu，这里转换成imu到相机

state->get_calib_IMUtoCAM(i)->set_value(cam_eigen);

state->get_calib_IMUtoCAM(i)->set_fej(cam_eigen);

e. 存储到内存容器map中

// Append to our maps for our feature trackers

camera_fisheye.insert({i,is_fisheye});

camera_calib.insert({i,cam_calib});

camera_wh.insert({i,wh});

step 6 特征点检测器的设置

nh.param<int>("num_pts", num_pts, 500);

nh.param<int>("fast_threshold", fast_threshold, 10);

nh.param<int>("grid_x", grid_x, 10);

nh.param<int>("grid_y", grid_y, 8);

 

step 7 陀螺仪和加速度计的噪声和随机游走

nh.param<double>("gyroscope_noise_density", imu_noises.sigma_w, 1.6968e-04);

nh.param<double>("accelerometer_noise_density", imu_noises.sigma_a, 2.0000e-3);

nh.param<double>("gyroscope_random_walk", imu_noises.sigma_wb, 1.9393e-05);

nh.param<double>("accelerometer_random_walk", imu_noises.sigma_ab, 3.0000e-03);

step 8 设置特征点跟踪的方式，可以用KLT和描述子的匹配跟踪

// Lets make a feature extractor

if(use_klt) {

trackFEATS = new TrackKLT(num_pts,state- >options().max_aruco_features,fast_threshold,grid_x,grid_y,min_px_dist);

trackFEATS->set_calibration(camera_calib, camera_fisheye);

} else {

trackFEATS = new TrackDescriptor(num_pts,state->options().max_aruco_features,fast_threshold,grid_x,grid_y,knn_ratio);

trackFEATS->set_calibration(camera_calib, camera_fisheye);

}

step 9 // Initialize our aruco tag extractor

if(use_aruco) {

trackARUCO = new TrackAruco(state->options().max_aruco_features,do_downsizing);

trackARUCO->set_calibration(camera_calib, camera_fisheye);

}

step 10 // Initialize our state propagator

propagator = new Propagator(imu_noises,gravity);

step 11 // Our state initialize

initializer = new InertialInitializer(gravity,init_window_time, init_imu_thresh);

step 12 // Make the updater!

updaterMSCKF = new UpdaterMSCKF(msckf_options, featinit_options);

updaterSLAM = new UpdaterSLAM(slam_options, aruco_options, featinit_options);

 

1.2 ROS设备同步机制 message_filters

// Logic for sync stereo subscriber

// https://answers.ros.org/question/96346/subscribe-to-two-image_raws-with-one-function/?answer=96491#post-id-96491

message_filters::Subscriber<sensor_msgs::Image> image_sub0(nh,topic_camera0.c_str(),1);

message_filters::Subscriber<sensor_msgs::Image> image_sub1(nh,topic_camera1.c_str(),1);

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;

message_filters::Synchronizer<sync_pol> sync(sync_pol(5), image_sub0,image_sub1);

同步左右相机的图像

1.3 回调函数

// Create subscribers

ros::Subscriber subimu = nh.subscribe(topic_imu.c_str(), 9999, callback_inertial); //IMU

subcam = nh.subscribe(topic_camera0.c_str(), 1, callback_monocular); //单目

sync.registerCallback(boost::bind(&callback_stereo, _1, _2)); //双目，则需要之前定义的同步句柄

2. IMU 回调 callback_inertial

void callback_inertial(const sensor_msgs::Imu::ConstPtr& msg) {

传递IMU读数给VIO系统

// send it to our VIO system

sys->feed_measurement_imu(timem, wm, am);

step 1 给propagator 提供IMU数据

// Push back to our propagator

propagator->feed_imu(timestamp,wm,am);

参见propagator文件具体定义。

step 2 给initializer提供IMU数据，若初始化成功之后，就不需要提供给它了

// Push back to our initializer

if(!is_initialized_vio) {

initializer->feed_imu(timestamp, wm, am);

}

注意，在initializer的函数定义中需要删除较老的IMU数据（it0->timestamp < timestamp-3*_window_length，_window_length从配置文件中读取的为0.75 单位是时间秒s，也就是IMU数据开始的一段不用于初始化）

 

3.单目回调 callback_monocular

void callback_monocular(const sensor_msgs::ImageConstPtr& msg0)

近期只关注双目模式，有空之后再更新

4.双目回调 callback_stereo

void callback_stereo(const sensor_msgs::ImageConstPtr& msg0, const sensor_msgs::ImageConstPtr& msg1)

 

4.1 获取时间戳和图像数据

// Fill our buffer if we have not

if(img0_buffer.rows == 0 || img1_buffer.rows == 0) {

time_buffer = cv_ptr0->header.stamp.toSec();

img0_buffer = cv_ptr0->image.clone();

time_buffer = cv_ptr1->header.stamp.toSec();

img1_buffer = cv_ptr1->image.clone();

return;

}

4.2 调用feed_measurement_stereo 启动MSCKF的各大核心模块

// send it to our VIO system

sys->feed_measurement_stereo(time_buffer, img0_buffer, img1_buffer, 0, 1);

viz->visualize();

step 1 给双目模块提供图像进行特征点跟踪

trackFEATS->feed_stereo(timestamp, img0, img1, cam_id0, cam_id1);

参考TrackBase 文件中定义

 

step 2 给双目模块提供图像进行Aruco特征点跟踪

trackARUCO->feed_stereo(timestamp, img0, img1, cam_id0, cam_id1);

 

step 3 尝试初始化VIO，若失败则从新开始（主要计算出重力方向，去除加速度的重力来计算IMU坐标系，估计加速度计的随机游走，还有陀螺仪的随机游走）

is_initialized_vio = try_to_initialize();

 

a. 初始化IMU状态（目前仍然为静止初始化，后续会改成SFM）

double time0;

Eigen::Matrix<double, 4, 1> q_GtoI0;

Eigen::Matrix<double, 3, 1> b_w0, v_I0inG, b_a0, p_I0inG;

// Try to initialize the system

bool success = initializer->initialize_with_imu(time0, q_GtoI0, b_w0, v_I0inG, b_a0, p_I0inG);

 

a-1. 将imu_data根据先后顺序分成两组IMU数据存储到数组中

window_newest.push_back(data); //最新，前0.75s 内的数据

window_secondnew.push_back(data); //次新， 前1.5s 到 0.75s 的数据

a-2. 先统计最新的状态是否有变化（避免出现静止，或者匀速运动），若加速度的变化太小则返回初始化失败

if(a_var < _imu_excite_threshold) { // 这里加速度平均值的阈值为 1.5

return false;

a-3. 若上述条件通过，则统计次新组数据的平均值

linavg = linsum/window_secondnew.size();

angavg = angsum/window_secondnew.size();

a-4. 构造全局的重力坐标系G（G到IMU的转换关系）

// Get z axis, which alines with -g (z_in_G=0,0,1)，当前平均值表示的是imu坐标系下的重力方向向量，以这个方向为全局重力坐标系的z轴

Eigen::Vector3d z_axis = linavg/linavg.norm();

// Create an x_axis 这里可以理解为IMU加速度计的x轴

Eigen::Vector3d e_1(1,0,0);

// Make x_axis perpendicular to z

Eigen::Vector3d x_axis = e_1-z_axis*z_axis.transpose()*e_1;

x_axis= x_axis/x_axis.norm();

注意这里的ez 即为z_axis归一化的坐标向量，·符号为点乘，*为数值乘法。

// Get z from the cross product of these two

Eigen::Matrix<double,3,1> y_axis = skew_x(z_axis)*x_axis;

最后，G到IMU的转换关系：

// From these axes get rotation

Eigen::Matrix<double,3,3> Ro;

Ro.block(0,0,3,1) = x_axis;

Ro.block(0,1,3,1) = y_axis;

Ro.block(0,2,3,1) = z_axis;

// Create our state variables

Eigen::Matrix<double,4,1> q_GtoI = rot_2_quat(Ro);

a-5 IMU陀螺仪和加速度计的随机游走bg，ba

// Set our biases equal to our noise (subtract our gravity from accelerometer bias)

Eigen::Matrix<double,3,1> bg = angavg;

Eigen::Matrix<double,3,1> ba = linavg - quat_2_Rot(q_GtoI)*_gravity;

a-6 设置初始状态

// Set our state variables

time0 = window_secondnew.at(window_secondnew.size()-1).timestamp;

q_GtoI0 = q_GtoI;

b_w0 = bg;

v_I0inG = Eigen::Matrix<double,3,1>::Zero();

b_a0 = ba;

p_I0inG = Eigen::Matrix<double,3,1>::Zero();

 

b.若初始化成功，则进行第一帧IMU状态赋值

// Make big vector (q,p,v,bg,ba), and update our state

// Note: start from zero position, as this is what our covariance is based off of

Eigen::Matrix<double,16,1> imu_val;

imu_val.block(0,0,4,1) = q_GtoI0;

imu_val.block(4,0,3,1) << 0,0,0;

imu_val.block(7,0,3,1) = v_I0inG;

imu_val.block(10,0,3,1) = b_w0;

imu_val.block(13,0,3,1) = b_a0;

//imu_val.block(10,0,3,1) << 0,0,0;

//imu_val.block(13,0,3,1) << 0,0,0;

state->imu()->set_value(imu_val);

state->set_timestamp(time0);

 

step 3 先预积分IMU状态，然后根据跟踪丢失的特征点用于更新VIO系统

// Call on our propagate and update function

do_feature_propagate_update(timestamp);

a. IMU运动学方程，预测IMU状态和IMU协方差预测

// Propagate the state forward to the current update time

// Also augment it with a new clone!

propagator->propagate_and_clone(state, timestamp); // 参见propagator文件具体的函数

 

b. 至少5个图像对完成IMU预测和协方差更新之后，才进行下一步三角化操作

// This isn't super ideal, but it keeps the logic after this easier...

// We can start processing things when we have at least 5 clones since we can start triangulating things...

if((int)state->n_clones() < std::min(state->options().max_clone_size,5)) {

 

c.避免MSCKF状态中维护的特征点过多，统计丢失的feats_lost 和老的marg候选点feats_marg

// Now, lets get all features that should be used for an update that are lost in the newest frame

std::vector<Feature*> feats_lost, feats_marg, feats_slam;

feats_lost = trackFEATS->get_feature_database()->features_not_containing_newer(state->timestamp());

// features_not_containing_newer 参考Feature文件中的定义，返回的feats_lost包含了到当前state时刻往前已经丢失的特征点ID 和坐标信息。

 

若MSCKF状态的大小超过上限，则需要去除老的状态。

// Don't need to get the oldest features untill we reach our max number of clones

if((int)state->n_clones() > state->options().max_clone_size) { // 这里euroc数据集设置11

feats_marg = trackFEATS->get_feature_database()->features_containing(state->margtimestep());

features_containing参考Feature文件中的定义。当MSCKF状态的个数超过了最大限制，则需要marg 掉一些多余点，所以这个函数返回的包含了某个时间戳（即为state->margtimestep()定义的滑窗内最老一帧，待marg的时间戳）包含的特征点坐标信息，为了便于理解，这里直接解释margtimestep()定义如下：

* @brief Will return the timestep that we will marginalize next.

* As of right now, since we are using a sliding window, this is the oldest clone.

* But if you wanted to do a keyframe system, you could selectively marginalize clones.

* @return timestep of clone we will marginalize

*/

double margtimestep() {

double time = INFINITY;

for (std::pair<const double, PoseJPL *> &clone_imu : _clones_IMU) {

if (clone_imu.first < time) {

time = clone_imu.first; // 可见就是找 _clones_IMU 中最老的时间戳

}

}

return time;

}

其中_clones_IMU 是在Propagator::propagate_and_clone IMU预积分之后，在用标定时间偏差来矫正的函数中调用insert_clone 插入新的IMU状态和时间戳

// Now perform stochastic cloning

StateHelper::augment_clone(state, last_w);

// Append the new clone to our clone vector

state->insert_clone(state->timestamp(), pose);

 

d. 在feats_lost找和剔除重复出现在marg内的点

auto it1 = feats_lost.begin();

while(it1 != feats_lost.end()) {

if(std::find(feats_marg.begin(),feats_marg.end(),(*it1)) != feats_marg.end()) {

it1 = feats_lost.erase(it1);

 

e.在feats_marg中找和剔除跟踪时间较长，认为是长期的特征点feats_maxtracks（从marg 候选点中去除，超过max_clone_size=11即可）

// If max track, then add it to our possible slam feature list

if(reached_max) {

feats_maxtracks.push_back(*it2);

it2 = feats_marg.erase(it2);

} else {

it2++;

f. 将feats_maxtracks中一部分存入feats_slam

先计算state->features_SLAM() 中有多少空间存储了Aruco点，计数器curr_aruco_tags

// Count how many aruco tags we have in our state

int curr_aruco_tags = 0;

auto it0 = state->features_SLAM().begin();

while(it0 != state->features_SLAM().end()) {

if ((int) (*it0).second->_featid <= state->options().max_aruco_features) curr_aruco_tags++; //Aruco点的个数 Aruco.size，max_aruco_features 默认是1024个，也就是说features_SLAM()中前1024个是预留存储aruco点的

it0++;

}

判断长期点feats_maxtracks集合个数是否超过 features_SLAM()中非Aruco点的容量大小

int amount_to_add = (state->options().max_slam_features+curr_aruco_tags)-(int)state->features_SLAM().size(); // 计算features_SLAM()中的余量，max_slam_features=50）

int valid_amount = (amount_to_add > (int)feats_maxtracks.size())? (int)feats_maxtracks.size() : amount_to_add; // 若余量足够装下上述的长期点feats_maxtracks，则全部加入SLAM特征点集feats_slam，否则只能加入余量的部分amount_to_add

if(valid_amount > 0) {

feats_slam.insert(feats_slam.end(), feats_maxtracks.end()-valid_amount, feats_maxtracks.end());

feats_maxtracks.erase(feats_maxtracks.end()-valid_amount, feats_maxtracks.end());

}

features_SLAM()中Aruco点不超过max_aruco_features =1024，slam特征点不超过max_slam_features=50

 

g. 在features_SLAM()的landmark 点，同时属于features_idlookup的点存储到feats_slam

for (std::pair<const size_t, Landmark*> &landmark : state->features_SLAM()) {

if(trackARUCO != nullptr) {

Feature* feat1 = trackARUCO->get_feature_database()->get_feature(landmark.second->_featid);

if(feat1 != nullptr) feats_slam.push_back(feat1); //Aruco 点存入feats_slam

}

Feature* feat2 = trackFEATS->get_feature_database()->get_feature(landmark.second->_featid);

if(feat2 != nullptr) feats_slam.push_back(feat2); // 非Aruco 点存入feats_slam

if(feat2 == nullptr) landmark.second->should_marg = true; // 若前端跟踪点集features_idlookup内没有对应ID的点需要marg掉该landmark点

h. 在msckf状态中marg掉老的点，而不会去除aruco标志点

// Lets marginalize out all old SLAM features here

// These are ones that where not successfully tracked into the current frame

// We do *NOT* marginalize out our aruco tags

StateHelper::marginalize_slam(state);

参考StateHelper 文件中的定义

 

i. 将feats_slam中的点进一步划分成老点feats_slam_UPDATE和新点feats_slam_DELAYED

std::vector<Feature*> feats_slam_DELAYED, feats_slam_UPDATE;

for(size_t i=0; i<feats_slam.size(); i++) {

if(state->features_SLAM().find(feats_slam.at(i)->featid) != state->features_SLAM().end()) {

feats_slam_UPDATE.push_back(feats_slam.at(i)); // 已在features_SLAM中，不需要三角化

//ROS_INFO("[UPDATE-SLAM]: found old feature %d (%d measurements)",(int)feats_slam.at(i)->featid,(int)feats_slam.at(i)->timestamps_left.size());

} else {

feats_slam_DELAYED.push_back(feats_slam.at(i)); //新的点，需要三角化

//ROS_INFO("[UPDATE-SLAM]: new feature ready %d (%d measurements)",(int)feats_slam.at(i)->featid,(int)feats_slam.at(i)->timestamps_left.size());

}

 

j. MSCKF 的点（加入丢失的点feats_lost，滑窗内最老的需要边缘化点feats_marg，长期跟踪的点feats_maxtracks）

// Concatenate our MSCKF feature arrays (i.e., ones not being used for slam updates)

std::vector<Feature*> featsup_MSCKF = feats_lost;

featsup_MSCKF.insert(featsup_MSCKF.end(), feats_marg.begin(), feats_marg.end());

featsup_MSCKF.insert(featsup_MSCKF.end(), feats_maxtracks.begin(), feats_maxtracks.end())

 

k. 更新（MSCKF点和SLAM点分别更新）

//===================================================================================

// Now that we have a list of features, lets do the EKF update for MSCKF and SLAM!

//===================================================================================

 

// Pass them to our MSCKF updater

// We update first so that our SLAM initialization will be more accurate??

updaterMSCKF->update(state, featsup_MSCKF);

rT4 = boost::posix_time::microsec_clock::local_time();

参考UpdaterMSCKF 的文件定义

 

// Perform SLAM delay init and update

updaterSLAM->update(state, feats_slam_UPDATE); // 已经成功三角化的点

updaterSLAM->delayed_init(state, feats_slam_DELAYED); //未三角化的点

rT5 = boost::posix_time::microsec_clock::local_time();

 

参考updaterSLAM的文件定义

 

l. 清除跟踪器中to_delete=ture的点

// Remove features that where used for the update from our extractors at the last timestep

// This allows for measurements to be used in the future if they failed to be used this time

// Note we need to do this before we feed a new image, as we want all new measurements to NOT be deleted

trackFEATS->get_feature_database()->cleanup();

if(trackARUCO != nullptr) {

trackARUCO->get_feature_database()->cleanup();

}

 

m. 若特征点的表达方式为局部，则需要更新anchor坐标系

// First do anchor change if we are about to lose an anchor pose

updaterSLAM->change_anchors(state);

参考updaterSLAM文件中的定义

 

n. marg去除较老的点

// Marginalize the oldest clone of the state if we are at max length

if((int)state->n_clones() > state->options().max_clone_size) {

StateHelper::marginalize_old_clone(state);

}

参见StateHelper文件定义

 

o. 更新相机内参数

// Finally if we are optimizing our intrinsics, update our trackers

if(state->options().do_calib_camera_intrinsics) {

// Get vectors arrays

std::map<size_t, Eigen::VectorXd> cameranew_calib;

std::map<size_t, bool> cameranew_fisheye;

for(int i=0; i<state->options().num_cameras; i++) {

Vec* calib = state->get_intrinsics_CAM(i);

bool isfish = state->get_model_CAM(i);

cameranew_calib.insert({i,calib->value()});

cameranew_fisheye.insert({i,isfish});

}

// Update the trackers and their databases

trackFEATS->set_calibration(cameranew_calib, cameranew_fisheye, true);

if(trackARUCO != nullptr) {

trackARUCO->set_calibration(cameranew_calib, cameranew_fisheye, true);

}

}