【MSCKF】零空间 UpdaterHelper::nullspace_project_inplace 的实现细节,MSCKF边缘化含义

大江东去浪淘尽千古风流人物2025-12-03 8:57

、

【MSCKF】零空间 UpdaterHelper::nullspace_project_inplace 的实现细节,MSCKF边缘化含义

      • 一、背景：MSCKF中特征点参数的冗余性
      • 二、左零空间投影的数学原理
        • [1. 左零空间的定义](#1. 左零空间的定义)
        • [2. 用QR分解构造左零空间基](#2. 用QR分解构造左零空间基)
      • 三、代码的数学实现解析
        • [1. Givens旋转的作用](#1. Givens旋转的作用)
        • [2. 循环实现QR分解](#2. 循环实现QR分解)
        • [3. 提取左零空间投影结果](#3. 提取左零空间投影结果)
      • 四、Eigen中`.block()`的使用方法
        • [1. 动态大小（运行时确定维度）](#1. 动态大小（运行时确定维度）)
        • [2. 静态大小（编译期确定维度，更高效）](#2. 静态大小（编译期确定维度，更高效）)
        • 代码中`.block()`的具体应用
      • 五、总结
      • 一、Givens旋转的本质功能：消去矩阵指定元素，构造上三角矩阵
      • [二、在代码中的具体作用：完成 H f H_f Hf的QR分解，累积正交矩阵 Q T Q^T QT](#二、在代码中的具体作用：完成 H f H_f Hf的QR分解，累积正交矩阵 Q T Q^T QT)
      • [三、同步变换 H x H_x Hx和 r \boldsymbol{r} r：保证约束的一致性](#三、同步变换 H x H_x Hx和 r \boldsymbol{r} r：保证约束的一致性)
      • 四、为提取左零空间基铺路
      • 总结
      • [一、Householder变换（Householder Reflections）](#一、Householder变换（Householder Reflections）)
        • [1. 原理：构造反射矩阵消去列元素](#1. 原理：构造反射矩阵消去列元素)
        • [2. 步骤示例（以 3 × 3 3 \times 3 3×3矩阵为例）](#2. 步骤示例（以 3 × 3 3 \times 3 3×3矩阵为例）)
        • [3. 特点与应用](#3. 特点与应用)
      • [二、Gram-Schmidt正交化（Gram-Schmidt Orthogonalization）](#二、Gram-Schmidt正交化（Gram-Schmidt Orthogonalization）)
        • [1. 原理：列向量的正交化过程](#1. 原理：列向量的正交化过程)
        • [2. 改进版本：Modified Gram-Schmidt（MGS）](#2. 改进版本：Modified Gram-Schmidt（MGS）)
        • [3. 特点与应用](#3. 特点与应用)
      • [三、QR迭代法（QR Iteration）](#三、QR迭代法（QR Iteration）)
        • [1. 原理：迭代实现QR分解与特征值求解](#1. 原理：迭代实现QR分解与特征值求解)
        • [2. 特点与应用](#2. 特点与应用)
      • 四、各方法对比（结合SLAM场景）
      • 五、在SLAM中的应用关联
      • [一、Householder变换（Householder Reflections）](#一、Householder变换（Householder Reflections）)
        • [1. 原理：构造反射矩阵消去列元素](#1. 原理：构造反射矩阵消去列元素)
        • [2. 步骤示例（以 3 × 3 3 \times 3 3×3矩阵为例）](#2. 步骤示例（以 3 × 3 3 \times 3 3×3矩阵为例）)
        • [3. 特点与应用](#3. 特点与应用)
      • [二、Gram-Schmidt正交化（Gram-Schmidt Orthogonalization）](#二、Gram-Schmidt正交化（Gram-Schmidt Orthogonalization）)
        • [1. 原理：列向量的正交化过程](#1. 原理：列向量的正交化过程)
        • [2. 改进版本：Modified Gram-Schmidt（MGS）](#2. 改进版本：Modified Gram-Schmidt（MGS）)
        • [3. 特点与应用](#3. 特点与应用)
      • [三、QR迭代法（QR Iteration）](#三、QR迭代法（QR Iteration）)
        • [1. 原理：迭代实现QR分解与特征值求解](#1. 原理：迭代实现QR分解与特征值求解)
        • [2. 特点与应用](#2. 特点与应用)
      • 四、各方法对比（结合SLAM场景）
      • 五、在SLAM中的应用关联
      • 步骤1：背景铺垫------观测方程与雅可比分块
      • [步骤2：对 H f H_f Hf做QR分解](#步骤2：对 H f H_f Hf做QR分解)
      • [步骤3：分块正交矩阵 Q Q Q](#步骤3：分块正交矩阵 Q Q Q)
      • [步骤4：利用正交性，推导 Q 1 Q_1 Q1与 Q 2 Q_2 Q2的关系](#步骤4：利用正交性，推导 Q 1 Q_1 Q1与 Q 2 Q_2 Q2的关系)
      • [步骤5：证明 Q 2 Q_2 Q2是 H f H_f Hf的左零空间基](#步骤5：证明 Q 2 Q_2 Q2是 H f H_f Hf的左零空间基)
      • 步骤6：左零空间投影，消去特征点参数
      • 总结：流程的核心逻辑

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

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

其中：

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

对观测方程做一阶泰勒展开，雅可比矩阵可分块为：
J = [ ∂ r ∂ x ∂ r ∂ f ] = [ H x H f ] J = \left[ \frac{\partial \boldsymbol{r}}{\partial \boldsymbol{x}} \quad \frac{\partial \boldsymbol{r}}{\partial \boldsymbol{f}} \right] = \left[ H_x \quad H_f \right] J=[∂x∂r∂f∂r]=[HxHf]

其中 H x ∈ R m × n H_x \in \mathbb{R}^{m \times n} Hx∈Rm×n（系统状态的雅可比）， H f ∈ R m × k H_f \in \mathbb{R}^{m \times k} Hf∈Rm×k（特征点参数的雅可比）。

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

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

1. 左零空间的定义

矩阵 H f H_f Hf的左零空间 （Left Null Space）定义为：
N left ( H f ) = { Z ∈ R m × ( m − k ) ∣ Z T H f = 0 } \mathcal{N}_\text{left}(H_f) = \left\{ \boldsymbol{Z} \in \mathbb{R}^{m \times (m-k)} \mid \boldsymbol{Z}^T H_f = \boldsymbol{0} \right\} Nleft(Hf)={Z∈Rm×(m−k)∣ZTHf=0}

其中 Z \boldsymbol{Z} Z的列是左零空间的基，满足 Z T H f = 0 \boldsymbol{Z}^T H_f = \boldsymbol{0} ZTHf=0（消去特征点参数的雅可比）。

将 Z T \boldsymbol{Z}^T ZT作用于残差和雅可比矩阵，得到新的约束：
Z T r = Z T H x x \boldsymbol{Z}^T \boldsymbol{r} = \boldsymbol{Z}^T H_x \boldsymbol{x} ZTr=ZTHxx

此时约束仅关于 x \boldsymbol{x} x，实现了特征点参数的消去。

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

对 H f H_f Hf做QR分解 （正交三角分解）：
H f = Q [ R 0 ] H_f = Q \begin{bmatrix} R \\ \boldsymbol{0} \end{bmatrix} Hf=Q[R0]

其中：

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

将 Q Q Q分块为：
Q = [ Q 1 Q 2 ] , Q 1 ∈ R m × k , Q 2 ∈ R m × ( m − k ) Q = \begin{bmatrix} Q_1 & Q_2 \end{bmatrix}, \quad Q_1 \in \mathbb{R}^{m \times k},\ Q_2 \in \mathbb{R}^{m \times (m-k)} Q=[Q1Q2],Q1∈Rm×k, Q2∈Rm×(m−k)

由于 Q Q Q正交， Q 2 T Q 1 = 0 Q_2^T Q_1 = \boldsymbol{0} Q2TQ1=0，因此：
Q 2 T H f = Q 2 T Q [ R 0 ] = [ 0 I m − k ] T [ R 0 ] = 0 Q_2^T H_f = Q_2^T Q \begin{bmatrix} R \\ \boldsymbol{0} \end{bmatrix} = \begin{bmatrix} \boldsymbol{0} \\ I_{m-k} \end{bmatrix}^T \begin{bmatrix} R \\ \boldsymbol{0} \end{bmatrix} = \boldsymbol{0} Q2THf=Q2TQ[R0]=[0Im−k]T[R0]=0

即 Q 2 Q_2 Q2是 H f H_f Hf左零空间的基（ Z = Q 2 \boldsymbol{Z}=Q_2 Z=Q2）。将 Q 2 T Q_2^T Q2T作用于残差和 H x H_x Hx，得到：
r ′ = Q 2 T r , H x ′ = Q 2 T H x \boldsymbol{r}' = Q_2^T \boldsymbol{r}, \quad H_x' = Q_2^T H_x r′=Q2Tr,Hx′=Q2THx

新的约束为 r ′ = H x ′ x \boldsymbol{r}' = H_x' \boldsymbol{x} r′=Hx′x，仅包含系统状态 x \boldsymbol{x} x。

三、代码的数学实现解析

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

1. Givens旋转的作用

Givens旋转是2×2正交矩阵，形式为：
G ( i , j , θ ) = [ 1 cos ⁡ θ − sin ⁡ θ sin ⁡ θ cos ⁡ θ ⋱ 1 ] G(i,j,\theta) = \begin{bmatrix} 1 & & & & \\ & \cos\theta & -\sin\theta & & \\ & \sin\theta & \cos\theta & & \\ & & & \ddots & \\ & & & & 1 \end{bmatrix} G(i,j,θ)= 1cosθsinθ−sinθcosθ⋱1

用于消去矩阵中指定位置的元素（如 H f ( m , n ) H_f(m,n) Hf(m,n)）。代码中tempHo_GR.makeGivens(a,b)会计算旋转角 θ \theta θ，使得：

cos ⁡ θ sin ⁡ θ − sin ⁡ θ cos ⁡ θ \] \[ a b \] = \[ a 2 + b 2 0 \] \\begin{bmatrix} \\cos\\theta \& \\sin\\theta \\\\ -\\sin\\theta \& \\cos\\theta \\end{bmatrix} \\begin{bmatrix} a \\\\ b \\end{bmatrix} = \\begin{bmatrix} \\sqrt{a\^2+b\^2} \\\\ 0 \\end{bmatrix} \[cosθ−sinθsinθcosθ\]\[ab\]=\[a2+b2 0

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

每次旋转后，需将Givens矩阵作用于 H f H_f Hf、 H x H_x Hx和 r \boldsymbol{r} r（因为 Q Q Q是Givens旋转的乘积，作用 G T G^T GT等价于累积 Q T Q^T QT）。

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

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

  • H x = H x . b l o c k ( H f . c o l s ( ) , 0 , H x . r o w s ( ) − H f . c o l s ( ) , H x . c o l s ( ) ) H_x = H_x.block(H_f.cols(), 0, H_x.rows()-H_f.cols(), H_x.cols()) Hx=Hx.block(Hf.cols(),0,Hx.rows()−Hf.cols(),Hx.cols())：提取 H x H_x Hx从第 k k k行开始的 ( m − k ) (m-k) (m−k)行（即 Q 2 T H x Q_2^T H_x Q2THx）；
  • r = r . b l o c k ( H f . c o l s ( ) , 0 , r . r o w s ( ) − H f . c o l s ( ) , r . c o l s ( ) ) \boldsymbol{r} = \boldsymbol{r}.block(H_f.cols(), 0, \boldsymbol{r}.rows()-H_f.cols(), \boldsymbol{r}.cols()) r=r.block(Hf.cols(),0,r.rows()−Hf.cols(),r.cols())：提取 r \boldsymbol{r} r从第 k k k行开始的 ( m − k ) (m-k) (m−k)行（即 Q 2 T r Q_2^T \boldsymbol{r} Q2Tr）。

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

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

1. 动态大小（运行时确定维度）
cpp 复制代码 
matrix.block(startRow, startCol, numRows, numCols)

参数说明：

  • startRow：子块起始行索引（从0开始）；
  • startCol：子块起始列索引；
  • numRows：子块的行数；
  • numCols：子块的列数。
2. 静态大小（编译期确定维度，更高效）
cpp 复制代码 
matrix.block<numRows, numCols>(startRow, startCol)
代码中.block()的具体应用

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

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

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

    从 H x H_x Hx的 ( m − 1 ) (m-1) (m−1)行、0列开始，取2行 、所有列的子块，应用相同的Givens旋转（保证变换一致性）。

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

    从残差向量 r \boldsymbol{r} r的 ( m − 1 ) (m-1) (m−1)行开始，取2行1列的子块，应用Givens旋转。

  4. H_x.block(H_f.cols(), 0, H_x.rows()-H_f.cols(), H_x.cols())：

    从 H x H_x Hx的 k k k行（ H f H_f Hf的列数）开始，取** m − k m-k m−k行**（左零空间维度）、所有列 的子块，即 Q 2 T H x Q_2^T H_x Q2THx。

五、总结

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

cpp 复制代码 
/**
 * @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 H_f Hf的QR分解（正交三角分解） ，并同步完成对 H x H_x Hx和残差 r \boldsymbol{r} r的正交变换，最终为构造 H f H_f Hf的左零空间、消去特征点参数冗余性奠定基础。具体作用可分为以下三层：

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

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

对于矩阵 H f H_f Hf中的元素 H f ( m − 1 , n ) H_f(m-1, n) Hf(m−1,n)和 H f ( m , n ) H_f(m, n) Hf(m,n)（第 n n n列的第 m − 1 m-1 m−1行与第 m m m行），Givens旋转矩阵 G G G满足：
G ⋅ [ H f ( m − 1 , n ) H f ( m , n ) ] = [ H f ( m − 1 , n ) 2 + H f ( m , n ) 2 0 ] G \cdot \begin{bmatrix} H_f(m-1, n) \\ H_f(m, n) \end{bmatrix} = \begin{bmatrix} \sqrt{H_f(m-1, n)^2 + H_f(m, n)^2} \\ 0 \end{bmatrix} G⋅[Hf(m−1,n)Hf(m,n)]=[Hf(m−1,n)2+Hf(m,n)2 0]

即通过旋转消去第 m m m行第 n n n列的元素，仅保留第 m − 1 m-1 m−1行第 n n n列的非零值。

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

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

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

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

三、同步变换 H x H_x Hx和 r \boldsymbol{r} r：保证约束的一致性

左零空间投影需要将正交矩阵 Q Q Q的左零空间部分 Q 2 T Q_2^T Q2T（ Q Q Q的后 m − k m-k m−k行）作用于 H x H_x Hx和 r \boldsymbol{r} r（得到 H x ′ = Q 2 T H x H_x' = Q_2^T H_x Hx′=Q2THx、 r ′ = Q 2 T r \boldsymbol{r}' = Q_2^T \boldsymbol{r} r′=Q2Tr）。

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

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

QR分解后，正交矩阵 Q Q Q可分块为 Q = [ Q 1 Q 2 ] Q = \begin{bmatrix} Q_1 & Q_2 \end{bmatrix} Q=[Q1Q2]（ Q 1 Q_1 Q1为前 k k k列， Q 2 Q_2 Q2为后 m − k m-k m−k列），其中 Q 2 Q_2 Q2正是 H f H_f Hf的左零空间基（满足 Q 2 T H f = 0 Q_2^T H_f = 0 Q2THf=0）。

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

总结

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

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

一、Householder变换（Householder Reflections）

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

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

Householder矩阵定义为：
H = I − 2 v v T v T v H = I - 2\frac{\boldsymbol{v}\boldsymbol{v}^T}{\boldsymbol{v}^T\boldsymbol{v}} H=I−2vTvvvT

其中 v \boldsymbol{v} v是反射向量，满足 H T = H H^T = H HT=H且 H T H = I H^T H = I HTH=I（正交性）。

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

2. 步骤示例（以 3 × 3 3 \times 3 3×3矩阵为例）
  • 对第1列构造 H 1 H_1 H1，消去第1列第2、3行元素；
  • 对第2列构造 H 2 H_2 H2，消去第2列第3行元素；
  • 最终 H 2 H 1 A = R H_2 H_1 A = R H2H1A=R，故 A = ( H 1 H 2 ) R = Q R A = (H_1 H_2) R = QR A=(H1H2)R=QR（ Q = H 1 H 2 Q = H_1 H_2 Q=H1H2）。
3. 特点与应用
  • 效率高：每次消去一列的下方元素，计算量约为Givens旋转的一半（尤其适合满秩矩阵）；
  • 数值稳定性好 ：是数值计算中最常用的QR分解方法 （如Eigen库的ColPivHouseholderQR默认实现）；
  • 适合大规模稠密矩阵的QR分解，在SLAM优化中常用于批量处理雅可比矩阵。

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

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

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

设 A = [ a 1 , a 2 , . . . , a n ] A = [\boldsymbol{a}_1, \boldsymbol{a}_2, ..., \boldsymbol{a}_n] A=[a1,a2,...,an]（列向量组），正交化步骤为：

  • 第一步 ： q 1 = a 1 ∥ a 1 ∥ \boldsymbol{q}_1 = \frac{\boldsymbol{a}_1}{\|\boldsymbol{a}1\|} q1=∥a1∥a1， R 11 = ∥ a 1 ∥ R{11} = \|\boldsymbol{a}_1\| R11=∥a1∥；
  • 第二步 ： r 12 = q 1 T a 2 \boldsymbol{r}_{12} = \boldsymbol{q}_1^T \boldsymbol{a}_2 r12=q1Ta2， b 2 = a 2 − r 12 q 1 \boldsymbol{b}_2 = \boldsymbol{a}2 - \boldsymbol{r}{12} \boldsymbol{q}_1 b2=a2−r12q1， q 2 = b 2 ∥ b 2 ∥ \boldsymbol{q}_2 = \frac{\boldsymbol{b}_2}{\|\boldsymbol{b}2\|} q2=∥b2∥b2， R 22 = ∥ b 2 ∥ R{22} = \|\boldsymbol{b}_2\| R22=∥b2∥；
  • 第 k k k步 ： r i k = q i T a k \boldsymbol{r}_{ik} = \boldsymbol{q}_i^T \boldsymbol{a}_k rik=qiTak（ i = 1 , . . . , k − 1 i=1,...,k-1 i=1,...,k−1）， b k = a k − ∑ i = 1 k − 1 r i k q i \boldsymbol{b}k = \boldsymbol{a}k - \sum{i=1}^{k-1} \boldsymbol{r}{ik} \boldsymbol{q}_i bk=ak−∑i=1k−1rikqi， q k = b k ∥ b k ∥ \boldsymbol{q}_k = \frac{\boldsymbol{b}_k}{\|\boldsymbol{b}k\|} qk=∥bk∥bk， R k k = ∥ b k ∥ R{kk} = \|\boldsymbol{b}_k\| Rkk=∥bk∥。

最终 A = Q R A = QR A=QR，其中 Q = [ q 1 , q 2 , . . . , q n ] Q = [\boldsymbol{q}_1, \boldsymbol{q}_2, ..., \boldsymbol{q}n] Q=[q1,q2,...,qn]（正交矩阵）， R R R为上三角矩阵（ R i k = q i T a k R{ik} = \boldsymbol{q}_i^T \boldsymbol{a}_k Rik=qiTak）。

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

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

  • 对每个 a k \boldsymbol{a}_k ak，先减去其在已正交化向量 q 1 , . . . , q k − 1 \boldsymbol{q}1,...,\boldsymbol{q}{k-1} q1,...,qk−1上的投影，再归一化。
3. 特点与应用
  • 直观易懂：原理基于向量正交化，适合理论推导；
  • 数值稳定性：MGS优于CGS，但仍不如Householder变换；
  • 适合小规模矩阵或需要显式构造正交基的场景（如线性代数教学、低维优化问题）。

三、QR迭代法（QR Iteration）

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

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

基本迭代步骤：

  • 初始化 A 0 = A A_0 = A A0=A；
  • 对 A k A_k Ak做QR分解： A k = Q k R k A_k = Q_k R_k Ak=QkRk；
  • 构造 A k + 1 = R k Q k A_{k+1} = R_k Q_k Ak+1=RkQk；
  • 重复迭代直到 A k A_k Ak收敛为上三角矩阵（对角元素为特征值）。
2. 特点与应用
  • 主要用于特征值/特征向量求解（如对称矩阵的特征值分解）；
  • 是数值线性代数中求解稠密矩阵特征值的核心算法；
  • 在SLAM中可用于协方差矩阵的特征值分析（如EKF的可观测性分析）。

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

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

五、在SLAM中的应用关联

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

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

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

一、Householder变换（Householder Reflections）

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

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

Householder矩阵定义为：
H = I − 2 v v T v T v H = I - 2\frac{\boldsymbol{v}\boldsymbol{v}^T}{\boldsymbol{v}^T\boldsymbol{v}} H=I−2vTvvvT

其中 v \boldsymbol{v} v是反射向量，满足 H T = H H^T = H HT=H且 H T H = I H^T H = I HTH=I（正交性）。

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

2. 步骤示例（以 3 × 3 3 \times 3 3×3矩阵为例）
  • 对第1列构造 H 1 H_1 H1，消去第1列第2、3行元素；
  • 对第2列构造 H 2 H_2 H2，消去第2列第3行元素；
  • 最终 H 2 H 1 A = R H_2 H_1 A = R H2H1A=R，故 A = ( H 1 H 2 ) R = Q R A = (H_1 H_2) R = QR A=(H1H2)R=QR（ Q = H 1 H 2 Q = H_1 H_2 Q=H1H2）。
3. 特点与应用
  • 效率高：每次消去一列的下方元素，计算量约为Givens旋转的一半（尤其适合满秩矩阵）；
  • 数值稳定性好 ：是数值计算中最常用的QR分解方法 （如Eigen库的ColPivHouseholderQR默认实现）；
  • 适合大规模稠密矩阵的QR分解，在SLAM优化中常用于批量处理雅可比矩阵。

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

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

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

设 A = [ a 1 , a 2 , . . . , a n ] A = [\boldsymbol{a}_1, \boldsymbol{a}_2, ..., \boldsymbol{a}_n] A=[a1,a2,...,an]（列向量组），正交化步骤为：

  • 第一步 ： q 1 = a 1 ∥ a 1 ∥ \boldsymbol{q}_1 = \frac{\boldsymbol{a}_1}{\|\boldsymbol{a}1\|} q1=∥a1∥a1， R 11 = ∥ a 1 ∥ R{11} = \|\boldsymbol{a}_1\| R11=∥a1∥；
  • 第二步 ： r 12 = q 1 T a 2 \boldsymbol{r}_{12} = \boldsymbol{q}_1^T \boldsymbol{a}_2 r12=q1Ta2， b 2 = a 2 − r 12 q 1 \boldsymbol{b}_2 = \boldsymbol{a}2 - \boldsymbol{r}{12} \boldsymbol{q}_1 b2=a2−r12q1， q 2 = b 2 ∥ b 2 ∥ \boldsymbol{q}_2 = \frac{\boldsymbol{b}_2}{\|\boldsymbol{b}2\|} q2=∥b2∥b2， R 22 = ∥ b 2 ∥ R{22} = \|\boldsymbol{b}_2\| R22=∥b2∥；
  • 第 k k k步 ： r i k = q i T a k \boldsymbol{r}_{ik} = \boldsymbol{q}_i^T \boldsymbol{a}_k rik=qiTak（ i = 1 , . . . , k − 1 i=1,...,k-1 i=1,...,k−1）， b k = a k − ∑ i = 1 k − 1 r i k q i \boldsymbol{b}k = \boldsymbol{a}k - \sum{i=1}^{k-1} \boldsymbol{r}{ik} \boldsymbol{q}_i bk=ak−∑i=1k−1rikqi， q k = b k ∥ b k ∥ \boldsymbol{q}_k = \frac{\boldsymbol{b}_k}{\|\boldsymbol{b}k\|} qk=∥bk∥bk， R k k = ∥ b k ∥ R{kk} = \|\boldsymbol{b}_k\| Rkk=∥bk∥。

最终 A = Q R A = QR A=QR，其中 Q = [ q 1 , q 2 , . . . , q n ] Q = [\boldsymbol{q}_1, \boldsymbol{q}_2, ..., \boldsymbol{q}n] Q=[q1,q2,...,qn]（正交矩阵）， R R R为上三角矩阵（ R i k = q i T a k R{ik} = \boldsymbol{q}_i^T \boldsymbol{a}_k Rik=qiTak）。

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

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

  • 对每个 a k \boldsymbol{a}_k ak，先减去其在已正交化向量 q 1 , . . . , q k − 1 \boldsymbol{q}1,...,\boldsymbol{q}{k-1} q1,...,qk−1上的投影，再归一化。
3. 特点与应用
  • 直观易懂：原理基于向量正交化，适合理论推导；
  • 数值稳定性：MGS优于CGS，但仍不如Householder变换；
  • 适合小规模矩阵或需要显式构造正交基的场景（如线性代数教学、低维优化问题）。

三、QR迭代法（QR Iteration）

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

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

基本迭代步骤：

  • 初始化 A 0 = A A_0 = A A0=A；
  • 对 A k A_k Ak做QR分解： A k = Q k R k A_k = Q_k R_k Ak=QkRk；
  • 构造 A k + 1 = R k Q k A_{k+1} = R_k Q_k Ak+1=RkQk；
  • 重复迭代直到 A k A_k Ak收敛为上三角矩阵（对角元素为特征值）。
2. 特点与应用
  • 主要用于特征值/特征向量求解（如对称矩阵的特征值分解）；
  • 是数值线性代数中求解稠密矩阵特征值的核心算法；
  • 在SLAM中可用于协方差矩阵的特征值分析（如EKF的可观测性分析）。

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

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

五、在SLAM中的应用关联

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

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

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

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

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

其中：

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

对观测方程做一阶泰勒展开 （线性化），残差可表示为雅可比与参数增量的乘积：
r ≈ ∂ h ∂ x Δ x + ∂ h ∂ f Δ f \boldsymbol{r} \approx \frac{\partial h}{\partial \boldsymbol{x}} \Delta\boldsymbol{x} + \frac{\partial h}{\partial \boldsymbol{f}} \Delta\boldsymbol{f} r≈∂x∂hΔx+∂f∂hΔf

将雅可比分块记为：
r ⏟ m × 1 = H x ⏟ m × n Δ x + H f ⏟ m × k Δ f \underbrace{\boldsymbol{r}}{m \times 1} = \underbrace{H_x}{m \times n} \Delta\boldsymbol{x} + \underbrace{H_f}_{m \times k} \Delta\boldsymbol{f} m×1 r=m×n HxΔx+m×k HfΔf

其中：

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

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

步骤2：对 H f H_f Hf做QR分解

对 H f ∈ R m × k H_f \in \mathbb{R}^{m \times k} Hf∈Rm×k（通常 m > k m > k m>k且 H f H_f Hf列满秩）做正交三角分解（QR分解） ，形式为：
H f = Q ⋅ [ R 0 ] H_f = Q \cdot \begin{bmatrix} R \\ \boldsymbol{0} \end{bmatrix} Hf=Q⋅[R0]

各矩阵的定义与原因：

  • Q ∈ R m × m Q \in \mathbb{R}^{m \times m} Q∈Rm×m：正交矩阵 ，满足 Q T Q = I Q^T Q = I QTQ=I（ I I I为单位矩阵）。

    原因：正交矩阵的变换保持内积和范数，数值稳定性强，且分块后可自然分离出零空间基。

  • R ∈ R k × k R \in \mathbb{R}^{k \times k} R∈Rk×k：上三角可逆矩阵 。

    原因：QR分解将列满秩矩阵分解为"正交矩阵+上三角矩阵"，上三角矩阵便于后续计算（如求逆）。

  • 0 ∈ R ( m − k ) × k \boldsymbol{0} \in \mathbb{R}^{(m-k) \times k} 0∈R(m−k)×k：零矩阵 。

    原因： H f H_f Hf是 m × k m \times k m×k矩阵（ m > k m > k m>k），QR分解后下方会自然出现零块，对应"行维度冗余"的部分。

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

将正交矩阵 Q Q Q按列分块 为：
Q = [ Q 1 Q 2 ] Q = \begin{bmatrix} Q_1 & Q_2 \end{bmatrix} Q=[Q1Q2]

其中：

  • Q 1 ∈ R m × k Q_1 \in \mathbb{R}^{m \times k} Q1∈Rm×k： Q Q Q的前 k k k列，维度匹配 R R R的行数 k k k；
  • Q 2 ∈ R m × ( m − k ) Q_2 \in \mathbb{R}^{m \times (m-k)} Q2∈Rm×(m−k)： Q Q Q的后 ( m − k ) (m-k) (m−k)列，维度匹配零矩阵的行数 ( m − k ) (m-k) (m−k)。
分块的原因：

为了匹配 H f H_f Hf的QR分解形式 [ R 0 ] \begin{bmatrix} R \\ \boldsymbol{0} \end{bmatrix} [R0]，后续可通过分块矩阵乘法分离出与 H f H_f Hf相关/无关的部分。

步骤4：利用正交性，推导 Q 1 Q_1 Q1与 Q 2 Q_2 Q2的关系

由于 Q Q Q是正交矩阵，满足 Q T Q = I Q^T Q = I QTQ=I。将 Q Q Q分块代入得：
Q T Q = [ Q 1 T Q 2 T ] [ Q 1 Q 2 ] = [ Q 1 T Q 1 Q 1 T Q 2 Q 2 T Q 1 Q 2 T Q 2 ] = [ I k 0 0 I m − k ] Q^T Q = \begin{bmatrix} Q_1^T \\ Q_2^T \end{bmatrix} \begin{bmatrix} Q_1 & Q_2 \end{bmatrix} = \begin{bmatrix} Q_1^T Q_1 & Q_1^T Q_2 \\ Q_2^T Q_1 & Q_2^T Q_2 \end{bmatrix} = \begin{bmatrix} I_k & \boldsymbol{0} \\ \boldsymbol{0} & I_{m-k} \end{bmatrix} QTQ=[Q1TQ2T][Q1Q2]=[Q1TQ1Q2TQ1Q1TQ2Q2TQ2]=[Ik00Im−k]

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

  1. Q 1 T Q 1 = I k Q_1^T Q_1 = I_k Q1TQ1=Ik（ Q 1 Q_1 Q1的列是正交单位向量）；
  2. Q 2 T Q 2 = I m − k Q_2^T Q_2 = I_{m-k} Q2TQ2=Im−k（ Q 2 Q_2 Q2的列是正交单位向量）；
  3. Q 2 T Q 1 = 0 Q_2^T Q_1 = \boldsymbol{0} Q2TQ1=0（ Q 1 Q_1 Q1与 Q 2 Q_2 Q2的列互相正交）。
这一步的原因：

正交性是后续证明" Q 2 Q_2 Q2是左零空间基"的核心前提。

步骤5：证明 Q 2 Q_2 Q2是 H f H_f Hf的左零空间基

左零空间的定义是：若 Z T H f = 0 \boldsymbol{Z}^T H_f = \boldsymbol{0} ZTHf=0，则 Z \boldsymbol{Z} Z是 H f H_f Hf左零空间的基。

我们将 H f = Q [ R 0 ] H_f = Q \begin{bmatrix} R \\ \boldsymbol{0} \end{bmatrix} Hf=Q[R0]代入 Q 2 T H f Q_2^T H_f Q2THf，计算得：
Q 2 T H f = Q 2 T ⋅ Q ⋅ [ R 0 ] Q_2^T H_f = Q_2^T \cdot Q \cdot \begin{bmatrix} R \\ \boldsymbol{0} \end{bmatrix} Q2THf=Q2T⋅Q⋅[R0]

结合 Q Q Q的分块形式 Q = [ Q 1 Q 2 ] Q = [Q_1 Q_2] Q=[Q1Q2]， Q 2 T Q = [ Q 2 T Q 1 Q 2 T Q 2 ] = [ 0 I m − k ] Q_2^T Q = \begin{bmatrix} Q_2^T Q_1 & Q_2^T Q_2 \end{bmatrix} = \begin{bmatrix} \boldsymbol{0} & I_{m-k} \end{bmatrix} Q2TQ=[Q2TQ1Q2TQ2]=[0Im−k]（由步骤4的结论3）。因此：
Q 2 T H f = [ 0 I m − k ] ⋅ [ R 0 ] = 0 ⋅ R + I m − k ⋅ 0 = 0 ( m − k ) × k Q_2^T H_f = \begin{bmatrix} \boldsymbol{0} & I_{m-k} \end{bmatrix} \cdot \begin{bmatrix} R \\ \boldsymbol{0} \end{bmatrix} = \boldsymbol{0} \cdot R + I_{m-k} \cdot \boldsymbol{0} = \boldsymbol{0}_{(m-k) \times k} Q2THf=[0Im−k]⋅[R0]=0⋅R+Im−k⋅0=0(m−k)×k

这说明 Q 2 Q_2 Q2满足左零空间的定义（ Q 2 T H f = 0 Q_2^T H_f = \boldsymbol{0} Q2THf=0），因此 Q 2 Q_2 Q2的列是 H f H_f Hf左零空间的一组正交基。

这一步的原因：

这是整个流程的核心------通过QR分解的分块正交矩阵，构造出了能消去 H f H_f Hf的左零空间基 Q 2 Q_2 Q2。

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

将左零空间基 Q 2 T Q_2^T Q2T作用于原残差 r \boldsymbol{r} r和** 系统状态雅可比 H x H_x Hx** ，得到新的残差和雅可比：
r ′ = Q 2 T r , H x ′ = Q 2 T H x \boldsymbol{r}' = Q_2^T \boldsymbol{r}, \quad H_x' = Q_2^T H_x r′=Q2Tr,Hx′=Q2THx

将原线性化残差 r = H x Δ x + H f Δ f \boldsymbol{r} = H_x \Delta\boldsymbol{x} + H_f \Delta\boldsymbol{f} r=HxΔx+HfΔf两边左乘 Q 2 T Q_2^T Q2T，得：
Q 2 T r = Q 2 T H x Δ x + Q 2 T H f Δ f Q_2^T \boldsymbol{r} = Q_2^T H_x \Delta\boldsymbol{x} + Q_2^T H_f \Delta\boldsymbol{f} Q2Tr=Q2THxΔx+Q2THfΔf

由于 Q 2 T H f = 0 Q_2^T H_f = \boldsymbol{0} Q2THf=0（步骤5的结论），上式简化为：
r ′ = H x ′ Δ x \boldsymbol{r}' = H_x' \Delta\boldsymbol{x} r′=Hx′Δx

这一步的原因：
  • 消去了冗余的特征点增量 Δ f \Delta\boldsymbol{f} Δf（ Q 2 T H f Δ f = 0 Q_2^T H_f \Delta\boldsymbol{f} = \boldsymbol{0} Q2THfΔf=0）；
  • 新约束 r ′ = H x ′ Δ x \boldsymbol{r}' = H_x' \Delta\boldsymbol{x} r′=Hx′Δx仅包含系统状态增量 Δ x \Delta\boldsymbol{x} Δx，实现了"只优化系统状态"的目标。

总结：流程的核心逻辑

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

相关推荐
AndrewHZ1 小时前
【图像处理基石】图像处理中的色彩经典算法原理与实战解析
图像处理·算法·计算机视觉·白平衡·色彩科学·经典算法·k means
Dev7z1 小时前
基于Matlab多算法的图像增强与客观质量评价系统
人工智能·算法·matlab
xcLeigh1 小时前
【新】Rust入门：基础语法应用
开发语言·算法·rust
小年糕是糕手1 小时前
【C++同步练习】类和对象（一）
java·开发语言·javascript·数据结构·c++·算法·排序算法
小O的算法实验室1 小时前
2025年IJPR SCI2区，基于混合邻域结构的高效稳定智能调度算法用于柔性作业车间调度，深度解析+性能实测
算法·论文复现·智能算法·智能算法改进
小年糕是糕手1 小时前
【C++同步练习】类和对象（二）
java·开发语言·javascript·数据结构·c++·算法·ecmascript
沙白猿1 小时前
B树 / B+树
数据结构·b树·算法
我不是彭于晏丶1 小时前
74. 搜索二维矩阵
数据结构·算法
一个处女座的程序猿1 小时前
AI之Algorithms：TheAlgorithms_Python(所有用 Python 实现的算法)的简介、安装和使用方法、案例应用之详细攻略
人工智能·python·算法
热门推荐
01GitHub 镜像站点02UV安装并设置国内源03安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）04BongoCat - 跨平台键盘猫动画工具05本地部署阿里最新开源的Z-Image06Linux下V2Ray安装配置指南07Meta第三代“分割一切”模型——SAM 3本地部署教程：首支持文本提示分割，400万概念、30毫秒响应，检测分割追踪一网打尽08【保姆级教程】免费使用Gemini3的5种方法！免翻墙/国内直连09Labelme从安装到标注：零基础完整指南1046个Nano-banana 精选提示词，持续更新中