当前位置：首页 > news >正文

OpenVINS代码解读---State.h

news 2025/11/11 9:12:40

State.h

State类的核心作用是统一管理 VIO 系统的所有状态变量，包括：

IMU 的当前状态（姿态、速度、位置、偏置）；
滑动窗口中的 IMU “克隆” 姿态（用于 MSCKF 的多视图约束）；
SLAM 特征的 3D 位置（长期跟踪的特征）；
相机与 IMU 的校准参数（外参、内参、时间偏移等）；
所有状态变量的协方差矩阵（描述估计不确定性）。

头文件

#include <map>
引入 C++ 标准库的map容器（有序键值对），用于存储有序关联数据（如按时间戳排序的相机参数）。#include <memory>
引入 C++11 + 的智能指针库（std::shared_ptr、std::unique_ptr等），用于安全管理动态分配的内存（如State中管理的IMU、Landmark等对象，避免内存泄漏）。#include <mutex>
引入 C++ 标准库的互斥锁（std::mutex），用于保证多线程环境下的线程安全（如多个线程同时访问 / 修改State中的状态变量时，通过锁避免数据竞争）。#include <unordered_map>
引入 C++ 标准库的unordered_map容器（哈希表实现的无序键值对），用于高效存储和查询关联数据（如State中_clones_IMU（克隆姿态的时间戳 - 姿态映射）、_cam_intrinsics（相机 ID - 内参映射））。#include <vector>
引入 C++ 标准库的vector容器（动态数组），用于存储有序序列数据（如State中_variables（所有状态变量列表）、_features_SLAM（SLAM 特征列表））。#include "StateOptions.h"
引入StateOptions结构体，用于存储 MSCKF 算法的核心配置（如是否启用 FEJ、滑动窗口最大克隆数、特征表示方式等），是State类的核心配置依赖#include "cam/CamBase.h"
引入相机基类CamBase，定义了相机的统一接口（如畸变校正、投影 / 反投影函数）。后续具体相机类型（如针孔相机、鱼眼相机）会继承该类，State中会通过该接口管理相机内参和图像处理逻辑。#include "types/IMU.h"
引入IMU类（继承自Type基类），用于存储 IMU 的状态变量（姿态、速度、位置、角速度偏置、加速度偏置等），是State类中核心的惯性导航状态载体。#include "types/Landmark.h"
引入Landmark类（继承自Type基类），用于存储 SLAM 特征的状态（3D 位置、特征表示方式、锚定帧信息等），是State中 SLAM 特征管理的核心类型。#include "types/PoseJPL.h"
引入PoseJPL类（继承自Type基类），采用 JPL 四元数表示姿态（用于避免四元数归一化约束），用于存储 IMU 的当前姿态和历史克隆姿态（_clones_IMU中的姿态对象）。#include "types/Type.h"
引入所有状态变量的基类Type，定义了状态变量的统一接口（如update（状态更新）、id（变量 ID）、size（变量维度）等）。IMU、PoseJPL、Landmark、Vec均继承此类，实现多态管理（如State中用std::vector<std::shared_ptr<Type>>统一存储所有状态变量）。#include "types/Vec.h"
引入Vec类（继承自Type基类），用于存储向量类型的状态变量（如相机内参、IMU 校准参数、相机 - IMU 时间偏移等），是通用的向量型状态载体。

State(StateOptions &options_);  // 构造函数：使用配置选项初始化状态

在 State 类中，有一个成员变量 StateOptions _options（用于存储滤波器的配置参数）；StateOptions &options_ 是一个结构体引用，用于接收外部传入的 StateOptions 结构体对象。通过它，State 类的构造函数可以将外部配置复制到自身的 _options 成员中，完成 State 类的配置初始化。引用在这里的作用是高效传递配置数据，而非定义新对象。

示例代码：

#include "State.h"
#include "StateOptions.h"int main(int argc, char**argv) {// 1. 外部创建 StateOptions 结构体对象，并配置参数ov_msckf::StateOptions options;  // 定义结构体对象（外部创建）// 配置核心参数（根据需求修改）options.do_fej = true;          // 启用一阶估计（FEJ）options.max_clones = 11;        // 滑动窗口最大克隆数为11options.max_slam = 50;          // SLAM特征最大数量为50options.feat_rep_msckf = ov_type::LandmarkRepresentation::GLOBAL_3D;  // MSCKF特征用全局3D表示options.imu_model = ov_msckf::StateOptions::ImuModel::KALIBR;  // IMU内参模型使用kalibr格式options.do_calib_imu_intrinsics = true;  // 启用IMU内参在线校准options.do_calib_cam_extrinsics = true;  // 启用相机外参在线校准// 2. 将配置好的 options 传入 State 构造函数，初始化状态auto state = std::make_shared<ov_msckf::State>(options);  // 外部传入，完成State初始化// 3. 后续使用 state 对象（如传递给Propagator、Updater等模块）// ...（省略VIO系统其他逻辑）return 0;
}

通过 “外部创建并传入 StateOptions” 的方式，实现了 ** 配置与核心逻辑的分离 **：

无需修改 State 类的源码，即可通过外部配置调整滤波器的行为（如切换 IMU 模型、开启 / 关闭校准功能）；
便于通过配置文件（如 YAML）解析参数（实际项目中，options 的参数通常从配置文件读取，而非硬编码），提高了代码的灵活性和可维护性。

~State() {}  // 析构函数：默认空实现

`margtimestep()`：获取下一个要边缘化的时间戳

double margtimestep() {std::lock_guard<std::mutex> lock(_mutex_state);  // 线程安全锁double time = INFINITY;for (const auto &clone_imu : _clones_IMU) {  // 遍历所有克隆姿态if (clone_imu.first < time) {time = clone_imu.first;  // 找到最旧的克隆时间戳}}return time;
}

std::mutex _mutex_state;
std::map<double, std::shared_ptr<ov_type::PoseJPL>> _clones_IMU;

`（1）_mutex_state`

_mutex_state 是 State 类的 成员变量，类型是 C++ 标准库的 std::mutex（互斥锁）。

它的核心作用是 保护 State 类中所有共享数据的访问安全—— 就像一个 “房间钥匙”：当多个线程（比如 VIO 中的「IMU 预测线程」和「视觉更新线程」）需要同时读写 State 里的状态（如 _clones_IMU、_Cov、_features_SLAM 等）时，必须先 “拿到钥匙”（锁定 _mutex_state），才能进入 “房间”（操作共享数据）；其他线程只能等待 “钥匙” 被归还（解锁），才能继续访问。

std::mutex 有两种内部状态：「未锁定」和「已锁定」，这个状态会在多线程操作中动态切换：

当线程执行 std::lock_guard<std::mutex> lock(_mutex_state) 时：lock_guard 的构造函数会调用 _mutex_state.lock()，将锁的状态从「未锁定」改为「已锁定」，此时其他线程再想锁定会被阻塞（等待）。
当线程退出临界区（比如 margtimestep() 函数执行结束）时：lock_guard 会自动析构，调用 _mutex_state.unlock()，将锁的状态从「已锁定」改回「未锁定」，释放资源让其他线程可以锁定。

（2）const auto &clone_imu : _clones_IMU

clone_imu 是 _clones_IMU 容器中的一个元素，表示某个时刻的 “IMU 克隆姿态”（包含姿态和位置信息）。

_clones_IMU 是一个 std::map<double, std::shared_ptr<ov_type::PoseJPL>> 类型的容器（有序键值对），因此 clone_imu 的类型是 std::pair<double, std::shared_ptr<ov_type::PoseJPL>>，即一个包含两个元素的键值对：

键（clone_imu.first）：double 类型，代表相机成像的时间戳（对应 IMU 克隆姿态的时刻）。
值（clone_imu.second）：std::shared_ptr<ov_type::PoseJPL> 类型，是指向 PoseJPL 对象的智能指针，存储该时刻的 IMU 克隆姿态（包括旋转 q_GtoIi 和位置 p_IiinG）：
- q_GtoIi：世界坐标系到该时刻 IMU 坐标系的旋转（JPL 四元数表示）；
- p_IiinG：该时刻 IMU 在世界坐标系中的位置。

“克隆（clone）” 的含义与作用
在 MSCKF（多状态约束卡尔曼滤波）中，“克隆” 是核心概念之一：
相机采集图像的时刻与 IMU 的采样时刻通常不同步（存在时间差）。为了将视觉观测（图像特征）与 IMU 状态关联，需要在 每个相机成像时刻 生成一个 “IMU 克隆姿态”—— 即通过 IMU 预积分，估计出相机拍照时 IMU 的姿态和位置（相当于 “克隆” 了一个该时刻的 IMU 状态）。
这些克隆姿态被存储在 _clones_IMU 中，形成一个滑动窗口（按时间戳排序）。当处理视觉特征时，同一特征在不同时刻的观测会与窗口中的多个克隆姿态关联，构建多视图约束，从而优化 IMU 状态和特征位置。
总结：为相机时刻 “补全” IMU 状态。

克隆核心原因是 解决相机与 IMU 的 “时间同步问题”，并为视觉观测和 IMU 状态建立 “一一对应的约束关系”—— 这是 VIO（视觉 - 惯性里程计）能融合两种传感器数据的关键前提。
（1）先明确核心矛盾：相机与 IMU 不同步
相机和 IMU 的工作方式完全不同，采样时刻天然不重合：
相机：“快照式” 采样（比如 10Hz），只在T_cam1、T_cam2、T_cam3等离散时刻拍摄图像，产生视觉特征观测（像素坐标）。
IMU：“连续式” 采样（比如 400Hz），在T_imu1、T_imu2、…、T_imuN等高频时刻输出角速度和加速度，提供运动信息。
举个例子：相机在T_cam=0.1s时拍了一张图，但 IMU 的采样时刻可能是0.098s和0.102s—— 没有任何一个 IMU 原始采样时刻刚好等于T_cam。
（2） “克隆姿态” 的本质：为相机时刻 “补全” IMU 状态
视觉特征的观测（比如某个特征在T_cam时刻的像素坐标），是在 T_cam时刻的相机姿态 下产生的。而相机姿态需要通过 IMU 状态推导（相机和 IMU 刚性连接，IMU 姿态 + 外参 = 相机姿态）。
但 IMU 没有T_cam时刻的原始状态，所以需要：
用T_cam前后的 IMU 高频数据，通过「预积分」技术，估计出T_cam时刻的 IMU 状态（姿态q_GtoI、位置p_IinG）。
这个 “估计出的、专门对应T_cam时刻的 IMU 状态”，就是「IMU 克隆姿态」—— 它不是真的 “复制” 了 IMU 硬件，而是 “克隆” 了T_cam时刻的 IMU 运动状态。
（3）为什么必须 “时间戳对应”？—— 让视觉观测能和 IMU 状态绑定
VIO 的核心是 “用视觉观测修正 IMU 的累积误差”，这个过程需要构建「观测方程」：
视觉观测值 = 基于IMU状态的预测值 + 噪声
要让这个方程成立，必须满足 “观测时刻” 和 “IMU 状态时刻” 一致：
视觉观测值（像素坐标）的时刻是T_cam（相机成像时刻）；
基于 IMU 状态的预测值，必须是T_cam时刻的 IMU 克隆姿态（转换为相机姿态后），才能把特征 3D 位置正确投影到像素平面，得到 “预测的像素坐标”。
如果两者时间戳不对应（比如用T_cam+0.01s的 IMU 状态），投影会因为时间差导致的运动（比如相机轻微移动）产生偏差，观测方程就会失真，修正结果也会出错。
（4）结合 MSCKF 的核心逻辑：多视图约束需要 “时刻对齐”
MSCKF 的关键是 “多状态约束”—— 同一个特征被多个相机帧观测，每个观测都要和对应时刻的 IMU 克隆姿态绑定，形成多个约束方程，共同优化 IMU 的状态（位置、姿态、偏置）。
比如：
特征 A 在T_cam1、T_cam2、T_cam3三个时刻被观测；
每个时刻都有对应的 IMU 克隆姿态（clone_imu1、clone_imu2、clone_imu3），时间戳分别等于T_cam1、T_cam2、T_cam3；
用这三个克隆姿态分别计算特征 A 的投影预测值，与三个时刻的实际观测值对比，产生三个残差，共同约束并修正 IMU 的状态。
总结
“相机成像的时间戳对应 IMU 克隆姿态的时刻”，本质是 通过 “预积分估计 + 时间戳绑定”，解决相机与 IMU 的不同步问题，确保每一个视觉观测都能找到唯一对应的 IMU 状态，从而构建有效的融合约束 —— 这是 VIO 能高精度估计位置、姿态的基础。
简单说：视觉观测是 “某一时刻的快照”，IMU 克隆姿态是 “该时刻的 IMU 状态替身”，两者时间戳对应，才能让 “视觉” 和 “惯性” 数据真正融合起来。

获取协方差矩阵的行数

int max_covariance_size(){return (int)_Cov.rows();}

_Cov.rows() 用于获取协方差矩阵的行数（由于协方差矩阵是方阵，行数 = 列数 = 总维度 N）。函数将其转换为 int 类型并返回，即当前所有状态变量的总自由度。

其中

  Eigen::MatrixXd _Cov;

为什么需要这个函数？
在 VIO 的滤波流程中，很多操作依赖于协方差矩阵的维度，例如：
状态更新：当用视觉观测修正状态时，需要构建观测矩阵 H（维度为「观测维度 × 状态总维度」），此时需通过 max_covariance_size() 确认 H 的列数是否正确。
边缘化：当滑动窗口满时，需要移除最旧的状态（如某个克隆姿态），此时需根据当前协方差维度调整矩阵大小（删除对应行和列）。
一致性检查：在调试或验证时，可通过该函数快速确认状态变量总维度是否符合预期（例如：新增 SLAM 特征后，维度是否正确增加）。

内参矩阵标准化

  static Eigen::Matrix3d Dm(StateOptions::ImuModel imu_model, const Eigen::MatrixXd &vec) {assert(vec.rows() == 6);assert(vec.cols() == 1);
//通过 assert 确保输入向量 vec 是6 行 1 列（因为 IMU 内参的尺度和轴偏参数共 6 个）。若不满足，程序会在调试模式下终止，提示参数错误。Eigen::Matrix3d D_matrix = Eigen::Matrix3d::Identity();
// 初始化 D_matrix 为 3x3 单位矩阵。IMU 的理想内参矩阵是单位矩阵（无误差时，测量值无需校正），实际误差参数是在单位矩阵基础上叠加的修正项。if (imu_model == StateOptions::ImuModel::KALIBR) {D_matrix << vec(0), 0, 0, vec(1), vec(3), 0, vec(2), vec(4), vec(5);
// 使用下三角部分填充参数（对角线及以下）} else {D_matrix << vec(0), vec(1), vec(3), 0, vec(2), vec(4), 0, 0, vec(5);
// 使用上三角部分填充参数（对角线及以上）}return D_matrix;}

static：静态成员函数，意味着不需要创建 State 类对象即可调用（通常用于工具类功能）。
返回值 Eigen::Matrix3d：3x3 的矩阵，即 IMU 内参矩阵（用于校正测量误差）。
StateOptions::ImuModel 是枚举类型（enum）；

校正陀螺仪的重力耦合误差

  static Eigen::Matrix3d Tg(const Eigen::MatrixXd &vec) {assert(vec.rows() == 9);assert(vec.cols() == 1);Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero();Tg << vec(0), vec(3), vec(6), vec(1), vec(4), vec(7), vec(2), vec(5), vec(8);return Tg;}

陀螺仪的核心功能是测量角速度，但实际硬件中，加速度（尤其是重力加速度）会通过机械结构或电路耦合到陀螺仪测量中，导致角速度测量出现偏差（这就是 “重力敏感度误差”）。

计算 IMU 内参相关的误差状态总维度

  int imu_intrinsic_size() const {int sz = 0;if (_options.do_calib_imu_intrinsics) {sz += 15;if (_options.do_calib_imu_g_sensitivity) {sz += 9;}}return sz;}

若 do_calib_imu_intrinsics = false（不校准 IMU 内参）：返回 0；
若 do_calib_imu_intrinsics = true 且 do_calib_imu_g_sensitivity = false（仅校准基础内参）：返回 15；
若 do_calib_imu_intrinsics = true 且 do_calib_imu_g_sensitivity = true（校准基础内参 + 重力敏感度）：返回 15 + 9 = 24。

const的作用：const 修饰符表明该函数不会修改 State 类的成员变量（仅读取配置参数）。

imu_intrinsic_size() 的返回值直接决定了：
协方差矩阵的维度：IMU 内参的协方差需要占据 sz × sz 的子矩阵（嵌入总协方差矩阵 _Cov 中）；
状态转移矩阵的大小：在 IMU 预积分（状态预测）时，转移矩阵需要包含内参误差的传播路径，维度依赖 sz；
观测方程的维度：视觉观测对 IMU 状态的修正会间接影响内参，观测矩阵的列数需要包含 sz 个维度。

成员变量

  std::mutex _mutex_state;
// 互斥锁，保护所有共享状态的线程安全/// Current timestamp (should be the last update time in camera clock frame!)// 当前时间戳（应该是相机时钟帧中的最后更新时间！）double _timestamp = -1;/// Struct containing filter options
// 滤波器配置选项（结构体）StateOptions _options;/// Pointer to the "active" IMU state (q_GtoI, p_IinG, v_IinG, bg, ba)std::shared_ptr<ov_type::IMU> _imu;/// Map between imaging times and clone poses (q_GtoIi, p_IiinG)// std::map 是 C++ 标准库中的有序关联容器，// 键（key）与值（value）成对存储，且会自动按照键的大小升序排序。
// 键为相机成像时间戳，值为该时刻的 IMU “克隆姿态”std::map<double, std::shared_ptr<ov_type::PoseJPL>> _clones_IMU;/// Our current set of SLAM features (3d positions)
// 键为特征 ID，值为 SLAM 特征的 3D 位置（Landmark 类型）std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> _features_SLAM;/// Time offset base IMU to camera (t_imu = t_cam + t_off)std::shared_ptr<ov_type::Vec> _calib_dt_CAMtoIMU;/// Calibration poses for each camera (R_ItoC, p_IinC)
// 键为相机 ID（多相机系统中区分不同相机），值为 IMU 到相机的外参（PoseJPL 类型）std::unordered_map<size_t, std::shared_ptr<ov_type::PoseJPL>> _calib_IMUtoCAM;/// Camera intrinsics
// 存储相机内参的向量形式（如焦距 fx/fy、主点 cx/cy、畸变系数等），键为相机 IDstd::unordered_map<size_t, std::shared_ptr<ov_type::Vec>> _cam_intrinsics;/// Camera intrinsics camera objects
// 存储相机对象（CamBase 基类的派生类，如针孔相机 CamPinhole），提供畸变校正、3D 点投影到像素平面等功能std::unordered_map<size_t, std::shared_ptr<ov_core::CamBase>> _cam_intrinsics_cameras;/// Gyroscope IMU intrinsics (scale imperfection and axis misalignment)
// 尺度与轴非正交性std::shared_ptr<ov_type::Vec> _calib_imu_dw;/// Accelerometer IMU intrinsics (scale imperfection and axis misalignment)std::shared_ptr<ov_type::Vec> _calib_imu_da;/// Gyroscope gravity sensitivitystd::shared_ptr<ov_type::Vec> _calib_imu_tg;/// Rotation from gyroscope frame to the "IMU" accelerometer frame (kalibr model)std::shared_ptr<ov_type::JPLQuat> _calib_imu_GYROtoIMU;/// Rotation from accelerometer to the "IMU" gyroscope frame frame (rpng model)
// 存储陀螺仪与加速度计之间的安装旋转（JPL 四元数），用于校正两者坐标系的非严格对齐误差（不同模型参数化方式不同）。std::shared_ptr<ov_type::JPLQuat> _calib_imu_ACCtoIMU;private:// Define that the state helper is a friend class of this class// This will allow it to access the below functions which should normally not be called// This prevents a developer from thinking that the "insert clone" will actually correctly add it to the covariance
// 声明StateHelper为友元类，允许其访问私有成员friend class StateHelper;/// Covariance of all active variablesEigen::MatrixXd _Cov;/// Vector of variables
// 所有状态变量的统一列表std::vector<std::shared_ptr<ov_type::Type>> _variables;
};

查看全文

http://www.dtcms.com/a/593319.html