论文：https://arxiv.org/abs/1712.00036

源码路径: https://github.com/daniilidis-group/msckf_mono

源码框架

上图展示了整个msckf源码框架，每当收到新图像时，首先判断是否进行imu初始化，如果没有则进行imu的初始化，否则记录当前图像帧和上一图像帧之间的imu信息，然后利用记录的imu信息进行EKF滤波的imu预测过程，之后更新图像跟踪器里面的图像和imu信息并跟踪和产生新特征；在跟踪完新图像之后，紧接着进行误差状态向量和协方差矩阵的增广，向误差状态向量里面添加新的相机状态和协方差．然后添加、更新和确定移除的特征点．紧接着进行特征的移除和EKF测量更新过程．最后删除掉老旧的不带有跟踪特征点的空相机状态．
基于上述过程，接下来将源码分为以下模块解析:

IMU初始化
EKF之IMU预测过程
跟踪器跟踪产生特征
误差状态向量和协方差矩阵的增广
移除特征点确定
EKF之测量更新过程
删除空状态

一、IMU初始化

系统静止状态的初始化

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; //0init_imu_state_.p_I_G.setZero(); //imu初始状态位置信息为0init_imu_state_.v_I_G.setZero(); //imu初始状态速度信息为0const auto q = init_imu_state_.q_IG;
}

理解：系统的初始化，位置、速度都为零，姿态是平均加速度和重力加速度的转换得到的；角速度的零偏是静止角速度的平均值，而加速度的零偏是0

二、EKF之IMU预测过程

2.1 理论部分（https://blog.csdn.net/hltt3838/article/details/113647649 ）

理解：因为IMU的更新频率是大于图像的更新屏率，当没有图像更新时，根据IMU的建模方程，推导系统的状态；

2.2 代码部分

  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 * dtF_ *= measurement_.dT;// Matrix exponentialPhi_ = F_.exp();// 看这篇论文，感觉是考虑到了EKF的不一致性，IMU在这作出了补偿，零空间// Apply observability constraints - enforce nullspace of Phi// Ref: Observability-constrained Vision-aided Inertial Navigation, Hesch J.// et al. Feb, 2012Matrix3<_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 directlyimu_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-timeerror 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-timeerror 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();}

三、跟踪器跟踪产生特征

3.1 理论部分

https://blog.csdn.net/hltt3838/article/details/112525638

3.2 代码部分

特征跟踪(光流法)

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 pointsint 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++;}}
}

产生特征（fast角点）

track_handler_->new_features()

源码中使用了https://github.com/uzh-rpg/fast库进行角点检测，就不深入细节了．可以直接参照opencv的FastFeatureDetector()角点检测器原理理解该部分，原理都是一样的，也可以用opencv的接口替换掉该fast角点检测库．

注意： 编译MSCKF的时候，一般会出现错误，就是因为没有安装这个库

四、误差状态向量和协方差矩阵的增广

4.1 理论部分

状态向量增广

当获得一张新的相机图像时，需要将相机位姿加入到当前状态向量中，相机位姿可以通过 IMU 估计出来：

协方差矩阵增广

当来一帧新图像后，新的协方差矩阵可写成：

4.2 代码部分

   // 与论文中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 poseQuaternion<_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 matrixif (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 matrixMatrixX<_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 structscam_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 trackedfor (auto feature_id : tracked_feature_ids_){// Check if old feature is seen in current measurementsauto 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 trackauto track = feature_tracks_.begin() + id_iter;// 如果跟踪的特征点出现在当前图像中，则将当前观测信息记录到对应特征的分组中// If we're still tracking this point, add the observationif (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 residualizedif (!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 listif (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;}elsetrack_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);}}}

六、EKF之测量更新过程

  void marginalize(){// 如果被丢弃的特征点数量不为0if (!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, LMbool 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 residualMatrixX<_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 friendsVectorX<_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 themauto 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}

MSCKF 源码解析一相关推荐

谷歌BERT预训练源码解析（二）：模型构建
版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明. 本文链接:https://blog.csdn.net/weixin_39470744/arti ...
谷歌BERT预训练源码解析（三）：训练过程
目录前言源码解析主函数自定义模型遮蔽词预测下一句预测规范化数据集前言本部分介绍BERT训练过程,BERT模型训练过程是在自己的TPU上进行的,这部分我没做过研究所以不做深入探讨.BE ...
谷歌BERT预训练源码解析（一）：训练数据生成
目录预训练源码结构简介输入输出源码解析参数主函数创建训练实例下一句预测&实例生成随机遮蔽输出结果一览预训练源码结构简介关于BERT,简单来说,它是一个基于Transfo ...
Gin源码解析和例子——中间件（middleware）
在<Gin源码解析和例子--路由>一文中,我们已经初识中间件.本文将继续探讨这个技术.(转载请指明出于breaksoftware的csdn博客) Gin的中间件,本质是一个匿名回调函数.这 ...
Colly源码解析——结合例子分析底层实现
通过<Colly源码解析--框架>分析,我们可以知道Colly执行的主要流程.本文将结合http://go-colly.org上的例子分析一些高级设置的底层实现.(转载请指明出于break ...
libev源码解析——定时器监视器和组织形式
我们先看下定时器监视器的数据结构.(转载请指明出于breaksoftware的csdn博客) /* invoked after a specific time, repeatable (based o ...
libev源码解析——定时器原理
本文将回答<libev源码解析--I/O模型>中抛出的两个问题.(转载请指明出于breaksoftware的csdn博客) 对于问题1:为什么backend_poll函数需要指定超时?我们 ...
libev源码解析——I/O模型
在<libev源码解析--总览>一文中,我们介绍过,libev是一个基于事件的循环库.本文将介绍其和事件及循环之间的关系.(转载请指明出于breaksoftware的csdn博客) 目前i ...
libev源码解析——调度策略
在<libev源码解析--监视器(watcher)结构和组织形式>中介绍过,监视器分为[2,-2]区间5个等级的优先级.等级为2的监视器最高优,然后依次递减.不区分监视器类型和关联的文件描 ...

MSCKF 源码解析一

源码框架

IMU初始化

EKF之IMU预测过程

跟踪器跟踪产生特征

误差状态向量和协方差矩阵的增广

移除特征点确定

EKF之测量更新过程

删除空状态

一、IMU初始化

二、EKF之IMU预测过程

2.1 理论部分（https://blog.csdn.net/hltt3838/article/details/113647649 ）

2.2 代码部分

三、跟踪器跟踪产生特征

3.1 理论部分

https://blog.csdn.net/hltt3838/article/details/112525638

3.2 代码部分

特征跟踪(光流法)

产生特征（fast角点）

四、误差状态向量和协方差矩阵的增广

4.1 理论部分

状态向量增广

协方差矩阵增广

4.2 代码部分

五、移除特征点确定

六、EKF之测量更新过程

七、删除空状态

MSCKF 源码解析一相关推荐

最新文章

热门文章

MSCKF 源码解析 一

源码框架

IMU初始化

EKF之IMU预测过程

跟踪器跟踪产生特征

误差状态向量和协方差矩阵的增广

移除特征点确定

EKF之测量更新过程

删除空状态

一、IMU初始化

二、EKF之IMU预测过程

2.1 理论部分 （https://blog.csdn.net/hltt3838/article/details/113647649 ）

2.2 代码部分

三、跟踪器跟踪产生特征

3.1 理论部分

https://blog.csdn.net/hltt3838/article/details/112525638

3.2 代码部分

特征跟踪(光流法)

产生特征（fast角点）

四、误差状态向量和协方差矩阵的增广

4.1 理论部分

状态向量增广

协方差矩阵增广

4.2 代码部分

五、移除特征点确定

六、EKF之测量更新过程

七、删除空状态

MSCKF 源码解析 一相关推荐

最新文章

热门文章

MSCKF 源码解析一

2.1 理论部分（https://blog.csdn.net/hltt3838/article/details/113647649 ）

MSCKF 源码解析一相关推荐