Closed-form solution of absolute orientation using unit quaternions(基于 3 点三元组的求解器进行最小估计)

前言

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

求解思路

让我们把三个点在两个坐标系的中坐标分别记作$r_{l1},r_{l2},r_{l3}$和$r_{r1},r_{r2},r_{r3}$。则：

$x_1=r_{l2}-r_{l1}$

然后

${\hat{x}}_1=\frac{x_1}{||x_1||}$

是在左坐标系中新x轴方向上的单位向量。然后设

$y_1=(r_{l3}-r_{l1})-[(r_{l3}-r_{l1})\cdot {\hat{x}}_1]{\hat{x}}_1$

1. $[(r_{l3}-r_{l1})\cdot {\hat{x}}_1]$表示$[(r_{l3}-r_{l1})$在$\hat{x}$上的投影（两向量点乘，得到结果为标量）。$[(r_{l3}-r_{l1})\cdot {\hat{x}}_1]$最后在乘以${\hat{x}}_1$（即$([(r_{l3}-r_{l1})\cdot {\hat{x}}_1]{\hat{x}}_1$）得到向量（因为此时${\hat{x}}_1$表示的是x轴的单位向量）。这里操作的目的是能够得到$r_{l3}$在$x$轴的分量（这里的$x$轴是由$r_{l1}$和$r_{l2}$组成的）。
   2.$(r_{l3}-r_{l1})-[(r_{l3}-r_{l1})\cdot {\hat{x}}_1]{\hat{x}}_1$最后相减得到了垂直于$x$轴的向量$y_1$（而且这个向量$y_1$是过$r_{l3}$的）。
   如图：
   

单位向量：

${\hat{y}}_1=\frac{y_1}{||y_1||}$

是左坐标系中新y轴的方向向量。最后，我们使用叉乘:：

${\hat{z}}_1=\widehat{x_1}\times {\hat{y}}_1$

得到左坐标系中新$z$轴的方向向量。同样的，我们能在右坐标系中得到相应的${\hat{x}}_r,{\hat{y}}_r,{\hat{z}}_r$。我们所求的旋转就是将${\hat{x}}_1,{\hat{y}}_1,{\hat{z}}_1$分别变换到${\hat{x}}_r,{\hat{y}}_r,{\hat{z}}_r$的旋转。现在将列向量邻接起来形成矩阵$M_1$和$M_r$：

$M_1=\begin{array}{ccc}[{\hat{x}}_1& {\hat{y}}_1& {\hat{z}}_1]\end{array}$ ，$M_r=\begin{array}{ccc}[{\hat{x}}_r& {\hat{y}}_r& {\hat{z}}_r]\end{array}$

给一个左坐标系中的向量$r_1$，则

$M_1^T{r}_1$

给出了矢量r沿着新构造的三角轴（其实就是新坐标系）的分量。将其乘以$M_r$就可以将它映射到右坐标系中，所以：

$r_r=M_r{M}_r^T{r}_r$

如何理解两个上面等式怎么来的？我是这么理解的：这里应该是少说了一个世界坐标系，一共是有三个坐标系，左右手坐标系+一个世界坐标系。上面的等式是都转化成了在世界坐标系下来做等号的。例如：在世界坐标系下有一个点$p$，$p$乘以$M_1$得到左手坐标系上的表示为$r_1$，$p$乘以$M_r$得到右手坐标系上的表示为$r_r$，即$r_1=M_{1}p$和$r_r=M_{r}p$。
所以进一步的，能推出：$p=M_1^{-1}{r}_1=M_1^T{r}_1$和$r_r=M_{r}p=M_r{M}_r^T{r}_r$。

从中能够得到旋转矩阵：

$R=M_r{M}_l^T$

R是一个正交矩阵，因为$M_r$和$M_l$都是正交的。以上就是通过三个点来求旋转的封闭解的方案。注意到它选择性的使用了三个点的信息。事实上，如果我们重编号这些点，我们会得到一个不同的旋转，除非数据是完美的。也要注意，这种方法不能扩展到超过三个点的情况。
即使仅有三个点，我们也应该使用最小二乘法去解这个问题，因为这里的约束大于未知参数的个数（也就是列出来的方程是一个超定方程组，需求其最小二乘解）。平移和比例因子的最小二乘解会在2.C和2.E小节中讲到。寻找最优旋转矩阵在第四章讲到。

转载，侵权请联系！