【OV】MSCKF与SLAM更新含义与操作流程解读
0. 零空间投影的作用和目的
零空间投影（Nullspace Projection）在优化和滤波算法中，特别是在多状态约束优化问题中，代码中的零空间投影，其主要目的如下：
- 消除冗余信息：通过零空间投影，可以去除雅可比矩阵中与某些约束无关的部分，确保优化过程中只保留有效的信息。
- 保证数值稳定性：在处理大规模、高维的优化问题时，直接使用原始雅克比矩阵可能导致数值不稳定或病态问题。零空间投影可以帮助改善矩阵条件数，提高数值稳定性。
- 满足约束条件：在某些情况下，系统可能包含特定的约束条件（例如刚体运动中的旋转和平移约束）。零空间投影可以确保这些约束在优化过程中得到满足，避免违反约束的情况。
零空间投影的具体作用可能是为了确保在更新过程中，状态估计不会偏离已知的约束条件，从而提高滤波器的准确性和鲁棒性。
小姐：零空间投影的主要目的是：消除冗余信息，保证数值稳定性，满足约束条件，提高整个系统的性能和可靠性，特别是在多状态扩展卡尔曼滤波（MSCKF）等复杂优化场景中。
1. 对比UpdaterSLAM::update、UpdaterSLAM::delayed_init和UpdaterMSCKF::update三个接口之间的差异、联系和各自的功能。
1.1 三个接口的主要作用
UpdaterSLAM::update:- 主要负责
更新SLAM状态，处理已经存在的特征点，并通过测量来更新状态估计。 - 包括特征点的清理、雅可比矩阵和残差的计算，以及状态的更新。
- 主要负责
UpdaterSLAM::delayed_init:- 用于延迟初始化新特征点，处理那些尚未被初始化的特征点。
- 包括清理、三角化特征、计算雅可比矩阵和残差，并最终初始化特征。
UpdaterMSCKF::update:- 专注于MSCKF（Multi-State Constraint Kalman Filter）方法的状态更新，处理IMU和视觉测量的融合。
- 主要通过对多个状态进行约束来更新状态估计。
1.2. 接口核心算法实现流程
1.2.1 特征点处理
-
update:- 清理特征点的测量，确保每个特征点有足够的测量。
- 计算雅可比矩阵和残差来更新状态。
-
delayed_init:- 清理特征点，确保每个特征点至少有两个测量，适合于三角化。
- 尝试三角化特征点，并在成功后进行高斯-牛顿优化。
-
updaterMSCKF::update:- 处理IMU和视觉测量，通过约束融合多个状态。
- 计算雅可比矩阵并更新状态，通常涉及到多次状态预测和更新步骤。
1.2.2 状态更新
-
update:- 计算线性系统并更新状态，使用扩展卡尔曼滤波（EKF）方法。
- 处理所有有效特征点的雅可比矩阵和残差，确保状态一致性。
-
delayed_init:- 初始化新的特征点，并将其添加到状态中。
- 使用卡方检验来决定特征点是否成功初始化。
-
updaterMSCKF::update:- 通过对IMU和视觉测量的约束来更新状态，通常包含多个状态的更新步骤。
- 计算状态的协方差并进行更新，确保估计的准确性。
1.3. 变量和数据结构
-
update:- 使用
Eigen库来处理矩阵和向量运算，构建大雅可比矩阵和残差向量。 - 维护一个大的雅可比矩阵（
Hx_big）和残差向量（res_big），以便进行状态更新。
- 使用
-
delayed_init:- 创建克隆相机姿态的映射，以便进行特征三角化。
- 使用
FeatureInitializer来处理特征的三角化和初始化。
-
updaterMSCKF::update:- 处理IMU和视觉数据，通常涉及到多个状态的雅可比矩阵和残差的计算。
- 使用状态协方差矩阵来评估状态的不确定性。
1.4. 特征表示
- 在
update中，特征的表示可能会根据不同的条件进行处理（如单逆深度），而在delayed_init中则更专注于特征的初始化和三角化。 updaterMSCKF::update则主要关注IMU和视觉数据的融合，处理状态约束。
1.5. 区别
UpdaterSLAM::update和UpdaterSLAM::delayed_init主要关注特征点的管理，而updaterMSCKF::update则专注于IMU和视觉测量的融合。update处理的是已有特征的状态更新，而delayed_init则是为新的特征点提供初始化机制。updaterMSCKF则是通过多状态约束来更新状态，处理不同类型的测量数据。
2. msckf update MSCKF相机视觉里程计更新
2.1 算法目的
利用多个相机观测到的特征点,通过优化计算得到当前状态的最优估计。它可以在不使用地图的情况下实现单目视觉里程计的功能。算法实现了特征点管理、相机位姿估计、特征点三角化和优化、测量矩阵构建和状态更新等主要步骤。
2.2 主要流程
- 清理特征点测量数据,去除测量次数少于2次的特征点。
- 根据当前状态中的IMU位姿和IMU到相机的外参,计算每个时刻相机的位姿。
- 对每个特征点进行三角化和高斯牛顿优化,去除三角化或优化失败的特征点。
- 构建包含所有特征点的大雅可比矩阵和残差向量,执行空间投影和Chi2距离检测去除离群点。
- 执行测量压缩,减小更新矩阵的尺寸。
- 使用EKF更新状态估计的位姿和其他参数。
2.3 代码实现
void UpdaterMSCKF::update(std::shared_ptr<State> state, std::vector<std::shared_ptr<Feature>> &feature_vec) {
// Return if no features
if (feature_vec.empty())
return;
//PRINT_DEBUG("\tMSCKF update: feature_vec size: %d\n", feature_vec.size());
// Start timing
boost::posix_time::ptime rT0, rT1, rT2, rT3, rT4, rT5;
rT0 = boost::posix_time::microsec_clock::local_time();
// 0. Get all timestamps our clones are at (and thus valid measurement times)
std::vector<double> clonetimes;
for (const auto &clone_imu : state->_clones_IMU) {
clonetimes.emplace_back(clone_imu.first);
}
// 1. Clean all feature measurements and make sure they all have valid clone times
auto it0 = feature_vec.begin();
while (it0 != feature_vec.end()) {
// Clean the feature
(*it0)->clean_old_measurements(clonetimes);
// Count how many measurements
int ct_meas = 0;
for (const auto &pair : (*it0)->timestamps) {
ct_meas += (*it0)->timestamps[pair.first].size();
}
// Remove if we don't have enough
if (ct_meas < 2) {
//PRINT_DEBUG("\tMSCKF update: feat %d ct_meas < 2\n", (*it0)->featid, ct_meas);
(*it0)->to_delete = true;
it0 = feature_vec.erase(it0);
} else {
//PRINT_DEBUG("\tMSCKF update: feat %d ct_meas = %d from %d cams\n", (*it0)->featid, ct_meas, (*it0)->timestamps.size());
it0++;
}
}
rT1 = boost::posix_time::microsec_clock::local_time();
// 2. Create vector of cloned *CAMERA* poses at each of our clone timesteps
std::unordered_map<size_t, std::unordered_map<double, FeatureInitializer::ClonePose>> clones_cam;
for (const auto &clone_calib : state->_calib_IMUtoCAM) {
// For this camera, create the vector of camera poses
std::unordered_map<double, FeatureInitializer::ClonePose> clones_cami;
for (const auto &clone_imu : state->_clones_IMU) {
// Get current camera pose
Eigen::Matrix<double, 3, 3> R_GtoCi = clone_calib.second->Rot() * clone_imu.second->Rot();
Eigen::Matrix<double, 3, 1> p_CioinG = clone_imu.second->pos() - R_GtoCi.transpose() * clone_calib.second->pos();
// Append to our map
clones_cami.insert({clone_imu.first, FeatureInitializer::ClonePose(R_GtoCi, p_CioinG)});
}
// Append to our map
clones_cam.insert({clone_calib.first, clones_cami});
}
// 3. Try to triangulate all MSCKF or new SLAM features that have measurements
auto it1 = feature_vec.begin();
while (it1 != feature_vec.end()) {
// Triangulate the feature and remove if it fails
bool success_tri = true;
if (initializer_feat->config().triangulate_1d) {
success_tri = initializer_feat->single_triangulation_1d(*it1, clones_cam);
} else {
success_tri = initializer_feat->single_triangulation(*it1, clones_cam);
}
// Gauss-newton refine the feature
bool success_refine = true;
if (initializer_feat->config().refine_features) {
success_refine = initializer_feat->single_gaussnewton(*it1, clones_cam);
}
// Remove the feature if not a success
if (!success_tri || !success_refine) {
//PRINT_DEBUG("\tMSCKF upadte: feat %d clones %d trig fail!\n", (*it1)->featid, clones_cam.size());
(*it1)->to_delete = true;
it1 = feature_vec.erase(it1);
continue;
}
it1++;
}
rT2 = boost::posix_time::microsec_clock::local_time();
// Calculate the max possible measurement size
size_t max_meas_size = 0;
for (size_t i = 0; i < feature_vec.size(); i++) {
for (const auto &pair : feature_vec.at(i)->timestamps) {
max_meas_size += 2 * feature_vec.at(i)->timestamps[pair.first].size();
}
}
// Calculate max possible state size (i.e. the size of our covariance)
// NOTE: that when we have the single inverse depth representations, those are only 1dof in size
size_t max_hx_size = state->max_covariance_size();
for (auto &landmark : state->_features_SLAM) {
max_hx_size -= landmark.second->size();
}
// Large Jacobian and residual of *all* features for this update
Eigen::VectorXd res_big = Eigen::VectorXd::Zero(max_meas_size);
Eigen::MatrixXd Hx_big = Eigen::MatrixXd::Zero(max_meas_size, max_hx_size);
std::unordered_map<std::shared_ptr<Type>, size_t> Hx_mapping;
std::vector<std::shared_ptr<Type>> Hx_order_big;
size_t ct_jacob = 0;
size_t ct_meas = 0;
// 4. Compute linear system for each feature, nullspace project, and reject
auto it2 = feature_vec.begin();
while (it2 != feature_vec.end()) {
// Convert our feature into our current format
UpdaterHelper::UpdaterHelperFeature feat;
feat.featid = (*it2)->featid;
feat.uvs = (*it2)->uvs;
feat.uvs_norm = (*it2)->uvs_norm;
feat.timestamps = (*it2)->timestamps;
// If we are using single inverse depth, then it is equivalent to using the msckf inverse depth
feat.feat_representation = state->_options.feat_rep_msckf;
if (state->_options.feat_rep_msckf == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) {
feat.feat_representation = LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH;
}
// Save the position and its fej value
if (LandmarkRepresentation::is_relative_representation(feat.feat_representation)) {
feat.anchor_cam_id = (*it2)->anchor_cam_id;
feat.anchor_clone_timestamp = (*it2)->anchor_clone_timestamp;
feat.p_FinA = (*it2)->p_FinA;
feat.p_FinA_fej = (*it2)->p_FinA;
} else {
feat.p_FinG = (*it2)->p_FinG;
feat.p_FinG_fej = (*it2)->p_FinG;
}
// Our return values (feature jacobian, state jacobian, residual, and order of state jacobian)
Eigen::MatrixXd H_f;
Eigen::MatrixXd H_x;
Eigen::VectorXd res;
std::vector<std::shared_ptr<Type>> Hx_order;
// Get the Jacobian for this feature
UpdaterHelper::get_feature_jacobian_full(state, feat, H_f, H_x, res, Hx_order);
// Nullspace project
UpdaterHelper::nullspace_project_inplace(H_f, H_x, res);
/// Chi2 distance check
Eigen::MatrixXd P_marg = StateHelper::get_marginal_covariance(state, Hx_order);
Eigen::MatrixXd S = H_x * P_marg * H_x.transpose();
S.diagonal() += _options.sigma_pix_sq * Eigen::VectorXd::Ones(S.rows());
double chi2 = res.dot(S.llt().solve(res));
// Get our threshold (we precompute up to 500 but handle the case that it is more)
double chi2_check;
if (res.rows() < 500) {
chi2_check = chi_squared_table[res.rows()];
} else {
boost::math::chi_squared chi_squared_dist(res.rows());
chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
PRINT_WARNING(YELLOW "chi2_check over the residual limit - %d\n" RESET, (int)res.rows());
}
// Check if we should delete or not
if (chi2 > _options.chi2_multipler * chi2_check) {
//PRINT_DEBUG("\tMSCKF update: feat %d fail! chi2 fail: %.2f > %.2f * %.2f\n", (*it2)->featid, chi2, _options.chi2_multipler, chi2_check);
(*it2)->to_delete = true;
it2 = feature_vec.erase(it2);
// PRINT_DEBUG("featid = %d\n", feat.featid);
// PRINT_DEBUG("chi2 = %f > %f\n", chi2, _options.chi2_multipler*chi2_check);
// std::stringstream ss;
// ss << "res = " << std::endl << res.transpose() << std::endl;
// PRINT_DEBUG(ss.str().c_str());
continue;
}
else {
//PRINT_DEBUG("\tMSCKF update: feat %d pass.\n", (*it2)->featid);
}
// We are good!!! Append to our large H vector
size_t ct_hx = 0;
for (const auto &var : Hx_order) {
// Ensure that this variable is in our Jacobian
if (Hx_mapping.find(var) == Hx_mapping.end()) {
Hx_mapping.insert({var, ct_jacob});
Hx_order_big.push_back(var);
ct_jacob += var->size();
}
// Append to our large Jacobian
Hx_big.block(ct_meas, Hx_mapping[var], H_x.rows(), var->size()) = H_x.block(0, ct_hx, H_x.rows(), var->size());
ct_hx += var->size();
}
// Append our residual and move forward
res_big.block(ct_meas, 0, res.rows(), 1) = res;
ct_meas += res.rows();
it2++;
}
rT3 = boost::posix_time::microsec_clock::local_time();
// We have appended all features to our Hx_big, res_big
// Delete it so we do not reuse information
for (size_t f = 0; f < feature_vec.size(); f++) {
feature_vec[f]->to_delete = true;
}
// Return if we don't have anything and resize our matrices
if (ct_meas < 1) {
return;
}
assert(ct_meas <= max_meas_size);
assert(ct_jacob <= max_hx_size);
res_big.conservativeResize(ct_meas, 1);
Hx_big.conservativeResize(ct_meas, ct_jacob);
// 5. Perform measurement compression
UpdaterHelper::measurement_compress_inplace(Hx_big, res_big);
if (Hx_big.rows() < 1) {
return;
}
rT4 = boost::posix_time::microsec_clock::local_time();
// Our noise is isotropic, so make it here after our compression
Eigen::MatrixXd R_big = _options.sigma_pix_sq * Eigen::MatrixXd::Identity(res_big.rows(), res_big.rows());
// 6. With all good features update the state
StateHelper::EKFUpdate(state, Hx_order_big, Hx_big, res_big, R_big);
rT5 = boost::posix_time::microsec_clock::local_time();
}
扫一扫
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
