【MSCKF && FEJ】SLAM中UpdaterHelper::get_feature_jacobian_representation FEJ的实现细节

SLAM中FEJ的实现与雅可比计算

原创已于 2025-12-01 19:05:54 修改 · 475 阅读

14 ·

CC 4.0 BY-SA版权

文章标签：

#数码相机 #ide #vr #mr #vscode #visual studio

于 2025-12-01 18:29:44 首次发布

MSCKF 专栏收录该内容

11 篇文章

订阅专栏

【MSCKF && FEJ】SLAM中FEJ的实现细节

1. FEJ (First Estimate Jacobian) 技术详细分析
2. 旋转矩阵左乘与右乘的物理意义及在FEJ实现中的应用
3. FEJ技术与雅可比矩阵计算详细分析
4. Eigen的noalias()优化与FEJ技术中的坐标变换分析

1. FEJ (First Estimate Jacobian) 技术详细分析

逐行分析FEJ代码段中每一行的数学原理、实现原因和推导过程。

/**
 * @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) {



  // 获取锚点相机的变换参数和相机标定信息
  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;
  }

1. FEJ技术概述

FEJ是VIO/SLAM系统中用于提高非线性优化一致性的重要技术。其核心思想是在整个优化过程中保持雅可比矩阵的线性化点不变，即使状态估计值在迭代过程中不断更新。这样可以避免由于线性化点变化导致的不一致性问题，从而提高系统的数值稳定性和收敛性。

2. 代码逐行分析与数学推导

第1行：条件判断

if (state->_options.do_fej) {

目的：检查系统是否启用了FEJ技术
原因：FEJ并非在所有场景下都是必须的，系统设计为可配置选项，允许根据实际需求启用或禁用

第2-5行：计算特征点在全局坐标系中的最佳估计位置

// 计算特征点在全局坐标系中的最佳估计位置（使用当前状态估计）
// 变换步骤：锚点相机坐标系 -> IMU坐标系 -> 全局坐标系
Eigen::Vector3d p_FinG_best = R_GtoI.transpose() * R_ItoC.transpose() * (feature.p_FinA - p_IinC) + p_IinG;

数学推导：

这里涉及坐标系变换链：锚点相机坐标系(A) → IMU坐标系(I) → 全局坐标系(G)

首先，有特征点在锚点相机坐标系中的位置：feature.p_FinA
IMU在相机坐标系中的位置：p_IinC
需要将特征点从相机坐标系转换到IMU坐标系：
- 公式：p_FinI = R_ItoC^T * (p_FinA - p_IinC)
- 这里，R_ItoC^T是IMU到相机旋转矩阵的转置，相当于相机到IMU的旋转矩阵
- 减去p_IinC是为了进行坐标系原点的平移
然后，将特征点从IMU坐标系转换到全局坐标系：
- 公式：p_FinG = R_GtoI^T * p_FinI + p_IinG
- 这里，R_GtoI^T是全局到IMU旋转矩阵的转置，相当于IMU到全局的旋转矩阵
- 加上p_IinG是为了将IMU原点的位置转换到全局坐标系中

合并上述两步，得到完整变换：

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

为什么这是"最佳估计"：因为这里使用的是当前优化迭代中的状态估计值（旋转矩阵和平移向量），而非初始估计值

第6-9行：更新锚点状态为FEJ值

// 使用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的状态（旋转和平移）回退到其初始估计值（FEJ值）
数学解释：
- Rot_fej()：获取IMU在锚点时刻的初始旋转矩阵估计
- pos_fej()：获取IMU在锚点时刻的初始位置估计
关键原因：这一步是FEJ技术的核心 - 我们要使用初始的状态估计值来计算雅可比矩阵，而不是当前迭代中的优化估计值

第10-13行：将特征点从全局坐标系转回锚点相机坐标系

// 将全局坐标系中的特征点位置转换回锚点相机坐标系
// 变换步骤：全局坐标系 -> IMU坐标系 -> 锚点相机坐标系
p_FinA = (R_GtoI.transpose() * R_ItoC.transpose()).transpose() * (p_FinG_best - p_IinG) + p_IinC;

数学推导：

这里执行的是逆向变换链：全局坐标系(G) → IMU坐标系(I) → 锚点相机坐标系(A)

我们已经有了特征点在全局坐标系中的最佳估计位置：p_FinG_best
首先，将特征点从全局坐标系转换到IMU坐标系：
- 公式：p_FinI = R_GtoI * (p_FinG_best - p_IinG)
- 这里，R_GtoI是全局到IMU的旋转矩阵
- 减去p_IinG是为了平移到IMU坐标系原点
然后，将特征点从IMU坐标系转换到相机坐标系：
- 公式：p_FinA = R_ItoC * p_FinI + p_IinC
- 这里，R_ItoC是IMU到相机的旋转矩阵
- 加上p_IinC是为了平移到相机坐标系原点

合并上述两步，得到：

p_FinA = R_ItoC * R_GtoI * (p_FinG_best - p_IinG) + p_IinC

注意到代码中的等价形式：
```
p_FinA = (R_GtoI.transpose() * R_ItoC.transpose()).transpose() * (p_FinG_best - p_IinG) + p_IinC
```
- 这利用了矩阵转置的性质：(AB)^T = B^T A^T
- 所以 R_ItoC * R_GtoI = (R_GtoI^T * R_ItoC^T)^T

核心原理：虽然我们使用了锚点IMU的初始状态（FEJ值），但我们仍然要保持特征点的实际3D位置不变（即等于当前优化得到的"最佳估计"位置）

3. FEJ技术的关键数学原理

FEJ技术的本质是在计算雅可比矩阵时使用固定的线性化点。具体来说：

非线性优化的基本问题：
- 在VIO/SLAM中，我们通常最小化重投影误差等非线性残差
- 每次迭代都需要计算残差对状态变量的雅可比矩阵
- 这个雅可比矩阵是在当前线性化点处计算的
不一致性问题：
- 当状态估计值更新后，如果同时更新线性化点，会导致雅可比矩阵的不一致性
- 这可能引起优化结果的偏差，甚至发散
FEJ的解决方案：
- 固定雅可比矩阵的线性化点（通常是初始估计值）
- 即使状态估计值在迭代中更新，雅可比矩阵仍然在初始点处计算
- 这确保了优化问题的一致性，提高了收敛的稳定性
坐标变换的巧妙之处：
- 代码中先将特征点变换到全局坐标系（使用当前状态估计）
- 然后再变换回锚点坐标系（使用FEJ状态估计）
- 这样既保持了特征点的3D位置不变，又确保了后续雅可比计算在线性化点处进行

4. 为什么这样实现FEJ

这种实现方式的优势在于：

计算效率：只需要在第一次迭代时计算雅可比矩阵，后续可以复用
数值稳定性：避免了由于线性化点变化导致的矩阵条件数恶化
一致性保证：确保了优化问题在数学上的一致性
实现简单：通过坐标系变换的方式优雅地实现了FEJ，不需要修改优化器本身

这种FEJ实现方式在许多开源VIO/SLAM系统中被广泛采用，如OKVIS、VINS-Mono等，是提高系统鲁棒性的关键技术之一。

2. 旋转矩阵左乘与右乘的物理意义及在FEJ实现中的应用

1. 旋转矩阵左乘与右乘的本质区别

在机器人学和计算机视觉中，旋转矩阵的乘法顺序（左乘或右乘）具有截然不同的物理意义，这直接关系到坐标变换的理解和实现：

1.1 左乘（Left Multiplication）的物理意义

左乘表示：坐标系变换

当我们对一个向量p左乘旋转矩阵R时：p' = R * p
这相当于保持向量不动，而旋转参考坐标系
从数学上看，左乘实现了将向量从一个坐标系转换到另一个坐标系
在机器人学中，左乘通常用于表示坐标变换（Coordinate Transformation）

1.2 右乘（Right Multiplication）的物理意义

右乘表示：向量旋转

当我们对一个向量p右乘旋转矩阵R时：p' = p * R（注意这里向量需要是行向量）
这相当于保持坐标系不动，而旋转向量本身
从数学上看，右乘实现了向量在同一坐标系内的旋转
在机器人学中，右乘通常用于表示姿态更新（Pose Update）

2. FEJ实现中使用左乘的原因

回到FEJ实现中的坐标变换代码：

p_FinA = (R_GtoI.transpose() * R_ItoC.transpose()).transpose() * (p_FinG_best - p_IinG) + p_IinC;

或者其等价形式：

p_FinA = R_ItoC * R_GtoI * (p_FinG_best - p_IinG) + p_IinC;

这里使用左乘的原因有以下几点：

1. 符合坐标变换的数学定义

在三维几何中，坐标变换的标准数学定义就是使用左乘。当我们需要将一个点从坐标系G（全局）转换到坐标系I（IMU）再转换到坐标系A（锚点相机），标准的做法是：

首先应用从G到I的变换
然后应用从I到A的变换

这种连续变换在数学上自然表现为矩阵的左乘序列：R_ItoA * R_GtoI * p_FinG

2. 旋转矩阵的组合顺序

在左乘约定下，旋转矩阵的组合顺序符合直观的变换顺序：

当我们写R_ItoC * R_GtoI时，表示先应用G→I的变换，再应用I→C的变换
这与我们执行变换的顺序（先从全局到IMU，再从IMU到相机）完全一致
如果使用右乘，矩阵的顺序需要颠倒，可能导致实现时的混淆

3. 列向量表示法的标准约定

在大多数机器人学和计算机视觉系统中，向量采用列向量表示（如Eigen库）：

列向量表示下，左乘旋转矩阵是进行坐标变换的标准方式
右乘通常需要将向量转置为行向量，这在Eigen等库中不太方便
代码中的Eigen::Vector3d是列向量，自然适合左乘操作

4. 与FEJ技术需求的匹配

在FEJ实现中，我们需要精确地追踪坐标变换链：

先将特征点从锚点相机坐标系转换到全局坐标系（使用当前估计）
然后将同一特征点从全局坐标系转换回锚点相机坐标系（使用FEJ值）

这种双向坐标变换在左乘约定下更容易实现和验证，特别是当我们需要确保变换的一致性和逆变换的正确性时。

3. 左乘与右乘的数学对比

为了更清晰地展示区别，我们进行一个简单的数学对比：

3.1 左乘方式（代码中使用的）

# 向量表示：列向量
p_FinI = R_GtoI * (p_FinG - p_IinG)      # 全局→IMU
p_FinA = R_ItoC * p_FinI + p_IinC        # IMU→相机

3.2 右乘方式（假设使用行向量）

# 向量表示：行向量
p_FinI = (p_FinG - p_IinG)^T * R_GtoI^T  # 全局→IMU
p_FinA = p_FinI * R_ItoC^T + p_IinC^T    # IMU→相机

可以看到，右乘方式不仅需要转置向量，还需要转置旋转矩阵，实现起来更复杂且容易出错。

4. 代码实现中的矩阵顺序分析

回到代码中的等价形式：

p_FinA = R_ItoC * R_GtoI * (p_FinG_best - p_IinG) + p_IinC;

这里的矩阵乘法顺序R_ItoC * R_GtoI看起来似乎与变换顺序相反，但实际上：

矩阵乘法R_ItoC * R_GtoI计算的是复合变换矩阵，表示从全局坐标系直接到相机坐标系的变换
当我们将这个复合矩阵应用于向量时，数学上仍然是先应用G→I变换，再应用I→C变换
这是因为矩阵乘法本身就表示变换的组合，AB表示先应用B变换，再应用A变换

5. 总结

在FEJ实现中使用左乘旋转矩阵是由以下因素决定的：

数学便利性：符合坐标变换的标准数学定义和实现方式
代码可读性：变换顺序与实现顺序一致，便于理解和维护
库兼容性：与Eigen等线性代数库的列向量表示法自然匹配
工程实践：这是机器人学和计算机视觉领域的广泛标准做法

通过使用左乘，代码能够清晰、高效地实现FEJ技术中所需的复杂坐标变换，同时保持数学上的正确性和物理意义的直观性。

3. FEJ技术与雅可比矩阵计算详细分析

3. 1 雅可比矩阵计算部分

3.1.1 计算相机坐标系到全局坐标系的旋转矩阵

Eigen::Matrix3d R_CtoG = R_GtoI.transpose() * R_ItoC.transpose();

原因与原理：

这是计算从相机坐标系到全局坐标系的旋转矩阵
由于旋转矩阵的逆等于其转置，等价于R_CtoG = R_GtoC⁻¹
此旋转矩阵将在后续的重投影误差或特征点观测中使用

3.1.2 初始化雅可比矩阵并计算旋转部分雅可比

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

原因与公式推导：

初始化一个3×6的雅可比矩阵，3行对应特征点的3个位置维度，6列对应IMU位姿的6个自由度（3个旋转，3个平移）
计算旋转部分（前3列）的雅可比：
- 首先计算特征点在相机坐标系中相对于IMU的向量：p_FinA - p_IinC
- 将此向量转换到IMU坐标系：R_ItoC.transpose() * (p_FinA - p_IinC)
- 使用反对称矩阵（叉乘矩阵）skew_x()表示旋转的导数关系
- 最后通过R_GtoI.transpose()转换到全局坐标系
- 负号表示雅可比的传递方向（从残差到状态参数的导数方向）
- 数学依据：旋转对特征点位置的导数可以表示为反对称矩阵的乘积

3.1.3 设置平移部分雅可比

H_anc.block(0, 3, 3, 3).setIdentity();

原因与原理：

设置雅可比矩阵的平移部分（后3列）为3×3单位矩阵
数学原理：当计算特征点位置相对于IMU平移位置的导数时，导数为单位矩阵
这是因为IMU位置的变化直接线性地影响特征点的全局位置
例如：如果IMU在x方向移动1单位，特征点在全局坐标系中也会移动1单位（在x方向）

3.1.4 添加雅可比矩阵和对应的状态到输出列表

x_order.push_back(state->_clones_IMU.at(feature.anchor_clone_timestamp));
H_x.push_back(H_anc);

原因与原理：

将锚点IMU位姿对象添加到状态变量顺序列表中，以保持雅可比矩阵与状态变量的对应关系
将计算好的雅可比矩阵添加到雅可比列表中
这样在后续的优化过程中，可以正确地构建整个系统的信息矩阵和梯度向量
确保特征点观测对锚点IMU位姿的约束能够被正确地纳入到优化问题中

3.2 技术要点总结

FEJ技术原理：通过固定雅可比矩阵计算时的线性化点，避免了非线性优化中常见的"线性化漂移"问题，提高了优化的一致性和收敛性
坐标变换链：代码中展示了完整的多坐标系变换链：锚点相机坐标系 → IMU坐标系 → 全局坐标系
雅可比矩阵结构：3×6的雅可比矩阵分别处理了旋转（前3列）和平移（后3列）的导数关系
计算优化：使用noalias()方法避免了Eigen中的临时对象创建，提高了计算效率
矩阵运算：巧妙地利用了旋转矩阵的性质（逆等于转置）和矩阵乘法的结合律，简化了坐标变换计算

这些代码实现了SLAM系统中关键的特征点雅可比计算，特别是结合FEJ技术来提高优化的稳定性和精度，是后端优化中非常核心的部分。

4. Eigen的noalias()优化与FEJ技术中的坐标变换分析

4.1 Eigen中noalias()方法的优化原理与实现

基本原理

H_anc.block(0, 0, 3, 3).noalias() = -R_GtoI.transpose() * skew_x(R_ItoC.transpose() * (p_FinA - p_IinC));

noalias()的技术原理：

Eigen库默认情况下会假设矩阵乘法操作可能存在别名（aliasing）问题
当执行A = B * C时，Eigen通常会先计算中间结果到临时变量，再赋值给A，以避免计算过程中数据被覆盖
noalias()方法显式告诉Eigen不需要创建临时变量，因为目标矩阵与源矩阵没有重叠内存

具体优化效果：

减少内存分配：避免创建临时矩阵对象
减少数据拷贝：直接在目标内存中计算结果
提高缓存利用率：数据局部性更好，减少缓存未命中
潜在的SIMD优化：允许更高效的SIMD指令执行

数学表达等价性：

// 使用noalias()
H_anc_block = -R_GtoI^T * skew_x(R_ItoC^T * (p_FinA - p_IinC))

// 等价但不使用noalias()
Temp = -R_GtoI^T * skew_x(R_ItoC^T * (p_FinA - p_IinC))
H_anc_block = Temp

适用条件：
只有当目标矩阵（H_anc_block）与源操作数（R_GtoI, R_ItoC, p_FinA, p_IinC）没有内存重叠时才能安全使用。在本例中，这是成立的，因为H_anc是新创建的矩阵，与其他操作数无重叠。

4.2 FEJ技术中的双向坐标变换

4.2.1 坐标变换的目的与流程

FEJ技术中需要两次坐标变换的完整流程：

// 步骤1：锚点相机坐标系 → IMU坐标系 → 全局坐标系
p_FinG_best = R_GtoI.transpose() * R_ItoC.transpose() * (p_FinA - p_IinC) + p_IinG;

// 步骤2：回退到FEJ状态
R_GtoI = Rot_fej();
p_IinG = pos_fej();

// 步骤3：全局坐标系 → IMU坐标系 → 锚点相机坐标系
p_FinA = (R_GtoI.transpose() * R_ItoC.transpose()).transpose() * (p_FinG_best - p_IinG) + p_IinC;

4.2.2 为什么需要这两次坐标变换？

根本目的：保持特征点几何位置的一致性，同时实现FEJ的线性化固定

特征点位置的几何保持：
- 第一次变换将特征点从锚点相机坐标系转换到全局坐标系，获得其在世界中的客观位置
- 第二次变换使用FEJ状态将特征点重新投影回锚点相机坐标系
- 这样，无论锚点状态如何变化，特征点的实际物理位置保持一致
FEJ技术的核心实现：
- FEJ要求雅可比矩阵在首次线性化点计算
- 通过这种"变换出去再变换回来"的方式，我们能够：
  - 首先使用最佳估计得到特征点的真实世界位置
  - 然后"冻结"这个位置，仅改变线性化点（锚点的FEJ状态）
  - 最终得到在FEJ状态下的特征点表示，用于计算雅可比矩阵
避免"线性化漂移"问题：
- 在非线性优化中，如果每次迭代都更新线性化点，可能导致不一致性
- 固定线性化点可以确保优化的收敛性和数值稳定性

4.2.3 这种实现方式的好处

精度与一致性：
- 保持了特征点几何位置的物理一致性
- 确保雅可比矩阵在固定点计算，避免优化过程中的不一致性
模块化实现：
- 清晰地分离了特征点位置计算和FEJ状态应用
- 便于理解和维护
数值稳定性：
- 两次变换形成闭环，理论上不会引入额外误差（仅受浮点精度限制）

4.2.4 可能的替代实现方式

直接代数方法：

// 计算从当前状态到FEJ状态的相对变换
Eigen::Matrix3d dR = R_GtoI_FeJ * R_GtoI_current.transpose();
Eigen::Vector3d dp = p_IinG_FeJ - dR * p_IinG_current;

// 直接应用相对变换到特征点
p_FinA_FeJ = dR * p_FinA_current + dp;

但这种方法可能在复杂情况下引入误差，且不够直观。

矩阵乘法优化版本：

// 优化版本，避免转置的转置运算
Eigen::Matrix3d R_CtoG_FeJ = R_GtoI_FeJ.transpose() * R_ItoC.transpose();
Eigen::Matrix3d R_GtoC_FeJ = R_CtoG_FeJ.transpose();  // 或者直接计算
p_FinA_FeJ = R_GtoC_FeJ * (p_FinG_best - p_IinG_FeJ) + p_IinC;

这是对现有实现的微小优化，保持了相同的逻辑。

四元数表示法：
如果系统使用四元数而非旋转矩阵，实现方式会有所不同，但基本思想一致——通过坐标变换链保持特征点位置一致性。

总结

noalias()优化：是Eigen库提供的一种内存优化技术，通过避免临时对象创建来提高矩阵运算效率，适用于无内存重叠的矩阵操作。
FEJ的双向坐标变换：是一种巧妙的技术实现，通过"变换出去再变换回来"的方式，在保持特征点几何位置一致性的同时，实现了FEJ技术对线性化点的固定，从而提高了SLAM优化的稳定性和收敛性。这种实现方式虽然涉及多次矩阵运算，但确保了优化过程的理论正确性。