Closed-form solution of absolute orientation using unit quaternions

---

这是一篇关于ORBSLAM2的一篇论文
Closed-form solution of absolute orientation using unit
quaternions
（使用单位四元数求绝对旋转的封闭解）
翻译进度:刚开始
题外话：
在这里稍微唠两句，七月初开始看的ORB_SLAM2框架，但是感觉看起来比较吃力，到写这篇博客的时候，已经过去十天了，但是感觉进展缓慢。这十天也发生了一些事，换了一个新的环境，一切都是从头适应。第一次猫狗双全，第一次摆地摊。第一次在闲鱼上买东西，结果被骗的血本无归……是该稳一稳心态了，毕竟时间还是比较紧的
博客还是要继续写的，写学习总结，写论文翻译。其实刚开始翻译论文只是看算法，看代码看的累了，就想翻译翻译论文放松一下，毕竟有道翻译还是很好用滴（哈哈哈），不过在翻译过程中确实能学到不少东西，一些专业术语，latex语法的使用,还有就是感觉这次六级刷分更加自信了呢.再积累一段时间,尽量早日脱离有道翻译,达到翻译论文和自己写日记一样顺滑流畅。
稳住心态，烦了累了，撸撸猫，溜溜大金毛，去夜市摆摆地摊，放松一下。苦难很多，但还是要继续生活啊。每日要勤勤恳恳修炼，终有神功大成的那一天的
另外这一篇可能会翻译比较粗糙，因为我发现理解文章意思和把它翻译通顺是两个概念，斟酌遣词造句不是我该干的（懒……），目前的翻译以自己理解为主，再积累一段时间，或者等以后有时间了可能会重新完善一下，到时候就当复习一遍了

---

作者：Berthold K. P. Horn
夏威夷大学马诺亚分校电气工程系，夏威夷檀香山96720
本论文1986年8月6日接收;1986年11月25日被收录

摘要

利用两个系统中多个点的坐标测量对来寻找两个坐标系统之间的关系是一项经典的摄影测量任务。这在双目测量和机器人学中有广泛的应用。我在这里给出了三个或更多点的最小二乘问题的一个封闭解。目前的求解方法有基于经验的，基于图解的和基于数值的迭代方法。利用单位四元数表示旋转，简化了求解的推导。我强调这个问题的解应该具有的对称性。最佳平动偏移量为一个系统的坐标质心与另一个系统的坐标质心的旋转和缩放之差。最佳尺度等于两个系统中坐标与各自中心的均方根偏差之比。通过这种方法得到的结果精度较高，它是基于测量若干个特定点的方法中较好的近似方法。表示最佳旋转的单位四元数是与对称4x4矩阵的最正特征值相关联的特征向量。该矩阵的元素是点对应坐标的乘积和的组合。

1. 介绍

假设我们已知在两个不同的笛卡尔坐标系中测量的一些点的坐标(图1)，从这些测量中恢复两个系统之间的转换的摄影测量问题称为绝对方向问题。有以下几种情况，最重要的是把从成对的航空照片发展成的立体模型与地理坐标系统联系起来。它在机器人技术中也很重要，在摄像机坐标系中的测量必须与机械臂的坐标系相联系。这里我们谈到了手眼变换的确定。

A. Previous work

绝对定向问题通常用经验的、图解的或数值迭代的方式来处理。当已知三个点的观测值时，Thompson 给出了这个问题的解决方案。他的方法和Schut中更简单的方法一样，都依赖于当三个点的所有坐标都已知时，选择性地忽略可用的额外约束。Schut使用单位四元数，得到一组线性方程。在2.A节中特定情况下，我给出了一个不要求求解系统的线性方程的更简单的解法。这些方法都存在不能解决三对以上点的缺陷。更为重要的是，这些方法甚至不能充分利用从三个点中得到的可用信息。
Oswal和Balasubramanian 提出了一种可以处理三个以上点的最小二乘方法，但他们的方法并不能保证旋转矩阵的正交性。然后使用一种迭代的方法来处理结果-使它更接近标准正交。这样做的方法是迭代的，其结果不是原来最小二乘问题的解。
我在第二节和第四节中提出了一种最小二乘问题的封闭解，并且在第五节说明了其在只使用三个点的时候是非常简洁的。这是非常重要的，因为有时候只有三个点可以使用。这种解决方案与这一节开头所描述的不同，因为它没有选择性的忽略观测所提供的信息，而是使用了观测所能提供的所有信息。
Schut和Thompson为四元数在摄影测量（虽然听起来怪怪的，但photogrammetry真就是这个意思）中的应用奠定了基础。在机器人技术领域，Salamin和Taylor是主要的倡导者。使用单位四元数来表示旋转在第3节和附录中进行了回顾(参见参考文献2)。

B. 最小点的数目

两个笛卡尔坐标系之间的变换可以看做是一个刚体变换，因此该变换可以分解为一个旋转和一个平移。另外，在摄影测量中，尺度可能是未知的。显然平移有三个自由度，旋转也有三个自由度（旋转轴的方向加上绕轴的旋转角度）。尺度又增加了一个自由度。（所以对于立体视觉来说，就是7自由度喽）在两个坐标系中的三个已知点提供了九个约束（每个坐标系提供三个），这足以确定7个未知量了。
通过舍弃其中的两个约束条件，七个未知数，七个方程，就可以解出所有的参数。我在2.A小节阐述了如何用类似的方法解出旋转，证明了这三个点不共线。两个点显然提供不了足够的约束。

C. 误差的最小平方和

在实践中，测量是不精确的，所以为了提高所求转换参数的精度，我们通常使用超过三个点。我们不再期望能够找到一个转换，将一个系统中测量到的点的坐标精确地映射到另一个系统中这些点的测量坐标。取而代之的而是，我们想方法使残差的平方和最小。找到转换参数的最优解是不容易的。在实践中，使用了各种经验的、图解的和数值的方法。它们本质上都是迭代法，即先给一个估计值，这种方法能够得出一个更好的解，但是依旧不是完美的。迭代法重复的使用直到误差下降到可以忽略不计。
有时候，利用已知的信息，能够得到变换（变换矩阵T）的一个比较好的的初始值参数，这样迭代一次就能得到一个接近最小二乘问题的真正解，以此来消除实际情况需要进一步的迭代。

D. 封闭解

在这篇论文中，我提出了一种不需要迭代的绝对方位的最小二乘问题的封闭解。封闭解的优点之一就是根据两个坐标系中点的观测值，它迭代一次就能得到变换矩阵的最优解。另外一个优点是它不像迭代方法中那样需要一个好的初始值。
我给出了一种使用单位四元数表达旋转的解决方案。所期望的四元数的解被证明是与最大特征值相关联的一个对称4×4矩阵的特征向量。这个矩阵的元素是点对应坐标乘积和的简单组合。为了找到特征值，必须解一个四次方程，其系数是矩阵元素的乘积的和。这证明了这个四次幂特别简单，因为它的一个系数是零。当一组测量中有一组是共面时，它会更加简化。

E. 正交矩阵

虽然单位四元数构成了一个优雅的表示旋转，我们大多数人更熟悉的标准正交矩阵的正行列式。幸运的是，可以很容易地由单位四元数的四个分量构造出一个合适的3 X 3旋转矩阵，如在第3. E小节所示。直接处理矩阵是困难的，因为需要处理六个非线性约束，以确保矩阵是标准正交的（即旋转矩阵有6个冗余约束）。对旋转矩阵使用矩阵操作的封闭形式的解决方案将在后续的论文中提出。这个封闭解需要计算这个3×3对称矩阵的平方根。

F. 解的对称性

不如假设有两个坐标系：左坐标系和有坐标系。右坐标系到左坐标系的变换和左坐标系到右坐标系的变换是一对互逆矩阵。我在第2.D小节中说明了比例因子必须以一种特殊的方式处理，以保证这将会发生。如果使用单位四元数来表达旋转，那么对称性也能得到保证。

2. 解决方案

我们能够看到，一旦旋转已知，那么平移和比例因子也很容易得到。问题的难点在于如何解出旋转。已知三个非共线的点，我们可以很容易地在左边和右边的坐标系中构造一个有用的三角(图2)。把第一个点作为原点，连接第一个点和第二个点，并把该方向作为新x轴的方向。在这三个点构成的平面上，将新的y轴与新的x轴成直角。然后使用右手规则，画出与x,y轴都正交的新z轴。在左右坐标系中都进行这种构造。将这些构造的三角中的一个转化为另一个三角的旋转也是将两个基本的笛卡尔坐标系统联系起来的旋转。这个旋转很容易得到，我们将在下面详细说明。

A. 选择性的丢弃约束

让我们把三个点在两个坐标系的中坐标分别记作${\mathbf{r}}_{l,1}$，${\mathbf{r}}_{l,2}$，${\mathbf{r}}_{l,3}$和${\mathbf{r}}_{r,1}$，${\mathbf{r}}_{r,2}$，${\mathbf{r}}_{r,3}$。则：
$${\mathbf{x}}_l={\mathbf{r}}_{l,2}-{\mathbf{r}}_{l,1}$$
然后
$${\hat{\mathbf{x}}}_l={\mathbf{x}}_l/||{\mathbf{x}}_l||$$
是左坐标系中新x轴方向上的单位向量。然后设
$${\mathbf{y}}_l=({\mathbf{r}}_{l,3}-{\mathbf{r}}_{l,1})-[({\mathbf{r}}_{l,3}-{\mathbf{r}}_{l,1}).{\hat{\mathbf{x}}}_l]{\hat{\mathbf{x}}}_l$$

试了试坚果云自带的画图功能，见下图。感觉还是挺好用的

为$({\mathbf{r}}_{l,3}-r_{l1})$在$\hat{\mathbf{x}}$方向的投影。单位向量
$${\hat{y}}_l={\mathbf{y}}_l/||{\mathbf{y}}_l||$$
是左坐标系中新y轴的方向向量。最后，我们使用叉乘
$${\hat{z}}_l={\hat{x}}_l\times {\hat{y}}_l$$
得到左坐标系中新z轴的方向向量。同样的，我们能在右坐标系中得到相应的${\hat{x}}_r,{\hat{y}}_r,{\hat{z}}_r$。我们所求的旋转就是将${\hat{x}}_l,{\hat{y}}_l,{\hat{z}}_l$分别变换到${\hat{x}}_r,{\hat{y}}_r,{\hat{z}}_r$的旋转。
现在将列向量邻接起来形成矩阵$M_l$和$M_r$:
$${\mathbf{M}}_l=\left|\begin{array}{ccc}{\hat{x}}_l& {\hat{y}}_l& {\hat{z}}_l\end{array}\right|,{\mathbf{M}}_r=\left|\begin{array}{ccc}{\hat{x}}_r& {\hat{y}}_r& {\hat{z}}_r\end{array}\right|$$
给一个左坐标系中的向量${\mathbf{r}}_l$，则
$${\mathbf{M}}_l^T{\mathbf{r}}_l$$
给出了矢量r沿着新构造的三角轴（其实就是新坐标系）的分量。将其乘以${\mathbf{M}}_r$就可以将它映射到右坐标系中，所以
$${\mathbf{r}}_r={\mathbf{M}}_r{\mathbf{M}}_l^T{\mathbf{r}}_l$$
从中能够得到旋转
$$\mathbf{R}={\mathbf{M}}_{\mathbf{r}}{\mathbf{M}}_{\mathbf{l}}^{\mathbf{T}}$$
R是一个正交矩阵，因为$M_r$和$M_l$都是正交的。以上就是通过三个点来求旋转的封闭解的方案。注意到它选择性的使用了三个点的信息。事实上，如果我们重编号这些点，我们会得到一个不同的旋转，除非数据是完美的。也要注意，这种方法不能扩展到超过三个点的情况。
即使仅有三个点，我们也应该使用最小二乘法去解这个问题，因为这里的约束大于未知参数的个数（也就是列出来的方程是一个超定方程组，需求其最小二乘解）。平移和比例因子的最小二乘解会在2.C和2.E小节中讲到。寻找最优旋转矩阵在第四章讲到。

B. 求平移t

（咕咕……）