子坐标系C在父坐标系W中的旋转问题

关键词：空间旋转、旋转轴、刚体旋转

用途：相机位姿估计、无人机位姿估计

文章类型：概念、公式总结（本文不带推倒过程，若想了解公式是如何推出来的请自习搜索文献），C++函数展示

@Author：VShawn(singlex@foxmail.com)

@Date:2016-11-04

@Lab: CvLab202@CSU

本文接上一篇《空间点绕轴旋转公式&程序(C++)》，继续讨论空间内的旋转问题，可能会用到上一篇中定义的函数。

问题四：空间内的坐标系旋转（相机坐标系在世界坐标系中的旋转）

好了，现在问题越来越复杂了，这次的问题涉及两个坐标系：世界坐标系W与相机坐标系C。

做过相机标定或者研究过相机模型的人对这两个坐标系应该是相当熟悉的了，对于不了解这方面的同学可以这样理解：

1. 世界坐标系就是前文一直在用着的坐标系，它规定了所有东西的绝对坐标。
2. 相机坐标系是人眼或者是相机看物体的一个坐标系，它能够表示我观测的这个物体距离我有多远，方向角是多少。（其z轴为视线正中线）

下图中的O即为相机坐标系原点，也就是人眼的位置，OZ_c表示的是当前视线的方向，

O_w则是世界坐标系原点。

于是我们的问题就是，人站在某点拍摄世界坐标系的原点O_w（下标w表示该坐标是在世界坐标系中的），拍摄出的原点正好落在图像中心，现在我通过某种方法（解PNP问题）计算出O_w在相机坐标系下的坐标为（下标c表示该坐标定义在相机坐标系内），求相机或者说是人位于世界坐标系的哪里。

这个问题，当相机坐标系跟世界坐标系完全平行（指两对坐标系的三个轴都相互对应平行）的时候很简单，无需考虑旋转，相机的位置就是。实际上，只要相机坐标系的三根轴与世界的三轴平行（这种平行不需要保证一一对应），这个问题都很好解决。比如下面几幅图的情况（黄点为世界坐标系原点O_w），只要知道世界坐标系的原点O_w在相机坐标系中的坐标，就可以很容易计算出相机的位置。

然而，一旦出现了下面的情况，那么问题就不那么好解决了。

比如说相机的状态是这样：

你就很难通过相机坐标系下的O_w坐标，计算出相机在世界坐标系下的位置。因为其中涉及到了相机坐标系在世界坐标系中的旋转问题。

那么如何求解这个问题呢，事实上，在上文中通过某种方法（解PNP问题）求出原点O_w在相机坐标系下的坐标的同时，我们也获得了相机坐标系的旋转矩阵（没错，就是求相机的外参数矩阵：旋转矩阵r&平移矩阵t），这个"某种方法"我将在其他文章中给大家展示。平移矩阵t可以告诉世界坐标系原点Ow在相机坐标系中的坐标，我们暂且假设为点P=(x₀,y₀,z₀)，向量O_cP为O_c指向O_w的向量。而旋转矩阵r，可以求出三个欧拉角【具体如何计算参考《根据相机旋转矩阵求解三个轴的旋转角/欧拉角》】。当相机坐标系C按照z轴、y轴、x轴的顺序旋转以上角度后变成坐标系C₃，C₃将与世界坐标系W完全平行。此时如果知道O_w在C₃系中的坐标，自然就知道了O_c在世界坐标系W中的坐标。

显然，相机坐标系经历了三次旋转后，原点O_c位置会保持不变，而点P跟随坐标系进行了三次旋转，那么向量O_cP不再指向O_w。

为了抵消旋转的作用，每次对坐标轴旋转后，我们需要对点P进行反向旋转，使得坐标系C₃中的向量O_cP依然能指向O_w。

于是得到解决问题的步骤：

第一次旋转：

原始相机坐标系C绕z轴旋转了变为C₁系，此时P=P₀=(x₀,y₀,z₀)，那么单独将P点绕z轴旋转，得到P₁=(x₁,y₁,z₁)，为C₁系中O_w的坐标。

第二次旋转：

C₁绕y轴旋转了变为C₂系，此时P₁=(x₁,y₁,z₁)，那么将P₁点绕y轴旋转，得到P₂=(x₂,y₂,z₂)，为C₂系中O_w的坐标。

第三次旋转：

C₂绕x轴旋转了变为C₃系，此时P₂=(x₂,y₂,z₂)，那么将P₂点绕x轴旋转，得到P₃=(x₃,y₃,z₃)，为C₃系中O_w的坐标。

于是世界坐标系中，相机的位置坐标为(-x₃,-y₃,-z₃)。

至于程序就很简单了，只需要调用几次《空间点绕轴旋转公式&程序(C++)》中的函数即可。

最后，本文的成果用在了《相机位姿估计1：根据四个特征点估计相机姿态》中。