【MSCKF】Givens的原理与实现细节（零空间 MSCKF边缘化中） &&QR分解的特点和常见方式

【MSCKF】零空间 MSCKF边缘化中Givens的原理与是西安细节

1. 【MSCKF】零空间 MSCKF边缘化中Givens的原理与是西安细节

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

一、`applyOnTheLeft` 与 `adjoint` 的含义

1. `applyOnTheLeft`（Eigen 方法）

对于 Eigen 中的矩阵/块（block），applyOnTheLeft(i, j, rot) 表示对当前块的第 i 行和第 j 行应用由 rot 定义的 Givens 旋转。
本质是左乘一个 Givens 旋转矩阵 ( G )：
$\text{block} = G \times \text{block}$
由于 Givens 旋转仅作用于两行（这里固定为第0行和第1行，对应原矩阵的 m-1 和 m 行），因此能精准消除目标位置的元素，同时保持其他行不变。

2. `adjoint`（伴随/共轭转置）

对于实矩阵（如这里的 Givens 旋转矩阵），adjoint() 等价于转置（ $G^\text{T}$ ）。
Givens 旋转矩阵是正交矩阵（满足 $G^\text{T}G = GG^\text{T} = I$ ），其转置等于逆。代码中用 tempHo_GR.adjoint() 的原因是：
makeGivens(a, b) 生成的旋转矩阵 ( G ) 用于将列向量 $a, b]^T$ 转为 $r, 0]^T$ （即消除 b）；而对矩阵行操作时，需要左乘 $G^\text{T}$ 才能实现相同的消元效果（行变换等价于左乘转置）。

二、3个 `block` 的具体位置变化（结合循环变量 ( n, m )）

Eigen 的 block(startRow, startCol, numRows, numCols) 表示：从 (startRow, startCol) 开始，取 numRows 行、numCols 列的子矩阵。结合循环逻辑（( n ) 遍历列，( m ) 从最后一行往下到 ( n+1 )），逐个分析：

1. `H_f.block(m - 1, n, 2, H_f.cols() - n)`

startRow = m - 1：当前处理的是原矩阵的 m-1 行和 m 行（共2行，numRows=2）；
startCol = n：第 n 列是当前消元的目标列，且前 n 列已完成上三角化（无需重复处理）；
numCols = H_f.cols() - n：从第 n 列到最后一列的所有列（仅处理未完成三角化的区域）。

位置变化示例（假设 ( H_f ) 是 5×3 矩阵）：

当 ( n=0 )（处理第0列）、( m=4 ) 时：block 是 (3, 0, 2, 3) → 覆盖行3-4、列0-2；
当 ( n=1 )（处理第1列）、( m=3 ) 时：block 是 (2, 1, 2, 2) → 覆盖行2-3、列1-2；
当 ( n=2 )（处理第2列）、( m=2 ) 时：block 是 (1, 2, 2, 1) → 覆盖行1-2、列2-2。

每次旋转后，H_f(m, n) 被消为0，最终 ( H_f ) 化为上三角矩阵。

2. `H_x.block(m - 1, 0, 2, H_x.cols())`

startRow = m - 1：与 ( H_f ) 同步，处理 m-1 行和 m 行（保证旋转的一致性）；
startCol = 0：( H_x ) 是状态雅可比矩阵，所有列（状态维度）都需跟随旋转（无列截断）；
numCols = H_x.cols()：覆盖 ( H_x ) 的所有列。

位置变化示例（假设 ( H_x ) 是 5×4 矩阵）：

当 ( n=0 )、( m=4 ) 时：block 是 (3, 0, 2, 4) → 覆盖行3-4、列0-3；
当 ( n=1 )、( m=3 ) 时：block 是 (2, 0, 2, 4) → 覆盖行2-3、列0-3。

目的是让 ( H_x ) 与 ( H_f ) 经历相同的正交变换，保证后续投影的正确性。

3. `res.block(m - 1, 0, 2, 1)`

startRow = m - 1：同样同步处理 m-1 行和 m 行；
startCol = 0、numCols=1：res 是列向量，仅需处理对应行的元素。

位置变化示例（假设 res 是 5×1 向量）：

当 ( m=4 ) 时：block 是 (3, 0, 2, 1) → 覆盖行3-4；
当 ( m=3 ) 时：block 是 (2, 0, 2, 1) → 覆盖行2-3。

残差向量需与雅可比矩阵同步旋转，才能保证投影后残差与状态的匹配性。

4. 核心逻辑总结

通过**固定行范围（m-1 和 m 行）、动态列范围（H_f 从 n 列开始，H_x/res 全列）**的 block 选择，结合 Givens 旋转的左乘转置操作，既高效完成 ( H_f ) 的上三角化，又保证 ( H_x ) 和 res 同步变换，最终实现左零空间的投影。


/**
 * @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());
}

2. QR分解的主要实现方式及核心特征

一、QR分解的主要实现方式及核心特征

QR分解的主流实现方式包括Givens旋转、Householder变换、Gram-Schmidt正交化（含经典/改进）和列旋转QR，各方法的原理、特点与适用场景差异显著：

实现方式	核心原理	关键特点	典型适用场景
Givens旋转	正交旋转矩阵作用于两行/两列，逐元素精准消元形成上三角R	局部性强（点对点消元）、数值稳定性好（正交变换）、灵活性高；计算常数因子略高	行数远大于列数的矩阵（如残差雅可比）、需同步变换多矩阵/向量（雅可比+残差）、局部精细消元需求
Householder变换	反射变换一次性消去一列所有目标元素（n+1行至最后），一次处理一整列	计算效率高（常数因子低）、批量整列消元；灵活性差，易产生冗余计算	满矩阵通用QR分解、大规模稠密矩阵、对计算效率要求高的场景
Gram-Schmidt正交化（CGS/MGS）	向量投影直接构造正交Q和上三角R，MGS改进CGS稳定性	原理直观、实现简单；数值稳定性差（CGS敏感，MGS仍弱于正交变换）	低维度矩阵演示、对精度要求低的简单场景
列旋转QR（QR with Column Pivoting）	QR分解中加入列交换，使R对角元降序排列	适配列秩不足场景、提升数值稳定性；增加列交换开销，列满秩时无必要	矩阵列线性相关/秩亏损（如参数冗余雅可比）、需鲁棒秩判定的场景

二、常见使用场景

QR分解的应用场景与实现方式强绑定：

高精度优化/估计（如SLAM的BA问题）：优先选择Givens旋转或Householder变换（正交变换保证稳定性），其中Givens更适配“残差雅可比+残差向量同步变换”的局部操作需求；
通用矩阵分解（如线性方程组求解）：Householder变换因效率优势成为主流；
秩亏损矩阵处理（如参数冗余场景）：列旋转QR可提升分解鲁棒性；
教学/低精度场景：Gram-Schmidt正交化因原理简单更易理解。

三、操作对象的要求

QR分解对实施的矩阵/向量有明确约束：

维度与秩：
- 若需左零空间投影，矩阵需满足行数m > 列数n；
- 常规场景要求矩阵列满秩（列旋转QR除外），否则R矩阵对角元出现零，影响分解精度；
- 同步变换的矩阵/向量（如雅可比矩阵、残差向量）需与目标矩阵行数完全一致。
数据类型与存储：
- 实数值矩阵/向量（复数需适配对应旋转类型，如JacobiRotation<std::complex<double>>）；
- 稠密矩阵更适配（稀疏矩阵需注意块操作效率，Givens/Householder均对稠密矩阵友好）；
- 支持高效块操作与行变换（如Eigen的MatrixXd，内存连续存储保证效率）。
数值范围：
元素需避免过大/过小（防止溢出/下溢），必要时通过残差归一化等预处理保证数值合理性。