Tsai两步法求手眼标定矩阵

• Tsai两步法求手眼标定矩阵
  • Tsai方法介绍
  • 术语概念
    • 齐次变换矩阵和坐标系的定义
    • 旋转轴和旋转角度
  • 引理的证明和解释
    • AX=XB构造
    • 引理1
    • 引理2
    • 引理3
    • 引理4
    • 引理5
    • 引理6 --得到公式（5）
    • 引理7
    • 引理8
    • 引理9
    • 引理10
    • 引理11
    • 引理的总结
  • 解法步骤
    • Step1: 计算 ${\mathbf{R}}_{cg}$
    • Step2：计算 ${\theta }_{{\mathbf{R}}_{cg}}$
    • Step3: 计算 ${\mathbf{P}}_{cg}$
    • Step4: 计算 ${\mathbf{t}}_{cg}$
  • 结尾

Tsai两步法求手眼标定矩阵

Tsai方法介绍

本篇介绍推导的手眼标定方法参考
ROGER Y. TSAI AND REIMAR K. LENZ
于1989年发表的论文
’A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration '
该论文提出了一种手眼标定的解析算法，并给出了这种方法下的手眼标定数据集的筛选原则

术语概念

齐次变换矩阵和坐标系的定义

$G_i$ : 末端坐标系（工具坐标系）
$C_i$ : 相机坐标系
$CW$ : 标定板世界坐标系
$RW$ : 机器人世界坐标系
${\mathbf{H}}_{gi}$: 从$G_i$到$RW$的齐次转换阵，即在机器人坐标系下的末端位姿矩阵
${\mathbf{H}}_{ci}$: 从$CW$到$C_i$的齐次转换矩阵，即可以理解为相机坐标系下的标定板位姿矩阵
${\mathbf{H}}_{gij}$: 从${\mathbf{H}}_{gi}$到${\mathbf{H}}_{gj}$的齐次转换矩阵，涉及到两次标定过程的参数

$${\mathbf{H}}_{gij}={\mathbf{H}}_{gj}^{-1}{\mathbf{H}}_{gi}$$

${\mathbf{H}}_{cij}$: 从${\mathbf{H}}_{ci}$到${\mathbf{H}}_{cj}$的齐次转换矩阵

$${\mathbf{H}}_{cij}={\mathbf{H}}_{cj}{\mathbf{H}}_{ci}^{-1}$$
为什么 ${\mathbf{H}}_{gij}$ 乘积中是取 $j$ 的逆矩阵，而 ${\mathbf{H}}_{cij}$ 是取 $i$ 的逆呢？
其实由于这种计算是专门用于eye-in-hand，也就是相机在机械臂的末端上，所以$CW$代表的是标定板的世界坐标，默认位置是不变的，而相机在两次标定中的位置是变化的，可以用以下方式重新说明会更清楚
$$\begin{array}{rl}& {\mathbf{H}}_{gi}={[}_{world}{\mathbf{M}}^{robot}{]}^{-1}{\cdot }_{world}{\mathbf{M}}^{gripper-i}{=}_{robot}{\mathbf{M}}^{gripper-i}\\ & {\mathbf{H}}_{gij}{=}_{gripper-j}{\mathbf{M}}^{gripper-i}\\ & ={[}_{robot}{\mathbf{M}}^{gripper-j}{]}^{-1}{\cdot }_{robot}{\mathbf{M}}^{gripper-i}\\ & ={\mathbf{H}}_{gj}^{-1}{\mathbf{H}}_{gi}\\ & \\ & {\mathbf{H}}_{ci}={[}_{world}{\mathbf{M}}^{cam-i}{]}^{-1}{\cdot }_{world}{\mathbf{M}}^{cal}{=}_{cam-i}{\mathbf{M}}^{cal}\\ & {\mathbf{H}}_{cij}{=}_{cam-j}{\mathbf{M}}^{cam-i}\\ & {=}_{cam-j}{\mathbf{M}}^{cal}\cdot {[}_{cam-i}{\mathbf{M}}^{cal}{]}^{-1}\\ & ={\mathbf{H}}_{cj}{\mathbf{H}}_{ci}^{-1}\end{array}$$

然后是关键的矩阵
${\mathbf{H}}_{cg}$: $Ci$到$Gi$的齐次转换矩阵，在eye-in-hand中，这个矩阵是不变的，也就是需要求取的关键矩阵
齐次变换矩阵由旋转和平移矩阵构成，具体不赘述

旋转轴和旋转角度

根据 Rodrigues 公式可以将旋转矩阵转换为旋转轴的单位向量和旋转角，三维空间中的旋转都可以看作是绕着某一旋转轴转动一定角度
公式的具体推导在另外一篇文章中已经推导过一遍
这里要补充的是旋转轴的单位向量是旋转矩阵的特征向量，特征值为1
$$\begin{array}{rl}& rotationaxis:{\mathbf{P}}_r=[n_1{n}_2{n}_3{]}^T\\ & {\mathbf{R}\mathbf{P}}_r={\mathbf{P}}_r--(1)\end{array}$$
在这里需要使用修改版的Rodriguees公式，
根据参考论文 'Optimal Spacecraft Rotational Manervers’
(这篇论文没看过，但是大概是推导了一种变形，使得公式中的角度量$\theta$在公式中隐去)
重新定义
$${\mathbf{P}}_r=2\sin \frac{\theta }{2}[n_1{n}_2{n}_3{]}^T--(2)$$
同样的，变形后的Rodrigues公式中不存在三角函数，这是优势,注意其中的$[n_1{n}_2{n}_3]$是单位向量
$$\begin{gathered} \mathbf{R}=\left(1-\frac{{\left|{\mathbf{P}}_r\right|}^2}{2}\right)\mathbf{I}+\frac{1}{2}\left({\mathbf{P}}_r{\mathbf{P}}_r^T+\alpha \cdot \mathrm{Skew}\left({\mathbf{P}}_r\right)\right) \\ where\alpha =\sqrt{4-{\left|{\mathbf{P}}_r\right|}^2}--(3) \end{gathered}$$

然后给出关于旋转轴和旋转角度的符号定义
${\theta }_{\mathbf{R}}$: $\mathbf{R}$ 旋转矩阵的旋转角度
${\mathbf{P}}_{gij}$: ${\mathbf{R}}_{gij}$的旋转轴
${\mathbf{P}}_{cij}$: ${\mathbf{R}}_{cij}$的旋转轴
${\mathbf{P}}_{cg}$: ${\mathbf{R}}_{cg}$的旋转轴

${\mathbf{P}}_{cg}$的变形:${\mathbf{P}}_{cg}^{\prime }$
${\mathbf{P}}_{cg}^{\prime }=\frac{1}{2\cos \left(\frac{{\theta }_{{\mathbf{R}}_{cg}}}{2}\right)}{\mathbf{P}}_{cg}=\frac{1}{\sqrt{4-{|{\mathbf{P}}_{cg}|}^2}}{\mathbf{P}}_{cg}--(4)$
反对称矩阵：
$\text{ Skew }(\mathbf{V})=\left[\begin{array}{ccc}0& -{\nu }_z& {\nu }_y\\ {\nu }_z& 0& -{\nu }_x\\ -{\nu }_y& {\nu }_x& 0\end{array}\right]$

引理的证明和解释

论文中首先给出了一个结论性式子
$\text{ Skew }\left({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij}\right){\mathbf{P}}_{cg}^{\prime }={\mathbf{P}}_{cij}-{\mathbf{P}}_{gi}--(5)$

AX=XB构造

这里首先证明这个式子是怎么来的
首先先简单证明一个基本的AX=XB公式
$$\begin{array}{rl}& {\mathbf{H}}_{gij}{\mathbf{H}}_{cg}={\mathbf{H}}_{cg}{\mathbf{H}}_{cij}\\ & left{=}_{gj}{\mathbf{M}}^{gi}{\cdot }_{gi}{\mathbf{M}}^{ci}{=}_{gj}{\mathbf{M}}^{ci}\\ & right{=}_{gj}{\mathbf{M}}^{cj}{\cdot }_{cj}{\mathbf{M}}^{ci}{=}_{gj}{\mathbf{M}}^{ci}\\ & left=right\end{array}$$
把这个齐次矩阵拆开，就有两个等式成立
分别是
$$\begin{array}{rl}& {\mathbf{R}}_{gij}{\mathbf{R}}_{cg}={\mathbf{R}}_{cg}{\mathbf{R}}_{cij}--(6)\\ & {\mathbf{R}}_{gij}{\mathbf{t}}_{cg}+{\mathbf{t}}_{gij}={\mathbf{R}}_{cg}{\mathbf{t}}_{cij}+{\mathbf{t}}_{cg}--(7)\end{array}$$
然后需要渐进地简单证明几个引理

引理1

引理1直接由公式5可以得到
$$\begin{array}{rl}LemmaI:& \\ & {\mathbf{R}}_{gij}=={\mathbf{R}}_{cg}{\mathbf{R}}_{cij}{\mathbf{R}}_{cg}^T--(8)\end{array}$$

引理2

引理2则是关于旋转轴之间的转换,可以用引理1证明,同时容易知道旋转轴向量是旋转矩阵的唯一特征值为1的实特征向量
$$\begin{array}{rl}LemmaII:& \\ & {\mathbf{P}}_{gij}={\mathbf{R}}_{cg}{\mathbf{P}}_{cij}--(9)\\ Proof:& \\ {\mathbf{R}}_{gij}& ={\mathbf{R}}_{cg}{\mathbf{R}}_{cij}{\mathbf{R}}_{cg}^T\\ {\mathbf{R}}_{gij}{\mathbf{P}}_{gij}& ={\mathbf{R}}_{cg}{\mathbf{R}}_{cij}{\mathbf{R}}_{cg}^T{\mathbf{P}}_{gij}--accordingtofomula(1)\\ {\mathbf{P}}_{gij}& ={\mathbf{R}}_{cg}{\mathbf{R}}_{cij}{\mathbf{R}}_{cg}^T{\mathbf{P}}_{gij}\\ \underline{{\mathbf{R}}_{cg}^T{\mathbf{P}}_{gij}}& ={\mathbf{R}}_{cij}\underline{{\mathbf{R}}_{cg}^T{\mathbf{P}}_{gij}}obviouslythisisaneigenvectorof{\mathbf{R}}_{cij}\\ {\mathbf{P}}_{cij}& ={\mathbf{R}}_{cg}^T{\mathbf{P}}_{gij}\\ {\mathbf{P}}_{gij}& ={\mathbf{R}}_{cg}{\mathbf{P}}_{cij}\\ Proofdone& \end{array}$$

引理3

引理3证明了三个旋转轴之间的垂直关系,这一部分的证明论文上好像有一点问题，不知道是不是符号写错，这里重新证明一下
$$\begin{array}{rl}LemmaIII:& \\ & {\mathbf{P}}_{cg}\perp ({\mathbf{P}}_{gij}-{\mathbf{P}}_{cij})--(10)\\ Proof:& \\ {\left({\mathbf{P}}_g-{\mathbf{P}}_c\right)}^T{\mathbf{P}}_{cg}& =({\mathbf{R}}_{cg}{\mathbf{P}}_c-{\mathbf{P}}_c{)}^T{\mathbf{R}}_{cg}{\mathbf{P}}_{cg}--accordingtolemmaII\\ & ={\mathbf{P}}_c^T{\mathbf{R}}_{cg}^T{\mathbf{R}}_{cg}{\mathbf{P}}_{cg}-{\mathbf{P}}_c^T{\mathbf{R}}_{cg}{\mathbf{P}}_{cg}\\ & ={\mathbf{P}}_c^T{\mathbf{P}}_{cg}-{\mathbf{P}}_c^T{\mathbf{P}}_{cg}\\ & =0\\ Proofdone& \end{array}$$

事实上根据引理3就能够计算得到 ${\mathbf{R}}_{cg}$ 的旋转轴 ${\mathbf{P}}_{cg}$ 了，即根据两次转移得到的两对${\mathbf{P}}_{g1}{\mathbf{P}}_{c1}$和${\mathbf{P}}_{g2}{\mathbf{P}}_{c2}$其叉乘后乘以一个缩放系数即可得到
$${\mathbf{P}}_{cg}=s\left({\mathbf{P}}_{g{i}_1{j}_1}-{\mathbf{P}}_{c{i}_1{j}_1}\right)\times \left({\mathbf{P}}_{g{i}_2{j}_2}-{\mathbf{P}}_{c{i}_2{j}_2}\right)$$
但是这一方法对误差的敏感性高，并且也没有考虑其他角度的影响，总之就是不太适用，但是论文没有给出具体的解释，这里不做讨论了

引理4

引理4证明了三个向量之间的共线关系-即平行关系
$$\begin{array}{rl}LemmaIV:& \\ & ({\mathbf{P}}_{gij}-{\mathbf{P}}_{cij})\parallel ({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})\times {\mathbf{P}}_{cg}\\ Proof:& \\ & 1.\{\begin{array}{c}{\mathbf{P}}_{cg}\perp ({\mathbf{P}}_{gij}-{\mathbf{P}}_{cij})--LemmaIII\\ {\mathbf{P}}_{cg}\perp ({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})\times {\mathbf{P}}_{cg}\end{array}\\ & 2.\\ & ({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})\perp ({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})\times {\mathbf{P}}_{cg}--obviously\\ & ({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})({\mathbf{P}}_{gij}-{\mathbf{P}}_{cij})\\ & ={\mathbf{P}}_{gij}^2-{\mathbf{P}}_{cij}^2\\ & =1-1=0\\ So,& \\ & ({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})\perp ({\mathbf{P}}_{gij}-{\mathbf{P}}_{cij})\\ & \\ & ({\mathbf{P}}_{gij}-{\mathbf{P}}_{cij})\parallel ({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})\times {\mathbf{P}}_{cg}\\ Proofdone& \end{array}$$
两个向量同时垂直于两条不平行的向量，证明这两条向量是平行的

引理5

引理5需要根据图来理解，使用解析几何的方式来证明

图中向量的意思不用说明，比较清楚了，其中的这个圆所在平面垂直于${\mathbf{P}}_{cg}$
引理5证明了两个向量的长度相同，主要是为了引出接下来两个向量相等的引理
$$\begin{array}{rl}LemmaV:& \\ & |{\mathbf{P}}_{gij}-{\mathbf{P}}_{cij}|=|({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})\times {\mathbf{P}}_{cg}^{\prime }|\\ Proof:& \\ & |({\mathbf{P}}_g+{\mathbf{P}}_c)\times {\mathbf{P}}_{cg}^{\prime }|=|({\mathbf{P}}_g+{\mathbf{P}}_c)||{\mathbf{P}}_{cg}^{\prime }|\sin \alpha --Omittheij\\ & =|({\mathbf{P}}_g+{\mathbf{P}}_c)|\cdot 2\sin \frac{\theta }{2}{\left(4-4{\sin }^2\frac{\theta }{2}\right)}^{-\frac{1}{2}}\sin \alpha --accordingtoformula(2)(4)\\ & =|({\mathbf{P}}_g+{\mathbf{P}}_c)|\cdot \tan \frac{\theta }{2}\cdot \sin \alpha --trigonometrictransfomation\\ & =2|\overrightarrow{OB}|\cdot \sin \alpha \cdot \tan \frac{\theta }{2}--asshowinfigure\\ & =2|\overrightarrow{AB}|\cdot \tan \frac{\theta }{2}\\ & =2|\overrightarrow{CB}|\\ & =|\overrightarrow{CG}|\\ & =|{\mathbf{P}}_g-{\mathbf{P}}_c|\\ Proofdone.& \end{array}$$

引理6 --得到公式（5）

根据引理4和引理5可以直接得到引理6
$$\begin{array}{rl}LemmaVI:& \\ & ({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})\times {\mathbf{P}}_{cg}^{\prime }={\mathbf{P}}_{cij}-{\mathbf{P}}_{gij}\end{array}$$
根据上图可以确定向量的方向问题
这一引理直接可以得到公式（5）
$\text{ Skew }\left({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij}\right){\mathbf{P}}_{cg}^{\prime }={\mathbf{P}}_{cij}-{\mathbf{P}}_{gi}--(5)$
接下来的引理则说明了如何求解公式（5）

引理7

引理7证明了 $\text{Skew}({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij})$ 的是奇异的，秩为2，通过初等行列式变换就能够容易证明这个反对称阵的秩为2
$$\begin{array}{rl}LemmaVII:& \\ & rank(\text{Skew}({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij}))=2\\ Proof:& \\ & A=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]=\left[\begin{array}{ccc}{a}_2{a}_3& 0& -a_1{a}_2\\ 0& a_1{a}_3& -a_1{a}_2\\ 0& 0& 0\end{array}\right]\\ & rank(A)=2\\ Proofdone.& \end{array}$$
引理7说明了公式（5）需要两个以上的方程组才能存在 ${\mathbf{R}}_{cg}$ 的唯一解，也就是需要三个以上的状态

引理8

引理8直接由公式（7）得到
$$\begin{array}{rl}LemmaVIII:& \\ & ({\mathbf{R}}_{gij}-\mathbf{I}){\mathbf{t}}_{cg}={\mathbf{R}}_{cg}{\mathbf{t}}_{cij}+{\mathbf{t}}_{gij}\end{array}$$
这一引理用于计算 ${\mathbf{t}}_{cg}$

引理9

从引理8得到的 $({\mathbf{R}}_{gij}-\mathbf{I})$ 是奇异的，秩小于2
论文中没有直接证明这个结论，但是我们可以根据Rodrigues公式和秩的性质来得到这一结论
L e m m a   I X : r a n k ( R g i j − I ) ≤ 2 P r o o f : r a n k ( R g i j − I ) = r a n k ( I + sin ⁡ θ ⋅ K + ( 1 − cos ⁡ θ ) ⋅ K 2 − I )    = r a n k ( sin ⁡ θ ⋅ K + ( 1 − cos ⁡ θ ) ⋅ K 2 )    = r a n k ( K ( sin ⁡ θ + ( 1 − cos ⁡ θ ) ⋅ K ) )    ≤ m i n { r a n k ( K ) , r a n k ( ( 1 − cos ⁡ θ ) ⋅ K ) } = 2 P r o o f   d o n e . \begin{aligned} Lemma\ IX:\\ &rank(\mathbf{R}_{gij}-\mathbf{I})\le 2\\ Proof:\\ &rank(\mathbf{R}_{gij}-\mathbf{I})=rank(\mathbf{I}+\sin \theta\cdot \mathbf{K}+(1-\cos \theta)\cdot\mathbf{K}^{2}-\mathbf{I})\\ &\quad\quad\quad\quad\quad\quad\ \ =rank(\sin \theta\cdot \mathbf{K}+(1-\cos \theta)\cdot\mathbf{K}^{2})\\ &\quad\quad\quad\quad\quad\quad\ \ =rank(\mathbf{K}(\sin \theta+(1-\cos \theta)\cdot\mathbf{K}))\\ &\quad\quad\quad\quad\quad\quad\ \ \le min\{rank(\mathbf{K}),rank((1-\cos \theta)\cdot\mathbf{K})\}=2\\ Proof\ done. \end{aligned} Lemma IX:Proof:Proof done.​rank(Rgij​−I)≤2rank(Rgij​−I)=rank(I+sinθ⋅K+(1−cosθ)⋅K2−I)  =rank(sinθ⋅K+(1−cosθ)⋅K2)  =rank(K(sinθ+(1−cosθ)⋅K))  ≤min{rank(K),rank((1−cosθ)⋅K)}=2​
这一引理说明了要计算引理8中的 ${\mathbf{t}}_{cg}$ 方程组要两个以上，即要测量三个以上的状态

引理10

观察公式（4）可以得出引理10
$$\begin{array}{rl}LemmaXI:& \\ & Ifandonlyif{\theta }_{{\mathbf{R}}_{cg}}\ne \pi ,orequivalently,|{\mathbf{P}}_{cg}|,\ne \pm 2,{\mathbf{P}}_{g{i}_1{j}_1}\nparallel {\mathbf{P}}_{g{i}_2{j}_2}({\mathbf{P}}_{c{i}_1{j}_1}\nparallel {\mathbf{P}}_{c{i}_2{j}_2})\\ & \\ & \left[\begin{array}{l}\text{ Skew }\left({\mathbf{P}}_{g{i}_1{j}_1}+{\mathbf{P}}_{c{i}_1{j}_1}\right)\\ \text{ Skew }\left({\mathbf{P}}_{g{i}_2{j}_2}-{\mathbf{P}}_{c{i}_2{j}_2}\right)\end{array}\right]hasfullcolumnrank3\end{array}$$
引理10保证给定至少三个状态后，${\mathbf{R}}_{cg}$的解是唯一的

引理11

与引理10类似的，也是为了证明唯一性
$$\begin{array}{rl}LemmaX:& \\ & Ifandonlyif{\mathbf{P}}_{g{i}_1{j}_1}\nparallel {\mathbf{P}}_{g{i}_2{j}_2}({\mathbf{P}}_{c{i}_1{j}_1}\nparallel {\mathbf{P}}_{c{i}_2{j}_2})\\ & \\ & \left[\begin{array}{l}{\mathbf{R}}_{g{i}_1{j}_1}-\mathbf{I}\\ {\mathbf{R}}_{g{i}_2{j}_2}-\mathbf{I}\end{array}\right]hasfullcolumnrank3\end{array}$$
引理11保证给定至少三个状态后，${\mathbf{t}}_{cg}$的解是唯一的

引理的总结

引理1到3是公式推导的准备阶段，理清了几个向量的基本关系
从引理4和5导出了引理6的公式可以用于计算旋转轴向量
引理7到引理11，证明了需要三个状态以上才能得到唯一解${\mathbf{H}}_{cg}$
后面几个引理没有具体证明，但是应该是正确的

解法步骤

Step1: 计算 ${\mathbf{R}}_{cg}$

对于任意一组的i j，${\mathbf{R}}_{gij}$或者${\mathbf{R}}_{cij}$旋转角要尽可能大一些
建立引理6中得到的线性方程
$\text{ Skew }\left({\mathbf{P}}_{gij}+{\mathbf{P}}_{cij}\right){\mathbf{P}}_{cg}^{\prime }={\mathbf{P}}_{cij}-{\mathbf{P}}_{gij}--(5)$
由于引理7说明了这个反对称阵的奇异性，必须要两对以上的ij才能得到方程的唯一解，即需要三个以上的状态
然后得到线性方程组
$$\left[\begin{array}{l}\text{ Skew }\left({\mathbf{P}}_{g{i}_1{j}_1}+{\mathbf{P}}_{c{i}_1{j}_1}\right)\\ \text{ Skew }\left({\mathbf{P}}_{g{i}_2{j}_2}+{\mathbf{P}}_{c{i}_2{j}_2}\right)\\ ⋮\\ \text{ Skew }\left({\mathbf{P}}_{g{i}_n{j}_n}+{\mathbf{P}}_{c{i}_n{j}_n}\right)\end{array}\right]\cdot {\mathbf{P}}_{cg}^{\prime }=\left[\begin{array}{l}{\mathbf{P}}_{c{i}_1{j}_1}-{\mathbf{P}}_{g{i}_1{j}_1}\\ {\mathbf{P}}_{c{i}_2{j}_2}-{\mathbf{P}}_{g{i}_2{j}_2}\\ ⋮\\ {\mathbf{P}}_{c{i}_n{j}_n}-{\mathbf{P}}_{g{i}_n{j}_n}\end{array}\right]--(11)$$
得到$\mathbf{A}\mathbf{x}=\mathbf{b}$,其中$\mathbf{A}\in {\mathfrak{R}}^{3n\times 3}$, $\mathbf{x}\in {\mathfrak{R}}^{3\times 1}$，$\mathbf{b}\in {\mathfrak{R}}^{3n\times 1}$
可以通过线性最小二乘的方法进行求解
需要注意的是，在结算之前需要检查一下 ${\mathbf{R}}_{cg}$的旋转角是否为180°
检查的方法是计算$({\mathbf{P}}_{g{i}_1{j}_1}+{\mathbf{P}}_{c{i}_1{j}_1})$是否与$({\mathbf{P}}_{g{i}_2{j}_2}+{\mathbf{P}}_{c{i}_2{j}_2})$共线的同时${\mathbf{P}}_{g{i}_1{j}_1}\nparallel {\mathbf{P}}_{g{i}_2{j}_2}$
这是由于计算式中的${\mathbf{P}}_{cg}^{\prime }$的定义问题导致的，具体可以看一下它的定义式（4）
${\mathbf{P}}_{cg}^{\prime }=\frac{1}{2\cos \left(\frac{{\theta }_{{\mathbf{R}}_{cg}}}{2}\right)}{\mathbf{P}}_{cg}=\frac{1}{\sqrt{4-{|{\mathbf{P}}_{cg}|}^2}}{\mathbf{P}}_{cg}--(4)$

Step2：计算 ${\theta }_{{\mathbf{R}}_{cg}}$

由公式（2）（4）可以得到${\theta }_{{\mathbf{R}}_{cg}}$的计算式
$$\begin{array}{rl}{\mathbf{P}}_{cg}^{\prime }& =\frac{1}{\sqrt{4-{\left|{\mathbf{P}}_{cg}\right|}^2}}{\mathbf{P}}_{cg}\\ & =\frac{1}{\sqrt{4-|2\sin \frac{\theta }{2}[n_1{n}_2{n}_3{]}^T{|}^2}}{\mathbf{P}}_{cg}\\ & =\frac{1}{\sqrt{4-4{\sin }^2\frac{\theta }{2}}}{\mathbf{P}}_{cg}\\ & =\frac{1}{2\cos \frac{\theta }{2}}{\mathbf{P}}_{cg}\\ & =\frac{1}{2\cos \frac{\theta }{2}}2\sin \frac{\theta }{2}[n_1{n}_2{n}_3{]}^T\\ & =\tan \frac{\theta }{2}[n_1{n}_2{n}_3{]}^T\\ |{\mathbf{P}}_{cg}^{\prime }|& =\tan \frac{\theta }{2}\\ {\theta }_{{\mathbf{R}}_{cg}}& =2\arctan |{\mathbf{P}}_{cg}^{\prime }|--(12)\end{array}$$
需要解释的是这个步骤2不是必须的，步骤3可以完整地包括进去，只是这一步骤比较直观

Step3: 计算 ${\mathbf{P}}_{cg}$

还是通过公式（4）可以反求
$$\begin{array}{rl}{\mathbf{P}}_{cg}^{\prime }& =\frac{1}{\sqrt{4-{\left|{\mathbf{P}}_{cg}\right|}^2}}{\mathbf{P}}_{cg}\\ {\mathbf{P}}_{cg}& =\frac{2{\mathbf{P}}_{cg}^{\prime }}{\sqrt{1+{\left|{\mathbf{P}}_{cg}^{\prime }\right|}^2}}--(13)\end{array}$$
这里直接给出结果，容易推导

Step4: 计算 ${\mathbf{t}}_{cg}$

用引理8的公式来计算${\mathbf{t}}_{cg}$
$({\mathbf{R}}_{gij}-\mathbf{I}){\mathbf{t}}_{cg}={\mathbf{R}}_{cg}{\mathbf{t}}_{cij}+{\mathbf{t}}_{gij}$
同样的，至少两组ij，或者说三个状态以上这个线性方程组才有唯一解
得到矩阵表示
$$\left[\begin{array}{l}{\mathbf{R}}_{g{i}_1{j}_1}-\mathbf{I}\\ {\mathbf{R}}_{g{i}_2{j}_2}-\mathbf{I}\\ ⋮\\ {\mathbf{R}}_{g{i}_n{j}_n}-\mathbf{I}\end{array}\right]{\mathbf{t}}_{cg}=\left[\begin{array}{l}{\mathbf{R}}_{cg}{\mathbf{t}}_{c{i}_1{j}_1}-{\mathbf{t}}_{g{i}_1{j}_1}\\ {\mathbf{R}}_{cg}{\mathbf{t}}_{c{i}_2{j}_2}-{\mathbf{t}}_{g{i}_2{j}_2}\\ ⋮\\ {\mathbf{R}}_{cg}{\mathbf{t}}_{c{i}_n{j}_n}-{\mathbf{t}}_{g{i}_n{j}_n}\end{array}\right]$$
同样的得到$\mathbf{A}\mathbf{x}=\mathbf{b}$,其中$\mathbf{A}\in {\mathfrak{R}}^{3n\times 3}$, $\mathbf{x}\in {\mathfrak{R}}^{3\times 1}$，$\mathbf{b}\in {\mathfrak{R}}^{3n\times 1}$
同样可以使用线性最小二乘方法求解

结尾

论文还对速度和误差做了定义和分析，这里不在叙述，以后有时间再仔细看看