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

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

），c++函式展示

@author：vshawn([email protected])

@date:2016-11-04

@lab: cvlab202@csu

好了，現在問題越來越複雜了，這次的問題涉及兩個座標系：世界座標系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繞y軸旋轉了

c2繞x軸旋轉了

於是世界座標系中，相機的位置座標為(-x3,-y3,-z3)。

至於程式就很簡單了，只需要呼叫幾次《空間點繞軸旋轉公式&程式(c++)》中的函式即可。

最後，本文的成果用在了《相機位姿估計1：根據四個特徵點估計相機姿態》中。

出處：