点到面（Point-to-Plane）点云配准中信息矩阵计算详解

点到面（Point-to-Plane）ICP 点云配准中，信息矩阵（Information Matrix） 是稀疏非线性优化中的重要组成部分，常用于：

• SLAM 后端中误差建模；
• 估计配准参数的可靠性；
• 构建优化问题的 Hessian 近似；
• 作为联合优化中各残差项的权重；

下面将从点到面 ICP 的基本残差定义出发，完整推导信息矩阵 的数学公式和含义，并说明其几何意义与代码实现方式。

一、预备知识

1. 点到面残差定义

对于源点 $\mathbf{p}\_s$ 和目标点 $\mathbf{p}\_t$ 及其单位法向量 $\mathbf{n}\_t$，点到面的残差定义为：

$$r={\mathbf{n}}_t^{\top }\left(R{\mathbf{p}}_s+\mathbf{t}-{\mathbf{p}}_t\right)$$

其中：

• $R\in SO(3)$ 为旋转矩阵
• $\mathbf{t}\in {\mathbb{R}}^3$ 为平移向量

设最终优化变量为 $T=[R|\mathbf{t}]\in SE(3)$。

2. 最小化目标函数

我们希望通过非线性最小二乘最小化以下代价函数：

$$E(T)=\sum _{i=1}^N{\left[r_i(T)\right]}^2$$

目标是对变换 $T$ 进行迭代优化。

二、雅可比与高斯牛顿方法回顾

1. 李代数扰动建模

设当前估计位姿为 $T=[R|\mathbf{t}]$，我们在 $SE(3)$ 上做一次微小扰动 $\delta \mathbf{\xi }\in {\mathbb{R}}^6$，其左乘作用为：

$$T^{\prime }=\exp (\delta {\mathbf{\xi }}^{\wedge })T$$

其中：

$$\delta \mathbf{\xi }=\left[\begin{array}{c}\delta \mathbf{\varphi }\\ \delta \mathbf{t}\end{array}\right]\in {\mathbb{R}}^6,\delta \mathbf{\varphi }\in {\mathbb{R}}^3\text{ 是旋转扰动}$$

2. 一阶近似残差函数

令残差 $r(\mathbf{\xi })$ 关于 $\delta \mathbf{\xi }$ 在当前估计处做泰勒展开：

$$r(\delta \mathbf{\xi })\approx r(0)+\mathbf{J}\cdot \delta \mathbf{\xi }$$

其中 $\mathbf{J}=\frac{\partial r}{\partial \mathbf{\xi }}\in {\mathbb{R}}^{1\times 6}$ 是残差关于位姿扰动的 雅可比矩阵。

三、点到面残差的雅可比推导

考虑源点 $\mathbf{p}\_s$ 变换后的位置为：

$${\mathbf{p}}_s^{\prime }=R{\mathbf{p}}_s+\mathbf{t}$$

目标为最小化：

$$r={\mathbf{n}}_t^{\top }({\mathbf{p}}_s^{\prime }-{\mathbf{p}}_t)$$

对李代数扰动 $\delta \mathbf{\xi }$ 展开后：

$${\mathbf{p}}_s^{\prime }(\delta \mathbf{\xi })\approx (I+[\delta \mathbf{\varphi }{]}_{\times })R{\mathbf{p}}_s+\mathbf{t}+\delta \mathbf{t}$$

$$r\approx {\mathbf{n}}_t^{\top }\left((I+[\delta \mathbf{\varphi }{]}_{\times })R{\mathbf{p}}_s+\mathbf{t}+\delta \mathbf{t}-{\mathbf{p}}_t\right)$$

忽略高阶项，取导：

$$\frac{\partial r}{\partial \delta \mathbf{\varphi }}={\mathbf{n}}_t^{\top }[R{\mathbf{p}}_s{]}_{\times }=-{\mathbf{n}}_t^{\top }[R{\mathbf{p}}_s{]}_{\times }$$

$$\frac{\partial r}{\partial \delta \mathbf{t}}={\mathbf{n}}_t^{\top }$$

因此残差对 $SE(3)$ 的雅可比为：

$${\mathbf{J}}_i=\left[\begin{array}{cc}{\mathbf{n}}_t^{\top }[R{\mathbf{p}}_s{]}_{\times }& {\mathbf{n}}_t^{\top }\end{array}\right]\in {\mathbb{R}}^{1\times 6}$$

设 $\mathbf{q}\_s=R\mathbf{p}\_s$ 是当前配准下的源点，则可记为：

$${\mathbf{J}}_i=\left[\begin{array}{cc}{\mathbf{n}}_t^{\top }\cdot \left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]& {\mathbf{n}}_t^{\top }\end{array}\right]$$

四、信息矩阵推导

1. 定义

高斯牛顿算法中，我们需要构建 Hessian 近似矩阵（即信息矩阵）：

$$\mathbf{H}=\sum _{i=1}^N{\mathbf{J}}_i^{\top }{\mathbf{J}}_i\in {\mathbb{R}}^{6\times 6}$$

这是每个点对的残差雅可比外积之和，用于构建线性系统：

$$\mathbf{H}\delta \mathbf{\xi }=-\sum _i{\mathbf{J}}_i^{\top }{r}_i$$

信息矩阵 $\mathbf{H}$ 本质上是最小二乘优化中的 正定近似 Hessian，反映了配准结果在各个自由度方向上的 刚性程度。

2. 信息矩阵构造公式

设每个点对的 Jacobian 为 $J\_i\in {\mathbb{R}}^{1\times 6}$，则：

$$\mathbf{H}=\sum _{i=1}^N{J}_i^{\top }{J}_i$$

每一项贡献为：

$$J_i^{\top }{J}_i=\left[\begin{array}{c}\text{6x1}\end{array}\right]\cdot \left[\begin{array}{c}\text{1x6}\end{array}\right]\text{6x6 对称矩阵}$$

五、计算流程小结

给定：

• 源点云 $\mathbf{p}\_s^i$
• 目标点云 $\mathbf{p}\_t^i$
• 目标法向量 $\mathbf{n}\_t^i$（单位向量）
• 当前估计位姿 $T=(R,\mathbf{t})$

计算流程：

1. 对每个点对 $i$：

   • 变换源点：$\mathbf{q}\_s^i=R\mathbf{p}\_s^i$
   • 计算残差：$r\_i=\mathbf{n}\_t^{i\top }(\mathbf{q}\_s^i+\mathbf{t}-\mathbf{p}\_t^i)$
   • 构造雅可比 $\mathbf{J}\_i\in {\mathbb{R}}^{1\times 6}$

2. 信息矩阵累加：

   $$\mathbf{H}=\sum _i{\mathbf{J}}_i^{\top }{\mathbf{J}}_i$$

六、C++代码实现计算信息矩阵H

Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();

for (int i = 0; i < N; ++i) {
    Eigen::Vector3d ps = source_points[i];
    Eigen::Vector3d pt = target_points[i];
    Eigen::Vector3d nt = target_normals[i];

    Eigen::Vector3d qs = R * ps;
    Eigen::Vector3d diff = qs + t - pt;

    double r = nt.dot(diff);
    Eigen::Matrix<double, 1, 6> J;
    J.block<1,3>(0,0) = (qs.cross(nt)).transpose(); // rotation part
    J.block<1,3>(0,3) = nt.transpose();             // translation part

    H += J.transpose() * J;
}

七、几何意义

信息矩阵 $\mathbf{H}$ 的本质：

• 是关于配准误差的二阶导数近似；
• 反映当前配准结果的局部几何稳定性；
• 条件数大 ⇒ 某方向上约束弱（例如纯平面场景，Z轴不稳定）；
• 可用于评估某一帧的贡献程度（比如 SLAM 中是否关键帧）；

八 、雅可比矩阵详细推导与展开

雅可比$J_i$的形式如下：

$${\mathbf{J}}_i=\left[\begin{array}{cc}{\mathbf{n}}_t^{\top }[R{\mathbf{p}}_s{]}_{\times }& {\mathbf{n}}_t^{\top }\end{array}\right]\in {\mathbb{R}}^{1\times 6}$$

记号说明：

• $\mathbf{p}\_s\in {\mathbb{R}}^3$：源点
• $\mathbf{p}\_t\in {\mathbb{R}}^3$：目标点
• $\mathbf{n}\_t\in {\mathbb{R}}^3$：目标点的单位法向量
• $R\in SO(3)$：当前位姿估计中的旋转矩阵
• $[\cdot ]\_\times$：反对称矩阵，定义如下

$$[\mathbf{v}{]}_{\times }=\left[\begin{array}{ccc}0& -v_z& v_y\\ v_z& 0& -v_x\\ -v_y& v_x& 0\end{array}\right]$$

8.1、向量表示展开

设：

$${\mathbf{q}}_s=R{\mathbf{p}}_s=\left[\begin{array}{c}x\\ y\\ z\end{array}\right],{\mathbf{n}}_t=\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]$$

则：

$$[{\mathbf{q}}_s{]}_{\times }=\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]$$

然后：

$${\mathbf{n}}_t^{\top }[{\mathbf{q}}_s{]}_{\times }\left[\begin{array}{ccc}{n}_x& n_y& n_z\end{array}\right]\left[\begin{array}{ccc}0& -z& y\\ z& 0& -x\\ -y& x& 0\end{array}\right]=\left[\begin{array}{c}{n}_{y}z-n_{z}y,n_{z}x-n_{x}z,n_{x}y-n_{y}x\end{array}\right]$$

即旋转部分雅可比为：

$$\frac{\partial r}{\partial \mathbf{\varphi }}={\mathbf{n}}_t^{\top }[R{\mathbf{p}}_s{]}_{\times }\in {\mathbb{R}}^{1\times 3}$$

平移部分是直接的：

$$\frac{\partial r}{\partial \mathbf{t}}={\mathbf{n}}_t^{\top }\in {\mathbb{R}}^{1\times 3}$$

所以整个雅可比展开为：

$${\mathbf{J}}_i=\left[\begin{array}{cccccc}{n}_{y}z-n_{z}y& n_{z}x-n_{x}z& n_{x}y-n_{y}x& n_x& n_y& n_z\end{array}\right]$$

8.2、几何解释：为什么是叉乘？

考虑 $SE(3)$ 李代数扰动中，旋转引起的误差是：

$$\delta \mathbf{p}=[\delta \mathbf{\varphi }{]}_{\times }R{\mathbf{p}}_s=-(R{\mathbf{p}}_s)\times \delta \mathbf{\varphi }$$

因此：

$$\frac{\partial r}{\partial \delta \mathbf{\varphi }}=\frac{\partial }{\partial \delta \mathbf{\varphi }}\left({\mathbf{n}}_t^{\top }(-R{\mathbf{p}}_s\times \delta \mathbf{\varphi })\right)=-{\mathbf{n}}_t^{\top }[R{\mathbf{p}}_s{]}_{\times }$$

所以这表示：当绕旋转轴扰动时，点 $R\mathbf{p}\_s$ 产生一个垂直于旋转轴的位移方向，该方向对法向量投影就是误差变化率。

8.3、信息矩阵中各项的含义

当构建信息矩阵：

$$\mathbf{H}=\sum _i{\mathbf{J}}_i^{\top }{\mathbf{J}}_i\in {\mathbb{R}}^{6\times 6}$$

它的结构如下：

每一项由雅可比矩阵的乘积贡献：

$${\mathbf{J}}_i^{\top }{\mathbf{J}}_i=\left[\begin{array}{cc}{\mathbf{J}}_{\text{rot}}^{\top }{\mathbf{J}}_{\text{rot}}& {\mathbf{J}}_{\text{rot}}^{\top }{\mathbf{J}}_{\text{trans}}\\ {\mathbf{J}}_{\text{trans}}^{\top }{\mathbf{J}}_{\text{rot}}& {\mathbf{J}}_{\text{trans}}^{\top }{\mathbf{J}}_{\text{trans}}\end{array}\right]$$

其中：

$${\mathbf{J}}_{\text{rot}}=-{\mathbf{n}}_t^{\top }[\mathbf{R}{\mathbf{p}}_s{]}_{\times }\in {\mathbb{R}}^{1\times 3}$$

$$\mathbf{J}\_\text{trans}=\mathbf{n}\_t^{\top }$$

8.4、典型应用场景

• 在 SLAM 后端中，用 $\mathbf{H}$ 的对角元素判断某帧或某约束的信息量；
• 在多帧优化中，使用 $\mathbf{H}$ 构造误差项协方差近似 $C\approx H^{-1}$；
• 对于退化检测，查看 $\mathbf{H}$ 的特征值分布是否存在很小的方向（如 Z 向不确定）；