【MSCKF】SLAM中UpdaterHelper::get_feature_jacobian_full 的实现细节

1. 依赖接口get_feature_jacobian_representation

呼应参考接口 UpdaterHelper::get_feature_jacobian_representation

/**

• @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, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f,
  std::vectorEigen::MatrixXd &H_x, std::vector<std::shared_ptr> &x_order)

2. 链式求导组合雅可比的推导过程与物理意义

在SLAM系统中，链式求导组合雅可比是进行非线性优化的核心。通过分析UpdaterHelper::get_feature_jacobian_full函数，我将详细解释链式求导的推导过程、物理意义和每一步转换。

链式求导的整体架构

链式求导的核心思想是：当我们需要计算残差对状态变量的导数时，如果它们之间存在多个中间变量，我们可以通过计算每两个相邻变量之间的导数，然后将这些导数相乘，得到最终的导数关系。

在代码中，链式求导的整体结构如下：

// 预计算部分雅可比组合
dz_dpfc = dz_dzn * dzn_dpfc;  // 观测对相机坐标系点的雅可比
dz_dpfg = dz_dpfc * dpfc_dpfg;  // 观测对全局坐标系点的雅可比

// 链式法则：计算残差对特征参数的总雅可比
H_f = dz_dpfg * dpfg_dlambda;

// 链式法则：计算残差对当前IMU位姿的雅可比
H_x = dz_dpfc * dpfc_dclone;

详细推导过程

1. 归一化图像坐标对相机坐标系点的雅可比 (dzn_dpfc)

物理意义：描述了相机坐标系中3D点位置的微小变化如何影响其在归一化图像平面上的投影坐标。

公式推导：
$$
设相机坐标系中的特征点为 $p_{FinCi}=[x,y,z{]}^T$，归一化坐标为 ${\mathbf{u}}_n=[x/z,y/z{]}^T$

计算导数：

- \( \frac{\partial u_n}{\partial x} = 1/z \), \( \frac{\partial u_n}{\partial y} = 0 \), \( \frac{\partial u_n}{\partial z} = -x/z^2 \)
- \( \frac{\partial v_n}{\partial x} = 0 \), \( \frac{\partial v_n}{\partial y} = 1/z \), \( \frac{\partial v_n}{\partial z} = -y/z^2 \)

代码实现：

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

2. 畸变对归一化坐标的雅可比 (dz_dzn)

物理意义：描述了归一化坐标的微小变化如何影响应用相机畸变后的图像坐标。

计算方法：通过调用相机模型函数cam_equi_compute_distort_jacobian计算，考虑了鱼眼等畸变模型的非线性映射关系。

代码实现：

dz_dzn << H_dz_dzn[0], H_dz_dzn[1],
          H_dz_dzn[2], H_dz_dzn[3];

3. 相机坐标系中特征点对全局坐标系中特征点的雅可比 (dpfc_dpfg)

物理意义：描述了全局坐标系中3D点位置的微小变化如何影响其在相机坐标系中的表示。

公式推导：

相机坐标系点与全局坐标系点的关系为：
$p_{FinCi}=R_{ItoC}\cdot R_{GtoIi}\cdot (p_{FinG}-p_{IiinG})+p_{IinC}$

忽略平移项对位置导数的影响（因为平移是常数项），导数为：
$\frac{\partial p_{FinCi}}{\partial p_{FinG}}=R_{ItoC}\cdot R_{GtoIi}$

代码实现：

dpfc_dpfg = R_ItoC * R_GtoIi;  // 3×3矩阵

4. 相机坐标系中特征点对IMU位姿的雅可比 (dpfc_dclone)

物理意义：描述了IMU位姿（旋转和平移）的微小变化如何影响相机坐标系中的3D点位置。

公式推导：

对于IMU旋转部分（用旋转向量参数化），导数为：
$\frac{\partial p_{FinCi}}{\partial heta}=R_{ItoC}\cdot [p_{FinIi}{]}_{imes}$

对于IMU平移部分，导数为：
$\frac{\partial p_{FinCi}}{\partial p_{IiinG}}=-R_{ItoC}\cdot R_{GtoIi}=-dpf{c}_{d}pfg$

代码实现：

dpfc_dclone.block(0, 0, 3, 3).noalias() = R_ItoC * skew_x(p_FinIi);
dpfc_dclone.block(0, 3, 3, 3) = -dpfc_dpfg;

5. 全局坐标系中特征点对特征参数的雅可比 (dpfg_dlambda)

物理意义：描述了特征点参数（如逆深度、方位角等）的微小变化如何影响其在全局坐标系中的3D位置。

计算方法：通过调用get_feature_jacobian_representation函数计算，根据特征点的不同参数化表示（全局3D、逆深度、锚点坐标系等）有不同的推导过程。

链式组合过程

1. 观测对相机坐标系点的雅可比 (dz_dpfc)

物理意义：综合考虑了归一化投影和畸变两个步骤，描述了相机坐标系中3D点的微小变化如何最终影响观测到的图像坐标。

组合公式：
$$dz\_dpfc=dz\_dznimesdzn\_dpfc$$

2. 观测对全局坐标系点的雅可比 (dz_dpfg)

物理意义：将相机坐标系中的变化进一步映射到全局坐标系，描述了全局坐标系中3D点的微小变化如何影响观测到的图像坐标。

组合公式：
$dz\_dpfg=dz\_dpfcimesdpfc\_dpfg$

3. 残差对特征参数的总雅可比 (H_f)

物理意义：这是优化问题中目标函数（残差）对优化变量（特征参数）的导数，用于梯度下降。

组合公式：
$H\_f=dz\_dpfgimesdpfg\_dlambda$

4. 残差对IMU位姿的雅可比 (H_x)

物理意义：目标函数（残差）对系统状态变量（IMU位姿）的导数，同样用于梯度下降。

组合公式：
$H\_x=dz\_dpfcimesdpfc\_dclone$

5. 残差对锚点状态和其他变量的雅可比

物理意义：当特征点使用基于锚点的表示时，需要考虑锚点状态对残差的影响。

组合公式：
( H_x += dz_dpfg imes dpfg_dx )

相机标定参数的链式求导

当启用相机外参或内参在线标定时，还需要计算残差对标定参数的雅可比：

1. 残差对相机外参的雅可比

物理意义：描述了相机外参（IMU到相机的旋转和平移）的微小变化如何影响重投影误差。

推导公式：

• 对旋转部分：$\frac{\partial p_{FinCi}}{\partial het{a}_{ItoC}}=[p_{FinCi}-p_{IinC}{]}_{imes}$
• 对平移部分：$\frac{\partial p_{FinCi}}{\partial p_{IinC}}=I_{3\times 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 += dz_dpfc * dpfc_dcalib;

2. 残差对相机内参的雅可比

物理意义：描述了相机内参的微小变化如何影响重投影误差。

代码实现：直接使用畸变对内参的雅可比

H_x = dz_dzeta;

FEJ技术对链式求导的影响

当启用FEJ（First Estimate Jacobian）技术时，雅可比矩阵的计算使用首次估计的状态，而不是当前状态：

if (state->_options.do_fej) {
  R_GtoIi = clone_Ii->Rot_fej();  // 使用首次估计的旋转矩阵
  p_IiinG = clone_Ii->pos_fej();  // 使用首次估计的位置
  p_FinIi = R_GtoIi * (p_FinG_fej - p_IiinG);
  p_FinCi = R_ItoC * p_FinIi + p_IinC;
}

物理意义：通过固定线性化点，可以提高优化的一致性和收敛性，避免线性化点漂移导致的问题。

总结

链式求导组合雅可比是SLAM系统中非线性优化的基础，它通过将复杂的导数关系分解为多个简单导数的乘积，实现了高效准确的梯度计算。整个过程涉及多个坐标转换、投影变换和参数化表示，每一步雅可比矩阵都有明确的物理意义，共同构成了一个完整的误差传播链。

在实际应用中，链式求导的计算效率对系统性能有重要影响，代码中使用了noalias()等优化技术来避免临时变量的创建，提高计算效率。同时，FEJ技术的应用也保证了优化过程的数值稳定性。

3. get_feature_jacobian_full 函数详细分析与公式推导

函数整体功能

该函数计算SLAM系统中特征点的重投影误差残差及对应的雅可比矩阵，用于非线性优化。核心数学原理是多视图几何的重投影误差计算和链式求导。

关键变量含义与大小

变量名	类型	大小	含义
state	std::shared_ptr<State>	-	系统状态，包含IMU克隆状态、相机内外参等
feature	UpdaterHelperFeature&	-	特征点信息，包含多相机观测数据
H_f	Eigen::MatrixXd	(2×N)×M	残差对特征参数的雅可比矩阵，N为总观测数，M为特征参数维度(3或1)
H_x	Eigen::MatrixXd	(2×N)×S	残差对系统状态的雅可比矩阵，S为状态总维度
res	Eigen::VectorXd	2×N	重投影误差残差向量
x_order	std::vector<std::shared_ptr<Type>>	S/dim	状态变量排序，用于雅可比列索引映射

坐标变换公式推导

1. 特征点全局位置计算

当特征点使用基于锚点的相对表示时，需要从锚点相机坐标系转换到全局坐标系：

坐标变换链：
锚点相机坐标系 → IMU坐标系 → 全局坐标系

// 代码实现
p_FinG = R_GtoI.transpose() * R_ItoC.transpose() * (feature.p_FinA - p_IinC) + p_IinG;

数学推导：

设：

• p_FinA: 特征点在锚点相机坐标系中的位置
• R_ItoC: IMU到锚点相机的旋转矩阵
• p_IinC: IMU在锚点相机坐标系中的位置
• R_GtoI: 全局到锚点IMU的旋转矩阵
• p_IinG: 锚点IMU在全局坐标系中的位置

转换步骤：

1. 相机坐标系 → IMU坐标系：

   p_FinI = R_ItoC^T * (p_FinA - p_IinC)
   

2. IMU坐标系 → 全局坐标系：

   p_FinG = R_GtoI^T * p_FinI + p_IinG
   

3. 合并得到：

   p_FinG = R_GtoI^T * R_ItoC^T * (p_FinA - p_IinC) + p_IinG
   

2. 特征点投影到当前相机平面

// 代码实现
Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);
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);

数学推导：

1. 全局坐标系 → 当前IMU坐标系：

   p_FinIi = R_GtoIi * (p_FinG - p_IiinG)
   

2. IMU坐标系 → 当前相机坐标系：

   p_FinCi = R_ItoC * p_FinIi + p_IinC
   

3. 透视投影到归一化平面：

   uv_norm = [p_FinCi.x/p_FinCi.z, p_FinCi.y/p_FinCi.z]^T
   

雅可比矩阵计算推导

1. 归一化坐标对相机坐标系点的雅可比

// 代码实现
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));

数学推导：

设归一化坐标为：

[u, v]^T = [x/z, y/z]^T

对相机坐标系点[x, y, z]^T求偏导：

du/dx = 1/z, du/dy = 0, du/dz = -x/z²

 dv/dx = 0, dv/dy = 1/z, dv/dz = -y/z²

因此雅可比矩阵：

┌ 1/z   0   -x/z² ┐
│                │
└  0   1/z  -y/z² ┘

2. 相机坐标系点对IMU位姿的雅可比

// 代码实现
dpfc_dclone.block(0, 0, 3, 3).noalias() = R_ItoC * skew_x(p_FinIi);
dpfc_dclone.block(0, 3, 3, 3) = -dpfc_dpfg;

数学推导：

相机坐标系点p_FinCi = R_ItoC * p_FinIi + p_IinC，其中p_FinIi = R_GtoIi * (p_FinG - p_IiinG)

1. 对旋转部分(φ)的导数：

   • 旋转扰动用角速度向量φ表示
   • 一阶近似下，R(φ) ≈ I + [φ]×，其中[φ]×是φ的反对称矩阵
   • 因此：dp_FinCi/dφ = R_ItoC * [p_FinIi]×

2. 对平移部分(p_IiinG)的导数：

   • 由于p_FinIi = R_GtoIi * (p_FinG - p_IiinG)
   • 因此：dp_FinCi/dp_IiinG = -R_ItoC * R_GtoIi = -dpfc_dpfg

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

数学推导：

相机坐标系点p_FinCi = R_ItoC * p_FinIi + p_IinC

1. 对外参旋转部分(φ)的导数：

   • R_ItoC(φ) ≈ I + [φ]×
   • 因此：dp_FinCi/dφ = [R_ItoC * p_FinIi]× = [p_FinCi - p_IinC]×

2. 对外参平移部分(p_IinC)的导数：

   • 直接求导得：dp_FinCi/dp_IinC = I（单位矩阵）

FEJ技术实现原理

// 代码实现
if (state->_options.do_fej) {
  R_GtoIi = clone_Ii->Rot_fej();
  p_IiinG = clone_Ii->pos_fej();
  p_FinIi = R_GtoIi * (p_FinG_fej - p_IiinG);
  p_FinCi = R_ItoC * p_FinIi + p_IinC;
}

FEJ技术原理：

First Estimate Jacobian (FEJ)技术是在非线性优化中保持雅可比矩阵一致性的重要方法：

1. 线性化点固定：使用变量的首次估计值作为雅可比计算的线性化点，而非当前估计值

   • 这样可以避免优化过程中线性化点不断变化导致的不一致性
   • 提高优化的收敛性和稳定性

2. 状态回退：

   • Rot_fej()：获取首次估计的旋转矩阵
   • pos_fej()：获取首次估计的位置
   • p_FinG_fej：特征点首次估计的全局位置

3. 重新计算特征点位置：使用FEJ状态重新计算特征点在相机坐标系中的位置，用于雅可比计算

4. 为什么注释掉相机外参的FEJ：

   • 代码中注释了相机外参的FEJ应用，这是一个设计选择
   • 在许多SLAM系统中，相机外参变化较慢或被认为是相对稳定的，因此可能不需要对其应用FEJ
   • 这也可能是为了简化实现或满足特定应用场景的需求

Huber鲁棒核函数

// 代码实现
double huber_scale = 1.0;
if (use_Loss) {
    const double r_l2 = res.squaredNorm();
    if (r_l2 > huberB) {
        const double radius = sqrt(r_l2);
        double rho1 = std::max(std::numeric_limits<double>::min(), huberA / radius);
        huber_scale = sqrt(rho1);
        res *= huber_scale;
    }
}

数学原理：

Huber核函数用于减小异常值对优化的影响：

ρ(r) = {
  (1/2)||r||²,            当||r|| ≤ δ
  δ||r|| - (1/2)δ²,       当||r|| > δ
}

代码中通过缩放因子huber_scale实现：

• 当残差较小时（r_l2 ≤ huberB），不进行缩放
• 当残差较大时，应用平方根缩放因子√(huberA/||r||)
• 同时对残差和雅可比矩阵应用相同的缩放，保持一致性

链式求导组合雅可比

// 代码实现
Eigen::MatrixXd dz_dpfc = dz_dzn * dzn_dpfc;
Eigen::MatrixXd dz_dpfg = dz_dpfc * dpfc_dpfg;

H_f.block(2 * c, 0, 2, H_f.cols()).noalias() = dz_dpfg * dpfg_dlambda;
H_x.block(2 * c, map_hx[clone_Ii], 2, clone_Ii->size()).noalias() = dz_dpfc * dpfc_dclone;

数学原理：

链式法则是雅可比计算的核心：

1. 残差对特征参数的雅可比：

   H_f = ∂res/∂λ = ∂res/∂p_FinG * ∂p_FinG/∂λ = dz_dpfg * dpfg_dlambda
   

2. 残差对IMU位姿的雅可比：

   H_x_Ii = ∂res/∂x_Ii = ∂res/∂p_FinCi * ∂p_FinCi/∂x_Ii = dz_dpfc * dpfc_dclone
   

3. 残差对其他状态的雅可比：

   H_x_other = ∂res/∂x_other = ∂res/∂p_FinG * ∂p_FinG/∂x_other = dz_dpfg * dpfg_dx
   

内存优化技巧

代码中多次使用noalias()方法：

H_f.block(2 * c, 0, 2, H_f.cols()).noalias() = dz_dpfg * dpfg_dlambda;

作用： 告诉Eigen库可以直接在目标内存位置计算结果，避免创建临时矩阵，提高计算效率和内存利用率。

总结

这个函数是SLAM后端优化的核心，通过复杂的坐标变换和链式求导，计算出残差向量和雅可比矩阵，为非线性优化提供必要的梯度信息。其实现考虑了FEJ技术、鲁棒核函数、多相机系统和在线标定等关键技术点，展现了现代SLAM系统的核心数学原理和工程实现。

源码

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 UpdaterHelp
er::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++;
    }
  }
}