前言

本文记录了一个机器人超声领域的配准问题。这和点云配准等问题在数学层面上都是一致的，本文的目的是从理论层面分析如何计算配准矩阵。（本文为了书写方便，向量等符号不用粗体表示，大家心里有数）

问题描述

如Fig. 1所示， { B } \{B\} {B}是机器人底座坐标系， { E } \{E\} {E}是机器人法兰坐标系， { I } \{I\} {I}是超声图像坐标系。 { I } \{I\} {I}的原点是超声图像中的索引为$(u,v)=(0,0)$（即超声图像最左上角的点）的像素点对应的物理位置，其中$u$和$v$分别是超声图像像素点的行索引和列索引。 { I } \{I\} {I}的$\text{x}$轴正方向是超声图像像素点列索引增加的方向， { I } \{I\} {I}的$\text{y}$轴正方向是超声图像像素点行索引增加的方向。

我们有一个水缸（水缸里面的水用于超声波传递，不然无法超声成像），里面有两条交叉的线，两条线交于一点${}^{B}p\in {\mathbb{R}}^3$，即$p$点在机器人底座坐标系 { B } \{B\} {B}中的坐标表示是${}^{B}p$。我们将超声探头移动并保证${}^{B}p$这点在超声图像中显示出来，我们记${}^{B}p$在超声图像中的成像点的像素为$(r,c)$。我们可以得到：
$${}^B{T}_I{}^E{T}_I{}^{I}p={}^{B}p\text{\hspace{1em}(1)}$$
其中${}^{I}p=(s_{v}v,s_{u}u,0,1{)}^T$，$s_v$和$s_u$是已知的像素到真实物理世界的缩放系数。

Fig. 1

和其它很多的标定方法（比如经典的eye-in-hand，eye-to-hand标定）一样，我们需要采集多组数据$<{}^B{T}_I(i),{}^{I}p(i)>$，$i=1,...,n$，且这些数据理论上满足
$${}^B{T}_I(i){}^E{T}_I{}^{I}p(i)={}^{B}p\text{\hspace{1em}(2)}$$

现在的问题是如何求解${}^E{T}_I$？最小二乘法？需要注意的是
$${}^E{T}_I=\left[\begin{array}{cc}R& t\\ 0_{1\times 3}& 1\end{array}\right],R\in SO(3)\text{\hspace{1em}(3)}$$
也就是说，这个优化是有约束优化（在$SO(3)$流形上的优化），所以直接最小二乘法似乎行不通（这个优化问题还是挺需要技巧的）。

我们正式把这个优化问题写成
$$\underset{{}^E{T}_I\in SE(3)}{\min }\sum _{i=1}^n\Vert {}^E{T}_I{}^{I}p(i)-({}^B{T}_I(i){)}^{-1}{}^{B}p{\Vert }_2^2\text{\hspace{1em}(4)}$$
为了简便，我们记$q_i$是$({}^B{T}_I(i){)}^{-1}{}^{B}p$的前三维度的列向量，记$p_i$是$p(i)$的前三维度的列向量，另外将${}^E{T}_I$用(3)式中的$R$和$t$来表示，得到
$$\underset{R\in SO(3),t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n\Vert R{p}_i+t-q_i{\Vert }_2^2\text{\hspace{1em}(5)}$$
对于(5)式，我们化简如下，
$$\begin{gathered} \underset{R\in SO(3),t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n\Vert R{p}_i+t-q_i{\Vert }_2^2 \\ =\underset{R\in SO(3),t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n(R{p}_i+t-q_i{)}^T(R{p}_i+t-q_i) \\ =\underset{R\in SO(3),t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n[p_i^T{R}^{T}R{p}_i+2(t-q_i{)}^{T}R{p}_i+(t-q_i{)}^T(t-q_i)] \\ =\underset{R\in SO(3),t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n[p_i^T{p}_i+2(t-q_i{)}^{T}R{p}_i+(t-q_i{)}^T(t-q_i)]\text{\hspace{1em}(6)} \end{gathered}$$
对于求解(6)式的最小值，一个直观的想法是对$R$和$t$分别求偏导数，然后令偏导数为0，可以得到：
$$\begin{gathered} R\sum _{i=1}^n(p_i{q}_i^T)=\sum _{i=1}^n(q_i-t)p_i^T \\ t=\frac{1}{n}\sum _{i=1}^n(q_i-R{p}_i)\text{\hspace{1em}(7)} \end{gathered}$$
注：在对$R$求导的时候，可以用$(uv{)}^{\prime }=u^{\prime }v+u{v}^{\prime }$来求，这样容易计算一些。对$t$求导直接计算就可以。
为了进一步简化符号，记$\bar{p}=\frac{1}{n}{\sum }_{i=1}^n{p}_i$，$\bar{q}=\frac{1}{n}{\sum }_{i=1}^n{q}_i$，${\tilde{p}}_i=p_i-\bar{p}$，${\tilde{q}}_i=q_i-\bar{q}$，代入(7)式化简得：
$$\begin{gathered} R\sum _{i=1}^n({\tilde{p}}_i{\tilde{p}}_i^T)=\sum _{i=1}^n({\tilde{q}}_i{\tilde{p}}_i^T) \\ t=\bar{q}-R\bar{p}\text{\hspace{1em}(8)} \end{gathered}$$
注意，(8)式中的${\sum }_{i=1}^n({\tilde{p}}_i{\tilde{p}}_i^T)$不一定可逆，即使可逆，$[{\sum }_{i=1}^n({\tilde{q}}_i{\tilde{p}}_i^T)][{\sum }_{i=1}^n({\tilde{p}}_i{\tilde{p}}_i^T){]}^{-1}$也不能保证属于$SO(3)$。所以求解偏导这个方法不可行。

求解方法

回到(6)式这个优化问题：
$$\underset{R\in SO(3),t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n[p_i^T{p}_i+2(t-q_i{)}^{T}R{p}_i+(t-q_i{)}^T(t-q_i)]$$
我们把这个优化问题看作是一个两个自变量（$R$和$t$）的优化问题，$t$是一个无约束自变量，$R\in SO(3)$是一个有约束自变量。
这个问题可以等价于
$$\begin{gathered} \underset{R\in SO(3),t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n[p_i^T{p}_i+2(t-q_i{)}^{T}R{p}_i+(t-q_i{)}^T(t-q_i)] \\ =\underset{R\in SO(3)}{\min }\underset{t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n[p_i^T{p}_i+2(t-q_i{)}^{T}R{p}_i+(t-q_i{)}^T(t-q_i)]\text{\hspace{1em}(9)} \end{gathered}$$

当我们固定$R$，
$$\underset{t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n[p_i^T{p}_i+2(t-q_i{)}^{T}R{p}_i+(t-q_i{)}^T(t-q_i)]\text{\hspace{1em}(10)}$$
(10)式这个优化问题很简单，直接对$t$求导就可以了，对$t$求导的结果(8)式已经得到，即$t=\bar{q}-R\bar{p}$。将$t=\bar{q}-R\bar{p}$代入(9)式，得
$$\begin{gathered} \underset{t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n[p_i^T{p}_i+2(t-q_i{)}^{T}R{p}_i+(t-q_i{)}^T(t-q_i)] \\ =\sum _{i=1}^n[({\tilde{p}}_i^T+{\bar{p}}^T)({\tilde{p}}_i+\bar{p})+2(-{\tilde{q}}_i-R\bar{p}{)}^{T}R({\tilde{p}}_i+\bar{p})+(-{\tilde{q}}_i-R\bar{p}{)}^T(-{\tilde{q}}_i-R\bar{p})] \\ =\sum _{i=1}^n[({\tilde{p}}_i^T+{\bar{p}}^T)({\tilde{p}}_i+\bar{p})-2({\tilde{q}}_i+R\bar{p}{)}^{T}R({\tilde{p}}_i+\bar{p})+({\tilde{q}}_i+R\bar{p}{)}^T({\tilde{q}}_i+R\bar{p})] \\ =\sum _{i=1}^n({\tilde{p}}_i^T{\tilde{p}}_i-2{\tilde{q}}_i^{T}R{\tilde{p}}_i+{\tilde{q}}_i^T{\tilde{q}}_i) \\ =\sum _{i=1}^n({\tilde{p}}_i^T{\tilde{p}}_i+{\tilde{q}}_i^T{\tilde{q}}_i)-2\sum _{i=1}^n({\tilde{q}}_i^{T}R{\tilde{p}}_i)\text{\hspace{1em}(11)} \end{gathered}$$
注：在(11)式的推导过程中，使用了${\sum }_{i=1}^n{\tilde{p}}_i=0$和${\sum }_{i=1}^n{\tilde{q}}_i=0$这两个等式进行化简。

将(11)式代入(9)式得，
$$\begin{gathered} \underset{R\in SO(3),t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n[p_i^T{p}_i+2(t-q_i{)}^{T}R{p}_i+(t-q_i{)}^T(t-q_i)] \\ =\underset{R\in SO(3)}{\min }\underset{t\in {\mathbb{R}}^3}{\min }\sum _{i=1}^n[p_i^T{p}_i+2(t-q_i{)}^{T}R{p}_i+(t-q_i{)}^T(t-q_i)] \\ =\underset{R\in SO(3)}{\min }[\sum _{i=1}^n({\tilde{p}}_i^T{\tilde{p}}_i+{\tilde{q}}_i^T{\tilde{q}}_i)-2\sum _{i=1}^n({\tilde{q}}_i^{T}R{\tilde{p}}_i)]\text{\hspace{1em}(12)} \end{gathered}$$
(12)式的求最小值问题等价于(13)式的求最大值问题。
$$\underset{R\in SO(3)}{\max }[\sum _{i=1}^n({\tilde{q}}_i^{T}R{\tilde{p}}_i)]\text{\hspace{1em}(13)}$$
求解(13)式这个优化问题，需要一些关于矩阵迹的技巧，符号$\text{tr}$表示矩阵的迹，
$$\begin{gathered} \underset{R\in SO(3)}{\max }[\sum _{i=1}^n({\tilde{q}}_i^{T}R{\tilde{p}}_i)] \\ =\underset{R\in SO(3)}{\max }\text{tr}[\sum _{i=1}^n({\tilde{q}}_i^{T}R{\tilde{p}}_i)](因为{\tilde{q}}_i^{T}R{\tilde{p}}_i\in \mathbb{R}) \\ =\underset{R\in SO(3)}{\max }[\sum _{i=1}^n\text{tr}({\tilde{q}}_i^{T}R{\tilde{p}}_i)] \\ =\underset{R\in SO(3)}{\max }[\sum _{i=1}^n\text{tr}(R{\tilde{p}}_i{\tilde{q}}_i^T)](因为\text{tr}(AB)=\text{tr}(BA),A\in {\mathbb{R}}^{m\times n},B\in {\mathbb{R}}^{n\times m}) \\ =\underset{R\in SO(3)}{\max }\text{tr}[\sum _{i=1}^n(R{\tilde{p}}_i{\tilde{q}}_i^T)] \\ =\underset{R\in SO(3)}{\max }\text{tr}[R\sum _{i=1}^n({\tilde{p}}_i{\tilde{q}}_i^T)] \\ =\underset{R\in SO(3)}{\max }\text{tr}(RC)(记C=\sum _{i=1}^n({\tilde{p}}_i{\tilde{q}}_i^T)) \\ =\underset{R\in SO(3)}{\max }\text{tr}(RU\Sigma V)(奇异值分解C=U\Sigma V,U和V都是正交矩阵，\Sigma 是对角线都是非负数的对角矩阵) \\ =\underset{R\in SO(3)}{\max }\text{tr}(VRU\Sigma )\text{\hspace{1em}(14)} \end{gathered}$$
因为$V$，$U$都是正交矩阵（即$V,U\in O(3)$），$R\in SO(3)$，所以$VRU$是正交矩阵（即$VRU\in O(3)$）。可以分为两种情况讨论，
第一种情况：$\text{det}(VU)=+1$
因为$R\in SO(3)$，所以$\text{det}(R)=+1$。因为$\text{det}(VU)=+1$，所以$\text{det}(VRU)=+1$，即$VRU\in SO(3)$。我们的优化目标是使得(14)式中的$$\text{tr}(VRU\Sigma )$$最大。不妨设$$\Sigma =\left[\begin{array}{ccc}{\sigma }_1& 0& 0\\ 0& {\sigma }_2& 0\\ 0& 0& {\sigma }_3\end{array}\right],{\sigma }_1\ge {\sigma }_2\ge {\sigma }_3\ge 0\text{\hspace{1em}(15)}$$（因为$\Sigma$是(14)式中的矩阵$C$的奇异值分解，所以$\Sigma$写成这样的一般形式是合理的）
进一步地，不妨设$$VRU=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right]\text{\hspace{1em}(16)}$$
因为$VRU\in SO(3)$，所以$-1\le a_{11}\le 1$，$-1\le a_{22}\le 1$和$-1\le a_{33}\le 1$。将(15)式和(16)式代入$\text{tr}(VRU\Sigma )$中，得
$$\text{tr}(VRU\Sigma )=a_{11}{\sigma }_1+a_{22}{\sigma }_2+a_{33}{\sigma }_3\text{\hspace{1em}(17)}$$
由(17)式知，$a_{11}=a_{22}=a_{33}=1$时，$\text{tr}(VRU\Sigma )={\sigma }_1+{\sigma }_2+{\sigma }_3$达到最大值，即$VRU=I$时。对应地，可以得出$R=V^T{U}^T$。

第二种情况：$\text{det}(VU)=-1$
因为$R\in SO(3)$，所以$\text{det}(R)=+1$。因为$\text{det}(VU)=-1$，所以$\text{det}(VRU)=-1$，即$VRU\in O(3)$且$VRU\notin SO(3)$。
我们给出以下引理，不加证明。
引理：$$\underset{A\in O(3),\det (A)=-1}{\max }\text{tr}(A\Sigma )={\sigma }_1+{\sigma }_2-{\sigma }_3$$
其中$\Sigma =\left[\begin{array}{ccc}{\sigma }_1& 0& 0\\ 0& {\sigma }_2& 0\\ 0& 0& {\sigma }_3\end{array}\right],{\sigma }_1\ge {\sigma }_2\ge {\sigma }_3\ge 0$。即$A=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& -1\end{array}\right]$时使得$\text{tr}(A\Sigma )$最大。

由这个引理知，$VRU=A$时，$\text{tr}(VRU\Sigma )$最大，其中$A=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& -1\end{array}\right]$。即$R=V^{T}A{U}^T$。

综上第一种情况和第二种情况，我们可以得出$R$的一般解为：
$$R=V^{T}Q{U}^T$$，其中$Q=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& \det (VU)\end{array}\right]$。

写在最后

上述如有错误，恳请指正，谢谢。

对于上文中的引理，不是太容易证明，我也没有太多时间去思考和查阅如何证明，自己大概想了1个小时，没想出来如何证明，去网上查了大概的证明思路，还是有些复杂的（如果大家有证明思路，欢迎分享讨论）。