【OV】VINS协方差矩阵维护：描述状态的不确定性及各状态变量之间的相关性

本文链接：https://blog.csdn.net/Darlingqiang/article/details/144909912

【OV】VINS协方差矩阵维护：描述状态的不确定性及各状态变量之间的相关性

cov 是协方差矩阵，用于描述状态的不确定性及各状态变量之间的相关性。在 SchurVINS::AugmentState 函数中，它通过扩展协方差矩阵来表示新增状态变量的不确定性。下面是 cov 的更新步骤的推导与原理说明：

1. 协方差维护更新数学推导

1.1 状态扩展的背景

在 VINS (Visual-Inertial Navigation System) 中，状态向量通常包含如下元素：
$\mathbf{x} = [\mathbf{x}_{imu}, \mathbf{x}_{k_1}, \mathbf{x}_{k_2}, \dots, \mathbf{x}_{k_n}]$
其中：

$\mathbf{x}_{imu}$ : IMU 的状态（如位姿、速度、偏置等）。
$\mathbf{x}_{k_i}$ : 第 $i$ 个关键帧（keyframe）的状态，包括位姿信息。

AugmentState 中新增了一个关键帧 $\mathbf{x}_{k_{n+1}}$ 。这需要更新协方差矩阵 $\mathbf{P}$ 以适应扩展后的状态向量。

1.2 原始协方差矩阵的结构

在状态扩展前，协方差矩阵的大小为：
$\mathbf{P}_{old} \in \mathbb{R}^{(15 + 6n) \times (15 + 6n)}$
其中：

15: 表示 IMU 状态的维度。
6n: 表示 (n) 个关键帧的状态维度，每个关键帧有 6 个自由度（旋转 + 平移）。

1.3. 协方差矩阵扩展的数学原理

扩展状态向量后，新的状态维度为 (15 + 6(n+1))。协方差矩阵扩展为：
$\mathbf{P}_{new} = \begin{bmatrix} \mathbf{P}_{old} & \mathbf{Q} \\ \mathbf{Q}^\top & \mathbf{R} \end{bmatrix}$
其中：

$\mathbf{P}_{old}$ : 原始协方差矩阵。
$\mathbf{Q}$ : 原状态与新增状态之间的协方差。
$\mathbf{R}$ : 新增状态的协方差。

在大多数实现中，为简化计算，假设新增关键帧的协方差等于某种初始化值，并具有如下特性：

与原状态的协方差矩阵 $(\mathbf{Q}$ ）可以通过特定假设或传递关系初始化。
新增状态的协方差 $\mathbf{R}$ 初始化为与 IMU 初始状态协方差一致。

1.4. 实现中的具体步骤

代码中的更新过程如下：

(1) 矩阵扩展

通过 cov.conservativeResize(old_rows + 6, old_cols + 6) 将协方差矩阵的维度从 $\times (15 + 6n)$ 扩展为 $\times (15 + 6(n+1))$ 。

(2) 新增部分的赋值

赋值过程如下：

新增行和列：
```
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);
```
将前 $\times old\_cols$ 块复制到新增行，表示新增关键帧与其他状态的初始相关性。
新增状态的协方差：
```
cov.block(old_rows, old_cols, 6, 6) = cov.block<6, 6>(0, 0);
```
初始化新增关键帧的协方差，假设与 IMU 初始协方差一致。

(3) 更新完成后的结构

最终扩展后的协方差矩阵形式为：
$\mathbf{P}_{new} = \begin{bmatrix} \mathbf{P}_{old} & \mathbf{Q} \\ \mathbf{Q}^\top & \mathbf{R} \end{bmatrix}$
其中：

$\mathbf{Q}$ 是由原始状态与新增状态之间的关系初始化的。
$\mathbf{R}$ 是新增关键帧状态的初始协方差。

1.5. 原理总结

协方差扩展的关键在于保持一致性：

协方差矩阵的维度变化：新增状态的维度与原状态向量保持一致。
初始化假设：新增状态的协方差与现有状态通过传递模型或初始化值关联。
数值稳定性：使用与原状态一致的协方差值作为新增部分的初值，避免数值不稳定。

这一过程确保了在扩展状态的同时，系统的不确定性能够正确传播并保持与观测模型一致。

这段代码是C++语言编写的，属于一个名为SchurVINS的类的成员函数AugmentState。这个函数的目的是将一个新的状态（由frame_bundle表示）添

加到当前的状态估计中，并且更新状态协方差矩阵。以下是对这个接口的目的、作用以及算法流程的总结：

2. 目的和作用：

状态扩展：将新的帧捆绑（frame_bundle）与当前状态合并，以更新状态估计。
状态ID管理：为新的状态分配一个唯一的ID。
状态存储：将新的状态存储在一个映射（states_map）中，以便后续访问。
协方差矩阵更新：更新状态协方差矩阵以包含新的状态信息。

2.1 算法流程：

检查时间戳：确保传入的frame_bundle的时间戳与当前状态（curr_state）的时间戳相匹配。
创建新状态：使用当前状态的信息创建一个新的增强状态（aug_ptr），包括时间戳、ID、四元数、位置、加速度和陀螺仪数据。
复制前向运动估计：将当前状态的四元数和位置的前向运动估计复制到新状态中。
存储新状态：将新状态添加到状态映射（states_map）中。
检查状态数量：确保状态映射中的状态数量不超过最大限制（state_max）。
协方差矩阵尺寸检查：检查当前协方差矩阵的尺寸，并确保其是一个方阵。
协方差矩阵尺寸更新：将协方差矩阵的行和列尺寸增加6，以容纳新状态的信息。
协方差矩阵块复制：复制协方差矩阵的块，以保持原有的协方差结构，并为新状态留出空间。
协方差矩阵块更新：更新协方差矩阵的块，以包含新状态的协方差信息。

2.2 代码中的一些关键点：

CHECK_EQ和CHECK宏用于断言，确保代码的执行符合预期的条件。
cov.conservativeResize用于保守地调整协方差矩阵的尺寸，这意味着它不会释放内存，而是可能分配更多的内存。
cov.block用于访问和操作协方差矩阵的特定块。
代码中注释掉了日志输出，如果需要调试，可以取消注释以查看协方差矩阵的变化。

这个函数是状态估计算法中的一部分，通常用于视觉-惯性导航系统（VINS）中，用于处理视觉和惯性测量数据，以估计系统的位姿和运动。

3. 代码实现：

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);

    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);

    //LOG(INFO) << " augmented state: \n" << std::setprecision(15) << cov;
}

4. part 2： RegisterPoints

目的是将新的帧捆绑（frame_bundle）中的角点特征注册到当前的状态中，并且更新局部点映射（local_pts）。

以下是对这个接口的目的、作用以及算法流程的总结：

4.1 目的和作用：

特征注册：将新帧中的角点特征与当前状态关联起来。
状态更新：更新当前状态的特征列表，以包含新帧中的角点特征。
局部点映射更新：更新局部点映射，以包含新帧中的角点。

4.2 算法流程：

检查状态映射：确保状态映射（states_map）不为空。
获取当前状态：从状态映射中获取最新的状态指针（state_ptr）。
获取状态ID：从当前状态指针中获取状态ID（state_id）。
遍历帧捆绑中的帧：对于frame_bundle中的每个帧（frame）：
- 获取帧的相机索引（camera_id）。
- 初始化特征计数器（num_feature）。
- 遍历帧中的特征：对于帧中的每个特征：
  - 检查特征是否有效（非空）并且是否为角点。
  - 如果特征有效，记录特征信息，并创建一个新的局部特征对象（LocalFeature）。
  - 设置局部特征的层级（level）。
  - 检查局部点映射：检查局部点映射（local_pts）中是否已经存在该点的ID：
    - 如果不存在，将点添加到局部点映射中，并更新当前状态的特征列表，以及点的局部观测列表（local_obs_）。
    - 如果存在，只更新当前状态的特征列表和点的局部观测列表。
  - 增加特征计数器。
- 记录特征数量：记录当前帧的特征数量。
- 记录相机ID和特征数量：记录相机ID和注册的特征数量。

4.3代码中的一些关键点：

CHECK宏用于断言，确保代码的执行符合预期的条件。
LOG(INFO)用于记录信息，有助于调试和监控程序的执行过程。
svo::isCorner函数用于检查特征是否为角点。
svo::LocalFeature是一个共享指针，用于表示局部特征。
local_pts是一个映射，用于存储局部点。
state_ptr->features是当前状态的特征列表。
point->local_obs_是点的局部观测列表。

这个函数是视觉-惯性导航系统（VINS）中的一部分，用于处理视觉测量数据，将新帧中的角点特征与当前状态关联起来，以便于后续的状态估计和优化。


void SchurVINS::RegisterPoints(const svo::FrameBundle::Ptr& frame_bundle) {
    CHECK(!states_map.empty());
    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])) {
                LOG(INFO) << "frame id: " << frame->id() << "point id: " << i << "pos: " << point->pos().transpose();
                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());
                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) << "num of features : " << frame->numFeatures() ;
        LOG(INFO) << "RegisterPoints camera id: " << camera_id << " num feature: " << num_feature;
    }
}