【SchurVins】核心核心改进与代码解析
OpenVins Run:
roscore
roslaunch ov_msckf subscribe.launch config:=euroc_mav
rosrun rviz rviz -d src/open_vins/ov_msckf/launch/display.rviz
rosbag play V1_01_easy.bag
SchurVINS算法在特征选择、关键帧选择策略和核心更新方面进行了以下改进：
- 特征选择：SchurVINS利用滑动窗口中所有成功三角化的地标及其观测值进行扩展卡尔曼滤波器（EKF）更新。这种方法能够最大限度地减少单个图像时间戳间隔内状态传播引起的漂移，从而在增强现实（AR）和虚拟现实（VR）场景中保证姿态的平滑性。
- 关键帧选择策略：如果候选帧（通常是当前帧）与前一关键帧之间的平均视差达到阈值，或者跟踪的地标点数量低于某个阈值，则该帧被定义为关键帧。一旦关键帧被选定，算法通过深度过滤器模块提取FAST角点以生成新的地标点。此外，如果候选帧与局部地图中维护的共视关键帧在方向和位置上的差距超出一定范围，该帧也会被定义为关键帧。
- 核心更新：SchurVINS将残差方程分解为与姿态相关的部分和与地标点相关的部分，然后分别进行EKF更新。对于地标点的EKF更新，算法针对每个地标点依次更新，显著降低了计算量。SchurVINS维护了一个基于首次初始化（FIFO）的局部地图，该地图包含关键帧，并且不对关键帧执行闭环调整（LBA）。作者认为，由于已经达到了高精度，因此不再需要传统的LBA，因此在SchurVINS中放弃了传统的LBA。
- 实验设置：在作者的实验中，局部地图中关键帧的数量被设置为10。
这些改进使得SchurVINS在保持高精度的同时，提高了计算效率，特别是在资源受限的设备上。通过这些策略，SchurVINS能够有效地处理视觉惯性数据，提供平滑且准确的姿态估计。
SchurVINS核心改进：
-
高精度定位：SchurVINS通过构建完整的残差模型来保证高精度定位，这包括显式建模梯度（Gradient）、Hessian矩阵和观测协方差。
-
低计算复杂性：SchurVINS利用Schur补码技术将完整模型分解为自我运动残差模型和地标残差模型，从而在保持高精度的同时降低计算复杂性。
-
滤波器基础的VINS框架：SchurVINS是一个基于滤波器的VINS框架，它通过EKF更新来实现对自我运动和地标的高效估计。
-
改进的地标求解器：SchurVINS提出了一种轻量级的基于EKF的地标求解器，以高效估计地标位置。这与SVO2.0中使用的基于GN的地标求解器相比，能够在保证高精度的同时降低计算复杂性。
-
消融研究：SchurVINS在消融研究中表现出色，即使在没有GN或基于EKF的地标求解器的情况下，SchurVINS也能够限制全局漂移，并且在某些挑战场景中避免系统发散。
总结来说，SchurVINS通过引入Schur补码和改进的地标求解器，实现了在高精度定位和低计算复杂性方面的显著提升。这些改进使得SchurVINS在资源受限的设备上也能提供高质量的定位服务。
// Copyright 2024 ByteDance and/or its affiliates.
/*
This program is free software: you can redistribute it and/or modify it
under the terms of the GNU General Public License as published by the
Free Software Foundation, either version 3 of the License, or (at your
option) any later version.
This program is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
more details.
You should have received a copy of the GNU General Public License along
with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include <svo/common/local_feature.h>
#include <svo/schur_vins.h>
#include <boost/math/distributions/chi_squared.hpp>
#include <opencv2/optflow.hpp>
#include <sstream>
#include <unordered_map>
#include <unordered_set>
namespace schur_vins {
SchurVINS::SchurVINS() : curr_state(new ImuState()) {
// LOG(INFO) << "init begins";
}
void SchurVINS::InitCov() {
double quat_cov = 0.0001;
double pos_cov = 0.001;
double vel_cov = 0.001;
double ba_cov = 2e-3;
double bg_cov = 1e-6;
cov = Eigen::MatrixXd::Zero(15, 15);
cov.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * quat_cov;
cov.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * pos_cov;
cov.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity() * vel_cov;
cov.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity() * ba_cov;
cov.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity() * bg_cov;
}
void SchurVINS::InitImuModel(double acc_n, double acc_w, double gyr_n, double gyr_w) {
imu_noise = Matrix12d::Zero();
// LOG(INFO) << "acc_n: " << acc_n << ", acc_w: " << acc_w << ", gyr_n: " << gyr_n << ", gyr_w: " << gyr_w;
imu_noise.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * gyr_n * gyr_n; // acc_n * acc_n;
imu_noise.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() * acc_n * acc_n; // acc_w * acc_w;
imu_noise.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity() * acc_w * acc_w; // gyr_n * gyr_n;
imu_noise.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity() * gyr_w * gyr_w; // gyr_w * gyr_w;
}
void SchurVINS::InitExtrinsic(const svo::CameraBundle::Ptr camera_bundle) {
CHECK(camera_bundle.get() != nullptr);
int camera_num = camera_bundle->getNumCameras();
cam0_param = camera_bundle->getCameraShared(0);
T_imu_cam0 = camera_bundle->get_T_C_B(0).inverse();
// LOG(INFO) << "cam0 extrinsic, q_i_c:\n" << T_imu_cam0;
if (camera_num > 1) {
cam0_param = camera_bundle->getCameraShared(1);
T_imu_cam1 = camera_bundle->get_T_C_B(1).inverse();
stereo = true;
// LOG(INFO) << "cam1 extrinsic, q_i_c:\n" << T_imu_cam1;
}
}
void SchurVINS::InitMaxState(int val) {
state_max = val;
// LOG(INFO) << "state_max: " << state_max;
}
void SchurVINS::InitFocalLength(double val) {
focal_length = val;
// LOG(INFO) << "focal_length: " << focal_length;
}
void SchurVINS::InitObsStddev(double _obs_dev) {
obs_dev = _obs_dev;
obs_invdev = 1.0 / obs_dev;
// LOG(INFO) << "obs_dev: " << obs_dev;
}
void SchurVINS::InitChi2(double chi2_rate) {
chi_square_lut.clear();
const int max_dof = 50;
std::vector<double> lut(max_dof + 1, 0.0);
for (int i = 1; i <= max_dof; ++i) {
boost::math::chi_squared chi_square_dist(i);
lut.at(i) = boost::math::quantile(chi_square_dist, chi2_rate);
}
lut.swap(chi_square_lut);
// LOG(INFO) << "chi2_rate: " << chi2_rate << ", max_dof: " << max_dof;
}
//主要功能是向当前状态（curr_state）中添加一个新的状态，并更新协方差矩阵（cov）以适应新的状态。用于动态地管理状态和协方差矩阵，以实现精确的定位和姿态估计。
void SchurVINS::AugmentState(const svo::FrameBundle::Ptr frame_bundle) {
CHECK_EQ(frame_bundle->getMinTimestampSeconds(), curr_state->ts);
curr_state->id = id_creator++;
svo::AugStatePtr aug_ptr(new svo::AugState(curr_state->ts, curr_state->id, curr_state->quat, curr_state->pos,
curr_state->acc, curr_state->gyr));
aug_ptr->quat_fej = curr_state->quat_fej;
aug_ptr->pos_fej = curr_state->pos_fej;
aug_ptr->frame_bundle = frame_bundle;
states_map.emplace(aug_ptr->id, aug_ptr);//更新状态映射：将新的增强状态对象添加到状态映射states_map中，并更新状态ID的创建器id_creator。
CHECK((int)states_map.size() <= state_max);
size_t old_rows = cov.rows();
size_t old_cols = cov.cols();
CHECK(old_rows == old_cols);
CHECK(old_rows == (15 + states_map.size() * 6 - 6));
cov.conservativeResize(old_rows + 6, old_cols + 6);
cov.block(old_rows, 0, 6, old_cols) = cov.block(0, 0, 6, old_cols);//调整协方差矩阵大小：根据新的状态数量调整协方差矩阵的大小，使其能够容纳新的状态。
cov.block(0, old_cols, old_rows, 6) = cov.block(0, 0, old_rows, 6);
cov.block(old_rows, old_cols, 6, 6) = cov.block<6, 6>(0, 0);//更新协方差矩阵：将旧的协方差矩阵数据移动到新的位置，并初始化新的协方差矩阵块。
}
void SchurVINS::PredictionState(const Eigen::Vector3d& acc, const Eigen::Vector3d& gyro, double dt) {
const Eigen::Vector3d wd_full = gyro * dt;
const Eigen::Vector3d wd_half = wd_full * 0.5;
Eigen::Quaterniond dq_half = Eigen::Quaterniond(1, wd_half(0) / 2, wd_half(1) / 2, wd_half(2) / 2);
Eigen::Quaterniond dq_full = Eigen::Quaterniond(1, wd_full(0) / 2, wd_full(1) / 2, wd_full(2) / 2);
dq_half.normalize();
dq_full.normalize();
const Eigen::Quaterniond q_half = curr_state->quat * dq_half;
const Eigen::Quaterniond q_full = curr_state->quat * dq_full;
// k1
const Eigen::Vector3d k1_dv = curr_state->quat * acc + gravity;
const Eigen::Vector3d k1_dp = curr_state->vel;
// LOG(INFO) << "k1_dv: " << k1_dv[0] << ", " << k1_dv[1] << ", " << k1_dv[2];
// k2
const Eigen::Vector3d k1_v = curr_state->vel + k1_dv * dt / 2;
const Eigen::Vector3d k2_dv = q_half * acc + gravity;
const Eigen::Vector3d k2_dp = k1_v;
// k3
const Eigen::Vector3d k2_v = curr_state->vel + k2_dv * dt / 2;
const Eigen::Vector3d k3_dv = q_half * acc + gravity;
const Eigen::Vector3d k3_dp = k2_v;
// k4
const Eigen::Vector3d k3_v = curr_state->vel + k3_dv * dt;
const Eigen::Vector3d k4_dv = q_full * acc + gravity;
const Eigen::Vector3d k4_dp = k3_v;
// output
curr_state->quat = q_full;
curr_state->pos += dt / 6 * (k1_dp + 2 * k2_dp + 2 * k3_dp + k4_dp);
curr_state->vel += dt / 6 * (k1_dv + 2 * k2_dv + 2 * k3_dv + k4_dv);
}
//于进行VIO的状态预测。该方法使用四阶龙格-库塔法（Runge-Kutta method）来预测下一时刻的状态，并更新状态协方差矩阵
// _dt：时间步长，即当前时刻到下一时刻的时间间隔。_acc：当前时刻的加速度测量值;_gyr：当前时刻的角速度测量值。
void SchurVINS::Prediction(double _dt, const Eigen::Vector3d& _acc, const Eigen::Vector3d& _gyr) {
CHECK(_dt >= 0 && _dt < 0.015) << "dt: " << _dt;
Eigen::Vector3d acc = _acc - curr_state->ba;//加速度和角速度的校正
Eigen::Vector3d gyr = _gyr - curr_state->bg;
const Eigen::Matrix3d rot = curr_state->quat.toRotationMatrix();
Matrix15d F = Matrix15d::Zero();
Eigen::Matrix<double, 15, 12> G = Eigen::Matrix<double, 15, 12>::Zero();
// Q, P, V, Ba, Bg 状态转移矩阵和噪声矩阵的构建
// F：状态转移矩阵，用于描述状态如何随时间变化。
F.block<3, 3>(0, 12) = -rot;
F.block<3, 3>(3, 6) = Eigen::Matrix3d::Identity();
F.block<3, 3>(6, 0) = -Utility::SkewMatrix(rot * acc);
F.block<3, 3>(6, 9) = -rot;
// G：噪声转移矩阵，用于描述噪声如何随时间变化。
G.block<3, 3>(0, 0) = -rot;
G.block<3, 3>(6, 3) = -rot;
G.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity();
G.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity();
//状态转移矩阵的更新：根据时间步长_dt，计算状态转移矩阵的幂次项，用于四阶龙格-库塔法。
Matrix15d Fdt = F * _dt;
Matrix15d Fdt_square = Fdt * Fdt;
Matrix15d Fdt_cube = Fdt_square * Fdt;
Matrix15d Phi = Matrix15d::Identity() + Fdt + 0.5 * Fdt_square + (1.0 / 6.0) * Fdt_cube;
// state prediction, 4th order Runge-Kutta
PredictionState(acc, gyr, _dt);//状态预测：调用PredictionState方法，使用四阶龙格-库塔法进行状态预测。
// fej impl
// {
// Eigen::Matrix3d rr = (curr_state->quat * curr_state->quat_fej.inverse()).toRotationMatrix();
// Eigen::Matrix3d yy = - Utility::SkewMatrix(curr_state->pos - curr_state->pos_fej - curr_state->vel_fej * _dt
// - 0.5 * gravity * _dt * _dt); Eigen::Matrix3d ss = - Utility::SkewMatrix(curr_state->vel -
// curr_state->vel_fej - gravity * _dt); Phi.block<3, 3>(0, 0) = rr; Phi.block<3, 3>(3, 0) = yy; Phi.block<3,
// 3>(6, 0) = ss;
// }
// 协方差矩阵的更新：根据状态转移矩阵和噪声转移矩阵，更新状态协方差矩阵。
cov.block(0, 0, 15, 15)
= Phi * cov.block(0, 0, 15, 15) * Phi.transpose() + Phi * G * imu_noise * G.transpose() * Phi.transpose() * _dt;
if (!states_map.empty()) {
int cov_len = cov.rows();
cov.block(0, 15, 15, cov_len - 15) = Phi * cov.block(0, 15, 15, cov_len - 15);
cov.block(15, 0, cov_len - 15, 15) = cov.block(15, 0, cov_len - 15, 15) * Phi.transpose();
}
// 协方差矩阵的稳定化：通过将协方差矩阵与其转置相加并取平均值，确保协方差矩阵的对称性和稳定性
Eigen::MatrixXd stable_cov = (cov + cov.transpose()) / 2.0;
cov = stable_cov;
// 状态和时间的更新：更新当前状态的四元数、位置、速度、时间戳、加速度和角速度。
curr_state->quat_fej = curr_state->quat;
curr_state->pos_fej = curr_state->pos;
curr_state->vel_fej = curr_state->vel;
curr_state->ts += _dt;
curr_state->acc = acc;
curr_state->gyr = gyr;
}
void SchurVINS::Solve3() {
const int state_size = (int)states_map.size();
if (state_size < 2) {
LOG(INFO) << "only one window, bypass solve()";
return;
}
const int min_frame_idx = states_map.begin()->second->frame_bundle->getBundleId();
const int prev_frame_id0 = (++states_map.rbegin())->second->frame_bundle->frames_[0]->id(),
prev_frame_id1 = (++states_map.rbegin())->second->frame_bundle->frames_[1]->id();
const int max_frame_idx = states_map.rbegin()->second->frame_bundle->getBundleId();
const int state_len = state_size * 6;
const int64_t curr_state_id = curr_state->id;
Eigen::MatrixXd Amtx = Eigen::MatrixXd::Zero(state_len, state_len);
Eigen::VectorXd Bvct = Eigen::VectorXd::Zero(state_len);
Matrix2o3d dr_dpc = Matrix2o3d::Zero();
Matrix3o6d dpc_dpos = Matrix3o6d::Zero();
Matrix2o6d jx = Matrix2o6d::Zero();
Matrix2o3d jf = Matrix2o3d::Zero();
Eigen::Vector2d r = Eigen::Vector2d::Zero();
// compute local points jacobian
int num_obs = 0;
double total_error = 0.0;
schur_pts_.clear();
for (svo::LocalPointMap::iterator pit = local_pts.begin(); pit != local_pts.end(); ++pit) {
const svo::PointPtr& curr_pt = pit->second;
if (curr_pt->register_id_ != curr_state_id) { // init point jacobian
curr_pt->gv.setZero();
curr_pt->V.setZero();
curr_pt->W.setZero();
curr_pt->register_id_ = curr_state_id; // avoid init multi times
}
if (curr_pt->CheckStatus() == false || curr_pt->CheckLocalStatus() == false
|| curr_pt->CheckLocalStatus(prev_frame_id0, prev_frame_id1, max_frame_idx) == false) {
// LOG(INFO) << "Solve2 pass invalid point: " << curr_pt->id()
// << " feature num : " << curr_pt->local_obs_.size() << " pos: " <<
// curr_pt->pos().transpose();
continue;
}
schur_pts_.insert(curr_pt);
const Eigen::Vector3d& Pw = curr_pt->pos();
// compute frame local observation
for (svo::LocalFeatureMap::iterator fit = curr_pt->local_obs_.begin(); fit != curr_pt->local_obs_.end();
++fit) {
const svo::LocalFeaturePtr& feature = fit->second;
const int curr_frame_idx = feature->frame->bundleId();
if (curr_frame_idx < min_frame_idx
|| curr_frame_idx > max_frame_idx) // pass observations on schurvins local sliding window
continue;
if (feature->state.expired())
continue;
const svo::AugStatePtr& ft_state = feature->state.lock();
const int& state_idx = ft_state->index;
const svo::Transformation& T_imu_cam = feature->camera_id == 0 ? T_imu_cam0 : T_imu_cam1;
const Eigen::Matrix3d& R_i_c = T_imu_cam.getRotationMatrix();
const Eigen::Quaterniond& q_i_c = T_imu_cam.getEigenQuaternion();
const Eigen::Vector3d& t_i_c = T_imu_cam.getPosition();
const Eigen::Matrix3d R_c_w = (ft_state->quat * q_i_c).toRotationMatrix().transpose();
const Eigen::Vector3d Pi = ft_state->quat.inverse() * (Pw - ft_state->pos);
const Eigen::Vector3d Pc = q_i_c.inverse() * (Pi - t_i_c);
// const double level_scale = (1 << feature->level);
const double level_scale = 1;
// calc residual
if (svo::LocalFeature::unit_sphere) {
r = feature->tan_space * (feature->xyz - Pc.normalized()) * (focal_length / level_scale);
} else {
r = (feature->xyz.head<2>() - Pc.head<2>() / Pc.z()) * (focal_length / level_scale);
}
total_error += r.norm();
// LOG(INFO) << "kf residual: " << r.transpose();
// huber
const double r_l2 = r.squaredNorm();
double huber_scale = 1.0;
if (r_l2 > huberB) {
const double radius = sqrt(r_l2);
double rho1 = std::max(std::numeric_limits<double>::min(), huberA / radius);
huber_scale = sqrt(rho1);
r *= huber_scale;
}
r *= obs_invdev;
// calc jacobian
dr_dpc.setZero();
if (svo::LocalFeature::unit_sphere) {
double Pc_norm = Pc.norm();
double inv_norm = 1.0 / Pc_norm;
double inv_norm3 = 1.0 / (Pc_norm * Pc_norm * Pc_norm);
Eigen::Matrix3d norm_jacob = Eigen::Matrix3d::Identity() * inv_norm - Pc * Pc.transpose() * inv_norm3;
dr_dpc = feature->tan_space * norm_jacob * (focal_length * obs_invdev * huber_scale / level_scale);
} else {
const double pc22 = Pc[2] * Pc[2];
dr_dpc(0, 0) = 1 / Pc[2];
dr_dpc(1, 1) = 1 / Pc[2];
dr_dpc(0, 2) = -Pc[0] / pc22;
dr_dpc(1, 2) = -Pc[1] / pc22;
dr_dpc *= (focal_length * obs_invdev * huber_scale / level_scale);
}
++num_obs;
dpc_dpos.leftCols(3).noalias()
= R_i_c.transpose() * Utility::SkewMatrix(Pi) * ft_state->quat.toRotationMatrix().transpose();
dpc_dpos.rightCols(3) = -R_c_w;
// dpc_dext.leftCols(3).noalias() = Utility::SkewMatrix(Pc) * R_i_c.transpose();
// dpc_dext.rightCols(3) = -R_i_c.transpose();
// jext.noalias() = dr_dpc * dpc_dext;
jx.noalias() = dr_dpc * dpc_dpos;
jf.noalias() = dr_dpc * R_c_w;
// LOG(INFO) << "jext:\n" << jext;
// LOG(INFO) << "jx:\n" << jx;
// LOG(INFO) << "jf:\n" << jf;
////////////////////////////////////////////////////////////
// Amtx.block(0, 0, 6, 6).noalias() += jext.transpose() * jext; // ext 2 ext
// Matrix6d blk_ext2pos = jext.transpose() * jx;
// const int pos_bias = 6 + ft->state->index * 6;
const int pos_bias = state_idx * 6;
// Amtx.block(0, pos_bias, 6, 6) += blk_ext2pos; // ext 2 pos
// Amtx.block(pos_bias, 0, 6, 6) += blk_ext2pos.transpose(); // pos 2 ext
Amtx.block(pos_bias, pos_bias, 6, 6).noalias() += jx.transpose() * jx; // pos 2 pos
// Bvct.segment(0, 6).noalias() += jext.transpose() * r; // ext grad
Bvct.segment(pos_bias, 6).noalias() += jx.transpose() * r; // pos grad
curr_pt->V.noalias() += jf.transpose() * jf; // pt 2 pt
curr_pt->gv.noalias() += jf.transpose() * r; // pt grad
// curr_pt->W.noalias() += jext.transpose() * jf; // ext 2 pt
feature->W.noalias() = jx.transpose() * jf; // pos 2 pt
// LOG(INFO) << "V:\n" << curr_pt->V;
// LOG(INFO) << "Vinv:\n" << curr_pt->V.inverse();
// LOG(INFO) << "gv:\n" << curr_pt->gv.transpose();
// LOG(INFO) << "W:\n" << ft->W;
}
}
// combine point observation on same body
for (const svo::PointPtr& pt : schur_pts_) {
pt->state_factors.clear();
for (svo::LocalFeatureMap::iterator iti = pt->local_obs_.begin(); iti != pt->local_obs_.end(); iti++) {
if (iti->second->state.expired())
continue;
const svo::AugStatePtr& ft_state = iti->second->state.lock();
svo::StateFactorMap::iterator iter = pt->state_factors.find(ft_state->index);
if (iter == pt->state_factors.end()) {
pt->state_factors.emplace(ft_state->index, svo::StateFactor(iti->second->W, ft_state));
} else {
iter->second.W += iti->second->W;
}
}
}
// schur completment
for (const svo::PointPtr& pt : schur_pts_) {
pt->Vinv = pt->V.inverse();
// LOG(INFO) << "pt->V:\n" << pt->V;
// LOG(INFO) << "pt->Vinv:\n" << pt->Vinv;
// const Matrix6o3d eWVinv = pt->W * pt->Vinv;
// LOG(INFO) << "eWVinv:\n" << eWVinv;
// Amtx.block(0, 0, 6, 6) -= eWVinv * pt->W.transpose(); // ext 2 ext schur
// Bvct.segment(0, 6) -= eWVinv * pt->gv; // ext grad schur
for (svo::StateFactorMap::iterator iti = pt->state_factors.begin(); iti != pt->state_factors.end(); iti++) {
if (iti->second.state.expired())
continue;
const svo::AugStatePtr& statei = iti->second.state.lock(); // int pi_bias = 6 + statei->index * 6;
const int pi_bias = statei->index * 6;
// const Matrix6d e2p_schur = eWVinv * fti->W.transpose(); // ext 2 pos schur
// Amtx.block(0, pi_bias, 6, 6) -= e2p_schur; //
// Amtx.block(pi_bias, 0, 6, 6) -= e2p_schur.transpose(); //
// LOG(INFO) << "e2p_schur:\n" << e2p_schur;
// LOG(INFO) << "Amtx:\n" << Amtx;
const Matrix6o3d WVinv = iti->second.W * pt->Vinv;
for (svo::StateFactorMap::iterator itj = iti; itj != pt->state_factors.end(); itj++) {
if (itj->second.state.expired())
continue;
const svo::AugStatePtr& statej = itj->second.state.lock();
// int pj_bias = 6 + statej->index * 6;
const int pj_bias = statej->index * 6;
Matrix6d p2p_schur = WVinv * itj->second.W.transpose(); // posi 2 posj schur
Amtx.block(pi_bias, pj_bias, 6, 6) -= p2p_schur;
if (pi_bias != pj_bias) {
Amtx.block(pj_bias, pi_bias, 6, 6) -= p2p_schur.transpose();
}
// LOG(INFO) << "Amtx:\n" << Amtx;
}
Bvct.segment(pi_bias, 6) -= WVinv * pt->gv; // pos grad schur
// LOG(INFO) << "Bvct:\n" << Bvct.transpose();
}
}
// LOG(INFO) << "Amtx:\n" << Amtx;
// LOG(INFO) << "Bvct:\n" << Bvct;
StateUpdate(Amtx, Bvct);
// LOG(ERROR) << "total local pts: " << local_pts.size() << " schur pts: " << schur_pts_.size();
}
void SchurVINS::StateUpdate(const Eigen::MatrixXd& hessian, const Eigen::VectorXd& gradient) {
// LOG(INFO) << "StateUpdate:\n";
// LOG(INFO) << "hessian: " << hessian;
// LOG(INFO) << "gradient: " << gradient.transpose();
int rows = (int)hessian.rows();
CHECK(rows == (int)states_map.size() * 6);
const Eigen::MatrixXd& R = hessian;
Eigen::MatrixXd Jacob_compress = Eigen::MatrixXd::Zero(rows, rows + 15);
Jacob_compress.block(0, 15, rows, rows) = hessian;
Eigen::MatrixXd S = hessian * cov.bottomRightCorner(rows, rows) * hessian.transpose() + R;
Eigen::MatrixXd K_T = S.ldlt().solve(hessian * cov.bottomRows(rows));
Eigen::MatrixXd K = K_T.transpose();
// std::cout << "K: " << S.rows() << " " << S.cols() << " " << hessian.rows() << " " << hessian.cols() << std::endl;
// std::cout << "K: " << cov.rows() << " " << cov.cols() << " " <<cov.bottomRows(rows).rows() << " " <<
// cov.bottomRows(rows).cols() << std::endl;
// Eigen::MatrixXd K = S.inverse() * hessian * cov.bottomRows(rows);
Eigen::VectorXd delta_x = K * gradient;
StateCorrection(K, Jacob_compress, delta_x, R);
dsw_ = delta_x.tail(delta_x.rows() - 15);
}
void SchurVINS::StateCorrection(const Eigen::MatrixXd& K, const Eigen::MatrixXd& J, const Eigen::VectorXd& dX,
const Eigen::MatrixXd& R) {
// Update the IMU state.
// quat, pos, vel, ba, bg
const Vector15d& dx_imu = dX.head(15);
const Eigen::Quaterniond dq_imu = Eigen::Quaterniond(1.0, 0.5 * dx_imu[0], 0.5 * dx_imu[1], 0.5 * dx_imu[2]);
Eigen::Quaterniond new_quat = dq_imu * curr_state->quat;
new_quat.normalize();
curr_state->quat = new_quat;
curr_state->pos += dx_imu.segment<3>(3);
curr_state->vel += dx_imu.segment<3>(6);
curr_state->ba += dx_imu.segment<3>(9);
curr_state->bg += dx_imu.segment<3>(12);
int idx = 0;
for (svo::StateMap::value_type& item : states_map) {
const Vector6d& dAX = dX.segment<6>(15 + idx);
Eigen::Quaterniond dq = Eigen::Quaterniond(1.0, 0.5 * dAX[0], 0.5 * dAX[1], 0.5 * dAX[2]);
dq.normalize();
item.second->quat = dq * item.second->quat;
item.second->rot = item.second->quat.toRotationMatrix();
item.second->pos += dAX.tail<3>();
idx += 6;
}
Eigen::MatrixXd I_KH = Eigen::MatrixXd::Identity(K.rows(), K.rows()) - K * J;
cov = I_KH * cov;
Eigen::MatrixXd stable_cov = (cov + cov.transpose()) / 2.0;
cov = stable_cov;
}
//该函数用于在VIO系统中动态管理状态和协方差矩阵，当某个状态（如一个关键帧）被移除时，更新系统以反映这一变化，确保系统的稳定性和准确性。
void SchurVINS::Management(int index) {
CHECK((int)states_map.size() == state_max);
int src_idx = 0;
int dst_idx = 0;
int cov_len = cov.rows();
while (src_idx < state_max && dst_idx < state_max) {
if (src_idx == index) {
src_idx++;
continue;
}
if (src_idx != dst_idx) {
cov.block(dst_idx * 6 + 15, 0, 6, cov_len) = cov.block(src_idx * 6 + 15, 0, 6, cov_len);
cov.block(0, dst_idx * 6 + 15, cov_len, 6) = cov.block(0, src_idx * 6 + 15, cov_len, 6);
}
src_idx++;
dst_idx++;
}
cov.conservativeResize(dst_idx * 6 + 15, dst_idx * 6 + 15);
int idx = 0;
for (auto it = states_map.begin(); it != states_map.end();) {
if (idx++ == index) {
for (auto& ft : it->second->features) {
if (const svo::PointPtr& pt = ft->point.lock()) {
int64_t state_id = it->second->id;
pt->local_obs_.erase(state_id);
}
}
it->second->features.clear();
it = states_map.erase(it);
break;
} else {
it++;
}
}
CHECK(((int)states_map.size() * 6 + 15) == cov.rows()) << states_map.size() << ", " << cov.rows();
ManageLocalMap();
}
void SchurVINS::ManageLocalMap() {
for (svo::LocalPointMap::iterator it = local_pts.begin(); it != local_pts.end();) {
svo::PointPtr& pt = it->second;
if (pt->local_obs_.empty()) {
it = local_pts.erase(it);
}
// else if(pt->features.rbegin()->first != curr_state->id) {
// pt->FeaturesClear();
// it = local_pts.erase(it);
// }
else {
it++;
}
}
}
bool SchurVINS::KeyframeSelect() {
bool delete_r2th = false;
// false - delete the oldest keyframe;
// true - delete the second new keyframe.
CHECK((int)states_map.size() == state_max)
<< "states_map.size: " << states_map.size() << ", state_max: " << state_max;
std::unordered_set<int64_t> tail_set;
size_t rindex = 0;
int common_cnt = 0;
for (svo::StateMap::reverse_iterator rit = states_map.rbegin(); rit != states_map.rend();) {
if (1 == rindex) {
for (const svo::LocalFeaturePtr& ft : rit->second->features) {
if (const svo::PointPtr& point = ft->point.lock()) {
CHECK(point);
tail_set.insert(point->id());
}
}
}
if (2 == rindex) {
for (svo::LocalFeaturePtr& ft : rit->second->features) {
if (const svo::PointPtr& point = ft->point.lock()) {
CHECK(point);
if (tail_set.find(point->id()) != tail_set.end()) {
common_cnt++;
}
}
}
break;
}
rit++;
rindex++;
}
curr_fts_cnt = (int)tail_set.size();
double track_rate = common_cnt / (curr_fts_cnt + 1e-6);
if (curr_fts_cnt < 10 || track_rate < 0.70) {
delete_r2th = false;
} else {
delete_r2th = true;
}
// LOG(ERROR) << "delete_r2th: " << delete_r2th << ", track_rate: " << track_rate
// << ", curr_fts_cnt: " << curr_fts_cnt;
return delete_r2th;
}
//主要功能是将一组图像帧中的特征点注册到当前的状态中
void SchurVINS::RegisterPoints(const svo::FrameBundle::Ptr& frame_bundle) {
CHECK(!states_map.empty());
//获取当前状态：通过states_map.rbegin()->second获取当前状态（state_ptr），并获取其ID（state_id）
auto& state_ptr = states_map.rbegin()->second;
const int state_id = state_ptr->id;
for (const svo::FramePtr& frame : frame_bundle->frames_) {
const int camera_id = frame->getNFrameIndex();
int num_feature = 0;
for (size_t i = 0; i < frame->numFeatures(); ++i) {
svo::PointPtr point = frame->landmark_vec_[i];
if (point != nullptr && svo::isCorner(frame->type_vec_[i])) {
//创建局部特征：通过std::make_shared<svo::LocalFeature>创建一个指向svo::LocalFeature的智能指针（ft），并初始化其参数，包括状态指针、特征点、帧、特征点在帧中的坐标和相机ID。
const svo::LocalFeaturePtr ft
= std::make_shared<svo::LocalFeature>(state_ptr, point, frame, frame->f_vec_.col(i), camera_id);
ft->level = frame->level_vec_[i];
svo::LocalPointMap::iterator iter = local_pts.find(point->id());
//注册特征点：通过local_pts.find(point->id())检查特征点是否已经注册。如果未注册，则将其插入到local_pts中，并将特征点添加到当前状态的特征列表中。同时，将特征点添加到特征点的局部观测列表中。
if (iter == local_pts.end()) {
local_pts.insert({point->id(), point});
state_ptr->features.emplace_back(ft);
point->local_obs_.insert({state_id, ft});
} else {
state_ptr->features.emplace_back(ft);
point->local_obs_.insert({state_id, ft});
}
++num_feature;
}
}
// LOG(INFO) << "RegisterPoints camera id: " << camera_id << " num feature: " << num_feature;
}
}
void SchurVINS::InitState(double _ts, const Eigen::Vector3d& _acc, const Eigen::Vector3d& _gyr) {
if (curr_state->ts > 0)
return;
Eigen::Vector3d acc_norm = _acc / _acc.norm();
Eigen::Vector3d grav_norm = Eigen::Vector3d(0, 0, 1);
// LOG(INFO) << "acc: " << _acc[0] << ", " << _acc[1] << ", " << _acc[2] << ", gyr: " << _gyr[0] << ", " << _gyr[1]
// << ", " << _gyr[2];
Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(acc_norm, grav_norm);
curr_state->ts = _ts;
curr_state->quat = quat;
// LOG(INFO) << "quat.rot: " << quat.coeffs().transpose();
curr_state->pos.setZero();
curr_state->vel.setZero();
curr_state->ba.setZero();
curr_state->bg.setZero();
// LOG(INFO) << "InitState: " << curr_state->ts << ", quat: " << curr_state->quat.w() << ", " <<
// curr_state->quat.x()
// << ", " << curr_state->quat.y() << ", " << curr_state->quat.z() << ", "
// << "pos: " << curr_state->pos[0] << ", " << curr_state->pos[1] << ", " << curr_state->pos[2] << ", "
// << "vel: " << curr_state->vel[0] << ", " << curr_state->vel[1] << ", " << curr_state->vel[2] << ", "
// << "ba: " << curr_state->ba[0] << ", " << curr_state->ba[1] << ", " << curr_state->ba[2] << ", "
// << "bg: " << curr_state->bg[0] << ", " << curr_state->bg[1] << ", " << curr_state->bg[2];
curr_state->quat_fej = curr_state->quat;
curr_state->pos_fej = curr_state->pos;
curr_state->vel_fej = curr_state->vel;
// Eigen::Quaterniond quat2 = Eigen::Quaterniond::FromTwoVectors(grav_norm, acc_norm);
// std::cout << "quat2.rot: " << std::endl << quat2.toRotationMatrix() << std::endl;
}
void SchurVINS::SetKeyframe(const bool _is_keyframe) {
const int L1 = 0, R2 = state_max - 2;
if ((int)states_map.size() == state_max) {
if (_is_keyframe == true) {
// LOG(ERROR) << "keyframe select outside";
const int idx = R2; // delete r2 frame when curr frame is keyframe
Management(idx);
} else {
// LOG(ERROR) << "keyframe select insside";
const int idx = KeyframeSelect() ? R2 : L1;
Management(idx);
}
}
}
//用于处理VIO的前向传播。主要目的是根据输入的帧包（frame_bundle）中的IMU数据和当前状态，对系统状态进行预测和更新。
void SchurVINS::Forward(const svo::FrameBundle::Ptr frame_bundle) {
constexpr double EPS = 1e-9;
// LOG(WARNING) << "schurvins Forward";
double img_ts = frame_bundle->getMinTimestampSeconds();
//处理IMU（惯性测量单元）数据，并根据这些数据对当前状态进行预测和更新
for (size_t i = 0; i < frame_bundle->imu_datas_.size(); i++) {
const double tmp_ts = frame_bundle->imu_datas_[i].timestamp_;
const Eigen::Vector3d tmp_acc = frame_bundle->imu_datas_[i].linear_acceleration_;
const Eigen::Vector3d tmp_gyr = frame_bundle->imu_datas_[i].angular_velocity_;
if (prev_imu_ts < 0) {//如果prev_imu_ts小于0（表示还没有初始化），则将当前IMU数据的加速度、角速度和时间戳保存到prev_acc、prev_gyr和prev_imu_ts中。
prev_acc = tmp_acc;
prev_gyr = tmp_gyr;
prev_imu_ts = tmp_ts;
}
if (curr_state->ts < 0) {//如果当前状态的时间戳小于0（表示还没有初始化），则调用InitState函数初始化当前状态。
InitState(prev_imu_ts, prev_acc, prev_gyr);
}
if (tmp_ts < curr_state->ts + EPS) {//如果当前IMU数据的时间戳小于当前状态的时间戳加上一个很小的值EPS，则跳过该数据。
// LOG(WARNING) << std::fixed << "pass imu data: " << tmp_ts;
continue;
}
if (tmp_ts < img_ts + EPS) {//如果当前IMU数据的时间戳小于图像时间戳加上EPS，则计算时间差deltaT，并调用Prediction函数进行预测。然后更新prev_acc、prev_gyr和prev_imu_ts。
double deltaT = tmp_ts - curr_state->ts;
CHECK(deltaT >= 0) << "deltaT: " << deltaT;
Prediction(deltaT, prev_acc, prev_gyr);
prev_acc = tmp_acc;
prev_gyr = tmp_gyr;
prev_imu_ts = tmp_ts;
} else { // Interpolation img time imu data
//如果当前IMU数据的时间戳在当前状态和图像时间戳之间，则计算两个时间差dt_1和dt_2，并计算插值权重w1和w2。然后调用Prediction函数进行预测，并更新prev_acc、prev_gyr和prev_imu_ts。
double dt_1 = img_ts - curr_state->ts;
double dt_2 = tmp_ts - img_ts;
CHECK(dt_1 >= 0) << "dt_1: " << dt_1;
CHECK(dt_2 >= 0) << "dt_2: " << dt_2;
CHECK(dt_1 + dt_2 > 0) << "dt_1 + dt_2: " << dt_1 + dt_2;
double w1 = dt_2 / (dt_1 + dt_2);
double w2 = dt_1 / (dt_1 + dt_2);
Prediction(dt_1, prev_acc, prev_gyr);
prev_acc = w1 * prev_acc + w2 * tmp_acc;
prev_gyr = w1 * prev_gyr + w2 * tmp_gyr;
prev_imu_ts = img_ts;
}
}
// update result
svo::Transformation T_world_imu = svo::Transformation(curr_state->quat, curr_state->pos);
for (const svo::FramePtr& frame : frame_bundle->frames_) {
frame->T_f_w_ = frame->T_cam_imu() * T_world_imu.inverse();
}
{
Eigen::Quaterniond quat = curr_state->quat;
Eigen::Vector3d pos = curr_state->pos;
Eigen::Vector3d vel = curr_state->vel;
Eigen::Vector3d ba = curr_state->ba;
Eigen::Vector3d bg = curr_state->bg;
// LOG(INFO) << "schurvins forward: " << std::fixed << std::setprecision(6) << curr_state->ts << ", quat: " <<
// quat.w()
// << ", " << quat.x() << ", " << quat.y() << ", " << quat.z() << ", "
// << "pos: " << pos[0] << ", " << pos[1] << ", " << pos[2] << ", "
// << "vel: " << vel[0] << ", " << vel[1] << ", " << vel[2] << ", "
// << "ba: " << ba[0] << ", " << ba[1] << ", " << ba[2] << ", "
// << "bg: " << bg[0] << ", " << bg[1] << ", " << bg[2];
// LOG(INFO) << "gravity: " << gravity[0] << ", " << gravity[1] << ", " << gravity[2];
}
}
//主要功能是处理VIO的后向优化步骤
int SchurVINS::Backward(const svo::FrameBundle::Ptr frame_bundle) {
//状态和特征点的增强
AugmentState(frame_bundle);
RegisterPoints(frame_bundle);
//状态索引的设置
int idx = 0;
std::for_each(states_map.begin(), states_map.end(),
[&](svo::StateMap::value_type& it) { it.second->index = idx++; });
Solve3();//优化求解。这个方法的具体实现细节没有在代码中给出，但通常涉及到使用Schur补和Levenberg-Marquardt算法进行非线性优化。
// update result
for (const svo::StateMap::value_type& item : states_map) {
const svo::Transformation T_world_imu = svo::Transformation(item.second->quat, item.second->pos);
const svo::FrameBundle::Ptr& tmp_frame_bundle = item.second->frame_bundle;
for (const svo::FramePtr& frame : tmp_frame_bundle->frames_) {
frame->T_f_w_ = frame->T_cam_imu() * T_world_imu.inverse();
}
}
int num_valid_feature = 0;
if (states_map.size() == 1) {
num_valid_feature = states_map.rbegin()->second->features.size();
} else {
num_valid_feature = RemoveOutliers(frame_bundle); // remove curr frame outlier first
RemovePointOutliers(); // remove local point outlier second
}
{
Eigen::Quaterniond quat = curr_state->quat;
Eigen::Vector3d pos = curr_state->pos;
Eigen::Vector3d vel = curr_state->vel;
Eigen::Vector3d ba = curr_state->ba;
Eigen::Vector3d bg = curr_state->bg;
// LOG(INFO) << "schurvins backward: " << std::fixed << std::setprecision(6) << curr_state->ts
// << ", quat: " << quat.w() << ", " << quat.x() << ", " << quat.y() << ", " << quat.z() << ", "
// << "pos: " << pos[0] << ", " << pos[1] << ", " << pos[2] << ", "
// << "vel: " << vel[0] << ", " << vel[1] << ", " << vel[2] << ", "
// << "ba: " << ba[0] << ", " << ba[1] << ", " << ba[2] << ", "
// << "bg: " << bg[0] << ", " << bg[1] << ", " << bg[2];
// LOG(INFO) << "gravity: " << gravity[0] << ", " << gravity[1] << ", " << gravity[2];
}
return num_valid_feature;
}
bool SchurVINS::StructureOptimize(const svo::PointPtr& optimize_point) {
if (schur_pts_.count(optimize_point)) {
const int min_idx = states_map.begin()->second->frame_bundle->getBundleId();
const int max_idx = states_map.rbegin()->second->frame_bundle->getBundleId();
optimize_point->EkfUpdate(dsw_, obs_dev, focal_length, min_idx, max_idx, huberA, huberB);
// optimize_point->EkfUpdate(dsw_, obs_dev);
return true;
}
// LOG(ERROR) << "miss schur point: " << optimize_point->local_obs_.size()
// << " key obs: " << optimize_point->obs_.size() << " pos: " << optimize_point->pos().norm()
// << " local status: " << (int)optimize_point->local_status_;
return false;
}
//移除VIO离群点,群点通常是指那些与大部分数据点不一致，可能由噪声或错误的测量引起的点
int SchurVINS::RemovePointOutliers() {
constexpr double MAX_REPROJECT_ERROR = 3.0;
const int min_frame_idx = states_map.begin()->second->frame_bundle->getBundleId();
const int max_frame_idx = states_map.rbegin()->second->frame_bundle->getBundleId();
const int state_len = states_map.size() * 6;
const int64_t curr_state_id = curr_state->id;
int outlier_points_num = 0, total_points_num = 0;
for (svo::LocalPointMap::iterator pit = local_pts.begin(); pit != local_pts.end(); ++pit) {
const svo::PointPtr& curr_pt = pit->second;
if (curr_pt->CheckStatus() == false) {
// LOG(INFO) << "Solve2 pass invalid point: " << curr_pt->id()
// << " feature num : " << curr_pt->local_obs_.size() << " pos: " <<
// curr_pt->pos().transpose();
continue;
}
double total_error = 0.0, num_obs = 0.0;
const Eigen::Vector3d Pw = curr_pt->pos();
// compute frame local observation
for (svo::LocalFeatureMap::iterator fit = curr_pt->local_obs_.begin(); fit != curr_pt->local_obs_.end();
++fit) {
const svo::LocalFeaturePtr& feature = fit->second;
const int curr_frame_idx = feature->frame->bundleId();
if (curr_frame_idx < min_frame_idx
|| curr_frame_idx > max_frame_idx) // pass observations on schurvins local sliding window
continue;
if (feature->state.expired())
continue;
// const svo::AugStatePtr& ft_state = feature->state.lock();
// const int state_idx = ft_state->index;
// const svo::Transformation& T_imu_cam = feature->camera_id == 0 ? T_imu_cam0 : T_imu_cam1;
// const Eigen::Matrix3d R_i_c = T_imu_cam.getRotationMatrix();
// const Eigen::Quaterniond q_i_c = T_imu_cam.getEigenQuaternion();
// const Eigen::Vector3d t_i_c = T_imu_cam.getPosition();
// const Eigen::Matrix3d R_c_w = (ft_state->quat * q_i_c).toRotationMatrix().transpose();
// const Eigen::Vector3d Pw = curr_pt->pos();
// const Eigen::Vector3d Pi = ft_state->quat.inverse() * (Pw - ft_state->pos);
const Eigen::Vector3d Pc = feature->frame->T_f_w_ * curr_pt->pos();
// const double level_scale = (1 << feature->level);
const double level_scale = 1;
// calc residual
// 计算重投影误差：对于每个点，计算其在所有观测帧中的重投影误差。重投影误差是通过将点的三维坐标投影到图像平面，并与观测到的二维坐标进行比较得到的。
const Eigen::Vector2d r = (feature->xyz.head<2>() - Pc.head<2>() / Pc.z()) * (focal_length / level_scale);
total_error += r.norm();
++num_obs;
}
const double avg_error = total_error / num_obs;
if (avg_error > MAX_REPROJECT_ERROR) {
++outlier_points_num;
curr_pt->local_status_ = false;
// curr_pt->last_structure_optim_ = -1;
} else {
curr_pt->local_status_ = true;
}
}
// LOG(ERROR) << "outlier_points_num: " << outlier_points_num << " ";
return outlier_points_num;
}
//移除VIO中的异常值（外点）:该方法通过计算重投影误差来识别和移除那些在重投影过程中产生较大误差的特征点，这些特征点被认为是异常值
int SchurVINS::RemoveOutliers(const svo::FrameBundle::Ptr frame_bundle) {
constexpr double MAX_REPROJECT_ERROR = 4.0;
double total_error = 0;
const svo::AugStatePtr& state = states_map.rbegin()->second;
int num_valid_feature = 0, num_invalid_feature = 0;
for (const svo::FramePtr& frame : frame_bundle->frames_) {
// const Eigen::Matrix3d R_i_c = frame->T_imu_cam().getRotationMatrix();
// const Eigen::Quaterniond q_i_c = frame->T_imu_cam().getEigenQuaternion();
// const Eigen::Vector3d t_i_c = frame->T_imu_cam().getPosition();
// const Eigen::Matrix3d R_c_w = (state->quat * q_i_c).toRotationMatrix().transpose();
// const Eigen::Matrix3d R_c_w = frame->T_f_w_.getRotationMatrix();
// const Eigen::Vector3d t_c_w = frame->T_f_w_.getPosition();
for (size_t i = 0; i < frame->numFeatures(); ++i) {
const svo::PointPtr curr_pt = frame->landmark_vec_[i];
const Eigen::Vector3d obs = frame->f_vec_.col(i);
// const double level_scale = (1 << frame->level_vec_[i]);
const double level_scale = 1;
if (curr_pt && local_pts.find(curr_pt->id()) != local_pts.end()) {
if (curr_pt->CheckStatus() == false)
continue;
const Eigen::Vector3d Pc = frame->T_f_w_ * curr_pt->pos();
const Eigen::Vector2d r
= (obs.head<2>() / obs.z() - Pc.head<2>() / Pc.z()) * (focal_length / level_scale);
total_error += r.norm();
if (r.norm() > MAX_REPROJECT_ERROR) {
// curr_pt->RemoveLocalObs(state->id, frame->getNFrameIndex());
frame->type_vec_[i] = svo::FeatureType::kOutlier;
frame->seed_ref_vec_[i].keyframe.reset();
frame->landmark_vec_[i] = nullptr; // delete landmark observation
num_invalid_feature++;
// LOG(ERROR) << "RemoveOutliers residual: " << r.transpose()
// << " camera id: " << frame->getNFrameIndex();
} else {
num_valid_feature++;
}
}
}
}
// LOG(INFO) << "RemoveOutliers total error: " << total_error << " num_total_feature: " << state->features.size()
// << " num_valid_feature: " << num_valid_feature << " num_invalid_feature: " << num_invalid_feature;
return num_valid_feature;
}
} // namespace schur_vins
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// This file is subject to the terms and conditions defined in the file
// 'LICENSE', which is part of this source code package.
// Modification Note:
// This file may have been modified by the authors of SchurVINS.
// (All authors of SchurVINS are with PICO department of ByteDance Corporation)
#include <svo/common/point.h>
#include <vikit/math_utils.h>
#include <svo/common/logging.h>
#include <svo/common/frame.h>
#include <svo/common/local_feature.h>
namespace svo {
std::atomic<int> PointIdProvider::last_id_ { 0 };
KeypointIdentifier::KeypointIdentifier(const FramePtr& _frame, const size_t _feature_index)
: frame(_frame)
, frame_id(_frame->id_)
, keypoint_index_(_feature_index)
{ ; }
Point::Point(const Eigen::Vector3d& pos)
: Point(PointIdProvider::getNewPointId(), pos)
{ ; }
Point::Point(const int id, const Eigen::Vector3d& pos)
: id_(id)
, pos_(pos)
{
last_projected_kf_id_.fill(-1);
}
Point::~Point()
{}
std::atomic_uint64_t Point::global_map_value_version_ {0u};
bool Point::CheckStatus() {
constexpr double MAX_DIST = 1e3;
constexpr int MIN_OBS = 3;
if (pos().norm() > MAX_DIST || local_obs_.size() < MIN_OBS)
return false;
return true;
}
bool Point::CheckLocalStatus() {
if (local_status_ == false)
return false;
return true;
}
bool Point::CheckLocalStatus(const int prev_frame_id0, const int prev_frame_id1, const int curr_framebundle_id) {
auto iter = local_obs_.find(curr_framebundle_id);
bool has_opt_prev = ((last_structure_optim_ == prev_frame_id0) || (last_structure_optim_ == prev_frame_id1));
bool has_obs_curr = (iter != local_obs_.end());
if (has_opt_prev || has_obs_curr)
return true;
return false;
}
void Point::RemoveLocalObs(const int state_id, const int camera_id) {
svo::LocalFeatureMap::iterator iter = local_obs_.find(state_id);
if (iter == local_obs_.end())
return;
for (; iter != local_obs_.end(); ++iter) {
if (iter->first != state_id)
break;
if (iter->first == state_id && iter->second->camera_id == camera_id) {
// LOG(ERROR) << "find local obs erase it";
local_obs_.erase(iter);
break;
}
}
}
void Point::addObservation(const FramePtr& frame, const size_t feature_index)
{
CHECK_NOTNULL(frame.get());
// check that we don't have yet a reference to this frame
// TODO(cfo): maybe we should use a std::unordered_map to store the observations.
const auto id = frame->id();
auto it = std::find_if(obs_.begin(), obs_.end(),
[&](const KeypointIdentifier& i){ return i.frame_id == id; });
if(it == obs_.end())
{
obs_.emplace_back(KeypointIdentifier(frame, feature_index));
}
else
{
CHECK_EQ(it->keypoint_index_, feature_index);
}
}
void Point::removeObservation(int frame_id)
{
obs_.erase(
std::remove_if(obs_.begin(), obs_.end(),
[&](const KeypointIdentifier& o){ return o.frame_id == frame_id; }),
obs_.end());
}
void Point::initNormal()
{
CHECK(!obs_.empty()) << "initializing normal without any observation";
if(const FramePtr& frame = obs_.front().frame.lock())
{
BearingVector f = frame->f_vec_.col(obs_.front().keypoint_index_);
normal_ = frame->T_f_w_.getRotation().inverseRotate(-f);
normal_information_ = Eigen::Matrix2d::Identity(); //DiagonalMatrix<double,3,3>(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0);
normal_set_ = true;
}
else
SVO_ERROR_STREAM("could not unlock weak_ptr<frame> in normal initialization");
}
bool Point::getCloseViewObs(
const Eigen::Vector3d& framepos,
FramePtr& ref_frame,
size_t& ref_feature_index) const
{
double min_cos_angle = 0.0;
Eigen::Vector3d obs_dir(framepos - pos_);
obs_dir.normalize();
//
// TODO: For edgelets, find another view that reduces an epipolar line that
// is orthogonal to the gradient!
//
// TODO: get frame with same point of view AND same pyramid level!
for(const KeypointIdentifier& obs : obs_)
{
if(FramePtr frame = obs.frame.lock())
{
Eigen::Vector3d dir(frame->pos() - pos_);
dir.normalize();
const double cos_angle = obs_dir.dot(dir);
if(cos_angle > min_cos_angle)
{
min_cos_angle = cos_angle;
ref_frame = frame;
ref_feature_index = obs.keypoint_index_;
}
}
else
{
SVO_DEBUG_STREAM("could not unlock weak_ptr<Frame> in Point::getCloseViewObs"
<<", Point-ID = " << id_
<<", Point-nObs = " << obs_.size()
<<", Frame-ID = " << obs.frame_id
<<", Feature-ID = " << obs.keypoint_index_
<<", Point-Type = " << type_);
return false;
}
}
if(min_cos_angle < 0.4) // assume that observations larger than 60° are useless
{
SVO_DEBUG_STREAM("getCloseViewObs(): obs is from too far away: " << min_cos_angle);
return false;
}
return true;
}
double Point::getTriangulationParallax() const
{
CHECK(!obs_.empty()) << "getTriangualtionParallax(): obs_ is empty!";
const FramePtr ref_frame = obs_.front().frame.lock();
if(!ref_frame)
{
SVO_ERROR_STREAM("getTriangualtionParallax(): Could not lock ref_frame");
return 0.0;
}
const Eigen::Vector3d r = (ref_frame->pos()-pos_).normalized();
double max_parallax = 0.0;
for(const KeypointIdentifier& obs : obs_)
{
if(const FramePtr frame = obs.frame.lock())
{
const Eigen::Vector3d v = (frame->pos()-pos_).normalized();
const double parallax = std::acos(r.dot(v));
max_parallax = std::max(parallax, max_parallax);
}
}
return max_parallax;
}
FramePtr Point::getSeedFrame() const
{
if(obs_.empty())
return nullptr;
// the seed should be in the observation with the smallest id
if(auto ref_frame = obs_.front().frame.lock())
return ref_frame;
else
SVO_ERROR_STREAM("could not lock weak_ptr<Frame> in point");
return nullptr;
}
bool Point::triangulateLinear()
{
const size_t n_obs = obs_.size();
if(n_obs < 2)
{
return false; // not enough measurements to triangulate.
}
Eigen::Matrix3Xd f_world; // bearing vectors in world coordinates
Eigen::Matrix3Xd p_world; // position vectors of cameras
f_world.resize(Eigen::NoChange, n_obs);
p_world.resize(Eigen::NoChange, n_obs);
size_t index = 0;
for(const KeypointIdentifier& obs : obs_)
{
if(const FramePtr frame = obs.frame.lock())
{
const Transformation T_world_cam = frame->T_world_cam();
f_world.col(index) = T_world_cam*frame->f_vec_.col(obs.keypoint_index_);
p_world.col(index) = T_world_cam.getPosition();
++index;
}
}
if(index != n_obs)
{
SVO_ERROR_STREAM("triangulateLinear failed, could not lock all frames.");
return false;
}
// from aslam: triangulation.cc
const Eigen::MatrixXd BiD = f_world *
f_world.colwise().squaredNorm().asDiagonal().inverse();
const Eigen::Matrix3d AxtAx = n_obs * Eigen::Matrix3d::Identity() -
BiD * f_world.transpose();
const Eigen::Vector3d Axtbx = p_world.rowwise().sum() - BiD *
f_world.cwiseProduct(p_world).colwise().sum().transpose();
Eigen::ColPivHouseholderQR<Eigen::Matrix3d> qr = AxtAx.colPivHouseholderQr();
static constexpr double kRankLossTolerance = 1e-5;
qr.setThreshold(kRankLossTolerance);
const size_t rank = qr.rank();
if (rank < 3) {
return false; // unobservable
}
pos_ = qr.solve(Axtbx);
return true;
}
void Point::updateHessianGradientUnitPlane(
const Eigen::Ref<BearingVector>& f,
const Eigen::Vector3d& p_in_f,
const Eigen::Matrix3d& R_f_w,
Eigen::Matrix3d& A,
Eigen::Vector3d& b,
double& new_chi2)
{
svo::Matrix23d J;
Point::jacobian_xyz2uv(p_in_f, R_f_w, J);
const Eigen::Vector2d e(vk::project2(f) - vk::project2(p_in_f));
A.noalias() += J.transpose() * J;
b.noalias() -= J.transpose() * e;
new_chi2 += e.squaredNorm();
}
void Point::updateHessianGradientUnitSphere(
const Eigen::Ref<BearingVector>& f,
const Eigen::Vector3d& p_in_f,
const Eigen::Matrix3d& R_f_w,
Eigen::Matrix3d& A,
Eigen::Vector3d& b,
double& new_chi2)
{
Eigen::Matrix3d J;
Point::jacobian_xyz2f(p_in_f, R_f_w, J);
const Eigen::Vector3d e = f - p_in_f.normalized();
A.noalias() += J.transpose() * J;
b.noalias() -= J.transpose() * e;
new_chi2 += e.squaredNorm();
}
void Point::EkfInit() {
ekf_init = true;
cov = Eigen::Matrix3d::Identity() * 0.1 * 0.1;
}
// 实现了一个扩展卡尔曼滤波（EKF）的更新步骤，用于估计一个点的状态。EKF是一种用于估计动态系统状态的递归滤波器，特别适用于非线性系统
// dx：一个Eigen::VectorXd类型的向量，表示状态估计的增量。
// obs_std：观测的标准差，用于计算观测噪声的协方差矩阵。
void Point::EkfUpdate(const Eigen::VectorXd& dx, double obs_std) {
if (!ekf_init) {
EkfInit();
}
CHECK(!state_factors.empty());
CHECK(state_factors.rbegin()->first <= (int)dx.rows());
// 计算残差：计算观测残差tmp_res，这是通过遍历状态因子并减去相应的状态增量来实现的。
Eigen::Vector3d tmp_res = gv; // calced residual.
for (svo::StateFactorMap::iterator it = state_factors.begin(); it != state_factors.end(); it++) {
CHECK(!it->second.state.expired());
int bias = (*it).first * 6;
tmp_res -= (*it).second.W.transpose() * dx.segment(bias, 6);
}
Eigen::Matrix3d Rcov = V * obs_std * obs_std;//计算协方差矩阵：计算观测噪声的协方差矩阵Rcov，然后计算总协方差矩阵S。
Eigen::Matrix3d S = V * cov * V.transpose() + Rcov;
// Eigen::Matrix3d K_T = S.ldlt().solve(V * cov);
Eigen::MatrixXd K_T = S.inverse() * V * cov;
Eigen::Matrix3d K = K_T.transpose();//计算卡尔曼增益：通过求解S的逆矩阵与V * cov的乘积来计算卡尔曼增益K。
// 更新状态估计：使用卡尔曼增益K和残差tmp_res来更新状态估计pos_。
Eigen::Vector3d delta_x = K * tmp_res;
pos_ += delta_x;
CHECK_LT(pos_.norm(), 1e4);
// 更新协方差矩阵：使用卡尔曼增益K和观测矩阵V来更新协方差矩阵cov，并确保协方差矩阵是对称的。
Eigen::Matrix3d I_KH = Eigen::Matrix3d::Identity() - K * V;
cov = I_KH * cov;
Eigen::Matrix3d stable_cov = (cov + cov.transpose()) / 2.0;
cov = stable_cov;
}
// dx: 更新向量，包含状态因子的增量。
// obs_std: 观测标准差。
// focal_length: 相机焦距。
// min_idx 和 max_idx: 用于限制关键帧的索引范围。
// huberA 和 huberB: 用于Huber损失函数的参数。
// 实现了一个扩展卡尔曼滤波（EKF）的更新步骤，用于优化三维空间中点的位置。EKF是一种用于估计动态系统状态的递归滤波器，特别适用于处理非线性系统
void Point::EkfUpdate(const Eigen::VectorXd& dx, const double obs_std, const double focal_length, const int min_idx,
const int max_idx, const double huberA, const double huberB) {
if (!ekf_init) {//如果EKF尚未初始化，则调用EkfInit()进行初始化。
EkfInit();
}
CHECK(!state_factors.empty());
CHECK(state_factors.rbegin()->first <= (int)dx.rows());
const double obs_invdev = 1.0 / obs_std;//计算观测逆方差
// TODO: compute keyframe observation
for (svo::Point::KeypointIdentifierList::iterator fit = obs_.begin(); fit != obs_.end(); ++fit) {//遍历观测
if (fit->frame.expired())
continue;
const svo::FramePtr& frame = fit->frame.lock();
const int& curr_frame_idx = frame->bundleId();
if (curr_frame_idx >= min_idx && curr_frame_idx <= max_idx) // pass observations on schurvins local sliding window遍历所有观测，跳过无效的帧和不在索引范围内的帧。
continue;
const Eigen::Vector3d& obs = frame->f_vec_.col(fit->keypoint_index_);
const Eigen::Matrix3d R_c_w = frame->T_f_w_.getRotationMatrix();
const Eigen::Vector3d t_c_w = frame->T_f_w_.getPosition();
const Eigen::Vector3d Pc = R_c_w * pos() + t_c_w;
// calc residual计算观测和估计位置之间的残差。
Eigen::Vector2d r
= (obs.head<2>() / obs.z() - Pc.head<2>() / Pc.z()) * (focal_length); // unsupport unit sphere
// LOG(ERROR) << "residual: " << r.transpose();
// huber
const double r_l2 = r.squaredNorm();
double huber_scale = 1.0;
if (r_l2 > huberB) {
const double radius = sqrt(r_l2);
double rho1 = std::max(std::numeric_limits<double>::min(), huberA / radius);
huber_scale = sqrt(rho1);
r *= huber_scale;
}
r *= obs_invdev;
// calc jacobian计算雅可比矩阵
Matrix2o3d dr_dpc = Matrix2o3d::Zero();
const double pc22 = Pc[2] * Pc[2];
dr_dpc(0, 0) = 1 / Pc[2];
dr_dpc(1, 1) = 1 / Pc[2];
dr_dpc(0, 2) = -Pc[0] / pc22;
dr_dpc(1, 2) = -Pc[1] / pc22;
dr_dpc *= (focal_length * obs_invdev * huber_scale);
Matrix2o3d jf = Matrix2o3d::Zero();
jf.noalias() = dr_dpc * R_c_w;
V.noalias() += jf.transpose() * jf; // pt 2 pt
gv.noalias() += jf.transpose() * r; // pt grad
}
// 计算增量
Eigen::Vector3d tmp_res = gv; // calced residual.
for (svo::StateFactorMap::iterator it = state_factors.begin(); it != state_factors.end(); it++) {
CHECK(!it->second.state.expired());
int bias = (*it).first * 6;
tmp_res -= (*it).second.W.transpose() * dx.segment(bias, 6);
}
// 更新状态和协方差
Eigen::Matrix3d Rcov = V;
Eigen::Matrix3d S = V * cov * V.transpose() + Rcov;
// Eigen::Matrix3d K_T = S.ldlt().solve(V * cov);
Eigen::MatrixXd K_T = S.inverse() * V * cov;
Eigen::Matrix3d K = K_T.transpose();
Eigen::Vector3d delta_x = K * tmp_res;
pos_ += delta_x;
CHECK_LT(pos_.norm(), 1e4);
Eigen::Matrix3d I_KH = Eigen::Matrix3d::Identity() - K * V;
cov = I_KH * cov;
Eigen::Matrix3d stable_cov = (cov + cov.transpose()) / 2.0;
cov = stable_cov;
}
void Point::optimize(const size_t n_iter, bool using_bearing_vector)
{
Eigen::Vector3d old_point = pos_;
double chi2 = 0.0;//用于存储当前的误差平方和。
Eigen::Matrix3d A;//A和b：分别用于存储雅可比矩阵和误差向量。
Eigen::Vector3d b;
if(obs_.size() < 2)
{
SVO_ERROR_STREAM("optimizing point with less than two observations");
return;
}
const double eps = 0.0000000001;
for(size_t i=0; i<n_iter; i++)//每次迭代都会重新计算雅可比矩阵和误差向量。
{
A.setZero();
b.setZero();
double new_chi2 = 0.0;
// compute residuals
for(const KeypointIdentifier& obs : obs_)
{
if(const FramePtr& frame = obs.frame.lock())
{
if(using_bearing_vector)
{
updateHessianGradientUnitSphere(
frame->f_vec_.col(obs.keypoint_index_), frame->T_f_w_*pos_,
frame->T_f_w_.getRotation().getRotationMatrix(),
A, b, new_chi2);
}
else
{
updateHessianGradientUnitPlane(
frame->f_vec_.col(obs.keypoint_index_), frame->T_f_w_*pos_,
frame->T_f_w_.getRotation().getRotationMatrix(),
A, b, new_chi2);
}
}
else
SVO_ERROR_STREAM("could not unlock weak_ptr<Frame> in Point::optimize");
}
// 求解线性系统：使用A.ldlt().solve(b)求解线性系统，得到点的更新量dp。
// solve linear system
const Eigen::Vector3d dp(A.ldlt().solve(b));
// check if error increased 检查更新是否有效：如果更新导致误差平方和增加，或者更新量dp包含NaN值，则回滚到上一次的位置，并停止优化。
if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0]))
{
#ifdef POINT_OPTIMIZER_DEBUG
cout << "it " << i
<< "\t FAILURE \t new_chi2 = " << new_chi2 << endl;
#endif
pos_ = old_point; // roll-back
break;
}
// update the model更新点位置：使用更新量dp更新点的位置，并更新误差平方和chi2。
Eigen::Vector3d new_point = pos_ + dp;
old_point = pos_;
pos_ = new_point;
chi2 = new_chi2;
#ifdef POINT_OPTIMIZER_DEBUG
cout << "it " << i
<< "\t Success \t new_chi2 = " << new_chi2
<< "\t norm(b) = " << vk::norm_max(b)
<< endl;
#endif
// stop when converged收敛检查：如果更新量dp的范数小于一个很小的值（eps），则认为优化已经收敛，停止迭代。
if(vk::norm_max(dp) <= eps)
break;
}
#ifdef POINT_OPTIMIZER_DEBUG
cout << endl;
#endif
}
void Point::print(const std::string& s) const
{
std::cout << s << std::endl;
std::cout << " id = " << id_ << std::endl;
std::cout << " pos = [" << pos_.transpose() << "]" << std::endl;
std::cout << " num obs = " << obs_.size() << std::endl;
}
} // namespace svo
扫一扫
255
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
