一个机器人中的配准问题
前言
本文记录了一个机器人超声领域的配准问题。这和点云配准等问题在数学层面上都是一致的，本文的目的是从理论层面分析如何计算配准矩阵。（本文为了书写方便，向量等符号不用粗体表示，大家心里有数）
问题描述
如Fig. 1所示， { B } \{B\} {B}是机器人底座坐标系， { E } \{E\} {E}是机器人法兰坐标系， { I } \{I\} {I}是超声图像坐标系。 { I } \{I\} {I}的原点是超声图像中的索引为 ( u , v ) = ( 0 , 0 ) (u,v)=(0,0) (u,v)=(0,0)（即超声图像最左上角的点）的像素点对应的物理位置，其中 u u u和 v v v分别是超声图像像素点的行索引和列索引。 { I } \{I\} {I}的 x \text{x} x轴正方向是超声图像像素点列索引增加的方向， { I } \{I\} {I}的 y \text{y} y轴正方向是超声图像像素点行索引增加的方向。
我们有一个水缸（水缸里面的水用于超声波传递，不然无法超声成像），里面有两条交叉的线，两条线交于一点 B p ∈ R 3 {}^{B}p \in \mathbb{R}^{3} Bp∈R3，即 p p p点在机器人底座坐标系 { B } \{B\} {B}中的坐标表示是 B p {}^{B}p Bp。我们将超声探头移动并保证 B p {}^{B}p Bp这点在超声图像中显示出来，我们记 B p {}^{B}p Bp在超声图像中的成像点的像素为 ( r , c ) (r,c) (r,c)。我们可以得到：
B T I E T I I p = B p (1) {}^{B}T_{I} {}^{E}T_{I} {}^{I}p = {}^{B}p \tag{1} BTIETIIp=Bp(1)
其中 I p = ( s v v , s u u , 0 , 1 ) T {}^{I}p=(s_{v}v,s_{u}u,0,1)^{T} Ip=(svv,suu,0,1)T， s v s_{v} sv和 s u s_{u} su是已知的像素到真实物理世界的缩放系数。
Fig. 1
和其它很多的标定方法（比如经典的eye-in-hand，eye-to-hand标定）一样，我们需要采集多组数据 < B T I ( i ) , I p ( i ) > <{}^{B}T_{I}(i), {}^{I}p(i)> <BTI(i),Ip(i)>， i = 1 , . . . , n i=1,...,n i=1,...,n，且这些数据理论上满足
B T I ( i ) E T I I p ( i ) = B p (2) {}^{B}T_{I}(i) {}^{E}T_{I} {}^{I}p(i) = {}^{B}p \tag{2} BTI(i)ETIIp(i)=Bp(2)
现在的问题是如何求解 E T I {}^{E}T_{I} ETI？最小二乘法？需要注意的是
E T I = R t 0 1 × 3 1 , R ∈ S O ( 3 ) (3) {}^{E}T_{I} = \left \\begin{array}{c:c} R \& t \\\\ \\hline 0_{1\\times3} \& 1 \\end{array} \\right, R \in SO(3) \tag{3} ETI=R01×3t1,R∈SO(3)(3)
也就是说，这个优化是有约束优化（在 S O ( 3 ) SO(3) SO(3)流形上的优化），所以直接最小二乘法似乎行不通（这个优化问题还是挺需要技巧的）。
我们正式把这个优化问题写成
min E T I ∈ S E ( 3 ) ∑ i = 1 n ∥ E T I I p ( i ) − ( B T I ( i ) ) − 1 B p ∥ 2 2 (4) \min_{{}^{E}T_{I} \in SE(3)} \sum^{n}{i=1} \|{}^{E}T{I}{}^{I}p(i) - ({}^{B}T_{I}(i))^{-1}{}^{B}p\|_{2}^{2} \tag{4} ETI∈SE(3)mini=1∑n∥ETIIp(i)−(BTI(i))−1Bp∥22(4)
为了简便，我们记 q i q_i qi是 ( B T I ( i ) ) − 1 B p ({}^{B}T_{I}(i))^{-1}{}^{B}p (BTI(i))−1Bp的前三维度的列向量，记 p i p_i pi是 p ( i ) p(i) p(i)的前三维度的列向量，另外将 E T I {}^{E}T_{I} ETI用(3)式中的 R R R和 t t t来表示，得到
min R ∈ S O ( 3 ) , t ∈ R 3 ∑ i = 1 n ∥ R p i + t − q i ∥ 2 2 (5) \min_{R \in SO(3), t \in \mathbb{R}^{3}} \sum^{n}{i=1} \|Rp_i + t - q_i\|{2}^{2} \tag{5} R∈SO(3),t∈R3mini=1∑n∥Rpi+t−qi∥22(5)
对于(5)式，我们化简如下，
min R ∈ S O ( 3 ) , t ∈ R 3 ∑ i = 1 n ∥ R p i + t − q i ∥ 2 2 = min R ∈ S O ( 3 ) , t ∈ R 3 ∑ i = 1 n ( R p i + t − q i ) T ( R p i + t − q i ) = min R ∈ S O ( 3 ) , t ∈ R 3 ∑ 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 ) = min R ∈ S O ( 3 ) , t ∈ R 3 ∑ i = 1 n p i T p i + 2 ( t − q i ) T R p i + ( t − q i ) T ( t − q i ) (6) \min_{R \in SO(3), t \in \mathbb{R}^{3}} \sum^{n}{i=1} \|Rp_i + t - q_i\|{2}^{2} \\ = \min_{R \in SO(3), t \in \mathbb{R}^{3}} \sum^{n}{i=1} (Rp_i + t - q_i)^{T}(Rp_i + t - q_i) \\ = \min{R \in SO(3), t \in \mathbb{R}^{3}} \sum^{n}{i=1} p_i\^TR\^TRp_i + 2(t - q_i)\^{T}Rp_i + (t - q_i)\^T(t - q_i) \\ = \min{R \in SO(3), t \in \mathbb{R}^{3}} \sum^{n}_{i=1} p_i\^Tp_i + 2(t - q_i)\^{T}Rp_i + (t - q_i)\^T(t - q_i) \tag{6} R∈SO(3),t∈R3mini=1∑n∥Rpi+t−qi∥22=R∈SO(3),t∈R3mini=1∑n(Rpi+t−qi)T(Rpi+t−qi)=R∈SO(3),t∈R3mini=1∑npiTRTRpi+2(t−qi)TRpi+(t−qi)T(t−qi)=R∈SO(3),t∈R3mini=1∑npiTpi+2(t−qi)TRpi+(t−qi)T(t−qi)(6)
对于求解(6)式的最小值，一个直观的想法是对 R R R和 t t t分别求偏导数，然后令偏导数为0，可以得到：
R ∑ i = 1 n ( p i q i T ) = ∑ i = 1 n ( q i − t ) p i T t = 1 n ∑ i = 1 n ( q i − R p i ) (7) R\sum^{n}{i=1}(p_iq_i^{T}) = \sum{i=1}^{n}(q_i-t)p_i^T \\ t = \frac{1}{n}\sum^{n}_{i=1}(q_i-Rp_i) \tag{7} Ri=1∑n(piqiT)=i=1∑n(qi−t)piTt=n1i=1∑n(qi−Rpi)(7)
注：在对 R R R求导的时候，可以用 ( u v ) ′ = u ′ v + u v ′ (uv)'=u'v+uv' (uv)′=u′v+uv′来求，这样容易计算一些。对 t t t求导直接计算就可以。
为了进一步简化符号，记 p ˉ = 1 n ∑ i = 1 n p i \bar{p}=\frac{1}{n}\sum^{n}{i=1}p_i pˉ=n1∑i=1npi， q ˉ = 1 n ∑ i = 1 n q i \bar{q}=\frac{1}{n}\sum^{n}{i=1}q_i qˉ=n1∑i=1nqi， p ~ i = p i − p ˉ \tilde{p}{i}=p_i-\bar{p} p~i=pi−pˉ， q ~ i = q i − q ˉ \tilde{q}{i}=q_{i}-\bar{q} q~i=qi−qˉ，代入(7)式化简得：
R ∑ i = 1 n ( p ~ i p ~ i T ) = ∑ i = 1 n ( q ~ i p ~ i T ) t = q ˉ − R p ˉ (8) R\sum^{n}{i=1}(\tilde{p}{i}\tilde{p}{i}^T)=\sum^{n}{i=1}(\tilde{q}{i}\tilde{p}{i}^T) \\ t=\bar{q}-R\bar{p} \tag{8} Ri=1∑n(p~ip~iT)=i=1∑n(q~ip~iT)t=qˉ−Rpˉ(8)
注意，(8)式中的 ∑ i = 1 n ( p ~ i p ~ i T ) \sum^{n}{i=1}(\tilde{p}{i}\tilde{p}_{i}^T) ∑i=1n(p~ip~iT)不一定可逆，即使可逆， ∑ i = 1 n ( q \~ i p \~ i T ) ∑ i = 1 n ( p \~ i p \~ i T ) − 1 \\sum\^{n}_{i=1}(\\tilde{q}_{i}\\tilde{p}_{i}\^T)\\sum\^{n}_{i=1}(\\tilde{p}_{i}\\tilde{p}_{i}\^T)^{-1} ∑i=1n(q\~ip\~iT)∑i=1n(p\~ip\~iT)−1也不能保证属于 S O ( 3 ) SO(3) SO(3)。所以求解偏导这个方法不可行。
求解方法
回到(6)式这个优化问题：
min R ∈ S O ( 3 ) , t ∈ R 3 ∑ i = 1 n p i T p i + 2 ( t − q i ) T R p i + ( t − q i ) T ( t − q i ) \min_{R \in SO(3), t \in \mathbb{R}^{3}} \sum^{n}_{i=1} p_i\^Tp_i + 2(t - q_i)\^{T}Rp_i + (t - q_i)\^T(t - q_i) R∈SO(3),t∈R3mini=1∑npiTpi+2(t−qi)TRpi+(t−qi)T(t−qi)
我们把这个优化问题看作是一个两个自变量（ R R R和 t t t）的优化问题， t t t是一个无约束自变量， R ∈ S O ( 3 ) R \in SO(3) R∈SO(3)是一个有约束自变量。
这个问题可以等价于
min R ∈ S O ( 3 ) , t ∈ R 3 ∑ i = 1 n p i T p i + 2 ( t − q i ) T R p i + ( t − q i ) T ( t − q i ) = min R ∈ S O ( 3 ) min t ∈ R 3 ∑ i = 1 n p i T p i + 2 ( t − q i ) T R p i + ( t − q i ) T ( t − q i ) (9) \min_{R \in SO(3), t \in \mathbb{R}^{3}} \sum^{n}{i=1} p_i\^Tp_i + 2(t - q_i)\^{T}Rp_i + (t - q_i)\^T(t - q_i) \\ = \min{R \in SO(3)} \min_{t \in \mathbb{R}^{3}} \sum^{n}_{i=1} p_i\^Tp_i + 2(t - q_i)\^{T}Rp_i + (t - q_i)\^T(t - q_i) \tag{9} R∈SO(3),t∈R3mini=1∑npiTpi+2(t−qi)TRpi+(t−qi)T(t−qi)=R∈SO(3)mint∈R3mini=1∑npiTpi+2(t−qi)TRpi+(t−qi)T(t−qi)(9)
当我们固定 R R R，
min t ∈ R 3 ∑ i = 1 n p i T p i + 2 ( t − q i ) T R p i + ( t − q i ) T ( t − q i ) (10) \min_{t \in \mathbb{R}^{3}} \sum^{n}_{i=1} p_i\^Tp_i + 2(t - q_i)\^{T}Rp_i + (t - q_i)\^T(t - q_i) \tag{10} t∈R3mini=1∑npiTpi+2(t−qi)TRpi+(t−qi)T(t−qi)(10)
(10)式这个优化问题很简单，直接对 t t t求导就可以了，对 t t t求导的结果(8)式已经得到，即 t = q ˉ − R p ˉ t=\bar{q}-R\bar{p} t=qˉ−Rpˉ。将 t = q ˉ − R p ˉ t=\bar{q}-R\bar{p} t=qˉ−Rpˉ代入(9)式，得
min t ∈ R 3 ∑ i = 1 n p i T p i + 2 ( t − q i ) T R p i + ( t − q i ) T ( t − q i ) = ∑ i = 1 n ( p \~ i T + p ˉ T ) ( p \~ i + p ˉ ) + 2 ( − q \~ i − R p ˉ ) T R ( p \~ i + p ˉ ) + ( − q \~ i − R p ˉ ) T ( − q \~ i − R p ˉ ) = ∑ i = 1 n ( p \~ i T + p ˉ T ) ( p \~ i + p ˉ ) − 2 ( q \~ i + R p ˉ ) T R ( p \~ i + p ˉ ) + ( q \~ i + R p ˉ ) T ( q \~ i + R p ˉ ) = ∑ i = 1 n ( p ~ i T p ~ i − 2 q ~ i T R p ~ i + q ~ i T q ~ i ) = ∑ i = 1 n ( p ~ i T p ~ i + q ~ i T q ~ i ) − 2 ∑ i = 1 n ( q ~ i T R p ~ i ) (11) \min_{t \in \mathbb{R}^{3}} \sum^{n}{i=1} p_i\^Tp_i + 2(t - q_i)\^{T}Rp_i + (t - q_i)\^T(t - q_i) \\ = \sum^{n}{i=1} (\\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^{n}{i=1} (\\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^{n}{i=1} (\tilde{p}i^T\tilde{p}i - 2\tilde{q}{i} ^{T}R\tilde{p}i + \tilde{q}{i}^T\tilde{q}{i}) \\ = \sum^{n}{i=1} (\tilde{p}i^T\tilde{p}i + \tilde{q}{i}^T\tilde{q}{i}) - 2\sum^{n}{i=1}(\tilde{q}_{i} ^{T}R\tilde{p}_i) \tag{11} t∈R3mini=1∑npiTpi+2(t−qi)TRpi+(t−qi)T(t−qi)=i=1∑n(p\~iT+pˉT)(p\~i+pˉ)+2(−q\~i−Rpˉ)TR(p\~i+pˉ)+(−q\~i−Rpˉ)T(−q\~i−Rpˉ)=i=1∑n(p\~iT+pˉT)(p\~i+pˉ)−2(q\~i+Rpˉ)TR(p\~i+pˉ)+(q\~i+Rpˉ)T(q\~i+Rpˉ)=i=1∑n(p~iTp~i−2q~iTRp~i+q~iTq~i)=i=1∑n(p~iTp~i+q~iTq~i)−2i=1∑n(q~iTRp~i)(11)
注：在(11)式的推导过程中，使用了 ∑ i = 1 n p ~ i = 0 \sum^{n}_{i=1} \tilde{p}i=0 ∑i=1np~i=0和 ∑ i = 1 n q ~ i = 0 \sum^{n}{i=1} \tilde{q}_i=0 ∑i=1nq~i=0这两个等式进行化简。
将(11)式代入(9)式得，
min R ∈ S O ( 3 ) , t ∈ R 3 ∑ i = 1 n p i T p i + 2 ( t − q i ) T R p i + ( t − q i ) T ( t − q i ) = min R ∈ S O ( 3 ) min t ∈ R 3 ∑ i = 1 n p i T p i + 2 ( t − q i ) T R p i + ( t − q i ) T ( t − q i ) = min R ∈ S O ( 3 ) ∑ i = 1 n ( p \~ i T p \~ i + q \~ i T q \~ i ) − 2 ∑ i = 1 n ( q \~ i T R p \~ i ) (12) \min_{R \in SO(3), t \in \mathbb{R}^{3}} \sum^{n}{i=1} p_i\^Tp_i + 2(t - q_i)\^{T}Rp_i + (t - q_i)\^T(t - q_i) \\ = \min{R \in SO(3)} \min_{t \in \mathbb{R}^{3}} \sum^{n}{i=1} p_i\^Tp_i + 2(t - q_i)\^{T}Rp_i + (t - q_i)\^T(t - q_i) \\ = \min{R \in SO(3)}\\sum\^{n}_{i=1} (\\tilde{p}_i\^T\\tilde{p}_i + \\tilde{q}_{i}\^T\\tilde{q}_{i}) - 2\\sum\^{n}_{i=1}(\\tilde{q}_{i} \^{T}R\\tilde{p}_i) \tag{12} R∈SO(3),t∈R3mini=1∑npiTpi+2(t−qi)TRpi+(t−qi)T(t−qi)=R∈SO(3)mint∈R3mini=1∑npiTpi+2(t−qi)TRpi+(t−qi)T(t−qi)=R∈SO(3)mini=1∑n(p\~iTp\~i+q\~iTq\~i)−2i=1∑n(q\~iTRp\~i)(12)
(12)式的求最小值问题等价于(13)式的求最大值问题。
max R ∈ S O ( 3 ) ∑ i = 1 n ( q \~ i T R p \~ i ) (13) \max_{R \in SO(3)}\\sum\^{n}_{i=1}(\\tilde{q}_{i} \^{T}R\\tilde{p}_i) \tag{13} R∈SO(3)maxi=1∑n(q\~iTRp\~i)(13)
求解(13)式这个优化问题，需要一些关于矩阵迹的技巧，符号 tr \text{tr} tr表示矩阵的迹，
max R ∈ S O ( 3 ) ∑ i = 1 n ( q \~ i T R p \~ i ) = max R ∈ S O ( 3 ) tr ∑ i = 1 n ( q \~ i T R p \~ i ) ( 因为 q ~ i T R p ~ i ∈ R ) = max R ∈ S O ( 3 ) ∑ i = 1 n tr ( q \~ i T R p \~ i ) = max R ∈ S O ( 3 ) ∑ i = 1 n tr ( R p \~ i q \~ i T ) ( 因为tr ( A B ) = tr ( B A ) , A ∈ R m × n , B ∈ R n × m ) = max R ∈ S O ( 3 ) tr ∑ i = 1 n ( R p \~ i q \~ i T ) = max R ∈ S O ( 3 ) tr R ∑ i = 1 n ( p \~ i q \~ i T ) = max R ∈ S O ( 3 ) tr ( R C ) ( 记 C = ∑ i = 1 n ( p ~ i q ~ i T ) ) = max R ∈ S O ( 3 ) tr ( R U Σ V ) ( 奇异值分解 C = U Σ V , U 和 V 都是正交矩阵， Σ 是对角线都是非负数的对角矩阵 ) = max R ∈ S O ( 3 ) tr ( V R U Σ ) (14) \max_{R \in SO(3)}\\sum\^{n}_{i=1}(\\tilde{q}_{i} \^{T}R\\tilde{p}_i) \\ = \max_{R \in SO(3)}\text{tr}\\sum\^{n}_{i=1}(\\tilde{q}_{i} \^{T}R\\tilde{p}_i) \quad (因为\tilde{q}{i} ^{T}R\tilde{p}i \in \mathbb{R}) \\ = \max{R \in SO(3)}\\sum\^{n}_{i=1}\\text{tr}(\\tilde{q}_{i} \^{T}R\\tilde{p}_i) \\ = \max{R \in SO(3)}\\sum\^{n}_{i=1}\\text{tr}(R\\tilde{p}_i\\tilde{q}_{i} \^{T}) \quad (因为\text{tr}(AB) = \text{tr}(BA), A \in \mathbb{R}^{m\times n}, B \in \mathbb{R}^{n\times m}) \\ = \max_{R \in SO(3)}\text{tr}\\sum\^{n}_{i=1}(R\\tilde{p}_i\\tilde{q}_{i} \^{T}) \\ = \max_{R \in SO(3)}\text{tr}R\\sum\^{n}_{i=1}(\\tilde{p}_i\\tilde{q}_{i} \^{T}) \\ = \max_{R \in SO(3)}\text{tr}(RC) \quad (记C=\sum^{n}{i=1}(\tilde{p}i\tilde{q}{i} ^{T})) \\ = \max{R \in SO(3)}\text{tr}(RU\Sigma V) \quad (奇异值分解C=U\Sigma V,U和V都是正交矩阵，\Sigma是对角线都是非负数的对角矩阵) \\ = \max_{R \in SO(3)}\text{tr}(VRU\Sigma) \tag{14} R∈SO(3)maxi=1∑n(q\~iTRp\~i)=R∈SO(3)maxtri=1∑n(q\~iTRp\~i)(因为q~iTRp~i∈R)=R∈SO(3)maxi=1∑ntr(q\~iTRp\~i)=R∈SO(3)maxi=1∑ntr(Rp\~iq\~iT)(因为tr(AB)=tr(BA),A∈Rm×n,B∈Rn×m)=R∈SO(3)maxtri=1∑n(Rp\~iq\~iT)=R∈SO(3)maxtrRi=1∑n(p\~iq\~iT)=R∈SO(3)maxtr(RC)(记C=i=1∑n(p~iq~iT))=R∈SO(3)maxtr(RUΣV)(奇异值分解C=UΣV,U和V都是正交矩阵，Σ是对角线都是非负数的对角矩阵)=R∈SO(3)maxtr(VRUΣ)(14)
因为 V V V， U U U都是正交矩阵（即 V , U ∈ O ( 3 ) V,U\in O(3) V,U∈O(3)）， R ∈ S O ( 3 ) R \in SO(3) R∈SO(3)，所以 V R U VRU VRU是正交矩阵（即 V R U ∈ O ( 3 ) VRU \in O(3) VRU∈O(3)）。可以分为两种情况讨论，
第一种情况 ： det ( V U ) = + 1 \text{det}(VU) = +1 det(VU)=+1
因为 R ∈ S O ( 3 ) R \in SO(3) R∈SO(3)，所以 det ( R ) = + 1 \text{det}(R)=+1 det(R)=+1。因为 det ( V U ) = + 1 \text{det}(VU)=+1 det(VU)=+1，所以 det ( V R U ) = + 1 \text{det}(VRU)=+1 det(VRU)=+1，即 V R U ∈ S O ( 3 ) VRU \in SO(3) VRU∈SO(3)。我们的优化目标是使得(14)式中的 tr ( V R U Σ ) \text{tr}(VRU\Sigma) tr(VRUΣ)最大。不妨设 Σ = σ 1 0 0 0 σ 2 0 0 0 σ 3 , σ 1 ≥ σ 2 ≥ σ 3 ≥ 0 (15) \Sigma = \begin{bmatrix} \sigma_1 & 0 & 0 \\ 0 & \sigma_2 & 0 \\ 0 & 0 & \sigma_3 \end{bmatrix}, \sigma_1 \geq \sigma_2 \geq \sigma_3 \geq 0 \tag{15} Σ= σ1000σ2000σ3 ,σ1≥σ2≥σ3≥0(15)（因为 Σ \Sigma Σ是(14)式中的矩阵 C C C的奇异值分解，所以 Σ \Sigma Σ写成这样的一般形式是合理的）
进一步地，不妨设 V R U = a 11 a 12 a 13 a 21 a 22 a 23 a 31 a 32 a 33 (16) VRU = \begin{bmatrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{bmatrix} \tag{16} VRU= a11a21a31a12a22a32a13a23a33 (16)
因为 V R U ∈ S O ( 3 ) VRU \in SO(3) VRU∈SO(3)，所以 − 1 ≤ a 11 ≤ 1 -1 \leq a_{11} \leq 1 −1≤a11≤1， − 1 ≤ a 22 ≤ 1 -1 \leq a_{22} \leq 1 −1≤a22≤1和 − 1 ≤ a 33 ≤ 1 -1 \leq a_{33} \leq 1 −1≤a33≤1。将(15)式和(16)式代入 tr ( V R U Σ ) \text{tr}(VRU\Sigma) tr(VRUΣ)中，得
tr ( V R U Σ ) = a 11 σ 1 + a 22 σ 2 + a 33 σ 3 (17) \text{tr}(VRU\Sigma) = a_{11}\sigma_{1}+a_{22}\sigma_{2}+a_{33}\sigma_{3} \tag{17} tr(VRUΣ)=a11σ1+a22σ2+a33σ3(17)
由(17)式知， a 11 = a 22 = a 33 = 1 a_{11}=a_{22}=a_{33}=1 a11=a22=a33=1时， tr ( V R U Σ ) = σ 1 + σ 2 + σ 3 \text{tr}(VRU\Sigma) = \sigma_{1}+\sigma_{2}+\sigma_{3} tr(VRUΣ)=σ1+σ2+σ3达到最大值，即 V R U = I VRU=I VRU=I时。对应地，可以得出 R = V T U T R=V^{T}U^{T} R=VTUT。
第二种情况 ： det ( V U ) = − 1 \text{det}(VU)=-1 det(VU)=−1
因为 R ∈ S O ( 3 ) R \in SO(3) R∈SO(3)，所以 det ( R ) = + 1 \text{det}(R)=+1 det(R)=+1。因为 det ( V U ) = − 1 \text{det}(VU)=-1 det(VU)=−1，所以 det ( V R U ) = − 1 \text{det}(VRU)=-1 det(VRU)=−1，即 V R U ∈ O ( 3 ) VRU \in O(3) VRU∈O(3)且 V R U ∉ S O ( 3 ) VRU \notin SO(3) VRU∈/SO(3)。
我们给出以下引理，不加证明。
引理 ： max A ∈ O ( 3 ) , det ( A ) = − 1 tr ( A Σ ) = σ 1 + σ 2 − σ 3 \max_{A \in O(3), \det(A)=-1}\text{tr}(A\Sigma)=\sigma_1 + \sigma_2 - \sigma_3 A∈O(3),det(A)=−1maxtr(AΣ)=σ1+σ2−σ3
其中 Σ = σ 1 0 0 0 σ 2 0 0 0 σ 3 , σ 1 ≥ σ 2 ≥ σ 3 ≥ 0 \Sigma = \begin{bmatrix} \sigma_1 & 0 & 0 \\ 0 & \sigma_2 & 0 \\ 0 & 0 & \sigma_3 \end{bmatrix}, \sigma_1 \geq \sigma_2 \geq \sigma_3 \geq 0 Σ= σ1000σ2000σ3 ,σ1≥σ2≥σ3≥0。即 A = 1 0 0 0 1 0 0 0 − 1 A=\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix} A= 10001000−1 时使得 tr ( A Σ ) \text{tr}(A\Sigma) tr(AΣ)最大。
由这个引理知， V R U = A VRU=A VRU=A时， tr ( V R U Σ ) \text{tr}(VRU\Sigma) tr(VRUΣ)最大，其中 A = 1 0 0 0 1 0 0 0 − 1 A=\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix} A= 10001000−1 。即 R = V T A U T R=V^TAU^T R=VTAUT。
综上第一种情况 和第二种情况 ，我们可以得出 R R R的一般解为：
R = V T Q U T R = V^TQU^T R=VTQUT，其中 Q = 1 0 0 0 1 0 0 0 det ( V U ) Q=\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(VU) \end{bmatrix} Q= 10001000det(VU) 。
写在最后
上述如有错误，恳请指正，谢谢。
对于上文中的引理，不是太容易证明，我也没有太多时间去思考和查阅如何证明，自己大概想了1个小时，没想出来如何证明，去网上查了大概的证明思路，还是有些复杂的（如果大家有证明思路，欢迎分享讨论）。