關鍵詞：空間旋轉、旋轉軸、剛體旋轉
用途：相機位姿估計、無人機位姿估計
文章類型：概念、公式總結（本文不帶推導過程，若想了解公式是如何推出來的請自習搜索文獻），C++函數展示
@Author：VShawn(singlex@foxmail.com)
@Date:2016-11-04
@Lab: CvLab202@CSU
本文接上一篇《空間點繞軸旋轉公式&程序(C++)》，繼續討論空間內的旋轉問題，可能會用到上一篇中定義的函數。
問題四：空間內的坐標系旋轉（相機坐標系在世界坐標系中的旋轉）
好了，現在問題越來越復雜了，這次的問題涉及兩個坐標系：世界坐標系W與相機坐標系C。
做過相機標定或者研究過相機模型的人對這兩個坐標系應該是相當熟悉的了，對於不了解這方面的同學可以這樣理解：
-
世界坐標系就是前文一直在用着的坐標系，它規定了所有東西的絕對坐標。
-
相機坐標系是人眼或者是相機看物體的一個坐標系，它能夠表示我觀測的這個物體距離我有多遠，方向角是多少。（其z軸為視線正中線）
下圖中的O即為相機坐標系原點，也就是人眼的位置，OZc表示的是當前視線的方向，
Ow則是世界坐標系原點。
於是我們的問題就是，人站在某點拍攝世界坐標系的原點Ow（下標w表示該坐標是在世界坐標系中的），拍攝出的原點正好落在圖像中心，現在我通過某種方法（解PNP問題）計算出Ow在相機坐標系下的坐標為（下標c表示該坐標定義在相機坐標系內），求相機或者說是人位於世界坐標系的哪里。
這個問題，當相機坐標系跟世界坐標系完全平行（指兩對坐標系的三個軸都相互對應平行）的時候很簡單，無需考慮旋轉，相機的位置就是。實際上，只要相機坐標系的三根軸與世界的三軸平行（這種平行不需要保證一一對應），這個問題都很好解決。比如下面幾幅圖的情況（黃點為世界坐標系原點Ow），只要知道世界坐標系的原點Ow在相機坐標系中的坐標，就可以很容易計算出相機的位置。
然而，一旦出現了下面的情況，那么問題就不那么好解決了。
比如說相機的狀態是這樣：
你就很難通過相機坐標系下的Ow坐標，計算出相機在世界坐標系下的位置。因為其中涉及到了相機坐標系在世界坐標系中的旋轉問題。
那么如何求解這個問題呢，事實上，在上文中通過某種方法（解PNP問題）求出原點Ow在相機坐標系下的坐標的同時，我們也獲得了相機坐標系的旋轉矩陣（沒錯，就是求相機的外參數矩陣：旋轉矩陣r&平移矩陣t），這個"某種方法"我將在其他文章中給大家展示。平移矩陣t可以告訴世界坐標系原點Ow在相機坐標系中的坐標，我們暫且假設為點P=(x0,y0,z0)，向量OcP為Oc指向Ow的向量。而旋轉矩陣r，可以求出三個歐拉角
【具體如何計算參考《根據相機旋轉矩陣求解三個軸的旋轉角/歐拉角》】。當相機坐標系C按照z軸、y軸、x軸的順序旋轉以上角度后變成坐標系C3，C3將與世界坐標系W完全平行。此時如果知道Ow在C3系中的坐標，自然就知道了Oc在世界坐標系W中的坐標。
顯然，相機坐標系經歷了三次旋轉后，原點Oc位置會保持不變，而點P跟隨坐標系進行了三次旋轉，那么向量OcP不再指向Ow。
為了抵消旋轉的作用，每次對坐標軸旋轉后，我們需要對點P進行反向旋轉，使得坐標系C3中的向量OcP依然能指向Ow。
於是得到解決問題的步驟：
第一次旋轉：
原始相機坐標系C繞z軸旋轉了
變為C1系，此時P=P0=(x0,y0,z0)，那么單獨將P點繞z軸旋轉
，得到P1=(x1,y1,z1)，為C1系中Ow的坐標。
第二次旋轉：
C1繞y軸旋轉了
變為C2系，此時P1=(x1,y1,z1)，那么將P1點繞y軸旋轉
，得到P2=(x2,y2,z2)，為C2系中Ow的坐標。
第三次旋轉：
C2繞x軸旋轉了
變為C3系，此時P2=(x2,y2,z2)，那么將P2點繞x軸旋轉
，得到P3=(x3,y3,z3)，為C3系中Ow的坐標。
於是世界坐標系中，相機的位置坐標為(-x3,-y3,-z3)。
至於程序就很簡單了，只需要調用幾次《空間點繞軸旋轉公式&程序(C++)》中的函數即可。
最后，本文的成果用在了《相機位姿估計1：根據四個特征點估計相機姿態》中。