【MSCKF】StateHelper 学习备注
-
-
- 一、整体背景与StateHelper角色
- 二、核心函数的数学推导与算法梳理
-
- [1. EKFPropagation：协方差传播](#1. EKFPropagation：协方差传播)
- [2. EKFUpdate：EKF更新](#2. EKFUpdate：EKF更新)
- [3. marginalize：边缘化操作](#3. marginalize：边缘化操作)
- [4. clone：状态克隆](#4. clone：状态克隆)
- [5. initialize：新变量初始化](#5. initialize：新变量初始化)
- [6. augment_clone：滑动窗口扩展](#6. augment_clone：滑动窗口扩展)
- [7. marginalize_old_clone：滑动窗口维护](#7. marginalize_old_clone：滑动窗口维护)
- [8. SRISWFUpdate：平方根信息滤波更新](#8. SRISWFUpdate：平方根信息滤波更新)
- 三、整体算法流程
- 四、关键技术总结
- 五、实现
-
一、整体背景与StateHelper角色
StateHelper是MSCKF（Multi-State Constraint Kalman Filter）的核心状态管理模块，负责处理EKF的状态（均值+协方差）操作，包括协方差传播、更新、边缘化、状态克隆/初始化 等。MSCKF是视觉惯性里程计（VIO）的经典方法，通过维护滑动窗口内的IMU位姿克隆（而非估计每个特征），利用多帧特征观测约束位姿，实现高精度运动估计。
MSCKF的状态向量通常包含：
- 核心IMU状态 ：位置 p G I \mathbf{p}{GI} pGI、速度 v G I \mathbf{v}{GI} vGI、姿态 q G I \mathbf{q}_{GI} qGI（四元数）、IMU偏差 b g , b a \mathbf{b}_g,\mathbf{b}_a bg,ba；
- 滑动窗口位姿克隆 ： q G I , k , p G I , k \mathbf{q}{GI,k},\mathbf{p}{GI,k} qGI,k,pGI,k（ k = 1 ⋯ N k=1\cdots N k=1⋯N， N N N为窗口大小）；
- 可选扩展 ：SLAM特征位置 p G f \mathbf{p}_{Gf} pGf、IMU-相机时间偏移 t d t_d td。
协方差矩阵 P \mathbf{P} P是状态向量的协方差，维度为 n × n n\times n n×n（ n n n为状态总维度）。StateHelper通过块矩阵操作高效管理 P \mathbf{P} P，避免全矩阵运算的低效性。
二、核心函数的数学推导与算法梳理
1. EKFPropagation：协方差传播
功能：状态均值已通过IMU预积分传播，此处仅更新协方差（过程噪声+状态转移）。
数学推导 ：
EKF协方差传播的核心公式为：
P ′ = Φ P Φ T + Q \mathbf{P}' = \mathbf{\Phi} \mathbf{P} \mathbf{\Phi}^T + \mathbf{Q} P′=ΦPΦT+Q
其中 Φ \mathbf{\Phi} Φ是状态转移矩阵 ， Q \mathbf{Q} Q是IMU过程噪声协方差（由角速度/加速度噪声和偏差噪声组成）。
由于仅核心IMU状态需要传播（克隆位姿是静态的），将状态分为：
- x o l d \mathbf{x}_{old} xold：待传播的核心IMU状态（对应
order_OLD）； - x c o n s t \mathbf{x}_{const} xconst：静态克隆位姿/其他状态（不变）。
则传播后的协方差可分块表示：
P ′ = [ Φ 0 0 I ] P [ Φ T 0 0 I ] T + [ Q 0 0 0 ] \mathbf{P}' = \begin{bmatrix} \mathbf{\Phi} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} \end{bmatrix} \mathbf{P} \begin{bmatrix} \mathbf{\Phi}^T & \mathbf{0} \\ \mathbf{0} & \mathbf{I} \end{bmatrix}^T + \begin{bmatrix} \mathbf{Q} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} \end{bmatrix} P′=[Φ00I]P[ΦT00I]T+[Q000]
算法逻辑：
- 从全协方差 P \mathbf{P} P中提取
order_OLD对应的块； - 用 Φ \mathbf{\Phi} Φ完成协方差传播，并叠加过程噪声 Q \mathbf{Q} Q；
- 将结果放回
order_NEW对应的连续协方差块（保证内存连续性以优化效率）。
2. EKFUpdate：EKF更新
功能：利用特征观测更新状态均值与协方差（对应MSCKF的多约束更新）。
数学推导 ：
EKF更新的标准流程基于线性化观测模型 ：
r = z − h ( x ) ( 残差 ) \mathbf{r} = \mathbf{z} - \mathbf{h}(\mathbf{x}) \quad (\text{残差}) r=z−h(x)(残差)
H = ∂ h ∂ x ( 观测雅各比 ) \mathbf{H} = \frac{\partial \mathbf{h}}{\partial \mathbf{x}} \quad (\text{观测雅各比}) H=∂x∂h(观测雅各比)
卡尔曼增益：
K = P H T ( H P H T + R ) − 1 \mathbf{K} = \mathbf{P} \mathbf{H}^T (\mathbf{H} \mathbf{P} \mathbf{H}^T + \mathbf{R})^{-1} K=PHT(HPHT+R)−1
状态更新：
x ← x + K r \mathbf{x} \leftarrow \mathbf{x} + \mathbf{K} \mathbf{r} x←x+Kr
协方差更新（约瑟夫形式，数值稳定）：
P ← ( I − K H ) P ( I − K H ) T + K R K T \mathbf{P} \leftarrow (\mathbf{I} - \mathbf{K} \mathbf{H}) \mathbf{P} (\mathbf{I} - \mathbf{K} \mathbf{H})^T + \mathbf{K} \mathbf{R} \mathbf{K}^T P←(I−KH)P(I−KH)T+KRKT
MSCKF中 H \mathbf{H} H通常是压缩雅各比 （仅包含滑动窗口内的克隆位姿），因此优化为块操作：
令 P a a \mathbf{P}{aa} Paa为克隆位姿对应的协方差块， H \mathbf{H} H为 m × a m\times a m×a（ m m m为观测维度， a a a为克隆位姿总维度），则：
S = H P a a H T + R ( 创新协方差 ) \mathbf{S} = \mathbf{H} \mathbf{P}{aa} \mathbf{H}^T + \mathbf{R} \quad (\text{创新协方差}) S=HPaaHT+R(创新协方差)
K a a = P a a H T S − 1 \mathbf{K}{aa} = \mathbf{P}{aa} \mathbf{H}^T \mathbf{S}^{-1} Kaa=PaaHTS−1
P a a ← ( I − K a a H ) P a a \mathbf{P}{aa} \leftarrow (\mathbf{I} - \mathbf{K}{aa} \mathbf{H}) \mathbf{P}{aa} Paa←(I−KaaH)Paa
P b a ← P b a ( I − K a a H ) T ( 交叉协方差更新 ) \mathbf{P}{ba} \leftarrow \mathbf{P}{ba} (\mathbf{I} - \mathbf{K}{aa} \mathbf{H})^T \quad (\text{交叉协方差更新}) Pba←Pba(I−KaaH)T(交叉协方差更新)
算法逻辑：
- 仅处理
H_order对应的状态子集（避免全矩阵运算）； - 计算创新协方差 S \mathbf{S} S和卡尔曼增益 K \mathbf{K} K；
- 分块更新协方差（核心+交叉项）与状态均值。
3. marginalize：边缘化操作
功能 ：移除状态中的冗余变量（如滑动窗口外的旧克隆位姿），通过Schur补保持协方差一致性。
数学推导 ：
将状态分为待边缘化变量 x m \mathbf{x}m xm和剩余变量 x o \mathbf{x}o xo，协方差分块为：
P = [ P m m P m o P o m P o o ] \mathbf{P} = \begin{bmatrix} \mathbf{P}{mm} & \mathbf{P}{mo} \\ \mathbf{P}{om} & \mathbf{P}{oo} \end{bmatrix} P=[PmmPomPmoPoo]
边缘化 x m \mathbf{x}m xm后，剩余状态的协方差为Schur补 ：
P ′ = P o o − P o m P m m − 1 P m o \mathbf{P}' = \mathbf{P}{oo} - \mathbf{P}{om} \mathbf{P}{mm}^{-1} \mathbf{P}_{mo} P′=Poo−PomPmm−1Pmo
算法逻辑：
- 定位 x m \mathbf{x}_m xm在协方差中的索引范围；
- 计算Schur补以更新剩余变量的协方差；
- 移除 x m \mathbf{x}_m xm对应的行列，并更新其他状态的索引（保证协方差维度匹配）。
4. clone：状态克隆
功能：复制核心IMU状态（如位姿）作为新的克隆位姿，添加到状态末尾（滑动窗口扩展）。
数学推导 ：
克隆位姿 x c ′ = x c \mathbf{x}_c' = \mathbf{x}_c xc′=xc（ x c \mathbf{x}c xc为原IMU位姿），因此协方差需新增块：
P n e w = [ P P c P c T P c c ] \mathbf{P}{new} = \begin{bmatrix} \mathbf{P} & \mathbf{P}_c \\ \mathbf{P}c^T & \mathbf{P}{cc} \end{bmatrix} Pnew=[PPcTPcPcc]
其中 P c \mathbf{P}_c Pc是原状态与 x c \mathbf{x}c xc的交叉协方差， P c c \mathbf{P}{cc} Pcc是 x c \mathbf{x}_c xc的自协方差。
若存在IMU-相机时间偏移 t d t_d td ，克隆需考虑 t d t_d td的影响：
q G I ( t + t d ) ≈ q G I ( t + t ^ d ) ⊗ exp ( ω ⋅ t ~ d ) \mathbf{q}{GI}(t+t_d) \approx \mathbf{q}{GI}(t+\hat{t}d) \otimes \exp(\boxed{\omega} \cdot \tilde{t}d) qGI(t+td)≈qGI(t+t^d)⊗exp(ω⋅t~d)
p G I ( t + t d ) ≈ p G I ( t + t ^ d ) + v G I ( t + t ^ d ) ⋅ t ~ d \mathbf{p}{GI}(t+t_d) \approx \mathbf{p}{GI}(t+\hat{t}d) + \mathbf{v}{GI}(t+\hat{t}_d) \cdot \tilde{t}_d pGI(t+td)≈pGI(t+t^d)+vGI(t+t^d)⋅t~d
雅各比需包含对 t d t_d td的导数，协方差需叠加 t d t_d td的交叉项。
算法逻辑：
- 在协方差末尾添加克隆变量的维度；
- 复制原变量的协方差块与交叉项；
- 若有时钟偏移，额外计算与 t d t_d td的交叉协方差。
5. initialize：新变量初始化
功能 ：初始化SLAM特征等新变量，支持不可逆雅各比 H L \mathbf{H}_L HL的场景（如2D观测约束3D特征）。
数学推导 ：
观测模型为：
z = H R x o t h e r + H L x n e w + n ( n ∼ N ( 0 , R ) ) \mathbf{z} = \mathbf{H}R \mathbf{x}{other} + \mathbf{H}L \mathbf{x}{new} + \mathbf{n} \quad (\mathbf{n}\sim\mathcal{N}(0,\mathbf{R})) z=HRxother+HLxnew+n(n∼N(0,R))
-
H L \mathbf{H}_L HL可逆场景 （
initialize_invertible）：直接求解新变量均值：
x n e w = H L − 1 ( z − H R x o t h e r ) \mathbf{x}_{new} = \mathbf{H}_L^{-1} (\mathbf{z} - \mathbf{H}R \mathbf{x}{other}) xnew=HL−1(z−HRxother)协方差：
P n e w , n e w = H L − 1 ( R + H R P o t h e r , o t h e r H R T ) ( H L − 1 ) T \mathbf{P}_{new,new} = \mathbf{H}_L^{-1} (\mathbf{R} + \mathbf{H}R \mathbf{P}{other,other} \mathbf{H}_R^T) (\mathbf{H}_L^{-1})^T Pnew,new=HL−1(R+HRPother,otherHRT)(HL−1)T交叉协方差：
P o t h e r , n e w = P o t h e r , o t h e r H R T ( H L − 1 ) T \mathbf{P}{other,new} = \mathbf{P}{other,other} \mathbf{H}_R^T (\mathbf{H}_L^{-1})^T Pother,new=Pother,otherHRT(HL−1)T -
H L \mathbf{H}_L HL不可逆场景 ：
利用Givens旋转 正交化 H L \mathbf{H}L HL，构造满秩系统，通过最小二乘求解 x n e w \mathbf{x}{new} xnew，并计算协方差。
算法逻辑：
- 若 H L \mathbf{H}_L HL可逆，直接解算均值与协方差；
- 否则用正交化方法构造可解系统，完成初始化后合并到状态中。
6. augment_clone：滑动窗口扩展
功能 ：将当前IMU位姿克隆到滑动窗口，支持时间偏移 t d t_d td的校准。
数学推导 ：
克隆位姿与 t d t_d td的关系（如前文所述）：
∂ θ ~ G I ∂ t ~ d = ω , ∂ p ~ G I ∂ t ~ d = v G I \frac{\partial \tilde{\boldsymbol{\theta}}_{GI}}{\partial \tilde{t}d} = \boxed{\omega}, \quad \frac{\partial \tilde{\mathbf{p}}{GI}}{\partial \tilde{t}d} = \mathbf{v}{GI} ∂t~d∂θ~GI=ω,∂t~d∂p~GI=vGI
雅各比矩阵需包含对 t d t_d td的导数，协方差需叠加 t d t_d td的交叉项。
算法逻辑：
- 克隆核心IMU位姿，添加到滑动窗口；
- 若有时钟偏移，计算克隆位姿与 t d t_d td的雅各比，更新协方差交叉项。
7. marginalize_old_clone：滑动窗口维护
功能：当滑动窗口超过最大尺寸时，边缘化最早的克隆位姿（保证计算效率）。
算法逻辑：
- 调用
marginalize函数移除最早的克隆变量； - 更新滑动窗口的位姿索引，保持窗口大小固定。
8. SRISWFUpdate：平方根信息滤波更新
功能 ：基于平方根信息矩阵（如Cholesky分解）的更新，提升数值稳定性（避免协方差矩阵正定损失）。
数学推导 ：
信息矩阵 Λ = P − 1 \mathbf{\Lambda} = \mathbf{P}^{-1} Λ=P−1，其平方根 S \mathbf{S} S满足 Λ = S T S \mathbf{\Lambda} = \mathbf{S}^T \mathbf{S} Λ=STS。观测的信息贡献为 H T R − 1 H \mathbf{H}^T \mathbf{R}^{-1} \mathbf{H} HTR−1H，因此更新后的平方根信息矩阵为：
S n e w = QR ( [ S H T R − 1 / 2 ] ) \mathbf{S}_{new} = \text{QR}(\begin{bmatrix} \mathbf{S} \\ \mathbf{H}^T \mathbf{R}^{-1/2} \end{bmatrix}) Snew=QR([SHTR−1/2])
算法逻辑：
- 将状态的平方根信息矩阵与观测的信息项合并；
- 通过QR分解完成平方根更新，避免协方差直接求逆。
三、整体算法流程
- IMU预积分：通过IMU数据传播核心IMU状态均值；
- 协方差传播 ：调用
EKFPropagation更新协方差； - 特征观测处理：提取多帧特征匹配，构造观测模型；
- EKF更新 ：调用
EKFUpdate用特征约束更新状态； - 滑动窗口维护 ：调用
augment_clone添加新克隆，调用marginalize_old_clone移除旧克隆； - SLAM特征初始化 （可选）：调用
initialize初始化新特征； - 边缘化冗余特征 （可选）：调用
marginalize_slam移除异常SLAM特征。
四、关键技术总结
- 分块矩阵操作：避免全协方差运算，提升效率；
- Schur补边缘化：保证状态移除后的协方差一致性；
- 时间偏移校准：克隆位姿时考虑IMU-相机时间差，提升同步精度；
- 平方根信息滤波：提升数值稳定性，避免协方差矩阵退化。
五、实现
StateHelper通过封装这些核心操作，实现了MSCKF状态的高效管理，是VIO系统的核心模块之一。
cpp
#ifndef OV_MSCKF_STATE_HELPER_H
#define OV_MSCKF_STATE_HELPER_H
#include <Eigen/Eigen>
#include <memory>
#include "common/feat/Feature.h"
#include "common/feat/FeatureInitializer.h"
class Type;
class State;
/**
* @brief Helper which manipulates the State and its covariance.
*
* In general, this class has all the core logic for an Extended Kalman Filter (EKF)-based system.
* This has all functions that change the covariance along with addition and removing elements from the state.
* All functions here are static, and thus are self-contained so that in the future multiple states could be tracked and updated.
* We recommend you look directly at the code for this class for clarity on what exactly we are doing in each and the matching documentation
* pages.
*/
class StateHelper {
public:
/**
* @brief Performs EKF propagation of the state covariance.
*
* The mean of the state should already have been propagated, thus just moves the covariance forward in time.
* The new states that we are propagating the old covariance into, should be **contiguous** in memory.
* The user only needs to specify the sub-variables that this block is a function of.
* \f[
* \tilde{\mathbf{x}}' =
* \begin{bmatrix}
* \boldsymbol\Phi_1 &
* \boldsymbol\Phi_2 &
* \boldsymbol\Phi_3
* \end{bmatrix}
* \begin{bmatrix}
* \tilde{\mathbf{x}}_1 \\
* \tilde{\mathbf{x}}_2 \\
* \tilde{\mathbf{x}}_3
* \end{bmatrix}
* +
* \mathbf{n}
* \f]
*
* @param state Pointer to state
* @param order_NEW Contiguous variables that have evolved according to this state transition
* @param order_OLD Variable ordering used in the state transition
* @param Phi State transition matrix (size order_NEW by size order_OLD)
* @param Q Additive state propagation noise matrix (size order_NEW by size order_NEW)
*/
static void EKFPropagation(std::shared_ptr<State> state, const std::vector<std::shared_ptr<Type>> &order_NEW,
const std::vector<std::shared_ptr<Type>> &order_OLD, const Eigen::MatrixXd &Phi,
const Eigen::MatrixXd &Q);
/**
* @brief Performs EKF update of the state (see @ref linear-meas page)
* @param state Pointer to state
* @param H_order Variable ordering used in the compressed Jacobian
* @param H Condensed Jacobian of updating measurement
* @param res residual of updating measurement
* @param R updating measurement covariance
*/
static void EKFUpdate(std::shared_ptr<State> state, const std::vector<std::shared_ptr<Type>> &H_order, const Eigen::MatrixXd &H,
const Eigen::VectorXd &res, const Eigen::MatrixXd &R);
/**
* @brief This will set the initial covaraince of the specified state elements.
* Will also ensure that proper cross-covariances are inserted.
* @param state Pointer to state
* @param covariance The covariance of the system state
* @param order Order of the covariance matrix
*/
static void set_initial_covariance(std::shared_ptr<State> state, const Eigen::MatrixXd &covariance,
const std::vector<std::shared_ptr<Type>> &order);
/**
* @brief For a given set of variables, this will this will calculate a smaller covariance.
*
* That only includes the ones specified with all crossterms.
* Thus the size of the return will be the summed dimension of all the passed variables.
* Normal use for this is a chi-squared check before update (where you don't need the full covariance).
*
* @param state Pointer to state
* @param small_variables Vector of variables whose marginal covariance is desired
* @return marginal covariance of the passed variables
*/
static Eigen::MatrixXd get_marginal_covariance(std::shared_ptr<State> state,
const std::vector<std::shared_ptr<Type>> &small_variables);
/**
* @brief This gets the full covariance matrix.
*
* Should only be used during simulation as operations on this covariance will be slow.
* This will return a copy, so this cannot be used to change the covariance by design.
* Please use the other interface functions in the StateHelper to progamatically change to covariance.
*
* @param state Pointer to state
* @return covariance of current state
*/
static Eigen::MatrixXd get_full_covariance(std::shared_ptr<State> state);
/**
* @brief Marginalizes a variable, properly modifying the ordering/covariances in the state
*
* This function can support any Type variable out of the box.
* Right now the marginalization of a sub-variable/type is not supported.
* For example if you wanted to just marginalize the orientation of a PoseJPL, that isn't supported.
* We will first remove the rows and columns corresponding to the type (i.e. do the marginalization).
* After we update all the type ids so that they take into account that the covariance has shrunk in parts of it.
*
* @param state Pointer to state
* @param marg Pointer to variable to marginalize
*/
static void marginalize(std::shared_ptr<State> state, std::shared_ptr<Type> marg);
/**
* @brief Clones "variable to clone" and places it at end of covariance
* @param state Pointer to state
* @param variable_to_clone Pointer to variable that will be cloned
*/
static std::shared_ptr<Type> clone(std::shared_ptr<State> state, std::shared_ptr<Type> variable_to_clone);
/**
* @brief Initializes new variable into covariance.
*
* Uses Givens to separate into updating and initializing systems (therefore system must be fed as isotropic).
* If you are not isotropic first whiten your system (TODO: we should add a helper function to do this).
* If your H_L Jacobian is already directly invertable, the just call the initialize_invertible() instead of this function.
* Please refer to @ref update-delay page for detailed derivation.
*
* @param state Pointer to state
* @param new_variable Pointer to variable to be initialized
* @param H_order Vector of pointers in order they are contained in the condensed state Jacobian
* @param H_R Jacobian of initializing measurements wrt variables in H_order
* @param H_L Jacobian of initializing measurements wrt new variable
* @param R Covariance of initializing measurements (isotropic)
* @param res Residual of initializing measurements
* @param chi_2_mult Value we should multiply the chi2 threshold by (larger means it will be accepted more measurements)
*/
static bool initialize(std::shared_ptr<State> state, std::shared_ptr<Type> new_variable,
const std::vector<std::shared_ptr<Type>> &H_order, Eigen::MatrixXd &H_R, Eigen::MatrixXd &H_L,
Eigen::MatrixXd &R, Eigen::VectorXd &res, double chi_2_mult);
/**
* @brief Initializes new variable into covariance (H_L must be invertible)
*
* Please refer to @ref update-delay page for detailed derivation.
* This is just the update assuming that H_L is invertable (and thus square) and isotropic noise.
*
* @param state Pointer to state
* @param new_variable Pointer to variable to be initialized
* @param H_order Vector of pointers in order they are contained in the condensed state Jacobian
* @param H_R Jacobian of initializing measurements wrt variables in H_order
* @param H_L Jacobian of initializing measurements wrt new variable (needs to be invertible)
* @param R Covariance of initializing measurements
* @param res Residual of initializing measurements
*/
static void initialize_invertible(std::shared_ptr<State> state, std::shared_ptr<Type> new_variable,
const std::vector<std::shared_ptr<Type>> &H_order, const Eigen::MatrixXd &H_R,
const Eigen::MatrixXd &H_L, const Eigen::MatrixXd &R, const Eigen::VectorXd &res);
/**
* @brief Augment the state with a stochastic copy of the current IMU pose
*
* After propagation, normally we augment the state with an new clone that is at the new update timestep.
* This augmentation clones the IMU pose and adds it to our state's clone map.
* If we are doing time offset calibration we also make our cloning a function of the time offset.
* Time offset logic is based on Mingyang Li and Anastasios I. Mourikis paper:
* http://journals.sagepub.com/doi/pdf/10.1177/0278364913515286 We can write the current clone at the true imu base clock time as the
* follow: \f{align*}{
* {}^{I_{t+t_d}}_G\bar{q} &= \begin{bmatrix}\frac{1}{2} {}^{I_{t+\hat{t}_d}}\boldsymbol\omega \tilde{t}_d \\
* 1\end{bmatrix}\otimes{}^{I_{t+\hat{t}_d}}_G\bar{q} \\
* {}^G\mathbf{p}_{I_{t+t_d}} &= {}^G\mathbf{p}_{I_{t+\hat{t}_d}} + {}^G\mathbf{v}_{I_{t+\hat{t}_d}}\tilde{t}_d
* \f}
* where we say that we have propagated our state up to the current estimated true imaging time for the current image,
* \f${}^{I_{t+\hat{t}_d}}\boldsymbol\omega\f$ is the angular velocity at the end of propagation with biases removed.
* This is off by some smaller error, so to get to the true imaging time in the imu base clock, we can append some small timeoffset error.
* Thus the Jacobian in respect to our time offset during our cloning procedure is the following:
* \f{align*}{
* \frac{\partial {}^{I_{t+t_d}}_G\tilde{\boldsymbol\theta}}{\partial \tilde{t}_d} &= {}^{I_{t+\hat{t}_d}}\boldsymbol\omega \\
* \frac{\partial {}^G\tilde{\mathbf{p}}_{I_{t+t_d}}}{\partial \tilde{t}_d} &= {}^G\mathbf{v}_{I_{t+\hat{t}_d}}
* \f}
*
* @param state Pointer to state
* @param last_w The estimated angular velocity at cloning time (used to estimate imu-cam time offset)
*/
static void augment_clone(std::shared_ptr<State> state, Eigen::Matrix<double, 3, 1> last_w);
/**
* @brief Remove the oldest clone, if we have more then the max clone count!!
*
* This will marginalize the clone from our covariance, and remove it from our state.
* This is mainly a helper function that we can call after each update.
* It will marginalize the clone specified by State::margtimestep() which should return a clone timestamp.
*
* @param state Pointer to state
*/
static void marginalize_old_clone(std::shared_ptr<State> state);
/**
* @brief Remove the oldest clone, if we have more then the max clone count!!
*
* This will marginalize the clone from our covariance, and remove it from our state.
* This is mainly a helper function that we can call after each update.
* It will marginalize the clone specified by State::margtimestep() which should return a clone timestamp.
*
* @param state Pointer to state
*/
static void marginalize_clone(std::shared_ptr<State> state,double time_to_marg);
/**
* @brief Marginalize bad SLAM features
* @param state Pointer to state
*/
static void marginalize_slam(std::shared_ptr<State> state);
static void save_landmark_state(double timestamp,
std::shared_ptr<Feature> feat,
const std::vector<std::map<double, FeatureInitializer::ClonePose>>& clones_cam,
std::string suffix);
static void margin_with_permutation(int start_id, int end_id, Eigen::MatrixXd& matrix_to_marg);
static void SRISWFUpdate(std::shared_ptr<State> state);
private:
/**
* All function in this class should be static.
* Thus an instance of this class cannot be created.
*/
StateHelper() {}
};
#endif // OV_MSCKF_STATE_HELPER_H
cpp
#include "StateHelper.h"
#include "common/state/State.h"
#include "common/types/Landmark.h"
#include "common/utils/colors.h"
#include "common/utils/print.h"
#include <boost/math/distributions/chi_squared.hpp>
#include <fstream>
bool EKFterminallog = false;
static std::ofstream debugger_statehelper("");
// static std::ofstream debugger_statehelper("debuginfo.csv");
extern std::ofstream debugger;
extern std::ofstream out;
/**
* @brief 对矩阵进行边缘化操作，通过置换和QR分解实现
*
* 该函数将指定范围内的变量进行边缘化处理。具体做法是：
* 1. 构造置换矩阵，将要边缘化的变量移到矩阵前部
* 2. 对置换后的矩阵进行QR分解
* 3. 提取QR分解结果中的上三角部分作为新的信息矩阵
*
* @param start_id 边缘化变量的起始索引（包含）
* @param end_id 边缘化变量的结束索引（不包含）
* @param matrix_to_marg 输入的待边缘化矩阵，函数执行后将被更新为边缘化后的结果矩阵
*/
void StateHelper::margin_with_permutation(int start_id, int end_id, Eigen::MatrixXd& matrix_to_marg) {
assert(matrix_to_marg.cols() == matrix_to_marg.rows());
// 保存原始矩阵并初始化置换矩阵
Eigen::MatrixXd origin_matrix = matrix_to_marg;
Eigen::PermutationMatrix<Eigen::Dynamic> perm(origin_matrix.rows());
perm.setIdentity();
int margin_size = end_id - start_id;
// 构造置换矩阵：将[start_id, end_id)范围内的行移动到前面
for (auto i = 0; i < start_id; i++) {
perm.indices()[i] = i + margin_size;
}
for (auto i = start_id; i < end_id; i++) {
perm.indices()[i] = i - start_id;
}
// 应用置换矩阵并对结果进行QR分解
Eigen::MatrixXd organized_info = origin_matrix * perm.transpose();
Eigen::HouseholderQR<Eigen::MatrixXd> qr(organized_info);
Eigen::MatrixXd RkM_RkMR_RkR = qr.matrixQR().triangularView<Eigen::Upper>();
// 提取边缘化后的子矩阵
int newsize = matrix_to_marg.rows() - margin_size;
matrix_to_marg = Eigen::MatrixXd::Zero(newsize, newsize);
matrix_to_marg = RkM_RkMR_RkR.block(end_id - start_id, end_id - start_id, newsize, newsize);
}
/**
* @brief 实现扩展卡尔曼滤波器(EKF)的协方差矩阵传播更新
*
* 该函数根据状态转移矩阵Phi和过程噪声矩阵Q，更新系统状态的协方差矩阵。
* 在EKF框架中，状态预测分为两个步骤：状态均值预测和协方差预测，
* 该函数专门处理协方差预测部分，实现了协方差矩阵的传播更新。
*
* 数学原理：根据EKF理论，协方差传播公式为 P_new = Φ * P_old * Φ^T + Q
* 其中Φ是状态转移矩阵，P_old是旧状态的协方差矩阵，Q是过程噪声协方差矩阵
*
* @param state 当前系统状态，包含需要更新的协方差矩阵
* @param order_NEW 新状态变量的有序列表，表示协方差更新后的变量顺序
* @param order_OLD 旧状态变量的有序列表，表示协方差更新前的变量顺序
* @param Phi 状态转移矩阵，维度为[新状态总维度×旧状态总维度]
* @param Q 过程噪声协方差矩阵，维度为[新状态总维度×新状态总维度]
*
* @throws std::runtime_error 当输入参数不满足要求（如空数组、非连续状态元素）或
* 协方差矩阵更新后出现负对角线元素时
*/
void StateHelper::EKFPropagation(std::shared_ptr<State> state, const std::vector<std::shared_ptr<Type>> &order_NEW,
const std::vector<std::shared_ptr<Type>> &order_OLD, const Eigen::MatrixXd &Phi,
const Eigen::MatrixXd &Q) {
// 验证输入参数：确保新旧变量列表不为空
if (order_NEW.empty() || order_OLD.empty()) {
PRINT_ERROR(RED "StateHelper::EKFPropagation() - Called with empty variable arrays!\n" RESET);
throw std::runtime_error("StateHelper::EKFPropagation() - Called with empty variable arrays!\n");
}
// 验证新状态变量在内存中是否连续，确保状态转移可以正确执行
// 这是为了确保状态向量中各变量的id是连续的，没有间隙
int size_order_NEW = order_NEW.at(0)->size();
for (size_t i = 0; i < order_NEW.size() - 1; i++) {
// 检查当前变量的结束id是否正好是下一个变量的开始id
if (order_NEW.at(i)->id() + order_NEW.at(i)->size() != order_NEW.at(i + 1)->id()) {
PRINT_ERROR(RED "StateHelper::EKFPropagation() - Called with non-contiguous state elements!\n" RESET);
PRINT_ERROR(
RED "StateHelper::EKFPropagation() - This code only support a state transition which is in the same order as the state\n" RESET);
throw std::runtime_error("StateHelper::EKFPropagation() - Called with non-contiguous state elements!\n");
}
// 累加计算新状态的总维度
size_order_NEW += order_NEW.at(i + 1)->size();
}
// 计算旧状态的总维度
int size_order_OLD = order_OLD.at(0)->size();
for (size_t i = 0; i < order_OLD.size() - 1; i++) {
size_order_OLD += order_OLD.at(i + 1)->size();
}
// 断言检查矩阵维度是否匹配
// Phi矩阵的行数应该等于新状态总维度
// Phi矩阵的列数应该等于旧状态总维度
// Q矩阵应该是方阵，维度等于新状态总维度
assert(size_order_NEW == Phi.rows());
assert(size_order_OLD == Phi.cols());
assert(size_order_NEW == Q.cols());
assert(size_order_NEW == Q.rows());
// 为每个旧状态变量计算在状态转移矩阵Phi中的起始位置（列索引）
int current_it = 0;
std::vector<int> Phi_id;
for (const auto &var : order_OLD) {
Phi_id.push_back(current_it);
current_it += var->size();
}
// 计算Cov_PhiT = P * Phi^T，这是协方差传播的中间结果
// 这里通过分块矩阵乘法实现，只处理与旧状态相关的部分
Eigen::MatrixXd Cov_PhiT = Eigen::MatrixXd::Zero(state->_Cov.rows(), Phi.rows());
for (size_t i = 0; i < order_OLD.size(); i++) {
std::shared_ptr<Type> var = order_OLD.at(i);
// 使用noalias()避免临时对象创建，提高计算效率
Cov_PhiT.noalias() +=
state->_Cov.block(0, var->id(), state->_Cov.rows(), var->size()) * Phi.block(0, Phi_id[i], Phi.rows(), var->size()).transpose();
}
// 计算Phi * P * Phi^T + Q，这是协方差更新的核心部分
// 先初始化结果矩阵为Q的上三角部分（利用对称性）
Eigen::MatrixXd Phi_Cov_PhiT = Q.selfadjointView<Eigen::Upper>();
for (size_t i = 0; i < order_OLD.size(); i++) {
std::shared_ptr<Type> var = order_OLD.at(i);
// 使用分块矩阵乘法累加计算Phi * P * Phi^T的各部分
Phi_Cov_PhiT.noalias() += Phi.block(0, Phi_id[i], Phi.rows(), var->size()) * Cov_PhiT.block(var->id(), 0, var->size(), Phi.rows());
}
// 更新状态协方差矩阵
// 协方差矩阵是对称的，我们只需要更新相应的块
int start_id = order_NEW.at(0)->id(); // 新状态在协方差矩阵中的起始位置
int phi_size = Phi.rows(); // 状态转移矩阵的行数（新状态总维度）
int total_size = state->_Cov.rows(); // 协方差矩阵的总维度
// 更新左上块：Cov_PhiT^T（利用对称性）
state->_Cov.block(start_id, 0, phi_size, total_size) = Cov_PhiT.transpose();
// 更新右上块：Cov_PhiT
state->_Cov.block(0, start_id, total_size, phi_size) = Cov_PhiT;
// 更新右下角块：Phi * P * Phi^T + Q
state->_Cov.block(start_id, start_id, phi_size, phi_size) = Phi_Cov_PhiT;
// 检查更新后的协方差矩阵是否半正定
// 半正定矩阵的所有对角线元素必须非负
Eigen::VectorXd diags = state->_Cov.diagonal();
bool found_neg = false;
for (int i = 0; i < diags.rows(); i++) {
if (diags(i) < 0.0) {
PRINT_WARNING(RED "StateHelper::EKFPropagation() - diagonal at %d is %.2f\n" RESET, i, diags(i));
found_neg = true;
}
}
// 如果发现负对角线元素，抛出异常以避免滤波器发散
if (found_neg) {
throw std::runtime_error("StateHelper::EKFPropagation() - Found negative diagonal value");
}
}
/**
* @brief 扩展卡尔曼滤波器(EKF)的测量更新函数
* @details 实现EKF测量更新步骤，根据雅可比矩阵H、残差res和观测噪声矩阵R更新系统状态和协方差
* 数学原理：
* - 卡尔曼增益计算: K = P*H^T*S^{-1}，其中 S = H*P*H^T + R
* - 状态更新: x_new = x_old + K*res
* - 协方差更新: P_new = P_old - K*H*P_old
*
* @param state 系统状态对象，包含状态变量和协方差矩阵
* @param H_order 雅可比矩阵H对应的变量列表，定义了H矩阵中各列的变量顺序
* @param H 雅可比矩阵，维度为[残差维度×状态变量维度]
* @param res 测量残差向量，维度为[残差维度×1]
* @param R 观测噪声协方差矩阵，维度为[残差维度×残差维度]
*/
void StateHelper::EKFUpdate(std::shared_ptr<State> state, const std::vector<std::shared_ptr<Type>> &H_order, const Eigen::MatrixXd &H,
const Eigen::VectorXd &res, const Eigen::MatrixXd &R) {
if (EKFterminallog) {
std::cout << "EKFUpdate checkpoint p0\n";
}
//==========================================================
//==========================================================
// Kalman增益计算的第一部分：K = (P*H^T)*S^{-1} = M*S^{-1}
// 参数验证：确保残差向量与观测噪声矩阵维度匹配，雅可比矩阵与残差向量行数一致
assert(res.rows() == R.rows());
assert(H.rows() == res.rows());
Eigen::MatrixXd M_a = Eigen::MatrixXd::Zero(state->_Cov.rows(), res.rows());
// 获取雅可比矩阵H中每个测量变量对应的起始列索引
int current_it = 0;
std::vector<int> H_id; // 存储每个变量在H矩阵中的起始列索引
for (const auto &meas_var : H_order) {
H_id.push_back(current_it);
current_it += meas_var->size();
}
if (EKFterminallog) {
std::cout << "EKFUpdate checkpoint p1\n";
}
//==========================================================
//==========================================================
// 计算M = P*H^T的每一块，通过分块矩阵乘法实现
// 遍历系统中的每个状态变量，计算其对应的M_i = P_im * Hm^T
for (const auto &var : state->_variables) {
// 初始化当前变量对应的M_i子矩阵
Eigen::MatrixXd M_i = Eigen::MatrixXd::Zero(var->size(), res.rows());
// 遍历所有测量变量，累加每个变量对当前变量的贡献
for (size_t i = 0; i < H_order.size(); i++) {
std::shared_ptr<Type> meas_var = H_order[i];
// 分块矩阵乘法：P的[var.id():meas_var.id()]块 × H的[0:H_id[i]]块的转置
// noalias()避免临时对象的创建，提高性能
M_i.noalias() += state->_Cov.block(var->id(), meas_var->id(), var->size(), meas_var->size()) *
H.block(0, H_id[i], H.rows(), meas_var->size()).transpose();
}
// 将计算得到的M_i子矩阵放到完整矩阵M_a的对应位置
M_a.block(var->id(), 0, var->size(), res.rows()) = M_i;
}
if (EKFterminallog) {
std::cout << "EKFUpdate checkpoint p1\n";
}
//==========================================================
//==========================================================
// 获取与测量相关的系统协方差矩阵子矩阵
Eigen::MatrixXd P_small = StateHelper::get_marginal_covariance(state, H_order);
// 计算残差协方差S = H*P_small*H^T + R
// 使用三角视图优化计算（只计算上三角部分）
Eigen::MatrixXd S(R.rows(), R.rows());
S.triangularView<Eigen::Upper>() = H * P_small * H.transpose();
S.triangularView<Eigen::Upper>() += R;
// 使用LLT分解求解S的逆，比直接求逆更稳定
Eigen::MatrixXd Sinv = Eigen::MatrixXd::Identity(R.rows(), R.rows());
S.selfadjointView<Eigen::Upper>().llt().solveInPlace(Sinv);
// 计算卡尔曼增益K = M_a * Sinv
Eigen::MatrixXd K = M_a * Sinv.selfadjointView<Eigen::Upper>();
// 更新协方差矩阵：P = P - K*M_a^T
// 利用对称性质，只更新上三角部分以提高效率
state->_Cov.triangularView<Eigen::Upper>() -= K * M_a.transpose();
// 确保协方差矩阵是对称的
state->_Cov = state->_Cov.selfadjointView<Eigen::Upper>();
if (EKFterminallog) {
std::cout << "EKFUpdate checkpoint p2\n";
}
// 数值稳定性检查：确保协方差矩阵是半正定的（对角线元素非负）
Eigen::VectorXd diags = state->_Cov.diagonal();
bool found_neg = false;
for (int i = 0; i < diags.rows(); i++) {
if (diags(i) < 0.0) {
// 输出详细的调试信息，包括问题变量、矩阵数据等
std::cout << "EKF update cov found neg: " << i << " " << diags(i)<< std::endl;
out << "EKF update cov found neg: " << i << " " << diags(i) << std::endl;
int count_hid = 0;
out << "H_order: ";
for (auto horder : H_order) {
out << horder->id() << " " << std::endl;
}
out << std::endl;
out << "Hid: ";
for (auto id : H_id) {
out << id << " " << std::endl;
}
out << std::endl;
out << "H size: " << H.cols() << " " << H.rows() << std::endl;
out << "H: \n" << H << std::endl;
out << "P_small: \n" << P_small << std::endl;
out << "Sinv: \n" << Sinv << std::endl;
out << "K: \n" << K << std::endl;
out << "vars: \n";
for (size_t i = 0; i < state->_variables.size(); i++) {
out << state->_variables.at(i)->id() << ": ";
out << state->_variables.at(i)->value().transpose() <<std::endl;
}
for (auto& ele : H_order) {
if (ele->id() <= i && ele->id() + 3 >= i) {
out << "ele id: " << ele->id() << std::endl;
out << "H_id[i]: " << H_id[count_hid] << std::endl;
}
count_hid++;
}
PRINT_WARNING(RED "StateHelper::EKFUpdate() - diagonal at %d is %.2f\n" RESET, i, diags(i));
found_neg = true;
}
}
// 如果发现负对角元素，抛出异常表示数值不稳定
if (found_neg) {
throw std::runtime_error("StateHelper::EKFUpdate() - Found negative diagonal value");
}
if (EKFterminallog) {
std::cout << "EKFUpdate checkpoint p3\n";
}
// 计算状态更新量dx = K * res
Eigen::VectorXd dx = K * res;
debugger << "dx : " << dx.transpose() << std::endl;
// 更新每个状态变量的均值
for (size_t i = 0; i < state->_variables.size(); i++) {
state->_variables.at(i)->update(dx.block(state->_variables.at(i)->id(), 0, state->_variables.at(i)->size(), 1));
}
// 如果启用了在线相机内参标定，更新相机对象的内参参数
if (state->_options.do_calib_camera_intrinsics) {
for (size_t cam_id = 0; cam_id < state->_cam_intrinsics.size(); ++cam_id) {
Eigen::VectorXf cam_vals = state->_cam_intrinsics[cam_id]->value().cast<float>();
float cam_vals_arr[8];
for (int idx = 0; idx < 8; ++idx) cam_vals_arr[idx] = cam_vals(idx);
cam_equi_set_value(state->_cam_intrinsics_cameras[cam_id].get(), cam_vals_arr);
}
}
if (EKFterminallog) {
std::cout << "EKFUpdate checkpoint p4\n";
}
}
/**
* @brief 平方根信息平滑滤波器(SRISWF)的状态更新函数
* @details 实现基于平方根信息矩阵的状态更新，通过求解线性方程组获取状态更新量，并更新系统状态变量
* SRISWF(Square Root Information Smoothing with Forgetting)是一种高效的滤波方法，
* 相比传统EKF具有更好的数值稳定性，尤其适合处理高维状态估计问题
*
* @param state 系统状态对象，包含信息矩阵(_Info)、总残差(res_total)和需更新的状态变量
*/
void StateHelper::SRISWFUpdate(std::shared_ptr<State> state) {
// 输出当前总残差向量，用于调试
debugger << " update res: \n" << state->res_total.transpose() << std::endl;
// 求解线性方程组: Info * delta_x = res_total
// 使用三角视图求解，利用信息矩阵的上三角结构提高计算效率
Eigen::VectorXd delta_x = state->_Info.triangularView<Eigen::Upper>().solve(state->res_total);
// 输出计算得到的状态更新量，用于调试
debugger << " update delta_x: \n" << delta_x.transpose() << std::endl;
// 更新SRISWF相关的状态变量
int current_update_id = 0; // 当前更新位置的索引
for (auto& var : state->_variables_SRISWF) {
// 提取当前变量对应的更新子向量
Eigen::VectorXd subvar = delta_x.segment(current_update_id, var->size());
// 更新变量的值
var->update(subvar);
// 更新索引位置
current_update_id += var->size();
}
// 更新IMU状态变量（位于信息矩阵的最后15维）
// 15维对应IMU的状态：位置(3)、速度(3)、四元数(4)、陀螺仪偏置(3)、加速度计偏置(2)
current_update_id = state->_Info.cols() - 15;
const Eigen::VectorXd state_toupdate = delta_x.segment(current_update_id, 15);
state->_imu->update(state_toupdate);
// 获取并输出更新后的位姿信息，用于调试
std::shared_ptr<PoseJPL> pose = std::dynamic_pointer_cast<PoseJPL>(state->_variables_SRISWF.back());
debugger << "updated pose: \n" << pose->pos().transpose() << "\n " << pose->quat().transpose() << std::endl;
// 断言检查：确保信息矩阵是方阵且维度与残差向量匹配
assert(state->_Info.rows() == state->_Info.cols());
assert(state->_Info.cols() == state->res_total.rows());
// 更新完成后重置总残差向量，为下次迭代做准备
state->res_total.setZero();
}
/**
* @brief 设置系统状态的初始协方差矩阵
* @details 将给定的协方差矩阵按照指定的变量顺序复制到系统状态的协方差矩阵中
* 该函数假设初始协方差矩阵是分块对角的（与其他未指定变量的交叉项为零）
* 这在系统初始化阶段（如传感器标定和初始状态设置时）通常是成立的
*
* @param state 系统状态对象，其协方差矩阵将被更新
* @param covariance 要设置的初始协方差矩阵
* @param order 指定协方差矩阵中各块对应的变量顺序
*/
void StateHelper::set_initial_covariance(std::shared_ptr<State> state, const Eigen::MatrixXd &covariance,
const std::vector<std::shared_ptr<Type>> &order) {
// 我们需要遍历每个元素并覆盖当前的协方差值
// 例如考虑以下情况：
// x = [ 姿态 位置 ] -> 插入到 -> x = [ 姿态 偏置 位置 ]
// P = [ P_姿态姿态 P_姿态位置 ] -> P = [ P_姿态姿态 0 P_姿态位置 ]
// [ P_位置姿态 P_位置位置 ] [ 0 P_偏置 0 ]
// [ P_位置姿态 0 P_位置位置 ]
// 关键假设是协方差矩阵是分块对角的（与P_偏置的交叉项为零，而P_偏置内部可以是稠密的）
// 这在系统启动时通常是成立的（例如在标定参数和初始状态之间）
// 遍历每个变量，复制所有其他变量的交叉项
// 注意：当i_index=k_index时，这也会复制变量自身的协方差块
int i_index = 0; // 当前处理的行块在输入协方差矩阵中的起始索引
for (size_t i = 0; i < order.size(); i++) {
int k_index = 0; // 当前处理的列块在输入协方差矩阵中的起始索引
for (size_t k = 0; k < order.size(); k++) {
// 从输入协方差矩阵复制对应块到系统状态协方差矩阵的对应位置
state->_Cov.block(order[i]->id(), order[k]->id(), order[i]->size(), order[k]->size()) =
covariance.block(i_index, k_index, order[i]->size(), order[k]->size());
k_index += order[k]->size(); // 更新列块索引
}
i_index += order[i]->size(); // 更新行块索引
}
// 确保协方差矩阵是对称的（使用上三角部分构造对称矩阵）
state->_Cov = state->_Cov.selfadjointView<Eigen::Upper>();
}
/**
* @brief 计算部分状态变量的边际协方差矩阵
*
* 该函数从系统完整协方差矩阵中提取指定状态变量子集的边际协方差
* 边际协方差表示在忽略其他状态变量影响的情况下，指定状态变量之间的协方差关系
*
* @param[in] state 系统状态对象，包含完整的协方差矩阵
* @param[in] small_variables 需要提取边际协方差的状态变量向量
* @return 提取的边际协方差矩阵，维度为(总变量维度×总变量维度)
*
* 数学原理：
* - 边际协方差矩阵是从完整协方差矩阵中提取的子矩阵
* - 对于状态向量x = [x₁, x₂]ᵀ，其中x₁是我们关心的变量，x₂是其他变量
* - 边际协方差矩阵定义为Cov(x₁) = E[(x₁-E[x₁])(x₁-E[x₁])ᵀ]
* - 在完整协方差矩阵中，这对应于取x₁变量对应的所有行和列的交叉项
*/
Eigen::MatrixXd StateHelper::get_marginal_covariance(std::shared_ptr<State> state,
const std::vector<std::shared_ptr<Type>> &small_variables) {
// 计算需要构建的边际协方差矩阵的大小
// 通过累加所有指定变量的维度得到边际协方差矩阵的总维度
int cov_size = 0;
for (size_t i = 0; i < small_variables.size(); i++) {
cov_size += small_variables[i]->size();
}
// 初始化返回的边际协方差矩阵，大小为cov_size×cov_size，初始值全为0
Eigen::MatrixXd Small_cov = Eigen::MatrixXd::Zero(cov_size, cov_size);
// 遍历每个变量对，复制对应的协方差交叉项
// 注意：当i_index=k_index时，这也会复制变量自身的方差
int i_index = 0; // 当前变量在行方向的起始索引
for (size_t i = 0; i < small_variables.size(); i++) {
int k_index = 0; // 当前变量在列方向的起始索引
for (size_t k = 0; k < small_variables.size(); k++) {
// 从完整协方差矩阵中提取指定变量对的协方差块，并复制到边际协方差矩阵
// block操作参数：目标块起始行、目标块起始列、块行数、块列数
Small_cov.block(i_index, k_index, small_variables[i]->size(), small_variables[k]->size()) =
state->_Cov.block(small_variables[i]->id(), small_variables[k]->id(), small_variables[i]->size(), small_variables[k]->size());
k_index += small_variables[k]->size(); // 更新列方向的起始索引
}
i_index += small_variables[i]->size(); // 更新行方向的起始索引
}
// 返回构建好的边际协方差矩阵
// 注意：由于协方差矩阵的对称性，理论上可以取消下面这行注释以确保数值对称性
// Small_cov = 0.5*(Small_cov+Small_cov.transpose());
return Small_cov;
}
/**
* @brief 获取系统完整的协方差矩阵
*
* 该函数从系统状态中提取当前活动状态变量的完整协方差矩阵
* 协方差矩阵表示状态变量之间的不确定性关系，是EKF等滤波器中的关键组成部分
*
* @param[in] state 系统状态对象，包含当前活动状态变量的协方差矩阵
* @return 完整的协方差矩阵，维度为(活动状态总维度×活动状态总维度)
*
* 实现说明：
* - 函数创建一个新的方阵，大小与状态对象中当前活动的协方差矩阵相同
* - 通过block操作将状态中的协方差矩阵复制到新矩阵的左上角部分
* - 虽然此实现看起来简单，但为代码提供了良好的封装性和一致性
*/
Eigen::MatrixXd StateHelper::get_full_covariance(std::shared_ptr<State> state) {
// 确定协方差矩阵的大小，等于活动状态变量的总维度
int cov_size = (int)state->_Cov.rows();
// 创建并初始化返回的协方差矩阵，大小为cov_size×cov_size，初始值全为0
Eigen::MatrixXd full_cov = Eigen::MatrixXd::Zero(cov_size, cov_size);
// 将状态对象中活动状态变量的协方差矩阵复制到返回矩阵的左上角部分
// block操作参数：目标块起始行、目标块起始列、块行数、块列数
full_cov.block(0, 0, state->_Cov.rows(), state->_Cov.rows()) = state->_Cov;
// 返回构建好的完整协方差矩阵
return full_cov;
}
/**
* @brief 对系统状态中的指定变量执行边缘化操作
*
* 边缘化是状态估计中的关键操作，用于移除不再需要的状态变量，同时保留其对剩余变量的约束信息
* 在VIO/SLAM系统中常用于处理计算资源限制或移除旧的历史状态
*
* @param[in,out] state 系统状态对象，包含需要边缘化的变量和协方差矩阵
* @param[in] marg 需要被边缘化的状态变量
*
* 数学原理：
* 对于联合概率分布 P(x₁, x_m, x₂)，边缘化x_m后得到 P(x₁, x₂) = ∫P(x₁, x_m, x₂)dx_m
* 在高斯分布假设下，这对应于从协方差矩阵中移除与x_m相关的行和列，保留x₁和x₂之间的关系
*
* 注意事项：
* - 当前实现不支持边缘化子变量
* - 使用共享指针管理变量生命周期
*/
void StateHelper::marginalize(std::shared_ptr<State> state, std::shared_ptr<Type> marg) {
// 检查要边缘化的变量是否存在于当前状态中
if (std::find(state->_variables.begin(), state->_variables.end(), marg) == state->_variables.end()) {
PRINT_ERROR(RED "StateHelper::marginalize() - Called on variable that is not in the state\n" RESET);
PRINT_ERROR(RED "StateHelper::marginalize() - Marginalization, does NOT work on sub-variables yet...\n" RESET);
throw std::runtime_error("StateHelper::marginalize() - Called on variable that is not in the state\n");
}
// 边缘化原理说明：
// 假设协方差矩阵具有如下结构（x₁, x_m, x₂），我们要移除x_m：
// P_(x₁,x₁) P(x₁,x_m) P(x₁,x₂)
// P_(x_m,x₁) P(x_m,x_m) P(x_m,x₂)
// P_(x₂,x₁) P(x₂,x_m) P(x₂,x₂)
//
// 边缘化后的协方差矩阵变为：
// P_(x₁,x₁) P(x₁,x₂)
// P_(x₂,x₁) P(x₂,x₂)
//
// 即：x₁在原协方差中从0到marg_id，x₂在原协方差中从marg_id+marg_size到Cov.rows()
// 获取边缘化变量的维度大小
int marg_size = marg->size();
// 获取边缘化变量在协方差矩阵中的起始索引
int marg_id = marg->id();
// 计算x₂部分的维度大小
int x2_size = (int)state->_Cov.rows() - marg_id - marg_size;
// 创建新的协方差矩阵，维度为原矩阵减去边缘化变量的维度
Eigen::MatrixXd Cov_new(state->_Cov.rows() - marg_size, state->_Cov.rows() - marg_size);
// 复制x₁部分的协方差子矩阵 [x₁, x₁]
Cov_new.block(0, 0, marg_id, marg_id) = state->_Cov.block(0, 0, marg_id, marg_id);
// 复制x₁和x₂之间的协方差子矩阵 [x₁, x₂]
Cov_new.block(0, marg_id, marg_id, x2_size) = state->_Cov.block(0, marg_id + marg_size, marg_id, x2_size);
// 利用对称性设置x₂和x₁之间的协方差子矩阵 [x₂, x₁]，即[x₁, x₂]的转置
Cov_new.block(marg_id, 0, x2_size, marg_id) = Cov_new.block(0, marg_id, marg_id, x2_size).transpose();
// 复制x₂部分的协方差子矩阵 [x₂, x₂]
Cov_new.block(marg_id, marg_id, x2_size, x2_size) = state->_Cov.block(marg_id + marg_size, marg_id + marg_size, x2_size, x2_size);
// 更新状态的协方差矩阵
// state->_Cov.resize(Cov_new.rows(),Cov_new.cols()); // 不需要显式调整大小，直接赋值会自动调整
state->_Cov = Cov_new;
// state->Cov() = 0.5*(Cov_new+Cov_new.transpose()); // 可选的对称化处理，确保数值稳定性
assert(state->_Cov.rows() == Cov_new.rows()); // 断言检查确保维度正确
// 更新状态变量列表，移除边缘化变量并调整剩余变量的索引
// 注意：当前实现不支持边缘化子变量！
std::vector<std::shared_ptr<Type>> remaining_variables;
for (size_t i = 0; i < state->_variables.size(); i++) {
// 只保留非边缘化的状态变量
if (state->_variables.at(i) != marg) {
if (state->_variables.at(i)->id() > marg_id) {
// 如果变量在边缘化变量之后，需要将其索引向前移动
state->_variables.at(i)->set_local_id(state->_variables.at(i)->id() - marg_size);
}
remaining_variables.push_back(state->_variables.at(i));
}
}
std::vector<std::shared_ptr<Type>> remaining_variables_sriswf;
// 设置边缘化变量的索引为-1，表示已被边缘化
// 注意：由于使用共享指针，无需手动释放内存
// delete marg; // 不再需要手动删除
marg->set_local_id(-1);
// 更新状态变量列表为剩余变量
state->_variables = remaining_variables;
}
/**
* @brief 克隆状态中的变量并将其添加到状态中
*
* 该函数在状态估计系统中用于创建现有状态变量的副本，并将其添加到系统状态中
* 同时自动调整协方差矩阵大小并复制相应的协方差信息，保持状态变量间的统计关系
*
* @param[in] state 系统状态对象，包含要克隆的变量
* @param[in] variable_to_clone 要克隆的目标变量（可以是主变量或子变量）
* @return 新创建的克隆变量指针
*
* 实现说明：
* - 支持克隆主变量或子变量
* - 维护协方差矩阵的对称性和一致性
* - 自动设置新变量在状态中的正确位置ID
*/
std::shared_ptr<Type> StateHelper::clone(std::shared_ptr<State> state, std::shared_ptr<Type> variable_to_clone) {
// 获取新克隆变量的总大小、旧协方差矩阵大小以及新变量在协方差矩阵中的位置
int total_size = variable_to_clone->size(); // 克隆变量的维度
int old_size = (int)state->_Cov.rows(); // 当前协方差矩阵大小
int new_loc = (int)state->_Cov.rows(); // 新变量在协方差矩阵中的起始位置（末尾）
// 保守地调整协方差矩阵大小，新增足够空间容纳新变量
// 使用Zero创建新矩阵保持现有数据不变，只扩展大小
state->_Cov.conservativeResizeLike(Eigen::MatrixXd::Zero(old_size + total_size, old_size + total_size));
// 保存当前变量列表，并初始化新克隆变量指针
const std::vector<std::shared_ptr<Type>> new_variables = state->_variables;
std::shared_ptr<Type> new_clone = nullptr;
// 遍历所有变量，查找要克隆的目标变量
for (size_t k = 0; k < state->_variables.size(); k++) {
// 首先检查顶层变量是否匹配，然后检查子变量
std::shared_ptr<Type> type_check = state->_variables.at(k)->check_if_subvariable(variable_to_clone);
// 如果顶层变量直接匹配，使用顶层变量
if (state->_variables.at(k) == variable_to_clone) {
type_check = state->_variables.at(k);
}
// 如果既不是顶层变量也不是子变量，则跳过
else if (type_check != variable_to_clone) {
continue;
}
// 找到要克隆的变量，获取其在协方差矩阵中的位置
int old_loc = type_check->id();
// 复制协方差元素到新位置：
// 1. 复制变量自身的方差（对角线块）
state->_Cov.block(new_loc, new_loc, total_size, total_size) = state->_Cov.block(old_loc, old_loc, total_size, total_size);
// 2. 复制新变量与所有旧变量的协方差（右边界块）
state->_Cov.block(0, new_loc, old_size, total_size) = state->_Cov.block(0, old_loc, old_size, total_size);
// 3. 复制所有旧变量与新变量的协方差（下边界块），利用协方差矩阵的对称性
state->_Cov.block(new_loc, 0, total_size, old_size) = state->_Cov.block(old_loc, 0, total_size, old_size);
// 创建变量的克隆实例并设置其在协方差矩阵中的位置ID
new_clone = type_check->clone();
new_clone->set_local_id(new_loc);
break; // 完成克隆后退出循环
}
// 错误检查：确保成功找到了并克隆了目标变量
if (new_clone == nullptr) {
PRINT_ERROR(RED "StateHelper::clone() - Called on variable is not in the state\n" RESET);
PRINT_ERROR(RED "StateHelper::clone() - Ensure that the variable specified is a variable, or sub-variable..\n" RESET);
throw std::runtime_error("StateHelper::clone() - Called on variable is not in the state\n");
}
// 将新克隆的变量添加到状态变量列表并返回
state->_variables.push_back(new_clone);
return new_clone;
}
/**
* @brief 初始化系统状态中的新变量，并执行相应的状态更新
*
* 该函数实现了新状态变量的初始化，通过QR分解和Givens旋转将系统分离为
* 与新变量相关的部分和不相关的部分。然后分别处理这两部分，确保新变量
* 能够合理地融入系统状态中，同时保持整个系统的一致性。
*
* @param state 系统状态对象，包含当前所有状态变量及其协方差矩阵
* @param new_variable 待初始化的新状态变量
* @param H_order 雅可比矩阵H的变量顺序
* @param H_R 雅可比矩阵右侧部分，对应已有状态变量
* @param H_L 雅可比矩阵左侧部分，对应新状态变量
* @param R 观测噪声协方差矩阵
* @param res 残差向量
* @param chi_2_mult 卡方检验的乘数因子
* @return bool 初始化是否成功
*/
bool StateHelper::initialize(std::shared_ptr<State> state, std::shared_ptr<Type> new_variable,
const std::vector<std::shared_ptr<Type>>& H_order, Eigen::MatrixXd& H_R, Eigen::MatrixXd& H_L,
Eigen::MatrixXd& R, Eigen::VectorXd& res, double chi_2_mult) {
// 检查新变量是否已在状态中初始化
if (std::find(state->_variables.begin(), state->_variables.end(), new_variable) != state->_variables.end()) {
PRINT_ERROR("StateHelper::initialize_invertible() - Called on variable that is already in the state\n");
PRINT_ERROR("StateHelper::initialize_invertible() - Found this variable at %d in covariance\n", new_variable->id());
throw std::runtime_error("StateHelper::initialize_invertible() - Called on variable that is already in the state\n");
}
// 检查观测噪声是否为各向同性（即对角矩阵且所有对角线元素相等）
// TODO: 可以简化这部分检查以提高效率
assert(R.rows() == R.cols());
assert(R.rows() > 0);
for (int r = 0; r < R.rows(); r++) {
for (int c = 0; c < R.cols(); c++) {
// 检查对角线元素是否相等
if (r == c && R(0, 0) != R(r, c)) {
PRINT_ERROR(RED "StateHelper::initialize() - Your noise is not isotropic!\n" RESET);
PRINT_ERROR(RED "StateHelper::initialize() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0));
throw std::runtime_error("StateHelper::initialize() - Noise is not isotropic!\n");
}
// 检查非对角线元素是否为零
else if (r != c && R(r, c) != 0.0) {
PRINT_ERROR(RED "StateHelper::initialize() - Your noise is not diagonal!\n" RESET);
PRINT_ERROR(RED "StateHelper::initialize() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c);
throw std::runtime_error("StateHelper::initialize() - Noise is not diagonal!\n");
}
}
}
//==========================================================
// 执行QR分解（使用Givens旋转）以分离系统
// 上部将依赖于新状态变量，下部则不依赖于新状态变量
//==========================================================
size_t new_var_size = new_variable->size();
assert((int)new_var_size == H_L.cols());
Eigen::JacobiRotation<double> tempHo_GR; // 存储Givens旋转矩阵的参数
// 对H_L矩阵进行QR分解（Givens旋转）
for (int n = 0; n < H_L.cols(); ++n) {
for (int m = (int)H_L.rows() - 1; m > n; m--) {
// 计算Givens旋转矩阵参数
tempHo_GR.makeGivens(H_L(m - 1, n), H_L(m, n));
// 将Givens旋转应用到对应行（m-1,m）的各个矩阵上
// 注意：我们只对非零列[n:H_L.cols()-n-1]应用G，这等价于对整个列[0:H_L.cols()-1]应用G
(H_L.block(m - 1, n, 2, H_L.cols() - n)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
(res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
(H_R.block(m - 1, 0, 2, H_R.cols())).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
}
}
// 将系统分为初始化部分和更新部分
// 1. 可逆的初始化系统（与新变量相关）
Eigen::MatrixXd Hxinit = H_R.block(0, 0, new_var_size, H_R.cols()); // 已有变量对应的雅可比
Eigen::MatrixXd H_finit = H_L.block(0, 0, new_var_size, new_var_size); // 新变量对应的雅可比（上三角）
Eigen::VectorXd resinit = res.block(0, 0, new_var_size, 1); // 相关残差
Eigen::MatrixXd Rinit = R.block(0, 0, new_var_size, new_var_size); // 相关噪声
// 2. 零空间投影的更新系统（不依赖新变量）
Eigen::MatrixXd Hup = H_R.block(new_var_size, 0, H_R.rows() - new_var_size, H_R.cols()); // 剩余雅可比
Eigen::VectorXd resup = res.block(new_var_size, 0, res.rows() - new_var_size, 1); // 剩余残差
Eigen::MatrixXd Rup = R.block(new_var_size, new_var_size, R.rows() - new_var_size, R.rows() - new_var_size); // 剩余噪声
//==========================================================
// 执行马氏距离检验，验证更新系统部分的一致性
//==========================================================
// 获取已有变量的边际协方差矩阵
Eigen::MatrixXd P_up = get_marginal_covariance(state, H_order);
assert(Rup.rows() == Hup.rows());
assert(Hup.cols() == P_up.cols());
// 计算创新协方差矩阵 S = Hup*P_up*Hup^T + Rup
Eigen::MatrixXd S = Hup * P_up * Hup.transpose() + Rup;
// 计算卡方值：chi^2 = resup^T * S^{-1} * resup
double chi2 = resup.dot(S.llt().solve(resup));
// 获取卡方检验阈值（95%置信度）
boost::math::chi_squared chi_squared_dist(res.rows());
double chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
// 如果卡方值过大，拒绝初始化（可能存在异常观测）
if (chi2 > chi_2_mult * chi2_check) {
//PRINT_DEBUG("chi2 fail: %.2f > %.2f * %.2f.\n", chi2, chi_2_mult, chi2_check);
return false;
}
//==========================================================
// 最终，在状态中初始化新变量
//==========================================================
// 检查H_finit的最后一个元素是否足够大（确保可逆性）
if (H_finit(new_var_size-1, new_var_size-1) < 1e-9) {
return false; // 接近奇异，初始化失败
}
// 记录协方差矩阵的旧大小
size_t oldSize = state->_Cov.rows();
// 调用initialize_invertible执行实际的变量初始化
StateHelper::initialize_invertible(state, new_variable, H_order, Hxinit, H_finit, Rinit, resinit);
// 记录协方差矩阵的新大小
size_t newSize = state->_Cov.rows();
// 如果协方差矩阵大小没有变化，说明初始化失败
if (oldSize == newSize) {
return false;
}
// 调试信息输出
debugger_statehelper << "Hup :\n" << Hup << std::endl;
debugger_statehelper << "resup :\n" << resup << std::endl;
// 使用更新部分对系统状态进行更新
if (Hup.rows() > 0) {
StateHelper::EKFUpdate(state, H_order, Hup, resup, Rup);
}
return true;
}
/**
* @brief 在状态估计系统中初始化可逆系统的新变量
*
* 该函数实现了在卡尔曼滤波框架下，将新变量（如路标点、相机位姿等）添加到状态向量中，
* 并通过可逆系统方程计算新变量的初始值和对应的协方差矩阵，同时更新整个系统的协方差。
*
* @param state 系统状态对象，包含当前所有状态变量及其协方差矩阵
* @param new_variable 待初始化的新状态变量
* @param H_order 雅可比矩阵H的变量顺序
* @param H_R 雅可比矩阵右侧部分，对应已有状态变量的雅可比
* @param H_L 雅可比矩阵左侧部分，对应新状态变量的雅可比（需可逆）
* @param R 观测噪声协方差矩阵（要求是各向同性的对角矩阵）
* @param res 残差向量
*/
void StateHelper::initialize_invertible(std::shared_ptr<State> state, std::shared_ptr<Type> new_variable,
const std::vector<std::shared_ptr<Type>> &H_order, const Eigen::MatrixXd &H_R,
const Eigen::MatrixXd &H_L, const Eigen::MatrixXd &R, const Eigen::VectorXd &res) {
// 检查新变量是否已在状态中初始化
if (std::find(state->_variables.begin(), state->_variables.end(), new_variable) != state->_variables.end()) {
PRINT_ERROR("StateHelper::initialize_invertible() - Called on variable that is already in the state\n");
PRINT_ERROR("StateHelper::initialize_invertible() - Found this variable at %d in covariance\n", new_variable->id());
throw std::runtime_error("StateHelper::initialize_invertible() - Called on variable that is already in the state\n");
}
// 检查观测噪声是否为各向同性（即对角矩阵且所有对角线元素相等）
// TODO: 可以简化这部分检查以提高效率
assert(R.rows() == R.cols());
assert(R.rows() > 0);
for (int r = 0; r < R.rows(); r++) {
for (int c = 0; c < R.cols(); c++) {
// 检查对角线元素是否相等
if (r == c && R(0, 0) != R(r, c)) {
PRINT_ERROR(RED "StateHelper::initialize_invertible() - Your noise is not isotropic!\n" RESET);
PRINT_ERROR(RED "StateHelper::initialize_invertible() - Found a value of %.2f verses value of %.2f\n" RESET, R(r, c), R(0, 0));
throw std::runtime_error("StateHelper::initialize_invertible() - Noise is not isotropic!\n");
}
// 检查非对角线元素是否为零
else if (r != c && R(r, c) != 0.0) {
PRINT_ERROR(RED "StateHelper::initialize_invertible() - Your noise is not diagonal!\n" RESET);
PRINT_ERROR(RED "StateHelper::initialize_invertible() - Found a value of %.2f at row %d and column %d\n" RESET, R(r, c), r, c);
throw std::runtime_error("StateHelper::initialize_invertible() - Noise is not diagonal!\n");
}
}
}
//==========================================================
// 计算卡尔曼增益的一部分: K = (P*H^T)*S^{-1} = M*S^{-1}
// 这里先计算M_a = P*H_R^T
//==========================================================
assert(res.rows() == R.rows());
assert(H_L.rows() == res.rows());
assert(H_L.rows() == H_R.rows());
// 初始化M_a矩阵，用于存储P*H_R^T的结果
Eigen::MatrixXd M_a = Eigen::MatrixXd::Zero(state->_Cov.rows(), res.rows());
// 获取每个测量变量在小雅可比矩阵中的位置索引
int current_it = 0;
std::vector<int> H_id; // 存储每个变量在H矩阵中的起始索引
for (const auto &meas_var : H_order) {
H_id.push_back(current_it);
current_it += meas_var->size();
}
//==========================================================
// 计算每个活动变量对应的M_i = P_im * Hm^T
//==========================================================
for (const auto &var : state->_variables) {
// 为当前变量初始化M_i矩阵
Eigen::MatrixXd M_i = Eigen::MatrixXd::Zero(var->size(), res.rows());
// 遍历所有测量变量，计算累积效果
for (size_t i = 0; i < H_order.size(); i++) {
std::shared_ptr<Type> meas_var = H_order.at(i);
// M_i += P_im * Hm^T，其中P_im是var和meas_var之间的协方差块
M_i += state->_Cov.block(var->id(), meas_var->id(), var->size(), meas_var->size()) *
H_R.block(0, H_id[i], H_R.rows(), meas_var->size()).transpose();
}
// 将计算得到的M_i保存到对应的M_a块中
M_a.block(var->id(), 0, var->size(), res.rows()) = M_i;
}
//==========================================================
// 获取与H_order对应变量的边际协方差矩阵
//==========================================================
Eigen::MatrixXd P_small = StateHelper::get_marginal_covariance(state, H_order);
// 计算创新协方差矩阵 M = H_R*P_small*H_R' + R
// 使用上三角视图进行计算，利用矩阵对称性优化性能
Eigen::MatrixXd M(H_R.rows(), H_R.rows());
M.triangularView<Eigen::Upper>() = H_R * P_small * H_R.transpose();
M.triangularView<Eigen::Upper>() += R;
// 计算新变量的协方差 P_LL = H_L^{-1} * M * H_L^{-T}
assert(H_L.rows() == H_L.cols()); // 确保H_L是方阵
assert(H_L.rows() == new_variable->size()); // 确保维度匹配
Eigen::MatrixXd H_Linv = H_L.inverse(); // 计算H_L的逆矩阵
Eigen::MatrixXd P_LL = H_Linv * M.selfadjointView<Eigen::Upper>() * H_Linv.transpose(); // 利用自伴随视图计算
//==========================================================
// 扩展系统协方差矩阵，包含新变量的协方差
//==========================================================
size_t oldSize = state->_Cov.rows();
// 扩展协方差矩阵大小
state->_Cov.conservativeResizeLike(Eigen::MatrixXd::Zero(oldSize + new_variable->size(), oldSize + new_variable->size()));
// 设置新变量与旧变量之间的交叉协方差
state->_Cov.block(0, oldSize, oldSize, new_variable->size()).noalias() = -M_a * H_Linv.transpose();
// 利用对称性设置下三角部分
state->_Cov.block(oldSize, 0, new_variable->size(), oldSize) = state->_Cov.block(0, oldSize, oldSize, new_variable->size()).transpose();
// 设置新变量的自协方差
state->_Cov.block(oldSize, oldSize, new_variable->size(), new_variable->size()) = P_LL;
// 如果协方差对角线元素过大，输出警告信息（可能数值不稳定）
if (P_LL.diagonal()(0) > 1e10) {
out << "init fail: ";
out << "oldSize : " << oldSize;
out << " new_variable->size(): " << new_variable->size() << std::endl;
out << "H_L: \n" << H_L << std::endl;
out << "M: \n" << M << std::endl;
out << " P_LL: " << P_LL << std::endl;
}
// 更新新变量的值（可逆系统只能更新新变量）
// 如果已经使用条件高斯-牛顿法求解了初始估计，这里的更新应该几乎为零
new_variable->update(H_Linv * res);
// 设置新变量的本地ID并添加到状态变量列表中
new_variable->set_local_id(oldSize);
state->_variables.push_back(new_variable);
}
/**
* @brief 在状态中增加一个IMU姿态的克隆，并更新相关的协方差
*
* 该函数实现了MSCKF（Multi-State Constraint Kalman Filter）中的关键操作：
* 克隆当前IMU姿态并将其添加到状态中，用于后续的多状态视觉约束更新。
* 同时处理时间偏移校准的特殊情况，确保协方差矩阵的一致性。
*
* @param state 系统状态对象，包含当前所有状态变量
* @param last_w 最后一次IMU角速度测量值，用于时间偏移校准的雅可比计算
*/
void StateHelper::augment_clone(std::shared_ptr<State> state, Eigen::Matrix<double, 3, 1> last_w) {
// 检查是否已经存在相同时间戳的克隆，如果存在则抛出异常
// MSCKF要求不同时刻的克隆姿态，同一时间戳不允许有多个克隆
if (state->_clones_IMU.find(state->_timestamp) != state->_clones_IMU.end()) {
PRINT_ERROR(RED "TRIED TO INSERT A CLONE AT THE SAME TIME AS AN EXISTING CLONE, EXITING!#!@#!@#\n" RESET);
throw std::runtime_error("TRIED TO INSERT A CLONE AT THE SAME TIME AS AN EXISTING CLONE, EXITING!\n");
}
// 调用clone函数克隆当前IMU的姿态
// 注意：克隆的姿态会被添加到协方差矩阵的末尾
std::shared_ptr<Type> posetemp = StateHelper::clone(state, state->_imu->pose());
// 将克隆对象动态转换为PoseJPL类型并验证转换是否成功
std::shared_ptr<PoseJPL> pose = std::dynamic_pointer_cast<PoseJPL>(posetemp);
if (pose == nullptr) {
PRINT_ERROR(RED "INVALID OBJECT RETURNED FROM STATEHELPER CLONE, EXITING!#!@#!@#\n" RESET);
throw std::runtime_error("INVALID OBJECT RETURNED FROM STATEHELPER CLONE, EXITING!");
}
// 将新克隆的姿态添加到克隆IMU姿态的映射中，键为时间戳
state->_clones_IMU[state->_timestamp] = pose;
// 如果启用了相机时间偏移校准，则克隆姿态是时间偏移的函数
// 实现逻辑基于Mingyang Li和Anastasios I. Mourikis的论文：
// http://journals.sagepub.com/doi/pdf/10.1177/0278364913515286
if (state->_options.do_calib_camera_timeoffset) {
// 计算时间偏移对克隆姿态的雅可比矩阵dnc_dt
Eigen::Matrix<double, 6, 1> dnc_dt = Eigen::MatrixXd::Zero(6, 1);
// 旋转部分的雅可比由角速度决定
dnc_dt.block(0, 0, 3, 1) = last_w;
// 平移部分的雅可比由速度决定
dnc_dt.block(3, 0, 3, 1) = state->_imu->vel();
// 使用雅可比矩阵更新协方差矩阵，考虑时间偏移校准参数的影响
// 协方差更新公式: P_new = P_old + P_old_d*J^T + J*P_old_d + J*P_d*J^T
// 这里实现了前两项，利用矩阵对称性
state->_Cov.block(0, pose->id(), state->_Cov.rows(), 6) +=
state->_Cov.block(0, state->_calib_dt_CAMtoIMU->id(), state->_Cov.rows(), 1) * dnc_dt.transpose();
// 利用对称性更新下三角部分
state->_Cov.block(pose->id(), 0, 6, state->_Cov.rows()) +=
dnc_dt * state->_Cov.block(state->_calib_dt_CAMtoIMU->id(), 0, 1, state->_Cov.rows());
}
}
/**
* @brief 边缘化最旧的IMU克隆姿态以保持系统状态大小的合理限制
*
* 该函数实现了MSCKF（Multi-State Constraint Kalman Filter）框架中的重要机制：
* 当系统中维护的IMU克隆姿态数量超过预设的最大限制时，边缘化最旧的克隆姿态，
* 从而控制状态向量和协方差矩阵的大小，保证滤波器的计算效率。
*
* @param state 系统状态对象，包含当前所有状态变量、协方差矩阵以及IMU克隆姿态集合
*/
void StateHelper::marginalize_old_clone(std::shared_ptr<State> state) {
// 检查IMU克隆姿态的数量是否超过了配置的最大限制
if ((int)state->_clones_IMU.size() > state->_options.max_clone_size) {
// 获取应该被边缘化的最旧克隆姿态的时间戳
double marginal_time = state->margtimestep();
// 确保获取到的时间戳有效（不是无穷大）
assert(marginal_time != INFINITY);
// 调用marginalize函数边缘化该克隆姿态
// 边缘化操作会将克隆姿态从状态向量中移除，但会保留其对剩余状态的约束信息
StateHelper::marginalize(state, state->_clones_IMU.at(marginal_time));
// 注意：marginalize函数应该已经在内部处理了克隆姿态的删除
// 因此这里只需要从_clones_IMU映射表中移除对应的指针引用即可
state->_clones_IMU.erase(marginal_time);
}
}
/**
* @brief 边缘化指定时间点的IMU克隆状态
*
* 此函数用于边缘化(边缘化是EKF中移除状态变量但保留其信息的技术)系统状态中特定时间点的IMU克隆姿态。
* 边缘化操作会从系统状态中移除指定的克隆状态，同时通过协方差矩阵操作保留其信息贡献。
*
* @param[in] state 系统状态对象，包含所有当前状态变量、协方差矩阵及IMU克隆姿态集合
* @param[in] time_to_marg 需要边缘化的IMU克隆姿态的时间戳
*
* @note 当time_to_marg为-1.0时，表示不需要边缘化任何克隆状态
* @note 该函数在MSCKF视觉惯性导航系统中用于灵活控制状态向量大小
*/
void StateHelper::marginalize_clone(std::shared_ptr<State> state, double time_to_marg) {
// 检查是否需要执行边缘化操作
if (time_to_marg != -1.0) {
// 设置边缘化时间点
double marginal_time = time_to_marg;
// 调用通用边缘化函数执行边缘化操作
StateHelper::marginalize(state, state->_clones_IMU.at(marginal_time));
// 注意：边缘化器已在marginalize函数中删除了克隆状态
// 因此这里只需要从状态的克隆映射表中移除对应的指针
state->_clones_IMU.erase(marginal_time);
}
}
/**
* @brief 边缘化SLAM系统中标记为需边缘化的特征点
*
* 此函数用于移除SLAM系统中那些被标记为需要边缘化的特征点，同时保留ARUCO标签特征点（如果存在）。
* 通过边缘化操作，系统能够控制状态向量的大小，在保持必要信息的同时优化计算效率。
*
* @param[in] state 系统状态对象，包含所有当前SLAM特征点及相关信息
*
* @note 函数会排除ARUCO标签特征点，确保它们不被边缘化
* @note 边缘化操作会将特征点从状态向量中移除，但通过修改协方差矩阵保留其约束信息
*/
void StateHelper::marginalize_slam(std::shared_ptr<State> state) {
// 移除标记了边缘化标志的SLAM特征点
// 同时确保不移除任何ARUCO标签特征点
int ct_marginalized = 0; // 记录边缘化的特征点数量
auto it0 = state->_features_SLAM.begin(); // 获取SLAM特征点集合的迭代器
// 遍历所有SLAM特征点
while (it0 != state->_features_SLAM.end()) {
// 检查特征点是否被标记为需要边缘化，且不是ARUCO标签特征点
// 注：ARUCO标签特征点通常有较低的ID值，通过ID范围判断来保护它们
if ((*it0).second->should_marg && (int)(*it0).first > 4 * state->_options.max_aruco_features) {
// 执行边缘化操作，从状态中移除特征点但保留其约束信息
StateHelper::marginalize(state, (*it0).second);
// 从特征点集合中删除该特征点并更新迭代器
it0 = state->_features_SLAM.erase(it0);
// 更新边缘化计数
ct_marginalized++;
} else {
// 特征点不需要边缘化，移动到下一个特征点
it0++;
}
}
}
void StateHelper::save_landmark_state(double timestamp,
std::shared_ptr<Feature> feat,
const std::vector<std::map<double, FeatureInitializer::ClonePose>>& clones_cam,
std::string suffix)
{
char path[200] = "E:\\git_repo\\slam-core\\build\\Release\\debug\\trig\\";
char filename[500];
sprintf(filename, "%s%.4f_%d_%s.json", path, timestamp, (int)feat->featid, suffix.c_str());
std::ofstream fout(filename);
fout << "{\n\t \"clones_cam\": [\n";
int cam_clones_size = 0;
for (size_t cam_id = 0; cam_id < clones_cam.size(); ++cam_id) {
fout << "\t\t{\n\t\t\t\"cam\": " << cam_id << ",\n";
fout << "\t\t\t\"clones\": [\n";
int times_size = 0;
for (auto &clone : clones_cam[cam_id]) {
fout << "\t\t\t\t{\n\t\t\t\t\t\"timestamp\": " << std::setprecision(15) << clone.first << ",\n";
fout << "\t\t\t\t\t\"T\": [\n";
auto R = clone.second._Rot.inverse();
auto t = clone.second._pos;
for (int i = 0; i < 3; i++) {
fout << "\t\t\t\t\t\t[";
for (int j = 0; j < 3; j++) {
fout << R(i, j) << ",";
}
fout << t(i) << "],\n";
}
times_size++;
fout << "\t\t\t\t\t\t[0, 0, 0, 1]\n\t\t\t\t\t]\n\t\t\t\t}";
if (times_size < clones_cam[cam_id].size()) { fout << ","; }
fout << "\n";
//fout << "R\n" << R << "\n";
//fout << "t\n" << t << "\n\n";
}
cam_clones_size++;
fout << "\t\t\t]\n\t\t}";
if (cam_clones_size < clones_cam.size()) { fout << ","; }
fout << "\n";
}
fout << "\t],\n";
fout << "\t\"feat\": [\n";
int cam_feat_size = 0;
size_t valid_cam_size = 0;
for (size_t cam_id = 0; cam_id < feat->timestamps.size(); ++cam_id) {
if (!feat->timestamps[cam_id].empty()) ++valid_cam_size;
}
const std::vector<std::vector<double>>& ts_all = feat->timestamps;
const std::vector<std::vector<Eigen::Vector2f>>& uvs_all = feat->uvs;
const std::vector<std::vector<Eigen::Vector2f>>& uvs_n_all = feat->uvs_norm;
for (size_t cam_id = 0; cam_id < ts_all.size(); ++cam_id) {
const std::vector<double>& ts = ts_all[cam_id];
const std::vector<Eigen::Vector2f>& uvs = uvs_all[cam_id];
const std::vector<Eigen::Vector2f>& uvs_n = uvs_n_all[cam_id];
// skip current loop for there is no observation
if (ts.empty()) continue;
fout << "\t\t{\n\t\t\t\"cam\": " << cam_id << ",\n";
fout << "\t\t\t\"observation\": [\n";
for (size_t i = 0; i < ts.size(); i++) {
fout << "\t\t\t\t{\n\t\t\t\t\t\"timestamp\": " << std::setprecision(15) << ts[i] << ",\n";
auto uv = uvs[i];
auto uv_norm = uvs_n[i];
fout << "\t\t\t\t\t\"uvs\": [\n";
fout << "\t\t\t\t\t\t" << uv[0] << ",\n\t\t\t\t\t\t" << uv[1] << "\n\t\t\t\t\t],\n";
fout << "\t\t\t\t\t\"uv_norms\": [\n";
fout << "\t\t\t\t\t\t" << uv_norm[0] << ",\n\t\t\t\t\t\t" << uv_norm[1] << "\n\t\t\t\t\t]\n";
fout << "\t\t\t\t }";
if (i != ts.size() - 1) { fout << ","; }
fout << "\n";
}
cam_feat_size++;
fout << "\t\t\t]\n\t\t}";
if (cam_feat_size < valid_cam_size) { fout << ","; }
fout << "\n";
}
fout << "\t]\n}\n";
}