【MSCKF】UpdaterHelper 学习备注

UpdaterHelper 学习备注

• • • 一、特征点参数化与雅可比矩阵推导
    • • [1. 全局3D表示（`GLOBAL_3D`）](#1. 全局3D表示（GLOBAL_3D）)
      • [2. 全局逆深度表示`GLOBAL_FULL_INVERSE_DEPTH`](#2. 全局逆深度表示GLOBAL_FULL_INVERSE_DEPTH)
      • • （1）参数到全局坐标的转换
        • （2）雅可比矩阵推导
      • [3. 锚点坐标系下的表示](#3. 锚点坐标系下的表示)
      • • （1）锚点3D表示（`ANCHORED_3D`）
        • [（2）锚点完整逆深度 `ANCHORED_FULL_INVERSE_DEPTH`](#（2）锚点完整逆深度 ANCHORED_FULL_INVERSE_DEPTH)
        • [（3）MSCKF风格逆深度 `ANCHORED_MSCKF_INVERSE_DEPTH`](#（3）MSCKF风格逆深度 ANCHORED_MSCKF_INVERSE_DEPTH)
        • [（4）单逆深度表示 `ANCHORED_INVERSE_DEPTH_SINGLE`](#（4）单逆深度表示 ANCHORED_INVERSE_DEPTH_SINGLE)
    • 二、重投影误差与残差计算
    • • [1. 坐标转换链](#1. 坐标转换链)
      • [2. 投影与畸变](#2. 投影与畸变)
      • [3. Huber鲁棒核](#3. Huber鲁棒核)
    • 三、雅可比矩阵的链式组合
    • [四、零空间投影（Nullspace Projection）](#四、零空间投影（Nullspace Projection）)
    • • [1. 数学原理](#1. 数学原理)
      • [2. 算法实现](#2. 算法实现)
    • [五、测量压缩（Measurement Compression）](#五、测量压缩（Measurement Compression）)
    • • [1. 数学原理](#1. 数学原理)
      • [2. 算法实现](#2. 算法实现)
    • 六、算法流程梳理

一、特征点参数化与雅可比矩阵推导

特征点的不同参数化方式决定了其雅可比矩阵的形式，代码中支持全局3D 、全局逆深度 、锚点3D 、锚点逆深度（含MSCKF风格）等表示。

1. 全局3D表示（GLOBAL_3D）

特征点直接用全局坐标系下的笛卡尔坐标表示：
p F G = [ x , y , z ] T p_F^G = [x, y, z]^T pFG=[x,y,z]T

雅可比矩阵为单位矩阵（特征点参数即全局坐标本身）：
H f = ∂ p F G ∂ p F G = I 3 × 3 H_f = \frac{\partial p_F^G}{\partial p_F^G} = I_{3 \times 3} Hf=∂pFG∂pFG=I3×3

2. 全局逆深度表示GLOBAL_FULL_INVERSE_DEPTH

参数化： λ = [ θ , ϕ , ρ ] T \lambda = [\theta, \phi, \rho]^T λ=[θ,ϕ,ρ]T，其中：

• θ = arctan ⁡ 2 ( y , x ) \theta = \arctan2(y, x) θ=arctan2(y,x)：方位角（x-y平面投影与x轴夹角）
• ϕ = arccos ⁡ ( z / ∥ p F G ∥ ) \phi = \arccos(z/\|p_F^G\|) ϕ=arccos(z/∥pFG∥)：极角（与z轴夹角）
• ρ = 1 / ∥ p F G ∥ \rho = 1/\|p_F^G\| ρ=1/∥pFG∥：逆深度

（1）参数到全局坐标的转换

球坐标转笛卡尔坐标（ r = 1 / ρ r = 1/\rho r=1/ρ为特征点到原点的距离）：
p F G = 1 ρ [ cos ⁡ θ sin ⁡ ϕ sin ⁡ θ sin ⁡ ϕ cos ⁡ ϕ ] p_F^G = \frac{1}{\rho} \begin{bmatrix} \cos\theta \sin\phi \\ \sin\theta \sin\phi \\ \cos\phi \end{bmatrix} pFG=ρ1 cosθsinϕsinθsinϕcosϕ

（2）雅可比矩阵推导

对参数 λ \lambda λ求偏导，组合为雅可比矩阵 H f = [ ∂ p F G ∂ θ , ∂ p F G ∂ ϕ , ∂ p F G ∂ ρ ] H_f = \left[ \frac{\partial p_F^G}{\partial \theta}, \frac{\partial p_F^G}{\partial \phi}, \frac{\partial p_F^G}{\partial \rho} \right] Hf=[∂θ∂pFG,∂ϕ∂pFG,∂ρ∂pFG]：

• 对 θ \theta θ的偏导：
  ∂ p F G ∂ θ = 1 ρ [ − sin ⁡ θ sin ⁡ ϕ cos ⁡ θ sin ⁡ ϕ 0 ] \frac{\partial p_F^G}{\partial \theta} = \frac{1}{\rho} \begin{bmatrix} -\sin\theta \sin\phi \\ \cos\theta \sin\phi \\ 0 \end{bmatrix} ∂θ∂pFG=ρ1 −sinθsinϕcosθsinϕ0

• 对 ϕ \phi ϕ的偏导：
  ∂ p F G ∂ ϕ = 1 ρ [ cos ⁡ θ cos ⁡ ϕ sin ⁡ θ cos ⁡ ϕ − sin ⁡ ϕ ] \frac{\partial p_F^G}{\partial \phi} = \frac{1}{\rho} \begin{bmatrix} \cos\theta \cos\phi \\ \sin\theta \cos\phi \\ -\sin\phi \end{bmatrix} ∂ϕ∂pFG=ρ1 cosθcosϕsinθcosϕ−sinϕ

• 对 ρ \rho ρ的偏导：
  ∂ p F G ∂ ρ = − 1 ρ 2 [ cos ⁡ θ sin ⁡ ϕ sin ⁡ θ sin ⁡ ϕ cos ⁡ ϕ ] \frac{\partial p_F^G}{\partial \rho} = -\frac{1}{\rho^2} \begin{bmatrix} \cos\theta \sin\phi \\ \sin\theta \sin\phi \\ \cos\phi \end{bmatrix} ∂ρ∂pFG=−ρ21 cosθsinϕsinθsinϕcosϕ

展开后得到代码中的雅可比矩阵：
H f = [ − sin ⁡ θ sin ⁡ ϕ ρ cos ⁡ θ cos ⁡ ϕ ρ − cos ⁡ θ sin ⁡ ϕ ρ 2 cos ⁡ θ sin ⁡ ϕ ρ sin ⁡ θ cos ⁡ ϕ ρ − sin ⁡ θ sin ⁡ ϕ ρ 2 0 − sin ⁡ ϕ ρ − cos ⁡ ϕ ρ 2 ] H_f = \begin{bmatrix} -\frac{\sin\theta \sin\phi}{\rho} & \frac{\cos\theta \cos\phi}{\rho} & -\frac{\cos\theta \sin\phi}{\rho^2} \\ \frac{\cos\theta \sin\phi}{\rho} & \frac{\sin\theta \cos\phi}{\rho} & -\frac{\sin\theta \sin\phi}{\rho^2} \\ 0 & -\frac{\sin\phi}{\rho} & -\frac{\cos\phi}{\rho^2} \end{bmatrix} Hf= −ρsinθsinϕρcosθsinϕ0ρcosθcosϕρsinθcosϕ−ρsinϕ−ρ2cosθsinϕ−ρ2sinθsinϕ−ρ2cosϕ

3. 锚点坐标系下的表示

锚点为某帧相机 C a C_a Ca，特征点在 C a C_a Ca下的坐标为 p F C a p_F^{C_a} pFCa，需转换到全局坐标系 p F G p_F^G pFG：
p F G = R I a G ( R C a I a ( p F C a − p I a C a ) ) + p I a G p_F^G = R_{I_a}^G (R_{C_a}^{I_a} (p_F^{C_a} - p_{I_a}^{C_a}) ) + p_{I_a}^G pFG=RIaG(RCaIa(pFCa−pIaCa))+pIaG

其中：

• R C a I a R_{C_a}^{I_a} RCaIa：IMU到锚点相机的旋转矩阵， p I a C a p_{I_a}^{C_a} pIaCa：IMU在锚点相机中的位置；
• R I a G R_{I_a}^G RIaG：全局到锚点IMU的旋转矩阵， p I a G p_{I_a}^G pIaG：锚点IMU的全局位置。

记 R C G = R I a G R C a I a R_C^G = R_{I_a}^G R_{C_a}^{I_a} RCG=RIaGRCaIa（锚点相机到全局的旋转），则 p F G = R C G ( p F C a − p I a C a ) + p I a G p_F^G = R_C^G (p_F^{C_a} - p_{I_a}^{C_a}) + p_{I_a}^G pFG=RCG(pFCa−pIaCa)+pIaG。

（1）锚点3D表示（ANCHORED_3D）

特征参数为 p F C a p_F^{C_a} pFCa，雅可比矩阵为：
H f = ∂ p F G ∂ p F C a = R C G H_f = \frac{\partial p_F^G}{\partial p_F^{C_a}} = R_C^G Hf=∂pFCa∂pFG=RCG

（2）锚点完整逆深度 ANCHORED_FULL_INVERSE_DEPTH

参数为锚点相机下的逆深度 λ a = [ θ a , ϕ a , ρ a ] T \lambda_a = [\theta_a, \phi_a, \rho_a]^T λa=[θa,ϕa,ρa]T，转换关系与全局逆深度一致：
p F C a = 1 ρ a [ cos ⁡ θ a sin ⁡ ϕ a sin ⁡ θ a sin ⁡ ϕ a cos ⁡ ϕ a ] p_F^{C_a} = \frac{1}{\rho_a} \begin{bmatrix} \cos\theta_a \sin\phi_a \\ \sin\theta_a \sin\phi_a \\ \cos\phi_a \end{bmatrix} pFCa=ρa1 cosθasinϕasinθasinϕacosϕa

雅可比矩阵为全局坐标对锚点参数的偏导（链式法则）：
H f = ∂ p F G ∂ λ a = ∂ p F G ∂ p F C a ⋅ ∂ p F C a ∂ λ a = R C G ⋅ J λ a H_f = \frac{\partial p_F^G}{\partial \lambda_a} = \frac{\partial p_F^G}{\partial p_F^{C_a}} \cdot \frac{\partial p_F^{C_a}}{\partial \lambda_a} = R_C^G \cdot J_{\lambda_a} Hf=∂λa∂pFG=∂pFCa∂pFG⋅∂λa∂pFCa=RCG⋅Jλa

其中 J λ a J_{\lambda_a} Jλa是锚点相机下逆深度参数的雅可比（与全局逆深度形式相同）。

（3）MSCKF风格逆深度 ANCHORED_MSCKF_INVERSE_DEPTH

参数为 λ m = [ α , β , ρ ] T \lambda_m = [\alpha, \beta, \rho]^T λm=[α,β,ρ]T，其中 α = x / z \alpha = x/z α=x/z， β = y / z \beta = y/z β=y/z， ρ = 1 / z \rho = 1/z ρ=1/z（锚点相机下的z分量逆）。转换关系：
p F C a = 1 ρ [ α β 1 ] p_F^{C_a} = \frac{1}{\rho} \begin{bmatrix} \alpha \\ \beta \\ 1 \end{bmatrix} pFCa=ρ1 αβ1

对 λ m \lambda_m λm的偏导：
∂ p F C a ∂ λ m = [ 1 ρ 0 − α ρ 2 0 1 ρ − β ρ 2 0 0 − 1 ρ 2 ] \frac{\partial p_F^{C_a}}{\partial \lambda_m} = \begin{bmatrix} \frac{1}{\rho} & 0 & -\frac{\alpha}{\rho^2} \\ 0 & \frac{1}{\rho} & -\frac{\beta}{\rho^2} \\ 0 & 0 & -\frac{1}{\rho^2} \end{bmatrix} ∂λm∂pFCa= ρ1000ρ10−ρ2α−ρ2β−ρ21

最终雅可比：
H f = R C G ⋅ ∂ p F C a ∂ λ m H_f = R_C^G \cdot \frac{\partial p_F^{C_a}}{\partial \lambda_m} Hf=RCG⋅∂λm∂pFCa

（4）单逆深度表示 ANCHORED_INVERSE_DEPTH_SINGLE

仅参数化锚点相机下的逆深度 ρ = 1 / z \rho = 1/z ρ=1/z，方向向量 u = [ x / z , y / z , 1 ] T \mathbf{u} = [x/z, y/z, 1]^T u=[x/z,y/z,1]T固定，因此：
p F C a = 1 ρ u p_F^{C_a} = \frac{1}{\rho} \mathbf{u} pFCa=ρ1u

偏导：
∂ p F C a ∂ ρ = − 1 ρ 2 u = − p F C a ρ \frac{\partial p_F^{C_a}}{\partial \rho} = -\frac{1}{\rho^2} \mathbf{u} = -\frac{p_F^{C_a}}{\rho} ∂ρ∂pFCa=−ρ21u=−ρpFCa

最终雅可比：
H f = R C G ⋅ ∂ p F C a ∂ ρ H_f = R_C^G \cdot \frac{\partial p_F^{C_a}}{\partial \rho} Hf=RCG⋅∂ρ∂pFCa

二、重投影误差与残差计算

重投影误差是特征点观测坐标与预测坐标的差值，核心步骤如下：

1. 坐标转换链

全局坐标 p F G p_F^G pFG → 当前IMU坐标 p F I i p_F^{I_i} pFIi → 当前相机坐标 p F C i p_F^{C_i} pFCi：
p F I i = R I i G ( p F G − p I i G ) p_F^{I_i} = R_{I_i}^G (p_F^G - p_{I_i}^G) pFIi=RIiG(pFG−pIiG)
p F C i = R C i I i p F I i + p I i C i p_F^{C_i} = R_{C_i}^{I_i} p_F^{I_i} + p_{I_i}^{C_i} pFCi=RCiIipFIi+pIiCi

2. 投影与畸变

• 归一化投影： u n o r m = [ x z , y z ] T \mathbf{u}_{norm} = \left[ \frac{x}{z}, \frac{y}{z} \right]^T unorm=[zx,zy]T（ x = p F C i ( 0 ) , y = p F C i ( 1 ) , z = p F C i ( 2 ) x=p_F^{C_i}(0), y=p_F^{C_i}(1), z=p_F^{C_i}(2) x=pFCi(0),y=pFCi(1),z=pFCi(2)）；
• 畸变矫正： u d i s t = D ( u n o r m , ζ ) \mathbf{u}{dist} = D(\mathbf{u}{norm}, \zeta) udist=D(unorm,ζ)，其中 ζ \zeta ζ为相机内参（含畸变参数）， D D D为畸变函数；
• 残差： r = u m e a s − u d i s t r = \mathbf{u}{meas} - \mathbf{u}{dist} r=umeas−udist。

3. Huber鲁棒核

若残差 ∥ r ∥ 2 > δ \|r\|^2 > \delta ∥r∥2>δ（ δ = 1. 5 2 \delta = 1.5^2 δ=1.52），应用Huber核缩放残差和雅可比：
r ′ = δ ∥ r ∥ ⋅ r r' = \sqrt{\frac{\delta}{\|r\|}} \cdot r r′=∥r∥δ ⋅r
H ′ = δ ∥ r ∥ ⋅ H H' = \sqrt{\frac{\delta}{\|r\|}} \cdot H H′=∥r∥δ ⋅H

三、雅可比矩阵的链式组合

残差对状态变量的雅可比需通过链式法则组合各环节的偏导，核心分解如下：

残差 r r r对特征参数 λ \lambda λ的雅可比：
H f = ∂ r ∂ λ = ∂ r ∂ u d i s t ⏟ − I 2 × 2 ⋅ ∂ u d i s t ∂ u n o r m ⏟ J D ⋅ ∂ u n o r m ∂ p F C i ⏟ J Π ⋅ ∂ p F C i ∂ p F G ⏟ R C i G ⋅ ∂ p F G ∂ λ ⏟ d p f g _ d l a m b d a H_f = \frac{\partial r}{\partial \lambda} = \underbrace{\frac{\partial r}{\partial \mathbf{u}{dist}}}{-I_{2×2}} \cdot \underbrace{\frac{\partial \mathbf{u}{dist}}{\partial \mathbf{u}{norm}}}{J_D} \cdot \underbrace{\frac{\partial \mathbf{u}{norm}}{\partial p_F^{C_i}}}{J\Pi} \cdot \underbrace{\frac{\partial p_F^{C_i}}{\partial p_F^G}}{R{C_i}^G} \cdot \underbrace{\frac{\partial p_F^G}{\partial \lambda}}_{dpfg\_dlambda} Hf=∂λ∂r=−I2×2 ∂udist∂r⋅JD ∂unorm∂udist⋅JΠ ∂pFCi∂unorm⋅RCiG ∂pFG∂pFCi⋅dpfg_dlambda ∂λ∂pFG

残差 r r r对系统状态 x x x（IMU位姿、相机标定）的雅可比：
H x = ∂ r ∂ x = − J D ⋅ J Π ⋅ ( ∂ p F C i ∂ x + R C i G ⋅ ∂ p F G ∂ x ) H_x = \frac{\partial r}{\partial x} = -J_D \cdot J_\Pi \cdot \left( \frac{\partial p_F^{C_i}}{\partial x} + R_{C_i}^G \cdot \frac{\partial p_F^G}{\partial x} \right) Hx=∂x∂r=−JD⋅JΠ⋅(∂x∂pFCi+RCiG⋅∂x∂pFG)

其中关键环节的雅可比：

• 投影雅可比 J Π J_\Pi JΠ：
  J Π = [ 1 z 0 − x z 2 0 1 z − y z 2 ] J_\Pi = \begin{bmatrix} \frac{1}{z} & 0 & -\frac{x}{z^2} \\ 0 & \frac{1}{z} & -\frac{y}{z^2} \end{bmatrix} JΠ=[z100z1−z2x−z2y]

• 当前IMU位姿的雅可比 ∂ p F C i ∂ ξ I i \frac{\partial p_F^{C_i}}{\partial \xi_{I_i}} ∂ξIi∂pFCi（ ξ I i = [ ϕ I i , p I i G ] T \xi_{I_i} = [\phi_{I_i}, p_{I_i}^G]^T ξIi=[ϕIi,pIiG]T，6维）：
  ∂ p F C i ∂ ξ I i = [ R C i I i [ p F I i ] × − R C i I i R I i G ] \frac{\partial p_F^{C_i}}{\partial \xi_{I_i}} = \begin{bmatrix} R_{C_i}^{I_i} [p_F^{I_i}]\times & -R{C_i}^{I_i} R_{I_i}^G \end{bmatrix} ∂ξIi∂pFCi=[RCiIi[pFIi]×−RCiIiRIiG]

• 相机外参的雅可比 ∂ p F C i ∂ ξ C i \frac{\partial p_F^{C_i}}{\partial \xi_{C_i}} ∂ξCi∂pFCi（ ξ C i = [ ϕ C i , p I i C i ] T \xi_{C_i} = [\phi_{C_i}, p_{I_i}^{C_i}]^T ξCi=[ϕCi,pIiCi]T，6维）：
  ∂ p F C i ∂ ξ C i = [ [ p F C i − p I i C i ] × I 3 × 3 ] \frac{\partial p_F^{C_i}}{\partial \xi_{C_i}} = \begin{bmatrix} [p_F^{C_i} - p_{I_i}^{C_i}]\times & I{3×3} \end{bmatrix} ∂ξCi∂pFCi=[[pFCi−pIiCi]×I3×3]

四、零空间投影（Nullspace Projection）

1. 数学原理

优化目标为 min ⁡ x ∥ r ( x , f ) ∥ 2 \min_x \|r(x, f)\|^2 minx∥r(x,f)∥2，雅可比分块为 J = [ H x , H f ] J = [H_x, H_f] J=[Hx,Hf]。为消去特征参数 f f f，将残差和雅可比投影到 H f H_f Hf的左零空间 （满足 v T H f = 0 v^T H_f = 0 vTHf=0的向量空间）。

对 H f H_f Hf做QR分解： H f = Q [ R 0 ] H_f = Q \begin{bmatrix} R \\ 0 \end{bmatrix} Hf=Q[R0]，其中 Q = [ Q 1 , Q 2 ] Q = [Q_1, Q_2] Q=[Q1,Q2]， Q 2 Q_2 Q2为左零空间基（ Q 2 T H f = 0 Q_2^T H_f = 0 Q2THf=0）。投影后：
r ′ = Q 2 T r , J ′ = Q 2 T H x r' = Q_2^T r, \quad J' = Q_2^T H_x r′=Q2Tr,J′=Q2THx

2. 算法实现

代码中通过Givens旋转 对 H f H_f Hf做QR分解，保留 Q 2 T Q_2^T Q2T对应的行（残差维度 m m m减去 f f f的维度 n n n），得到仅关于 x x x的优化问题。

五、测量压缩（Measurement Compression）

1. 数学原理

当残差维度 m > m > m>状态维度 n n n时， H x H_x Hx的秩最大为 n n n。对 H x H_x Hx做QR分解： H x = Q [ R 0 ] H_x = Q \begin{bmatrix} R \\ 0 \end{bmatrix} Hx=Q[R0]，取前 n n n行（ Q 1 T Q_1^T Q1T）压缩残差：
r ′ = Q 1 T r , J ′ = R r' = Q_1^T r, \quad J' = R r′=Q1Tr,J′=R

2. 算法实现

通过Givens旋转对 H x H_x Hx做QR分解，保留前 n n n行，将残差维度从 m m m降至 n n n，降低优化计算量。

六、算法流程梳理

1. 特征参数化选择 ：根据配置选择特征点的参数化方式，计算其到全局坐标的雅可比 H f H_f Hf；
2. 残差计算：遍历所有观测帧，将特征点投影到图像平面，计算重投影残差；
3. 雅可比组合：通过链式法则计算残差对特征参数和系统状态的雅可比；
4. 零空间投影：消去特征参数，得到仅关于系统状态的优化问题；
5. 测量压缩：降低残差维度，提升优化效率；
6. 非线性优化：最小化投影后的残差，更新系统状态。

该代码实现了SLAM后端中特征点参数化、雅可比计算、约束投影的核心逻辑，通过FEJ（First Estimate Jacobian）保持线性化点一致性，结合鲁棒核和零空间投影提升优化的稳定性与效率。

#ifndef OV_MSCKF_UPDATER_HELPER_H
#define OV_MSCKF_UPDATER_HELPER_H

#include <Eigen/Eigen>
#include <memory>

#include "common/types/LandmarkRepresentation.h"


class Type;




class State;

/**
 * @brief Class that has helper functions for our updaters.
 *
 * Can compute the Jacobian for a single feature representation.
 * This will create the Jacobian based on what representation our state is in.
 * If we are using the anchor representation then we also have additional Jacobians in respect to the anchor state.
 * Also has functions such as nullspace projection and full jacobian construction.
 * For derivations look at @ref update-feat page which has detailed equations.
 *
 */
class UpdaterHelper {
public:
  /**
   * @brief Feature object that our UpdaterHelper leverages, has all measurements and means
   */
  struct UpdaterHelperFeature {

    /// Unique ID of this feature
    size_t featid;

    /// UV coordinates that this feature has been seen from (mapped by camera ID)
    std::vector<std::vector<Eigen::Vector2f>> uvs;

    // UV normalized coordinates that this feature has been seen from (mapped by camera ID)
    std::vector<std::vector<Eigen::Vector2f>> uvs_norm;

    /// Timestamps of each UV measurement (mapped by camera ID)
    std::vector<std::vector<double>> timestamps;

    /// What representation our feature is in
    LandmarkRepresentation::Representation feat_representation;

    /// What camera ID our pose is anchored in!! By default the first measurement is the anchor.
    int anchor_cam_id = -1;

    /// Timestamp of anchor clone
    double anchor_clone_timestamp = -1;

    /// Triangulated position of this feature, in the anchor frame
    Eigen::Vector3d p_FinA;

    /// Triangulated position of this feature, in the anchor frame first estimate
    Eigen::Vector3d p_FinA_fej;

    /// Triangulated position of this feature, in the global frame
    Eigen::Vector3d p_FinG;

    /// Triangulated position of this feature, in the global frame first estimate
    Eigen::Vector3d p_FinG_fej;
  };

  /**
   * @brief This gets the feature and state Jacobian in respect to the feature representation
   *
   * @param[in] state State of the filter system
   * @param[in] feature Feature we want to get Jacobians of (must have feature means)
   * @param[out] H_f Jacobians in respect to the feature error state (will be either 3x3 or 3x1 for single depth)
   * @param[out] H_x Extra Jacobians in respect to the state (for example anchored pose)
   * @param[out] x_order Extra variables our extra Jacobian has (for example anchored pose)
   */
  static void get_feature_jacobian_representation(std::shared_ptr<State> state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f,
                                                  std::vector<Eigen::MatrixXd> &H_x, std::vector<std::shared_ptr<Type>> &x_order);

  /**
   * @brief Will construct the "stacked" Jacobians for a single feature from all its measurements
   *
   * @param[in] state State of the filter system
   * @param[in] feature Feature we want to get Jacobians of (must have feature means)
   * @param[out] H_f Jacobians in respect to the feature error state
   * @param[out] H_x Extra Jacobians in respect to the state (for example anchored pose)
   * @param[out] res Measurement residual for this feature
   * @param[out] x_order Extra variables our extra Jacobian has (for example anchored pose)
   */
  static void get_feature_jacobian_full(std::shared_ptr<State> state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f,
                                        Eigen::MatrixXd &H_x, Eigen::VectorXd &res, std::vector<std::shared_ptr<Type>> &x_order);

  /**
   * @brief This will project the left nullspace of H_f onto the linear system.
   *
   * Please see the @ref update-null for details on how this works.
   * This is the MSCKF nullspace projection which removes the dependency on the feature state.
   * Note that this is done **in place** so all matrices will be different after a function call.
   *
   * @param H_f Jacobian with nullspace we want to project onto the system [res = Hx*(x-xhat)+Hf(f-fhat)+n]
   * @param H_x State jacobian
   * @param res Measurement residual
   */
  static void nullspace_project_inplace(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res);

  /**
   * @brief This will perform measurement compression
   *
   * Please see the @ref update-compress for details on how this works.
   * Note that this is done **in place** so all matrices will be different after a function call.
   *
   * @param H_x State jacobian
   * @param res Measurement residual
   */
  static void measurement_compress_inplace(Eigen::MatrixXd &H_x, Eigen::VectorXd &res);
};



#endif // OV_MSCKF_UPDATER_HELPER_H

#include "UpdaterHelper.h"

#include "common/state/State.h"

#include "common/utils/quat_ops.h"



/**
 * @brief 计算特征点在不同表示形式下的雅可比矩阵
 * 
 * 此函数根据特征点的不同参数化表示形式，计算相应的雅可比矩阵，用于SLAM后端的非线性优化。
 * 支持全局3D坐标、全局逆深度、锚点坐标系下的3D坐标和多种逆深度参数化形式。
 * 
 * @param state 系统状态共享指针，包含IMU状态、相机标定等信息
 * @param feature 当前处理的特征点对象
 * @param H_f 输出参数，特征点参数相对于全局坐标系的雅可比矩阵
 * @param H_x 输出参数，特征点相对于系统状态变量的雅可比矩阵向量
 * @param x_order 输出参数，状态变量的顺序列表，与H_x中的雅可比矩阵一一对应
 */
void UpdaterHelper::get_feature_jacobian_representation(std::shared_ptr<State> state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f,
                                                        std::vector<Eigen::MatrixXd> &H_x, std::vector<std::shared_ptr<Type>> &x_order) {

  // Global XYZ representation
  if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_3D) {
    H_f.resize(3, 3);
    H_f.setIdentity();  // 对于全局3D表示，雅可比矩阵是单位矩阵
    return;
  }

  // Global inverse depth representation
  if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_FULL_INVERSE_DEPTH) {

    // 获取特征点的线性化点（FEJ或当前估计值）
    Eigen::Matrix<double, 3, 1> p_FinG = (state->_options.do_fej) ? feature.p_FinG_fej : feature.p_FinG;

    // 计算逆深度参数表示（方位角、极角和逆深度）
    double g_rho = 1 / p_FinG.norm();       // 逆深度
    double g_phi = std::acos(g_rho * p_FinG(2));  // 极角（相对于z轴的角度）
    double g_theta = std::atan2(p_FinG(1), p_FinG(0));  // 方位角（相对于x轴的角度）
    Eigen::Matrix<double, 3, 1> p_invFinG;  // 逆深度参数向量
    p_invFinG(0) = g_theta;  // 方位角
    p_invFinG(1) = g_phi;    // 极角
    p_invFinG(2) = g_rho;    // 逆深度

    // 计算三角函数值以构建雅可比矩阵
    double sin_th = std::sin(p_invFinG(0, 0));  // 方位角的正弦
    double cos_th = std::cos(p_invFinG(0, 0));  // 方位角的余弦
    double sin_phi = std::sin(p_invFinG(1, 0));  // 极角的正弦
    double cos_phi = std::cos(p_invFinG(1, 0));  // 极角的余弦
    double rho = p_invFinG(2, 0);  // 逆深度值

    // 构建全局逆深度参数化的雅可比矩阵
    // 雅可比矩阵表示全局3D坐标相对于逆深度参数的导数
    H_f.resize(3, 3);
    H_f << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
        (1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
        -(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
    return;
  }

  //======================================================================
  // 以下代码处理基于锚点的特征表示
  //======================================================================

  // 确保特征点有有效的锚点相机ID
  assert(feature.anchor_cam_id != -1);

  // 获取锚点相机的变换参数和相机标定信息
  Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->Rot();  // IMU到相机的旋转矩阵
  Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->pos();  // IMU在相机坐标系中的位置
  Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot();  // 全局坐标系到IMU的旋转矩阵
  Eigen::Vector3d p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos();  // IMU在全局坐标系中的位置
  Eigen::Vector3d p_FinA = feature.p_FinA;  // 特征点在锚点坐标系中的位置

  // 如果启用FEJ（First Estimate Jacobian）技术
  if (state->_options.do_fej) {
    // 计算特征点在全局坐标系中的最佳估计位置（使用当前状态估计）
    // 变换步骤：锚点相机坐标系 -> IMU坐标系 -> 全局坐标系
    Eigen::Vector3d p_FinG_best = R_GtoI.transpose() * R_ItoC.transpose() * (feature.p_FinA - p_IinC) + p_IinG;
    
    // 使用FEJ的锚点状态（旋转和位置）重新计算特征点在锚点相机坐标系中的位置
    // 这样可以保持雅可比矩阵在初始估计处的线性化，提高优化一致性
    R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot_fej();  // 使用FEJ旋转矩阵
    p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos_fej();  // 使用FEJ位置
    
    // 将全局坐标系中的特征点位置转换回锚点相机坐标系
    // 变换步骤：全局坐标系 -> IMU坐标系 -> 锚点相机坐标系
    p_FinA = (R_GtoI.transpose() * R_ItoC.transpose()).transpose() * (p_FinG_best - p_IinG) + p_IinC;
  }
  
  // 计算相机坐标系到全局坐标系的旋转矩阵
  Eigen::Matrix3d R_CtoG = R_GtoI.transpose() * R_ItoC.transpose();

  // 计算特征点相对于锚点IMU位姿的雅可比矩阵
  Eigen::Matrix<double, 3, 6> H_anc;  // 3个位置维度对6个IMU位姿参数的雅可比
  H_anc.block(0, 0, 3, 3).noalias() = -R_GtoI.transpose() * skew_x(R_ItoC.transpose() * (p_FinA - p_IinC));  // 旋转部分雅可比
  H_anc.block(0, 3, 3, 3).setIdentity();  // 平移部分雅可比为单位矩阵

  // 将锚点IMU位姿的雅可比添加到输出列表中
  x_order.push_back(state->_clones_IMU.at(feature.anchor_clone_timestamp));
  H_x.push_back(H_anc);

  // 如果需要计算相机标定参数的雅可比
  if (state->_options.do_calib_camera_pose) {
    Eigen::Matrix<double, 3, 6> H_calib;  // 特征点位置对相机标定参数的雅可比
    H_calib.block(0, 0, 3, 3).noalias() = -R_CtoG * skew_x(p_FinA - p_IinC);  // 相机旋转标定参数的雅可比
    H_calib.block(0, 3, 3, 3) = -R_CtoG;  // 相机平移标定参数的雅可比
    
    // 将相机标定参数的雅可比添加到输出列表
    x_order.push_back(state->_calib_IMUtoCAM.at(feature.anchor_cam_id));
    H_x.push_back(H_calib);
  }

  // 处理锚点坐标系下的3D特征表示
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_3D) {
    H_f = R_CtoG;  // 雅可比矩阵即为相机到全局的旋转矩阵
    return;
  }

  // 处理锚点坐标系下的完整逆深度表示
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_FULL_INVERSE_DEPTH) {

    // 计算锚点坐标系中的逆深度参数表示
    double a_rho = 1 / p_FinA.norm();  // 逆深度
    double a_phi = std::acos(a_rho * p_FinA(2));  // 极角
    double a_theta = std::atan2(p_FinA(1), p_FinA(0));  // 方位角
    Eigen::Matrix<double, 3, 1> p_invFinA;  // 逆深度参数向量
    p_invFinA(0) = a_theta;
    p_invFinA(1) = a_phi;
    p_invFinA(2) = a_rho;

    // 计算三角函数值用于构建雅可比矩阵
    double sin_th = std::sin(p_invFinA(0, 0));
    double cos_th = std::cos(p_invFinA(0, 0));
    double sin_phi = std::sin(p_invFinA(1, 0));
    double cos_phi = std::cos(p_invFinA(1, 0));
    double rho = p_invFinA(2, 0);

    // 构建锚点坐标系下特征点3D位置相对于逆深度参数的雅可比矩阵
    Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
    d_pfinA_dpinv << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
        (1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
        -(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
    
    // 最终雅可比矩阵：全局坐标系中的特征点位置相对于逆深度参数的导数
    H_f = R_CtoG * d_pfinA_dpinv;
    return;
  }

  // 处理MSCKF风格的逆深度表示
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH) {

    // 计算MSCKF风格的逆深度参数（alpha, beta, rho）
    Eigen::Matrix<double, 3, 1> p_invFinA_MSCKF;
    p_invFinA_MSCKF(0) = p_FinA(0) / p_FinA(2);  // alpha = x/z
    p_invFinA_MSCKF(1) = p_FinA(1) / p_FinA(2);  // beta = y/z
    p_invFinA_MSCKF(2) = 1 / p_FinA(2);  // rho = 1/z

    // 提取参数值
    double alpha = p_invFinA_MSCKF(0, 0);
    double beta = p_invFinA_MSCKF(1, 0);
    double rho = p_invFinA_MSCKF(2, 0);

    // 构建MSCKF逆深度参数化的雅可比矩阵
    Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
    d_pfinA_dpinv << (1.0 / rho), 0.0, -(1.0 / (rho * rho)) * alpha,
        0.0, (1.0 / rho), -(1.0 / (rho * rho)) * beta,
        0.0, 0.0, -(1.0 / (rho * rho));
    
    // 最终雅可比矩阵：全局坐标系中的特征点位置相对于MSCKF逆深度参数的导数
    H_f = R_CtoG * d_pfinA_dpinv;
    return;
  }

  /// 处理仅估计单一深度值的情况（假设初始方向已知）
  if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) {

    // 计算逆深度和方向向量
    double rho = 1.0 / p_FinA(2);  // 逆深度（z轴分量的倒数）
    Eigen::Vector3d bearing = rho * p_FinA;  // 单位方向向量（乘以rho后）

    // 构建单一深度参数化的雅可比矩阵
    Eigen::Vector3d d_pfinA_drho;
    d_pfinA_drho << -(1.0 / (rho * rho)) * bearing;  // 位置对逆深度参数的导数
    
    // 最终雅可比矩阵：全局坐标系中的特征点位置相对于单一逆深度参数的导数
    H_f = R_CtoG * d_pfinA_drho;
    return;
  }

  // 如果执行到这里，说明特征表示类型无效
  assert(false);
}


double huberA = 1.5;
double huberB = huberA * huberA;
bool use_Loss = true;

/**
 * @brief 计算特征点的完整雅可比矩阵和重投影误差残差
 * 
 * 该函数是SLAM后端优化的核心组件，负责计算特征点在所有观测帧中的重投影误差，
 * 以及这些误差对特征参数和系统状态变量的雅可比矩阵，为非线性优化提供梯度信息。
 * 
 * @param state 系统状态，包含IMU克隆状态、相机内外参等信息
 * @param feature 特征点信息，包含多相机的观测、时间戳、特征表示类型等
 * @param H_f 输出参数，残差对特征点参数的雅可比矩阵
 * @param H_x 输出参数，残差对系统状态变量的雅可比矩阵
 * @param res 输出参数，重投影误差残差向量
 * @param x_order 输出参数，状态变量的排序顺序，用于雅可比矩阵列索引映射
 */
void UpdaterHelper::get_feature_jacobian_full(std::shared_ptr<State> state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f,
                                              Eigen::MatrixXd &H_x, Eigen::VectorXd &res, std::vector<std::shared_ptr<Type>> &x_order) {

  // 统计当前特征点在所有相机中的总观测次数
  int total_meas = 0;
  const std::vector<std::vector<double>>& ts_all = feature.timestamps;
  for (size_t cam_id = 0; cam_id < ts_all.size(); ++cam_id) {
    total_meas += (int)ts_all[cam_id].size();
  }

  // 计算与当前特征点相关的所有状态变量的总大小
  int total_hx = 0;
  // 建立状态变量到雅可比矩阵列索引的映射，用于快速定位状态变量在雅可比中的位置
  std::unordered_map<std::shared_ptr<Type>, size_t> map_hx;
  // 遍历所有相机的观测
  for (size_t cam_id = 0; cam_id < ts_all.size(); ++cam_id) {

    // 获取当前相机与IMU之间的外参和内参
    std::shared_ptr<PoseJPL> calibration = state->_calib_IMUtoCAM.at(cam_id);
    std::shared_ptr<Vec> distortion = state->_cam_intrinsics.at(cam_id);

    // 如果启用相机外参在线标定，将外参添加到状态变量中
    if (state->_options.do_calib_camera_pose) {
      map_hx.insert({calibration, total_hx});
      x_order.push_back(calibration);
      total_hx += calibration->size(); // 外参大小为6（3旋转+3平移）
    }

    // 如果启用相机内参在线标定，将内参添加到状态变量中
    if (state->_options.do_calib_camera_intrinsics) {
      map_hx.insert({distortion, total_hx});
      x_order.push_back(distortion);
      total_hx += distortion->size(); // 内参大小根据相机模型而定
    }

    // 遍历当前相机的所有观测
    for (size_t m = 0; m < feature.timestamps[cam_id].size(); ++m) {

      // 获取对应时间戳的IMU克隆状态，如果尚未添加到状态变量列表中，则添加
      std::shared_ptr<PoseJPL> clone_Ci = state->_clones_IMU.at(feature.timestamps[cam_id][m]);
      if (map_hx.find(clone_Ci) == map_hx.end()) {
        map_hx.insert({clone_Ci, total_hx});
        x_order.push_back(clone_Ci);
        total_hx += clone_Ci->size(); // IMU位姿大小为6（3旋转+3平移）
      }
    }
  }

  // 如果特征点使用基于锚点的相对表示，确保锚点状态也被添加到状态变量中
  if (LandmarkRepresentation::is_relative_representation(feature.feat_representation)) {

    // 确保特征点有有效的锚点相机ID
    assert(feature.anchor_cam_id != -1);

    // 添加锚点IMU克隆状态（如果尚未添加）
    std::shared_ptr<PoseJPL> clone_Ai = state->_clones_IMU.at(feature.anchor_clone_timestamp);
    if (map_hx.find(clone_Ai) == map_hx.end()) {
      map_hx.insert({clone_Ai, total_hx});
      x_order.push_back(clone_Ai);
      total_hx += clone_Ai->size();
    }

    // 如果启用相机外参标定，也需要添加锚点相机的外参
    if (state->_options.do_calib_camera_pose) {
      std::shared_ptr<PoseJPL> clone_calib = state->_calib_IMUtoCAM.at(feature.anchor_cam_id);
      if (map_hx.find(clone_calib) == map_hx.end()) {
        map_hx.insert({clone_calib, total_hx});
        x_order.push_back(clone_calib);
        total_hx += clone_calib->size();
      }
    }
  }

  //=========================================================================
  // 计算特征点在全局坐标系中的位置
  //=========================================================================

  // 获取特征点的全局位置（如果是绝对表示则直接使用）
  Eigen::Vector3d p_FinG = feature.p_FinG;
  // 如果特征点使用基于锚点的相对表示，则需要从锚点相机坐标系转换到全局坐标系
  if (LandmarkRepresentation::is_relative_representation(feature.feat_representation)) {
    // 确保有有效的锚点
    assert(feature.anchor_cam_id != -1);
    // 获取锚点相机与IMU之间的外参
    Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->Rot(); // IMU到相机的旋转矩阵
    Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->pos(); // IMU在相机坐标系中的位置
    // 获取锚点时刻的IMU位姿
    Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot(); // 全局到IMU的旋转矩阵
    Eigen::Vector3d p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos(); // IMU在全局坐标系中的位置
    // 将特征点从锚点相机坐标系转换到全局坐标系
    // 转换过程：锚点相机坐标系 → IMU坐标系 → 全局坐标系
    p_FinG = R_GtoI.transpose() * R_ItoC.transpose() * (feature.p_FinA - p_IinC) + p_IinG;
  }

  // 计算特征点在FEJ（First Estimate Jacobian）状态下的全局位置
  // 如果是基于锚点的表示，则FEJ位置与当前计算的位置相同，因为锚点参数不参与FEJ
  Eigen::Vector3d p_FinG_fej = feature.p_FinG_fej;
  if (LandmarkRepresentation::is_relative_representation(feature.feat_representation)) {
    p_FinG_fej = p_FinG;
  }

  //=========================================================================
  // 初始化残差和雅可比矩阵
  //=========================================================================
  // 计数器，用于跟踪当前处理的测量索引
  int c = 0;

  // 根据特征点表示类型确定雅可比矩阵的列数
  // 大多数表示使用3个参数，只有单参数逆深度表示使用1个参数
  int jacobsize = (feature.feat_representation != LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) ? 3 : 1;

  // 初始化残差向量，每个观测贡献2个残差（x和y坐标）
  res = Eigen::VectorXd::Zero(2 * total_meas);

  // 初始化特征点参数的雅可比矩阵
  H_f = Eigen::MatrixXd::Zero(2 * total_meas, jacobsize);

  // 初始化状态变量的雅可比矩阵
  H_x = Eigen::MatrixXd::Zero(2 * total_meas, total_hx);

  // 计算特征点全局位置对特征参数的导数
  // 这个计算只需要执行一次，因此提取到循环外以提高效率
  Eigen::MatrixXd dpfg_dlambda;
  std::vector<Eigen::MatrixXd> dpfg_dx;
  std::vector<std::shared_ptr<Type>> dpfg_dx_order;
  // 调用辅助函数计算特征表示雅可比（注意：这里原始代码中缺少第二个参数feature）
  UpdaterHelper::get_feature_jacobian_representation(state, feature, dpfg_dlambda, dpfg_dx, dpfg_dx_order);

  // 调试断言：确保所有在 dpfg_dx_order 中的状态变量都在map_hx中有对应的映射
#ifndef NDEBUG
  for (auto &type : dpfg_dx_order) {
    assert(map_hx.find(type) != map_hx.end());
  }
#endif

  //=========================================================================
  // 遍历所有相机观测，计算残差和雅可比矩阵
  //=========================================================================
  // 遍历每个相机
  for (size_t cam_id = 0; cam_id < ts_all.size(); ++cam_id) {

    // 获取当前相机的内参和与IMU的外参
    std::shared_ptr<Vec> distortion = state->_cam_intrinsics.at(cam_id);
    std::shared_ptr<PoseJPL> calibration = state->_calib_IMUtoCAM.at(cam_id);
    Eigen::Matrix3d R_ItoC = calibration->Rot();  // IMU到相机的旋转矩阵
    Eigen::Vector3d p_IinC = calibration->pos();  // IMU在相机坐标系中的位置

    // 遍历当前相机的所有观测
    for (size_t m = 0; m < feature.timestamps[cam_id].size(); ++m) {

      //=====================================================================
      // 计算特征点在当前相机坐标系中的投影并计算残差
      //=====================================================================

      // 获取当前观测时刻的IMU克隆状态
      std::shared_ptr<PoseJPL> clone_Ii = state->_clones_IMU.at(feature.timestamps[cam_id][m]);
      Eigen::Matrix3d R_GtoIi = clone_Ii->Rot(); // 全局到当前IMU的旋转矩阵
      Eigen::Vector3d p_IiinG = clone_Ii->pos(); // 当前IMU在全局坐标系中的位置

      // 将特征点从全局坐标系转换到当前IMU坐标系
      Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);

      // 将特征点从IMU坐标系转换到相机坐标系
      Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;
      // 计算归一化图像坐标（投影到归一化平面）
      Eigen::Vector2d uv_norm;
      uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);

      // 应用相机畸变模型（支持鱼眼等畸变模型）
      Eigen::Vector2d uv_dist;
      float uv_norm_array[2] = {uv_norm.cast<float>()[0], uv_norm.cast<float>()[1]};
      float uv_dist_array[2];
      // 使用相机模型计算畸变后的坐标
      cam_equi_distort_f(state->_cam_intrinsics_cameras.at(cam_id).get(), uv_norm_array, uv_dist_array);
      uv_dist = Eigen::Vector2f(uv_dist_array[0], uv_dist_array[1]).cast<double>();

      // 计算重投影误差残差（观测值减去预测值）
      Eigen::Vector2d uv_m((double)feature.uvs[cam_id][m](0), (double)feature.uvs[cam_id][m](1));
      res.block(2 * c, 0, 2, 1) = uv_m - uv_dist;

      // 应用Huber鲁棒核函数，减小异常值对优化的影响
      double huber_scale = 1.0;
      if (use_Loss) {
          const double r_l2 = res.squaredNorm();  // 计算残差的L2范数平方
          if (r_l2 > huberB) {  // 如果残差大于阈值，应用Huber核
              const double radius = sqrt(r_l2);  // 残差的L2范数
              // 计算Huber缩放因子，避免除零错误
              double rho1 = std::max(std::numeric_limits<double>::min(), huberA / radius);
              huber_scale = sqrt(rho1);  // 对残差应用平方根缩放
              res *= huber_scale;  // 缩放残差
          }
      }

      //=====================================================================
      // 应用FEJ（First Estimate Jacobian）技术
      //=====================================================================

      // 如果启用了FEJ，则使用首次估计的状态来计算雅可比，以保持线性化点的一致性
      if (state->_options.do_fej) {
        // 使用FEJ状态下的IMU旋转和平移
        R_GtoIi = clone_Ii->Rot_fej();  // 首次估计的旋转矩阵
        p_IiinG = clone_Ii->pos_fej();  // 首次估计的位置
        // 注意：注释掉的部分表示相机外参也可以使用FEJ，但在这里未启用
        // R_ItoC = calibration->Rot_fej();
        // p_IinC = calibration->pos_fej();
        // 使用FEJ状态重新计算特征点位置
        p_FinIi = R_GtoIi * (p_FinG_fej - p_IiinG);
        p_FinCi = R_ItoC * p_FinIi + p_IinC;
        // 注意：这里不重新计算uv_norm，因为FEJ只影响雅可比计算
        // uv_norm << p_FinCi(0)/p_FinCi(2),p_FinCi(1)/p_FinCi(2);
      }

      //=====================================================================
      // 计算各层雅可比矩阵
      //=====================================================================

      // 计算畸变对归一化坐标的雅可比，以及对相机内参的雅可比
      float uv_norm_jacobian_array[2] = {uv_norm.cast<float>()[0], uv_norm.cast<float>()[1]};
      float H_dz_dzn[4];  // 畸变对归一化坐标的雅可比
      float H_dz_dzeta[16];  // 畸变对内参的雅可比
      // 调用相机模型计算畸变雅可比
      cam_equi_compute_distort_jacobian(state->_cam_intrinsics_cameras.at(cam_id).get(), 
                                       uv_norm_jacobian_array, H_dz_dzn, H_dz_dzeta);
      
      // 将C风格数组转换为Eigen矩阵
      Eigen::MatrixXd dz_dzn(2, 2);  // 2×2矩阵，畸变对归一化坐标的雅可比
      dz_dzn << H_dz_dzn[0], H_dz_dzn[1],
                H_dz_dzn[2], H_dz_dzn[3];
      
      Eigen::MatrixXd dz_dzeta(2, 8);  // 2×8矩阵，畸变对内参的雅可比（最多8个内参参数）
      dz_dzeta << H_dz_dzeta[0], H_dz_dzeta[1], H_dz_dzeta[2], H_dz_dzeta[3], 
                 H_dz_dzeta[4], H_dz_dzeta[5], H_dz_dzeta[6], H_dz_dzeta[7],
                 H_dz_dzeta[8], H_dz_dzeta[9], H_dz_dzeta[10], H_dz_dzeta[11], 
                 H_dz_dzeta[12], H_dz_dzeta[13], H_dz_dzeta[14], H_dz_dzeta[15];

      // 计算归一化图像坐标对相机坐标系中特征点的雅可比
      Eigen::MatrixXd dzn_dpfc = Eigen::MatrixXd::Zero(2, 3);  // 2×3矩阵
      // 透视投影的导数，z是特征点在相机坐标系中的深度
      dzn_dpfc << 1 / p_FinCi(2), 0, -p_FinCi(0) / (p_FinCi(2) * p_FinCi(2)), 
                  0, 1 / p_FinCi(2), -p_FinCi(1) / (p_FinCi(2) * p_FinCi(2));
      
      // 如果使用了Huber核，也要缩放雅可比矩阵
      if (use_Loss) {
          dzn_dpfc *= huber_scale;
      }

      // 计算相机坐标系中特征点对全局坐标系中特征点的雅可比
      // 这是一个旋转变换的导数
      Eigen::MatrixXd dpfc_dpfg = R_ItoC * R_GtoIi;  // 3×3矩阵

      // 计算相机坐标系中特征点对IMU位姿的雅可比（旋转部分和平移部分）
      Eigen::MatrixXd dpfc_dclone = Eigen::MatrixXd::Zero(3, 6);  // 3×6矩阵（3旋转+3平移）
      // 对旋转部分的导数：使用反对称矩阵表示旋转的扰动
      dpfc_dclone.block(0, 0, 3, 3).noalias() = R_ItoC * skew_x(p_FinIi);
      // 对平移部分的导数：等于负的dpfc_dpfg
      dpfc_dclone.block(0, 3, 3, 3) = -dpfc_dpfg;

      //=====================================================================
      // 应用链式法则组合雅可比矩阵
      //=====================================================================

      // 预计算部分雅可比组合
      Eigen::MatrixXd dz_dpfc = dz_dzn * dzn_dpfc;  // 2×3矩阵，观测对相机坐标系点的雅可比
      Eigen::MatrixXd dz_dpfg = dz_dpfc * dpfc_dpfg;  // 2×3矩阵，观测对全局坐标系点的雅可比

      // 链式法则：计算残差对特征参数的总雅可比
      H_f.block(2 * c, 0, 2, H_f.cols()).noalias() = dz_dpfg * dpfg_dlambda;

      // 链式法则：计算残差对当前IMU位姿的雅可比
      H_x.block(2 * c, map_hx[clone_Ii], 2, clone_Ii->size()).noalias() = dz_dpfc * dpfc_dclone;

      // 链式法则：计算残差对其他相关状态变量的雅可比（如锚点状态）
      // 注意：这里使用加法是因为可能存在多个贡献来源
      for (size_t i = 0; i < dpfg_dx_order.size(); i++) {
        H_x.block(2 * c, map_hx[dpfg_dx_order.at(i)], 2, dpfg_dx_order.at(i)->size()).noalias() += 
            dz_dpfg * dpfg_dx.at(i);
      }

      //=====================================================================
      // 计算相机标定参数的雅可比（如果启用）
      //=====================================================================

      // 计算残差对相机外参的雅可比（如果启用外参标定）
      if (state->_options.do_calib_camera_pose) {

        // 计算相机坐标系中特征点对相机外参的雅可比
        Eigen::MatrixXd dpfc_dcalib = Eigen::MatrixXd::Zero(3, 6);  // 3×6矩阵（3旋转+3平移）
        // 对旋转部分的导数：使用反对称矩阵
        dpfc_dcalib.block(0, 0, 3, 3) = skew_x(p_FinCi - p_IinC);
        // 对平移部分的导数：单位矩阵
        dpfc_dcalib.block(0, 3, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity();

        // 链式法则：组合得到残差对外参的雅可比
        H_x.block(2 * c, map_hx[calibration], 2, calibration->size()).noalias() += 
            dz_dpfc * dpfc_dcalib;
      }

      // 计算残差对相机内参的雅可比（如果启用内参标定）
      if (state->_options.do_calib_camera_intrinsics) {
        H_x.block(2 * c, map_hx[distortion], 2, distortion->size()) = dz_dzeta;
      }

      // 更新计数器，处理下一个观测
      c++;
    }
  }
}

/**
 * @brief 将雅可比矩阵和残差向量投影到特征点参数雅可比H_f的左零空间中
 * 
 * 此函数实现了一个重要的投影操作，用于MSCKF或滑动窗口优化中消除特征点参数化带来的不确定性。
 * 通过将优化问题投影到H_f的左零空间，我们可以移除特征点参数相关的自由度，直接优化系统状态。
 * 
 * 数学原理：对于观测方程r = h(x, f)，其中x是系统状态，f是特征点参数，我们可以将雅可比矩阵分块为[H_x H_f]。
 * 左零空间投影可以构建一个新的优化问题，仅包含系统状态x的信息，同时保留原始问题的所有约束。
 * 
 * @param H_f 特征点参数的雅可比矩阵，形状为[残差维度×特征参数维度]
 * @param H_x 系统状态变量的雅可比矩阵，形状为[残差维度×状态变量维度]
 * @param res 重投影误差残差向量，形状为[残差维度×1]
 */
void UpdaterHelper::nullspace_project_inplace(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {

  // 对H_f应用左零空间投影到所有变量
  // 基于《Matrix Computations》第4版（Golub和Van Loan著）
  // 详见第252页，算法5.2.4了解这两个循环的工作原理
  // 注意：书中使用matlab索引表示法，因此我们需要从所有索引中减去1
  Eigen::JacobiRotation<double> tempHo_GR;  // 用于存储Givens旋转矩阵
  
  // 使用Givens旋转对 H_f 进行QR分解
  for (int n = 0; n < H_f.cols(); ++n) {  // 遍历每一列
    for (int m = (int)H_f.rows() - 1; m > n; m--) {  // 从下往上遍历每一行
      // 创建一个Givens旋转矩阵，用于消除H_f(m, n)元素
      tempHo_GR.makeGivens(H_f(m - 1, n), H_f(m, n));
      
      // 将Givens旋转应用到H_f、H_x和res的相应行
      // 注意：我们只需要将G应用到非零列[n:Ho.cols()-n-1]，
      // 这等效于将G应用到整个列[0:Ho.cols()-1]
      (H_f.block(m - 1, n, 2, H_f.cols() - n)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
      (H_x.block(m - 1, 0, 2, H_x.cols())).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
      (res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
    }
  }

  // H_f雅可比矩阵的最大秩为3（如果它表示3D位置），因此左零空间的大小为Hf.rows()-3
  // 注意：这里需要使用eigen3的eval()方法来避免别名问题！
  // H_f = H_f.block(H_f.cols(),0,H_f.rows()-H_f.cols(),H_f.cols()).eval();  // 注释掉的代码：特征参数雅可比在投影后通常不需要保留
  
  // 提取H_x和res中对应左零空间的部分
  // 投影后的H_x只包含与系统状态相关且独立于特征参数的信息
  H_x = H_x.block(H_f.cols(), 0, H_x.rows() - H_f.cols(), H_x.cols()).eval();
  res = res.block(H_f.cols(), 0, res.rows() - H_f.cols(), res.cols()).eval();

  // 验证代码：确保投影后的H_x和res行数一致
  assert(H_x.rows() == res.rows());
}

/**
 * @brief 执行测量压缩操作，减小雅可比矩阵和残差向量的维度
 * 
 * 此函数实现了MSCKF等算法中的测量压缩技术，通过QR分解将高维测量信息压缩为低维等价形式，
 * 同时保留原始测量的所有约束信息。这可以显著降低后续优化计算的计算复杂度。
 * 
 * 测量压缩的核心思想是：当残差维度（H_x.rows()）大于状态变量维度（H_x.cols()）时，
 * 我们可以通过投影操作将问题转换为等价的低维形式，而不会丢失任何信息。
 * 
 * @param H_x 系统状态变量的雅可比矩阵，形状为[残差维度×状态变量维度]，将被原地修改
 * @param res 重投影误差残差向量，形状为[残差维度×1]，将被原地修改
 */
void UpdaterHelper::measurement_compress_inplace(Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {

  // 如果H_x是一个宽矩阵（行数<=列数），则不需要压缩，直接返回
  // 只有当观测残差维度大于状态变量维度时，压缩才会带来计算效率提升
  if (H_x.rows() <= H_x.cols())
    return;

  // 使用Givens旋转执行测量压缩
  // 基于《Matrix Computations》第4版（Golub和Van Loan著）
  // 详见第252页，算法5.2.4了解这两个循环的工作原理
  // 注意：书中使用matlab索引表示法，因此我们需要从所有索引中减去1
  Eigen::JacobiRotation<double> tempHo_GR;  // 用于存储Givens旋转矩阵
  
  // 使用Givens旋转对H_x进行QR分解
  for (int n = 0; n < H_x.cols(); n++) {  // 遍历每一列
    for (int m = (int)H_x.rows() - 1; m > n; m--) {  // 从下往上遍历每一行
      // 创建一个Givens旋转矩阵，用于消除H_x(m, n)元素
      tempHo_GR.makeGivens(H_x(m - 1, n), H_x(m, n));
      
      // 将Givens旋转应用到H_x和res的相应行
      // 注意：我们只需要将G应用到非零列[n:Ho.cols()-n-1]，
      // 这等效于将G应用到整个列[0:Ho.cols()-1]
      (H_x.block(m - 1, n, 2, H_x.cols() - n)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
      (res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
    }
  }

  // 确定压缩后的矩阵大小
  // 如果H是宽矩阵，则使用所有行；否则使用与状态变量维度相同的大小
  int r = std::min(H_x.rows(), H_x.cols());

  // 构建压缩后的雅可比矩阵和残差向量
  // 经过QR分解后，只有前r行包含有效信息，其余行可以丢弃
  assert(r <= H_x.rows());  // 验证r不超过原始行数
  H_x.conservativeResize(r, H_x.cols());  // 保留前r行，其余行被移除
  res.conservativeResize(r, res.cols());   // 相应地调整残差向量大小
}