论文：https://arxiv.org/abs/1712.00036
源码路径: https://github.com/daniilidis-group/msckf_mono
上图展示了整个msckf源码框架，每当收到新图像时，首先判断是否进行imu初始化，如果没有则进行imu的初始化，否则记录当前图像帧和上一图像帧之间的imu信息，然后利用记录的imu信息进行EKF滤波的imu预测过程，之后更新图像跟踪器里面的图像和imu信息并跟踪和产生新特征；在跟踪完新图像之后，紧接着进行误差状态向量和协方差矩阵的增广，向误差状态向量里面添加新的相机状态和协方差．然后添加、更新和确定移除的特征点．紧接着进行特征的移除和EKF测量更新过程．最后删除掉老旧的不带有跟踪特征点的空相机状态．
基于上述过程，接下来将源码分为以下模块解析:
系统静止状态的初始化
void RosInterface::initialize_imu()
{
Eigen::Vector3f accel_accum;
Eigen::Vector3f gyro_accum;
int num_readings = 0;
accel_accum.setZero();
gyro_accum.setZero();
// 累加初始一段时间内imu的测量值
for (const auto &entry : imu_queue_)
{
auto imu_time = std::get<0>(entry);
auto imu_reading = std::get<1>(entry);
accel_accum += imu_reading.a;
gyro_accum += imu_reading.omega;
num_readings++;
}
// 计算初始静止时间段内加速度和角速度的平均值
Eigen::Vector3f accel_mean = accel_accum / num_readings;
Eigen::Vector3f gyro_mean = gyro_accum / num_readings;
// 将静止时刻陀螺仪角速度均值作为角速度偏置
init_imu_state_.b_g = gyro_mean;
init_imu_state_.g << 0.0, 0.0, -9.81; //默认重力加速度
// 由静止时刻imu加速度均值向量与默认重力加速度向量之间的轴角转换作为初始时刻imu状态的姿态信息
init_imu_state_.q_IG = Quaternion<float>::FromTwoVectors(
-init_imu_state_.g, accel_mean);
init_imu_state_.b_a = init_imu_state_.q_IG * init_imu_state_.g + accel_mean; //0
init_imu_state_.p_I_G.setZero(); //imu初始状态位置信息为0
init_imu_state_.v_I_G.setZero(); //imu初始状态速度信息为0
const auto q = init_imu_state_.q_IG;
}
理解：系统的初始化，位置、速度都为零， 姿态是平均加速度和重力加速度的转换得到的； 角速度的零偏是静止角速度的平均值，而加速度的零偏是0
理解：因为IMU的更新频率是大于图像的更新屏率，当没有图像更新时，根据IMU的建模方程，推导系统的状态；
void propagate(imuReading<_S> &measurement_)
{
//按论文中的公式(10)计算F矩阵和G矩阵
calcF(imu_state_, measurement_);
calcG(imu_state_);
// 预测k+1时刻imu状态(使用5阶龙格库塔法积分)
imuState<_S> imu_state_prop = propogateImuStateRK(imu_state_, measurement_);
// 预测协方差
// F * dt
F_ *= measurement_.dT;
// Matrix exponential
Phi_ = F_.exp();
// 看这篇论文，感觉是考虑到了EKF的不一致性，IMU在这作出了补偿，零空间
// Apply observability constraints - enforce nullspace of Phi
// Ref: Observability-constrained Vision-aided Inertial Navigation, Hesch J.
// et al. Feb, 2012
Matrix3<_S> R_kk_1(imu_state_.q_IG_null);
Phi_.template block<3, 3>(0, 0) =
imu_state_prop.q_IG.toRotationMatrix() * R_kk_1.transpose();
Vector3<_S> u = R_kk_1 * imu_state_.g;
RowVector3<_S> s = (u.transpose() * u).inverse() * u.transpose();
Matrix3<_S> A1 = Phi_.template block<3, 3>(6, 0);
Vector3<_S> tmp = imu_state_.v_I_G_null - imu_state_prop.v_I_G;
Vector3<_S> w1 = vectorToSkewSymmetric(tmp) * imu_state_.g;
Phi_.template block<3, 3>(6, 0) = A1 - (A1 * u - w1) * s;
Matrix3<_S> A2 = Phi_.template block<3, 3>(12, 0);
tmp = measurement_.dT * imu_state_.v_I_G_null +
imu_state_.p_I_G_null - imu_state_prop.p_I_G;
Vector3<_S> w2 = vectorToSkewSymmetric(tmp) * imu_state_.g;
Phi_.template block<3, 3>(12, 0) = A2 - (A2 * u - w2) * s;
Matrix<_S, 15, 15> imu_covar_prop = Phi_ * (imu_covar_ + G_ * noise_params_.Q_imu * G_.transpose() * measurement_.dT) * Phi_.transpose();
// Apply updates directly
imu_state_ = imu_state_prop;
imu_state_.q_IG_null = imu_state_.q_IG;
imu_state_.v_I_G_null = imu_state_.v_I_G;
imu_state_.p_I_G_null = imu_state_.p_I_G;
imu_covar_ = (imu_covar_prop + imu_covar_prop.transpose()) / 2.0;
imu_cam_covar_ = Phi_ * imu_cam_covar_;
}
void calcF（）， void calcG
inline void calcF(const imuState<_S> &imu_state_k,
const imuReading<_S> &measurement_k)
{
/* Multiplies the error state in the linearized continuous-time
error state model */
F_.setZero();
// 计算角速度和加速度名义状态
Vector3<_S> omegaHat, aHat;
omegaHat = measurement_k.omega - imu_state_k.b_g;
aHat = measurement_k.a - imu_state_k.b_a; //论文中有考虑地球自转影响，这里没考虑
Matrix3<_S> C_IG = imu_state_k.q_IG.toRotationMatrix();
F_.template block<3, 3>(0, 0) = -vectorToSkewSymmetric(omegaHat);
F_.template block<3, 3>(0, 3) = -Matrix3<_S>::Identity();
F_.template block<3, 3>(6, 0) = -C_IG.transpose() * vectorToSkewSymmetric(aHat);
F_.template block<3, 3>(6, 9) = -C_IG.transpose();
F_.template block<3, 3>(12, 6) = Matrix3<_S>::Identity();
}
inline void calcG(const imuState<_S> &imu_state_k)
{
/* Multiplies the noise std::vector in the linearized continuous-time
error state model */
G_.setZero();
Matrix3<_S> C_IG = imu_state_k.q_IG.toRotationMatrix();
G_.template block<3, 3>(0, 0) = -Matrix3<_S>::Identity();
G_.template block<3, 3>(3, 3) = Matrix3<_S>::Identity();
G_.template block<3, 3>(6, 6) = -C_IG.transpose();
G_.template block<3, 3>(9, 9) = Matrix3<_S>::Identity();
}
void CornerTracker::track_features(cv::Mat img_1, cv::Mat img_2, Point2fVector& points1, Point2fVector& points2, IdVector& id1, IdVector& id2)
{
std::vector<uchar> status;
std::vector<float> err;
//使用具有金字塔的迭代Lucas-Kanade方法计算稀疏特征集的光流
cv::calcOpticalFlowPyrLK(img_1, img_2,
points1, points2,
status, err,
window_size_, max_level_,
termination_criteria_,
cv::OPTFLOW_USE_INITIAL_FLOW, min_eigen_threshold_);
int h = img_1.rows;
int w = img_1.cols;
// 移除跟踪失败的或者在图像边界之外的特征点
// remove failed or out of image points
int indexCorrection = 0;
for(int i=0; i<status.size(); i++)
{
cv::Point2f pt = points2.at(i- indexCorrection);
cv::Point2f dist_vector = points2.at(i-indexCorrection)-points1.at(i-indexCorrection);
double dist = std::sqrt(dist_vector.x*dist_vector.x+dist_vector.y*dist_vector.y);
if(dist>25.0 || (status.at(i) == 0)||(pt.x<0)||(pt.y<0)||(pt.x>w)||(pt.y>h))
{
if((pt.x<0)||(pt.y<0)||(pt.x>w)||(pt.y>h))
status.at(i) = 0;
points1.erase (points1.begin() + (i - indexCorrection));
points2.erase (points2.begin() + (i - indexCorrection));
id1.erase (id1.begin() + (i - indexCorrection));
id2.erase (id2.begin() + (i - indexCorrection));
indexCorrection++;
}
}
}
track_handler_->new_features()
源码中使用了https://github.com/uzh-rpg/fast库进行角点检测，就不深入细节了．可以直接参照opencv的FastFeatureDetector()角点检测器原理理解该部分，原理都是一样的，也可以用opencv的接口替换掉该fast角点检测库．
注意： 编译MSCKF的时候，一般会出现错误，就是因为没有安装这个库
当获得一张新的相机图像时，需要将相机位姿加入到当前状态向量中，相机 位姿可以通过 IMU 估计出来：
当来一帧新图像后，新的协方差矩阵可写成：
// 与论文中III-C一致
// Generates a new camera state and adds it to the full state and covariance.
void augmentState(const int &state_id, const _S &time)
{
map_.clear();
// 依据当前IMU位姿计算出当前相机位姿(论文中公式14)
// Compute camera_ pose from current IMU pose
Quaternion<_S> q_CG = camera_.q_CI * imu_state_.q_IG;
q_CG.normalize();
camState<_S> cam_state;
cam_state.last_correlated_id = -1;
cam_state.q_CG = q_CG;
cam_state.p_C_G =
imu_state_.p_I_G + imu_state_.q_IG.inverse() * camera_.p_C_I;
cam_state.time = time;
cam_state.state_id = state_id;
// 协方差维度增广
// Build MSCKF covariance matrix
if (cam_states_.size())
{
P_.resize(15 + cam_covar_.cols(), 15 + cam_covar_.cols());
P_.template block<15, 15>(0, 0) = imu_covar_;
P_.block(0, 15, 15, cam_covar_.cols()) = imu_cam_covar_;
P_.block(15, 0, cam_covar_.cols(), 15) = imu_cam_covar_.transpose();
P_.block(15, 15, cam_covar_.rows(), cam_covar_.cols()) = cam_covar_;
}
else
{
P_ = imu_covar_;
}
if (P_.determinant() < -0.000001)
{
//ROS_ERROR("Covariance determinant is negative! %f", P.determinant());
}
// 计算新相机状态相对于状态向量的jacobi矩阵(论文中公式16)
MatrixX<_S> J = MatrixX<_S>::Zero(6, 15 + 6 * cam_states_.size());
J.template block<3, 3>(0, 0) = camera_.q_CI.toRotationMatrix();
J.template block<3, 3>(3, 0) =
vectorToSkewSymmetric(imu_state_.q_IG.inverse() * camera_.p_C_I);
J.template block<3, 3>(3, 12) = Matrix3<_S>::Identity();
// Camera<_S> State Jacobian
// MatrixX<_S> J = calcJ(imu_state_, cam_states_);
MatrixX<_S> tempMat = MatrixX<_S>::Identity(15 + 6 * cam_states_.size() + 6,
15 + 6 * cam_states_.size());
tempMat.block(15 + 6 * cam_states_.size(), 0, 6,
15 + 6 * cam_states_.size()) = J;
// 增广协方差矩阵
// Augment the MSCKF covariance matrix
MatrixX<_S> P_aug = tempMat * P_ * tempMat.transpose();
MatrixX<_S> P_aug_sym = (P_aug + P_aug.transpose()) / 2.0;
P_aug = P_aug_sym;
// 增广状态向量
// Break everything into appropriate structs
cam_states_.push_back(cam_state);
imu_covar_ = P_aug.template block<15, 15>(0, 0);
// 更新相机协方差(P_CC)和相机imu协方差(P_IC)
cam_covar_.resize(P_aug.rows() - 15, P_aug.cols() - 15);
cam_covar_ = P_aug.block(15, 15, P_aug.rows() - 15, P_aug.cols() - 15);
imu_cam_covar_.resize(15, P_aug.cols() - 15);
imu_cam_covar_ = P_aug.block(0, 15, 15, P_aug.cols() - 15);
VectorX<_S> cov_diag = imu_covar_.diagonal();
}
void update(const std::vector<Vector2<_S>, Eigen::aligned_allocator<Vector2<_S>>> &measurements,
const std::vector<size_t> &feature_ids)
{
feature_tracks_to_residualize_.clear();
tracks_to_remove_.clear();
int id_iter = 0;
// feature_ids指代当前特征点索引集
// tracked_feature_ids_指代已跟踪的特征的索引集
// Loop through all features being tracked
for (auto feature_id : tracked_feature_ids_)
{
// Check if old feature is seen in current measurements
auto input_feature_ids_iter =
find(feature_ids.begin(), feature_ids.end(), feature_id);
bool is_valid = (input_feature_ids_iter != feature_ids.end());
// If so, get the relevant track
auto track = feature_tracks_.begin() + id_iter;
// 如果跟踪的特征点出现在当前图像中，则将当前观测信息记录到对应特征的分组中
// If we're still tracking this point, add the observation
if (is_valid)
{
// 根据特征点对观测数据进行分组
size_t feature_ids_dist =
distance(feature_ids.begin(), input_feature_ids_iter);
track->observations.push_back(measurements[feature_ids_dist]);
// 更新相机状态量中的特征点集
auto cam_state_iter = cam_states_.end() - 1;
cam_state_iter->tracked_feature_ids.push_back(feature_id);
// 记录特征点分组中相机状态索引
track->cam_state_indices.push_back(cam_state_iter->state_id);
}
// 如果特征点未出现在当前图像帧中，或者特征点被跟踪的次数超过给定阈值，则移除特征点
// If corner is not valid or track is too long, remove track to be residualized
if (!is_valid || (track->observations.size() >=
msckf_params_.max_track_length))
{
// 移除观测到"移除特征点"的相机状态中的"移除特征点"，并记录对应的相机状态
featureTrackToResidualize<_S> track_to_residualize;
removeTrackedFeature(feature_id, track_to_residualize.cam_states,
track_to_residualize.cam_state_indices);
// 如果"移除特征点"被跟踪的次数超过设定的阈值，则将track的相关信息记录到 track_to_residualize 中
// feature_tracks_to_residualize_中记录了被丢弃的特征点信息
// If track is long enough, add to the residualized list
if (track_to_residualize.cam_states.size() >=
msckf_params_.min_track_length)
{
track_to_residualize.feature_id = track->feature_id;
track_to_residualize.observations = track->observations;
track_to_residualize.initialized = track->initialized;
if (track->initialized)
track_to_residualize.p_f_G = track->p_f_G;
feature_tracks_to_residualize_.push_back(track_to_residualize);
}
// 记录要移除的特征点索引
tracks_to_remove_.push_back(feature_id);
}
id_iter++;
}
// TODO: Double check this stuff and maybe use use non-pointers for accessing
// elements so that it only requires one pass
// 移除 tracks_to_remove_ 中的特征点
for (auto feature_id : tracks_to_remove_)
{
auto track_iter = feature_tracks_.begin();
while (track_iter != feature_tracks_.end())
{
if (track_iter->feature_id == feature_id)
{
size_t last_id = track_iter->cam_state_indices.back();
for (size_t index : track_iter->cam_state_indices)
{
for (auto &camstate : cam_states_)
{
if (!camstate.tracked_feature_ids.size() &&
camstate.state_id == index)
{
camstate.last_correlated_id = last_id;
}
}
}
track_iter = feature_tracks_.erase(track_iter);
break;
}
else
track_iter++;
}
auto corresponding_id = std::find(tracked_feature_ids_.begin(),
tracked_feature_ids_.end(), feature_id);
if (corresponding_id != tracked_feature_ids_.end())
{
tracked_feature_ids_.erase(corresponding_id);
}
}
}
void marginalize()
{
// 如果被丢弃的特征点数量不为0
if (!feature_tracks_to_residualize_.empty())
{
int num_passed, num_rejected, num_ransac, max_length, min_length;
_S max_norm, min_norm;
num_passed = 0;
num_rejected = 0;
num_ransac = 0;
max_length = -1;
min_length = std::numeric_limits<int>::max();
max_norm = -1;
min_norm = std::numeric_limits<_S>::infinity();
std::vector<bool> valid_tracks;
std::vector<Vector3<_S>, Eigen::aligned_allocator<Vector3<_S>>> p_f_G_vec;
int total_nObs = 0;
for (auto track = feature_tracks_to_residualize_.begin();
track != feature_tracks_to_residualize_.end(); track++)
{
// 如果被丢弃的有效特征点数量超过3个时，需要检查丢弃特征点内的相机状态是否移动了足够距离
if (num_feature_tracks_residualized_ > 3 &&
!checkMotion(track->observations.front(), track->cam_states))
{
num_rejected += 1;
valid_tracks.push_back(false);
continue;
}
Vector3<_S> p_f_G;
_S Jcost, RCOND;
// 估计特征点位置
// Estimate feature 3D location with intersection, LM
bool isvalid =
initializePosition(track->cam_states, track->observations, p_f_G);
if (isvalid)
{
track->initialized = true;
track->p_f_G = p_f_G;
map_.push_back(p_f_G); //map_记录特征点位置
}
p_f_G_vec.push_back(p_f_G); //记录新还原出来的特征点位置
int nObs = track->observations.size();
// 转换特征点位置到c1坐标系
Vector3<_S> p_f_C1 = (track->cam_states[0].q_CG.toRotationMatrix()) *
(p_f_G - track->cam_states[0].p_C_G);
Eigen::Array<_S, 3, 1> p_f_G_array = p_f_G.array();
if (!isvalid)
{
num_rejected += 1;
valid_tracks.push_back(false);
}
else
{
num_passed += 1;
valid_tracks.push_back(true);
total_nObs += nObs;
if (nObs > max_length)
{
max_length = nObs;
}
if (nObs < min_length)
{
min_length = nObs;
}
num_feature_tracks_residualized_ += 1;
}
}
if (!num_passed)
{
return;
}
// 初始化投影到零空间后的观测方程中的变量
MatrixX<_S> H_o = MatrixX<_S>::Zero(2 * total_nObs - 3 * num_passed,
15 + 6 * cam_states_.size());
MatrixX<_S> R_o = MatrixX<_S>::Zero(2 * total_nObs - 3 * num_passed,
2 * total_nObs - 3 * num_passed);
VectorX<_S> r_o(2 * total_nObs - 3 * num_passed);
Vector2<_S> rep;
rep << noise_params_.u_var_prime, noise_params_.v_var_prime;
int stack_counter = 0;
for (int iter = 0; iter < feature_tracks_to_residualize_.size(); iter++)
{
if (!valid_tracks[iter])
continue;
featureTrackToResidualize<_S> track = feature_tracks_to_residualize_[iter];
Vector3<_S> p_f_G = p_f_G_vec[iter];
VectorX<_S> r_j = calcResidual(p_f_G, track.cam_states, track.observations); //计算视觉测量残差，与论文中公式18,19,20一致
int nObs = track.observations.size();
MatrixX<_S> R_j = (rep.replicate(nObs, 1)).asDiagonal();
// Calculate H_o_j and residual
MatrixX<_S> H_o_j, A_j;
calcMeasJacobian(p_f_G, track.cam_state_indices, H_o_j, A_j); //计算论文公式(23)中的A_T和H_0
// Stacked residuals and friends
VectorX<_S> r_o_j = A_j.transpose() * r_j; //将论文公式(22)中的r_j投影到左零空间
MatrixX<_S> R_o_j = A_j.transpose() * R_j * A_j; //将测量噪声投影到左零空间
if (gatingTest(H_o_j, r_o_j, track.cam_states.size() - 1))
{
r_o.segment(stack_counter, r_o_j.size()) = r_o_j;
H_o.block(stack_counter, 0, H_o_j.rows(), H_o_j.cols()) = H_o_j;
R_o.block(stack_counter, stack_counter, R_o_j.rows(), R_o_j.cols()) =
R_o_j;
stack_counter += H_o_j.rows();
}
}
H_o.conservativeResize(stack_counter, H_o.cols());
r_o.conservativeResize(stack_counter);
R_o.conservativeResize(stack_counter, stack_counter);
// 至此计算出论文中的观测方程中的H_0, r_0, 以及n_0对应的噪声方差矩阵
// 然后根据ekf的观测更新过程更新状态和协方差
measurementUpdate(H_o, r_o, R_o);
}
}
// 删除不包含任何激活特征点的相机状态，并更新对应的协方差矩阵
void pruneEmptyStates()
{
int max_states = msckf_params_.max_cam_states;
if (cam_states_.size() < max_states)
return;
std::vector<size_t> deleteIdx;
deleteIdx.clear();
size_t num_states = cam_states_.size();
// 找到所有的不带有特征点信息的相机状态，并删除它们
// Find all cam_states_ with no tracked landmarks and prune them
auto camState_it = cam_states_.begin();
size_t num_deleted = 0;
int camstate_pos = 0;
int num_cam_states = cam_states_.size();
int last_to_remove = num_cam_states - max_states - 1;
if (cam_states_.front().tracked_feature_ids.size())
{
return;
}
for (int i = 1; i < num_cam_states - max_states; i++)
{
if (cam_states_[i].tracked_feature_ids.size())
{
last_to_remove = i - 1;
break;
}
}
//删除无效相机状态
for (int i = 0; i <= last_to_remove; ++i)
{
deleteIdx.push_back(camstate_pos + num_deleted);
pruned_states_.push_back(*camState_it);
camState_it = cam_states_.erase(camState_it);
num_deleted++;
}
if (deleteIdx.size() != 0)
{
int n_remove = 0;
int n_keep = 0;
std::vector<bool> to_keep(num_states, false);
for (size_t IDx = 0; IDx < num_states; IDx++)
{
if (find(deleteIdx.begin(), deleteIdx.end(), IDx) != deleteIdx.end())
n_remove++;
else
{
to_keep[IDx] = true;
n_keep++;
}
}
//更新对应的协方差(直接移除状态位对应的协方差位)
int remove_counter = 0;
int keep_counter = 0;
VectorXi keepCovarIdx(6 * n_keep);
VectorXi removeCovarIdx(6 * n_remove);
for (size_t IDx = 0; IDx < num_states; IDx++)
{
if (!to_keep[IDx])
{
removeCovarIdx.segment<6>(6 * remove_counter) =
VectorXi::LinSpaced(6, 6 * IDx, 6 * (IDx + 1) - 1);
remove_counter++;
}
else
{
keepCovarIdx.segment<6>(6 * keep_counter) =
VectorXi::LinSpaced(6, 6 * IDx, 6 * (IDx + 1) - 1);
keep_counter++;
}
}
MatrixX<_S> prunedCamCovar;
square_slice(cam_covar_, keepCovarIdx, prunedCamCovar);
cam_covar_.resize(prunedCamCovar.rows(), prunedCamCovar.cols());
cam_covar_ = prunedCamCovar;
Matrix<_S, 15, Dynamic> prunedImuCamCovar;
column_slice(imu_cam_covar_, keepCovarIdx, prunedImuCamCovar);
imu_cam_covar_.resize(prunedImuCamCovar.rows(), prunedImuCamCovar.cols());
imu_cam_covar_ = prunedImuCamCovar;
}
// TODO: Additional outputs = deletedCamCovar (used to compute sigma),
// deletedCamStates
}