【MSCKF】零空间的实现细节及其在MSCKF边缘化中含义（UpdaterHelper::nullspace_project_inplace ）

、

一、背景：MSCKF中特征点参数的冗余性

在**多状态约束卡尔曼滤波（MSCKF）**或滑动窗口优化中，观测方程可表示为：
$$\mathbf{r}=h(\mathbf{x},\mathbf{f})$$
其中：

• $\mathbf{r}\in {\mathbb{R}}^m$：重投影误差残差向量（$m$为残差维度，例如单目特征点的残差维度为2）；
• $\mathbf{x}\in {\mathbb{R}}^n$：系统状态（如相机位姿、速度、IMU偏置等）；
• $\mathbf{f}\in {\mathbb{R}}^k$：特征点参数（如3D空间点的坐标，$k=3$）。

对观测方程做一阶泰勒展开，雅可比矩阵可分块为：
$$J=\left[\frac{\partial \mathbf{r}}{\partial \mathbf{x}}\frac{\partial \mathbf{r}}{\partial \mathbf{f}}\right]=\left[H_x{H}_f\right]$$
其中 $H_x\in {\mathbb{R}}^{m\times n}$（系统状态的雅可比），$H_f\in {\mathbb{R}}^{m\times k}$（特征点参数的雅可比）。

特征点$\mathbf{f}$是冗余参数（优化目标仅需系统状态$\mathbf{x}$），因此需通过左零空间投影消去$\mathbf{f}$的影响，将优化问题转化为仅关于$\mathbf{x}$的约束。

二、左零空间投影的数学原理

1. 左零空间的定义

矩阵$H_f$的左零空间（Left Null Space）定义为：
$${\mathcal{N}}_{\text{left}}(H_f)=\left\{\mathbf{Z}\in {\mathbb{R}}^{m\times (m-k)}\mid {\mathbf{Z}}^T{H}_f=0\right\}$$
其中$\mathbf{Z}$的列是左零空间的基，满足${\mathbf{Z}}^T{H}_f=0$（消去特征点参数的雅可比）。

将${\mathbf{Z}}^T$作用于残差和雅可比矩阵，得到新的约束：
$${\mathbf{Z}}^T\mathbf{r}={\mathbf{Z}}^T{H}_x\mathbf{x}$$
此时约束仅关于$\mathbf{x}$，实现了特征点参数的消去。

2. 用QR分解构造左零空间基

对$H_f$做QR分解（正交三角分解）：
$$H_f=Q\left[\begin{array}{c}R\\ 0\end{array}\right]$$
其中：

• $Q\in {\mathbb{R}}^{m\times m}$：正交矩阵（$Q^{T}Q=I$）；
• $R\in {\mathbb{R}}^{k\times k}$：上三角矩阵；
• $0\in {\mathbb{R}}^{(m-k)\times k}$：零矩阵。

将$Q$分块为：
$$Q=\left[\begin{array}{cc}{Q}_1& Q_2\end{array}\right],Q_1\in {\mathbb{R}}^{m\times k},Q_2\in {\mathbb{R}}^{m\times (m-k)}$$

由于$Q$正交，$Q_2^T{Q}_1=0$，因此：
$$Q_2^T{H}_f=Q_2^{T}Q\left[\begin{array}{c}R\\ 0\end{array}\right]={\left[\begin{array}{c}0\\ I_{m-k}\end{array}\right]}^T\left[\begin{array}{c}R\\ 0\end{array}\right]=0$$

即$Q_2$是$H_f$左零空间的基（$\mathbf{Z}=Q_2$）。将$Q_2^T$作用于残差和$H_x$，得到：
$${\mathbf{r}}^{\prime }=Q_2^T\mathbf{r},H_x^{\prime }=Q_2^T{H}_x$$
新的约束为${\mathbf{r}}^{\prime }=H_x^{\prime }\mathbf{x}$，仅包含系统状态$\mathbf{x}$。

三、代码的数学实现解析

代码通过Givens旋转实现$H_f$的QR分解（逐元素消去下三角，得到上三角$R$），并提取$Q_2^T$作用后的结果。

1. Givens旋转的作用

Givens旋转是2×2正交矩阵，形式为：
$$G(i,j,\theta )=\left[\begin{array}{ccccc}1& & & & \\ & \cos \theta & -\sin \theta & & \\ & \sin \theta & \cos \theta & & \\ & & & \ddots & \\ & & & & 1\end{array}\right]$$
用于消去矩阵中指定位置的元素（如$H_f(m,n)$）。代码中tempHo_GR.makeGivens(a,b)会计算旋转角$\theta$，使得：
$$\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}a\\ b\end{array}\right]=\left[\begin{array}{c}\sqrt{a^2+b^2}\\ 0\end{array}\right]$$

2. 循环实现QR分解

• 外层循环：遍历$H_f$的每一列$n$（从0到$k-1$）；
• 内层循环：从最后一行到$n+1$行遍历$m$，对$H_f(m-1,n)$和$H_f(m,n)$构造Givens旋转，消去$H_f(m,n)$，将$H_f$转化为上三角矩阵。

每次旋转后，需将Givens矩阵作用于$H_f$、$H_x$和$\mathbf{r}$（因为$Q$是Givens旋转的乘积，作用$G^T$等价于累积$Q^T$）。

3. 提取左零空间投影结果

$H_f$的列数为$k$（特征参数维度），因此$Q_2$对应$Q$的后$(m-k)$行。代码中：

• $H_x=H_x.block(H_f.cols(),0,H_x.rows()-H_f.cols(),H_x.cols())$：提取$H_x$从第$k$行开始的$(m-k)$行（即$Q_2^T{H}_x$）；
• $\mathbf{r}=\mathbf{r}.block(H_f.cols(),0,\mathbf{r}.rows()-H_f.cols(),\mathbf{r}.cols())$：提取$\mathbf{r}$从第$k$行开始的$(m-k)$行（即$Q_2^T\mathbf{r}$）。

四、Eigen中.block()的使用方法

.block()是Eigen中提取/操作矩阵子块的核心函数，有动态大小和静态大小两种调用方式：

1. 动态大小（运行时确定维度）

matrix.block(startRow, startCol, numRows, numCols)

参数说明：

• startRow：子块起始行索引（从0开始）；
• startCol：子块起始列索引；
• numRows：子块的行数；
• numCols：子块的列数。

2. 静态大小（编译期确定维度，更高效）

matrix.block<numRows, numCols>(startRow, startCol)

代码中.block()的具体应用

1. H_f.block(m-1, n, 2, H_f.cols()-n)：
   从$H_f$的$(m-1)$行、$n$列开始，取2行（Givens旋转作用于相邻两行）、$H_f.cols()-n$列（仅处理当前列及右侧列，左侧已消为上三角）的子块，应用Givens旋转。

2. H_x.block(m-1, 0, 2, H_x.cols())：
   从$H_x$的$(m-1)$行、0列开始，取2行、所有列的子块，应用相同的Givens旋转（保证变换一致性）。

3. res.block(m-1, 0, 2, 1)：
   从残差向量$\mathbf{r}$的$(m-1)$行开始，取2行1列的子块，应用Givens旋转。

4. H_x.block(H_f.cols(), 0, H_x.rows()-H_f.cols(), H_x.cols())：
   从$H_x$的$k$行（$H_f$的列数）开始，取**$m-k$行**（左零空间维度）、所有列的子块，即$Q_2^T{H}_x$。

五、总结

代码通过Givens旋转实现$H_f$的QR分解，构造左零空间基$Q_2$，并将$Q_2^T$作用于$H_x$和$\mathbf{r}$，最终消去特征点参数的影响，得到仅关于系统状态$\mathbf{x}$的优化约束。.block()的作用是精准提取/操作矩阵子块，确保Givens旋转的正确应用和左零空间投影结果的提取。

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

注释说明：
函数概述：解释了函数的核心功能是将雅可比矩阵和残差向量投影到特征点参数雅可比的左零空间，这是MSCKF等算法中的关键操作。

数学原理：详细说明了左零空间投影的数学背景，包括如何将包含特征点和状态变量的优化问题转换为仅包含状态变量的问题。

参数说明：清晰描述了输入参数H_f、H_x和res的含义和维度。

实现细节：对QR分解过程中的Givens旋转操作进行了逐行注释，解释了矩阵操作的目的和数学意义。

关键技术点：

引用了算法来源（Golub和Van Loan的《Matrix Computations》）
解释了索引调整的原因（Matlab索引到C++索引的转换）
说明了使用eval()方法避免别名问题的重要性
解释了为何只保留左零空间部分（H_f.cols()之后的行）
验证机制：说明了最后的断言用于验证投影操作的正确性。

这个注释版本详细解释了nullspace_project_inplace函数的工作原理、数学基础和实现细节，便于理解其在SLAM优化中的作用。

在代码中，Givens旋转的核心作用是实现对特征点雅可比矩阵$H_f$的QR分解（正交三角分解），并同步完成对$H_x$和残差$\mathbf{r}$的正交变换，最终为构造$H_f$的左零空间、消去特征点参数冗余性奠定基础。具体作用可分为以下三层：

一、Givens旋转的本质功能：消去矩阵指定元素，构造上三角矩阵

Givens旋转是一种正交变换（满足$G^{T}G=I$），其核心作用是通过构造特定的2×2旋转矩阵，精准消去矩阵中指定位置的非零元素，将矩阵逐步转化为上三角形式——这正是QR分解的关键目标。

对于矩阵$H_f$中的元素$H_f(m-1,n)$和$H_f(m,n)$（第$n$列的第$m-1$行与第$m$行），Givens旋转矩阵$G$满足：
$$G\cdot \left[\begin{array}{c}{H}_f(m-1,n)\\ H_f(m,n)\end{array}\right]=\left[\begin{array}{c}\sqrt{H_f(m-1,n{)}^2+H_f(m,n{)}^2}\\ 0\end{array}\right]$$
即通过旋转消去第$m$行第$n$列的元素，仅保留第$m-1$行第$n$列的非零值。

二、在代码中的具体作用：完成$H_f$的QR分解，累积正交矩阵$Q^T$

代码通过嵌套循环对$H_f$逐列、逐行应用Givens旋转：

• 外层循环遍历$H_f$的每一列$n$（从0到特征参数维度$k-1$）；
• 内层循环从$H_f$的最后一行向上遍历到第$n+1$行，对每一对相邻行$(m-1,m)$构造Givens旋转，消去第$m$行第$n$列的元素。

经过所有Givens旋转后，$H_f$被转化为上三角矩阵$R$（仅对角线及上方元素非零），而所有Givens旋转矩阵的乘积的转置即为QR分解中的正交矩阵$Q^T$（$H_f=Q\cdot \left[\begin{array}{c}R\\ 0\end{array}\right]$）。

三、同步变换$H_x$和$\mathbf{r}$：保证约束的一致性

左零空间投影需要将正交矩阵$Q$的左零空间部分$Q_2^T$（$Q$的后$m-k$行）作用于$H_x$和$\mathbf{r}$（得到$H_x^{\prime }=Q_2^T{H}_x$、${\mathbf{r}}^{\prime }=Q_2^T\mathbf{r}$）。

由于Givens旋转的累积等价于$Q^T$的作用，因此代码中每次对$H_f$应用Givens旋转时，必须同步对$H_x$和$\mathbf{r}$的对应行执行相同的旋转——这确保了$H_x$和$\mathbf{r}$始终跟随$H_f$完成正交变换，最终提取的左零空间投影结果是自洽的。

四、为提取左零空间基铺路

QR分解后，正交矩阵$Q$可分块为$Q=\left[\begin{array}{cc}{Q}_1& Q_2\end{array}\right]$（$Q_1$为前$k$列，$Q_2$为后$m-k$列），其中$Q_2$正是$H_f$的左零空间基（满足$Q_2^T{H}_f=0$）。

代码中通过Givens旋转完成QR分解后，直接提取$H_x$和$\mathbf{r}$的后$m-k$行（对应$Q_2^T$的作用结果），本质上是利用Givens旋转累积的$Q^T$实现了左零空间投影。

总结

Givens旋转在代码中是实现QR分解的“数值工具”：一方面将$H_f$转化为上三角矩阵，完成QR分解；另一方面通过同步变换$H_x$和$\mathbf{r}$，确保左零空间投影的一致性，最终消去特征点参数的冗余性，得到仅关于系统状态的优化约束。

QR分解是将矩阵分解为正交矩阵$Q$和上三角矩阵$R$（$A=QR$）的重要数值方法，除了Givens旋转外，主流实现方法还包括Householder变换（反射变换）、Gram-Schmidt正交化，以及针对特殊矩阵的QR迭代法等。以下逐一介绍：

一、Householder变换（Householder Reflections）

1. 原理：构造反射矩阵消去列元素

Householder变换通过构造Householder矩阵（对称正交矩阵），一次性消去矩阵某一列中主对角线以下的所有元素（比Givens旋转更高效，Givens需逐元素消去）。

Householder矩阵定义为：
$$H=I-2\frac{\mathbf{v}{\mathbf{v}}^T}{{\mathbf{v}}^T\mathbf{v}}$$
其中$\mathbf{v}$是反射向量，满足$H^T=H$且$H^{T}H=I$（正交性）。

对矩阵$A\in {\mathbb{R}}^{m\times n}$的第$k$列（$k=1,2,...,n-1$），构造$\mathbf{v}$使得$HA$的第$k$列主对角线以下元素全为0。重复此过程，最终将$A$转化为上三角矩阵$R$，而所有Householder矩阵的乘积即为正交矩阵$Q$（$A=QR$，$Q=H_1{H}_2...H_{n-1}$）。

2. 步骤示例（以$3\times 3$矩阵为例）

• 对第1列构造$H_1$，消去第1列第2、3行元素；
• 对第2列构造$H_2$，消去第2列第3行元素；
• 最终$H_2{H}_{1}A=R$，故$A=(H_1{H}_2)R=QR$（$Q=H_1{H}_2$）。

3. 特点与应用

• 效率高：每次消去一列的下方元素，计算量约为Givens旋转的一半（尤其适合满秩矩阵）；
• 数值稳定性好：是数值计算中最常用的QR分解方法（如Eigen库的ColPivHouseholderQR默认实现）；
• 适合大规模稠密矩阵的QR分解，在SLAM优化中常用于批量处理雅可比矩阵。

二、Gram-Schmidt正交化（Gram-Schmidt Orthogonalization）

1. 原理：列向量的正交化过程

Gram-Schmidt正交化通过对矩阵$A$的列向量进行正交化，直接构造正交矩阵$Q$和上三角矩阵$R$。

设$A=[{\mathbf{a}}_1,{\mathbf{a}}_2,...,{\mathbf{a}}_n]$（列向量组），正交化步骤为：

• 第一步：${\mathbf{q}}_1=\frac{{\mathbf{a}}_1}{\Vert {\mathbf{a}}_1\Vert }$，$R_{11}=\Vert {\mathbf{a}}_1\Vert$；
• 第二步：${\mathbf{r}}_{12}={\mathbf{q}}_1^T{\mathbf{a}}_2$，${\mathbf{b}}_2={\mathbf{a}}_2-{\mathbf{r}}_{12}{\mathbf{q}}_1$，${\mathbf{q}}_2=\frac{{\mathbf{b}}_2}{\Vert {\mathbf{b}}_2\Vert }$，$R_{22}=\Vert {\mathbf{b}}_2\Vert$；
• 第$k$步：${\mathbf{r}}_{ik}={\mathbf{q}}_i^T{\mathbf{a}}_k$（$i=1,...,k-1$），${\mathbf{b}}_k={\mathbf{a}}_k-{\sum }_{i=1}^{k-1}{\mathbf{r}}_{ik}{\mathbf{q}}_i$，${\mathbf{q}}_k=\frac{{\mathbf{b}}_k}{\Vert {\mathbf{b}}_k\Vert }$，$R_{kk}=\Vert {\mathbf{b}}_k\Vert$。

最终$A=QR$，其中$Q=[{\mathbf{q}}_1,{\mathbf{q}}_2,...,{\mathbf{q}}_n]$（正交矩阵），$R$为上三角矩阵（$R_{ik}={\mathbf{q}}_i^T{\mathbf{a}}_k$）。

2. 改进版本：Modified Gram-Schmidt（MGS）

经典Gram-Schmidt（CGS）存在数值稳定性差的问题（舍入误差累积导致正交性退化），改进版MGS通过调整计算顺序（逐列正交化而非逐行）提升稳定性：

• 对每个${\mathbf{a}}_k$，先减去其在已正交化向量${\mathbf{q}}_1,...,{\mathbf{q}}_{k-1}$上的投影，再归一化。

3. 特点与应用

• 直观易懂：原理基于向量正交化，适合理论推导；
• 数值稳定性：MGS优于CGS，但仍不如Householder变换；
• 适合小规模矩阵或需要显式构造正交基的场景（如线性代数教学、低维优化问题）。

三、QR迭代法（QR Iteration）

1. 原理：迭代实现QR分解与特征值求解

QR迭代法并非直接用于普通矩阵的QR分解，而是通过反复QR分解+矩阵乘法迭代逼近矩阵的Schur分解（上三角化），主要用于特征值求解，但本质依赖QR分解。

基本迭代步骤：

• 初始化$A_0=A$；
• 对$A_k$做QR分解：$A_k=Q_k{R}_k$；
• 构造$A_{k+1}=R_k{Q}_k$；
• 重复迭代直到$A_k$收敛为上三角矩阵（对角元素为特征值）。

2. 特点与应用

• 主要用于特征值/特征向量求解（如对称矩阵的特征值分解）；
• 是数值线性代数中求解稠密矩阵特征值的核心算法；
• 在SLAM中可用于协方差矩阵的特征值分析（如EKF的可观测性分析）。

四、各方法对比（结合SLAM场景）

方法	核心操作	效率	数值稳定性	适用场景
Givens旋转	逐元素消去	中	高	稀疏矩阵、逐步变换（如代码中）
Householder变换	逐列消去下方元素	高	高	稠密矩阵、大规模QR分解
Gram-Schmidt（MGS）	列向量正交化	低	中	小规模矩阵、理论推导
QR迭代法	迭代QR分解+乘法	中	高	特征值求解

五、在SLAM中的应用关联

在你提供的代码中，Givens旋转被用于逐步处理雅可比矩阵$H_f$（因$H_f$可能是稀疏或需要逐列消去的结构）；若需批量处理稠密雅可比矩阵，Householder变换会更高效（如Eigen的HouseholderQR类）；而Gram-Schmidt正交化则常用于理论分析（如证明左零空间的存在性）。

综上，QR分解的实现方法需根据矩阵结构（稠密/稀疏）、规模和计算需求选择，其中Householder变换是工程实践中最常用的通用方法。

QR分解是将矩阵分解为正交矩阵$Q$和上三角矩阵$R$（$A=QR$）的重要数值方法，除了Givens旋转外，主流实现方法还包括Householder变换（反射变换）、Gram-Schmidt正交化，以及针对特殊矩阵的QR迭代法等。以下逐一介绍：

一、Householder变换（Householder Reflections）

1. 原理：构造反射矩阵消去列元素

Householder变换通过构造Householder矩阵（对称正交矩阵），一次性消去矩阵某一列中主对角线以下的所有元素（比Givens旋转更高效，Givens需逐元素消去）。

Householder矩阵定义为：
$$H=I-2\frac{\mathbf{v}{\mathbf{v}}^T}{{\mathbf{v}}^T\mathbf{v}}$$
其中$\mathbf{v}$是反射向量，满足$H^T=H$且$H^{T}H=I$（正交性）。

对矩阵$A\in {\mathbb{R}}^{m\times n}$的第$k$列（$k=1,2,...,n-1$），构造$\mathbf{v}$使得$HA$的第$k$列主对角线以下元素全为0。重复此过程，最终将$A$转化为上三角矩阵$R$，而所有Householder矩阵的乘积即为正交矩阵$Q$（$A=QR$，$Q=H_1{H}_2...H_{n-1}$）。

2. 步骤示例（以$3\times 3$矩阵为例）

• 对第1列构造$H_1$，消去第1列第2、3行元素；
• 对第2列构造$H_2$，消去第2列第3行元素；
• 最终$H_2{H}_{1}A=R$，故$A=(H_1{H}_2)R=QR$（$Q=H_1{H}_2$）。

3. 特点与应用

• 效率高：每次消去一列的下方元素，计算量约为Givens旋转的一半（尤其适合满秩矩阵）；
• 数值稳定性好：是数值计算中最常用的QR分解方法（如Eigen库的ColPivHouseholderQR默认实现）；
• 适合大规模稠密矩阵的QR分解，在SLAM优化中常用于批量处理雅可比矩阵。

二、Gram-Schmidt正交化（Gram-Schmidt Orthogonalization）

1. 原理：列向量的正交化过程

Gram-Schmidt正交化通过对矩阵$A$的列向量进行正交化，直接构造正交矩阵$Q$和上三角矩阵$R$。

设$A=[{\mathbf{a}}_1,{\mathbf{a}}_2,...,{\mathbf{a}}_n]$（列向量组），正交化步骤为：

• 第一步：${\mathbf{q}}_1=\frac{{\mathbf{a}}_1}{\Vert {\mathbf{a}}_1\Vert }$，$R_{11}=\Vert {\mathbf{a}}_1\Vert$；
• 第二步：${\mathbf{r}}_{12}={\mathbf{q}}_1^T{\mathbf{a}}_2$，${\mathbf{b}}_2={\mathbf{a}}_2-{\mathbf{r}}_{12}{\mathbf{q}}_1$，${\mathbf{q}}_2=\frac{{\mathbf{b}}_2}{\Vert {\mathbf{b}}_2\Vert }$，$R_{22}=\Vert {\mathbf{b}}_2\Vert$；
• 第$k$步：${\mathbf{r}}_{ik}={\mathbf{q}}_i^T{\mathbf{a}}_k$（$i=1,...,k-1$），${\mathbf{b}}_k={\mathbf{a}}_k-{\sum }_{i=1}^{k-1}{\mathbf{r}}_{ik}{\mathbf{q}}_i$，${\mathbf{q}}_k=\frac{{\mathbf{b}}_k}{\Vert {\mathbf{b}}_k\Vert }$，$R_{kk}=\Vert {\mathbf{b}}_k\Vert$。

最终$A=QR$，其中$Q=[{\mathbf{q}}_1,{\mathbf{q}}_2,...,{\mathbf{q}}_n]$（正交矩阵），$R$为上三角矩阵（$R_{ik}={\mathbf{q}}_i^T{\mathbf{a}}_k$）。

2. 改进版本：Modified Gram-Schmidt（MGS）

经典Gram-Schmidt（CGS）存在数值稳定性差的问题（舍入误差累积导致正交性退化），改进版MGS通过调整计算顺序（逐列正交化而非逐行）提升稳定性：

• 对每个${\mathbf{a}}_k$，先减去其在已正交化向量${\mathbf{q}}_1,...,{\mathbf{q}}_{k-1}$上的投影，再归一化。

3. 特点与应用

• 直观易懂：原理基于向量正交化，适合理论推导；
• 数值稳定性：MGS优于CGS，但仍不如Householder变换；
• 适合小规模矩阵或需要显式构造正交基的场景（如线性代数教学、低维优化问题）。

三、QR迭代法（QR Iteration）

1. 原理：迭代实现QR分解与特征值求解

QR迭代法并非直接用于普通矩阵的QR分解，而是通过反复QR分解+矩阵乘法迭代逼近矩阵的Schur分解（上三角化），主要用于特征值求解，但本质依赖QR分解。

基本迭代步骤：

• 初始化$A_0=A$；
• 对$A_k$做QR分解：$A_k=Q_k{R}_k$；
• 构造$A_{k+1}=R_k{Q}_k$；
• 重复迭代直到$A_k$收敛为上三角矩阵（对角元素为特征值）。

2. 特点与应用

• 主要用于特征值/特征向量求解（如对称矩阵的特征值分解）；
• 是数值线性代数中求解稠密矩阵特征值的核心算法；
• 在SLAM中可用于协方差矩阵的特征值分析（如EKF的可观测性分析）。

四、各方法对比（结合SLAM场景）

方法	核心操作	效率	数值稳定性	适用场景
Givens旋转	逐元素消去	中	高	稀疏矩阵、逐步变换（如代码中）
Householder变换	逐列消去下方元素	高	高	稠密矩阵、大规模QR分解
Gram-Schmidt（MGS）	列向量正交化	低	中	小规模矩阵、理论推导
QR迭代法	迭代QR分解+乘法	中	高	特征值求解

五、在SLAM中的应用关联

在你提供的代码中，Givens旋转被用于逐步处理雅可比矩阵$H_f$（因$H_f$可能是稀疏或需要逐列消去的结构）；若需批量处理稠密雅可比矩阵，Householder变换会更高效（如Eigen的HouseholderQR类）；而Gram-Schmidt正交化则常用于理论分析（如证明左零空间的存在性）。

综上，QR分解的实现方法需根据矩阵结构（稠密/稀疏）、规模和计算需求选择，其中Householder变换是工程实践中最常用的通用方法。

要理解“用QR分解构造左零空间基”的流程，我们需要从问题背景出发，逐步推导数学过程，并解释每一步的核心目的。

步骤1：背景铺垫——观测方程与雅可比分块

在MSCKF/滑动窗口优化中，观测方程描述了“系统状态+特征点”与观测残差的关系：
$$\mathbf{r}=h(\mathbf{x},\mathbf{f})$$
其中：

• $\mathbf{r}\in {\mathbb{R}}^m$：重投影误差残差（如单目特征的残差维度$m=2$，多帧观测时$m=2N$，$N$为观测帧数）；
• $\mathbf{x}\in {\mathbb{R}}^n$：系统状态（相机位姿、IMU参数等，是我们的优化目标）；
• $\mathbf{f}\in {\mathbb{R}}^k$：特征点参数（如3D点坐标，$k=3$，是冗余参数，无需优化）。

对观测方程做一阶泰勒展开（线性化），残差可表示为雅可比与参数增量的乘积：
$$\mathbf{r}\approx \frac{\partial h}{\partial \mathbf{x}}\Delta \mathbf{x}+\frac{\partial h}{\partial \mathbf{f}}\Delta \mathbf{f}$$
将雅可比分块记为：
$$\underset{m\times 1}{\underbrace{\mathbf{r}}}=\underset{m\times n}{\underbrace{H_x}}\Delta \mathbf{x}+\underset{m\times k}{\underbrace{H_f}}\Delta \mathbf{f}$$
其中：

• $H_x=\frac{\partial h}{\partial \mathbf{x}}$：系统状态的雅可比；
• $H_f=\frac{\partial h}{\partial \mathbf{f}}$：特征点参数的雅可比。

我们的目标是消去冗余的特征点增量$\Delta \mathbf{f}$，只保留关于$\Delta \mathbf{x}$的约束。这需要找到$H_f$的左零空间基（满足${\mathbf{Z}}^T{H}_f=0$的矩阵$\mathbf{Z}$），从而消去$H_f\Delta \mathbf{f}$项。

步骤2：对$H_f$做QR分解

对$H_f\in {\mathbb{R}}^{m\times k}$（通常$m>k$且$H_f$列满秩）做正交三角分解（QR分解），形式为：
$$H_f=Q\cdot \left[\begin{array}{c}R\\ 0\end{array}\right]$$

各矩阵的定义与原因：

• $Q\in {\mathbb{R}}^{m\times m}$：正交矩阵，满足$Q^{T}Q=I$（$I$为单位矩阵）。
  原因：正交矩阵的变换保持内积和范数，数值稳定性强，且分块后可自然分离出零空间基。

• $R\in {\mathbb{R}}^{k\times k}$：上三角可逆矩阵。
  原因：QR分解将列满秩矩阵分解为“正交矩阵+上三角矩阵”，上三角矩阵便于后续计算（如求逆）。

• $0\in {\mathbb{R}}^{(m-k)\times k}$：零矩阵。
  原因：$H_f$是$m\times k$矩阵（$m>k$），QR分解后下方会自然出现零块，对应“行维度冗余”的部分。

步骤3：分块正交矩阵$Q$

将正交矩阵$Q$按列分块为：
$$Q=\left[\begin{array}{cc}{Q}_1& Q_2\end{array}\right]$$
其中：

• $Q_1\in {\mathbb{R}}^{m\times k}$：$Q$的前$k$列，维度匹配$R$的行数$k$；
• $Q_2\in {\mathbb{R}}^{m\times (m-k)}$：$Q$的后$(m-k)$列，维度匹配零矩阵的行数$(m-k)$。

分块的原因：

为了匹配$H_f$的QR分解形式$\left[\begin{array}{c}R\\ 0\end{array}\right]$，后续可通过分块矩阵乘法分离出与$H_f$相关/无关的部分。

步骤4：利用正交性，推导$Q_1$与$Q_2$的关系

由于$Q$是正交矩阵，满足$Q^{T}Q=I$。将$Q$分块代入得：
$$Q^{T}Q=\left[\begin{array}{c}{Q}_1^T\\ Q_2^T\end{array}\right]\left[\begin{array}{cc}{Q}_1& Q_2\end{array}\right]=\left[\begin{array}{cc}{Q}_1^T{Q}_1& Q_1^T{Q}_2\\ Q_2^T{Q}_1& Q_2^T{Q}_2\end{array}\right]=\left[\begin{array}{cc}{I}_k& 0\\ 0& I_{m-k}\end{array}\right]$$

由此可得3个关键结论（正交矩阵的分块性质）：

1. $Q_1^T{Q}_1=I_k$（$Q_1$的列是正交单位向量）；
2. $Q_2^T{Q}_2=I_{m-k}$（$Q_2$的列是正交单位向量）；
3. $Q_2^T{Q}_1=0$（$Q_1$与$Q_2$的列互相正交）。

这一步的原因：

正交性是后续证明“$Q_2$是左零空间基”的核心前提。

步骤5：证明$Q_2$是$H_f$的左零空间基

左零空间的定义是：若${\mathbf{Z}}^T{H}_f=0$，则$\mathbf{Z}$是$H_f$左零空间的基。

我们将$H_f=Q\left[\begin{array}{c}R\\ 0\end{array}\right]$代入$Q_2^T{H}_f$，计算得：
$$Q_2^T{H}_f=Q_2^T\cdot Q\cdot \left[\begin{array}{c}R\\ 0\end{array}\right]$$

结合$Q$的分块形式$Q=[Q_1{Q}_2]$，$Q_2^{T}Q=\left[\begin{array}{cc}{Q}_2^T{Q}_1& Q_2^T{Q}_2\end{array}\right]=\left[\begin{array}{cc}0& I_{m-k}\end{array}\right]$（由步骤4的结论3）。因此：
$$Q_2^T{H}_f=\left[\begin{array}{cc}0& I_{m-k}\end{array}\right]\cdot \left[\begin{array}{c}R\\ 0\end{array}\right]=0\cdot R+I_{m-k}\cdot 0=0_{(m-k)\times k}$$

这说明$Q_2$满足左零空间的定义（$Q_2^T{H}_f=0$），因此$Q_2$的列是$H_f$左零空间的一组正交基。

这一步的原因：

这是整个流程的核心——通过QR分解的分块正交矩阵，构造出了能消去$H_f$的左零空间基$Q_2$。

步骤6：左零空间投影，消去特征点参数

将左零空间基$Q_2^T$作用于原残差$\mathbf{r}$和系统状态雅可比$H_x$，得到新的残差和雅可比：
$${\mathbf{r}}^{\prime }=Q_2^T\mathbf{r},H_x^{\prime }=Q_2^T{H}_x$$

将原线性化残差$\mathbf{r}=H_x\Delta \mathbf{x}+H_f\Delta \mathbf{f}$两边左乘$Q_2^T$，得：
$$Q_2^T\mathbf{r}=Q_2^T{H}_x\Delta \mathbf{x}+Q_2^T{H}_f\Delta \mathbf{f}$$

由于$Q_2^T{H}_f=0$（步骤5的结论），上式简化为：
$${\mathbf{r}}^{\prime }=H_x^{\prime }\Delta \mathbf{x}$$

这一步的原因：

• 消去了冗余的特征点增量$\Delta \mathbf{f}$（$Q_2^T{H}_f\Delta \mathbf{f}=0$）；
• 新约束${\mathbf{r}}^{\prime }=H_x^{\prime }\Delta \mathbf{x}$仅包含系统状态增量$\Delta \mathbf{x}$，实现了“只优化系统状态”的目标。

总结：流程的核心逻辑

通过“QR分解→分块正交矩阵→构造左零空间基→投影消冗余”的步骤，我们将包含特征点参数的约束，转化为仅关于系统状态的约束。每一步的设计都服务于“消去冗余参数、保留有效约束”的目标，是MSCKF中“边缘化特征点”的关键数学工具。