子座標系C在父座標系W中的旋轉問題

標籤：經曆   family   歐拉   alt   座標系   htm   justify   sub   簡單   

關鍵詞：空間旋轉、旋轉軸、剛體旋轉

用途：相機位姿估計、無人機位姿估計

文章類型：概念、公式總結（本文不帶推倒過程，若想瞭解公式是如何推出來的請自習搜尋文獻），C++函數展示

@Author：V_Shawn

@Date:2016-11-04

@Lab: [email protected]

本文接上一篇《空間點繞軸旋轉公式&程式(C++)》，繼續討論空間內的旋轉問題，可能會用到上一篇中定義的函數。

問題四：空間內的座標系旋轉（相機座標系在全局座標系中的旋轉）

好了，現在問題越來越複雜了，這次的問題涉及兩個座標系：全局座標系W與相機座標系C。

做過相機標定或者研究過相機模型的人對這兩個座標系應該是相當熟悉的了，對於不瞭解這方面的同學可以這樣理解：

1. 全局座標系就是前文一直在用著的座標系，它規定了所有東西的絕對座標。
2. 相機座標系是人眼或者是相機看物體的一個座標系，它能夠表示我觀測的這個物體距離我有多遠，方向角是多少。（其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軸、x軸、y軸的順序旋轉以上角度後變成座標系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++)》中的函數即可。

子座標系C在父座標系W中的旋轉問題