背景

最近在做骨科导航的项目，有个需求是这样，有个标定好的探针，有一个双目摄像头通过看到探针上的小球可以算出来探针针尖的位置，医生在骨头上用探针点点，然后拿到进行配准。

我们现在的做法是先找三个标志点，然后进行一次粗配准拿到一个变换矩阵，随后进行精配准，即在骨头上点更多的点，一次次迭代计算。

这个地方的本质就是求一个刚体变换，找到一个点集{Pi}（探针点的点——Vega空间）到另一个点集{Qi}（CT上的点——CT空间）的对应关系，也就是{Pi}↔{Qi}。我们求解的就是 Qi = RPi + t

由于项目里的算法都是用VTK实现的，本文讨论的大多都是VTK的内部实现。

粗配准

这里的粗配准是解决了一个初始位姿未知的问题。其实理论上讲，在刚体变换问题中，三个不共线的点可以求得唯一的解来确定旋转矩阵和平移向量。但由于我们规划的点和医生点的点不可能是完全对应的，所以实际中这个地方仍会产生误差。

我们的最终目标就是找到最合适的旋转矩阵R，和平移向量t，让点集整体的误差最小，也就是下面的公式： \(\min_{R,t} \sum_{i=1}^N \|RP_i + t - Q_i\|^2\)

这个地方有两种方式求解，如果是PCL，Egien流派 更多的是用手写SVD的方式求解，而VTK则是用四元数求解，两种方式在结果上是一样的，只是方式不一样，下面我分别介绍一下两种方法：

SVD

一般这个地方直接求解计算难度会比较大，所以一般都是先消去平移，再单独求旋转；固定旋转，反向去求平移。

去除旋转的方式很简单，可以通过质心，然后每个点减去质心就可以使整体点集的质心在原点：

质心求解公式： \(\bar P = \frac{1}{N}\sum_{i=1}^{N} P_i\) 去中心化： \(P_i' = P_i - \bar P\) 经过这样处理之后的点集即摆脱了平移带来的干扰，让后续只需要关注求解旋转矩阵即可。

构建协方差矩阵

处理完平移干扰后，我们可以就可以从一堆杂乱的三维点中，提取出P点旋转到Q点需要旋转多少角度。这个地方我们就可以构造出一个协方差矩阵： \(H = \sum_{i=1}^{N} P_i' Q_i'^T\)

  • P_i’：去中心化后的术中探针点，是 3×1 列向量
  • Q_i’：去中心化后的CT点转置，是 1×3 行向量

两者相乘3×1 × 1×3 = 3×3 矩阵，遍历所有点累加，最终得到一个唯一的 3×3 协方差矩阵 H。这个地方的H就是把所有点位的xyz三个轴之间的偏转关系，关联程度，全部压缩成了一个结构化的数据。旋转矩阵确定后，两组点集的角度偏差已经完全矫正，仅剩整体位置偏移问题。此时结合原始点集的质心，通过质心差值减去旋转带来的位置偏移，即可精准算出平移向量。

四元数

这里的前置流程是一样的，都需要计算质心、去中心化、构建协方差矩阵。区别主要在于后面的解码旋转信息的数学工具。

这里主要是利用了四元数表征三维旋转，构造一个4*4对称矩阵K

从协方差矩阵提取9个分量，按照固定规则填充进去： \(K= \begin{bmatrix} S_{xx}+S_{yy}+S_{zz} & S_{yz}-S_{zy} & S_{zx}-S_{xz} & S_{xy}-S_{yx}\\ S_{yz}-S_{zy} & S_{xx}-S_{yy}-S_{zz} & S_{xy}+S_{yx} & S_{zx}+S_{xz}\\ S_{zx}-S_{xz} & S_{xy}+S_{yx} & -S_{xx}+S_{yy}-S_{zz} & S_{yz}+S_{zy}\\ S_{xy}-S_{yx} & S_{zx}+S_{xz} & S_{yz}+S_{zy} & -S_{xx}-S_{yy}+S_{zz} \end{bmatrix}\) 然后这个地方可以解释一下：

  • 首行首列：对角线为协方差矩阵迹（三轴累加和），其余项由协方差不对称分量
  • 右下 3×3 子块：对角线由三轴分量加减组合而成，交叉项由协方差对称分量
  • 矩阵为实对称矩阵，转置后与自身相等，保证全部特征值为实数，数值求解稳定。

随后对矩阵K做特征分解，取最小特征值对应的特征向量即最优的单位四元数。将四元数转换为旋转矩阵带入质心公式求解平移向量完成粗配准。 \(t=\bar Q - R\bar P\) 至此，我们就完成了快速粗配准。

精配准

由于很多手术都是毫米级的，仅靠粗配准仍无法满足精度要求，故需要精配准来进一步优化。这里我采用了ICP算法。

ICP核心流程如下：

  1. 用粗配准得到的矩阵做一次变换，匹配CT点集中距离最近的对应点
  2. 基于新的匹配关系，再求解一轮旋转矩阵和平移向量
  3. 判断是否收敛或达到最大迭代次数，若没有则重复迭代