カメラ画像と拘束条件から点の座標を求める？

カメラ画像の座標から、拘束条件(カメラの地上高h、俯角θ、画像は平面上のものしか写っていない)

を用いて、ワールド座標系での点(x,y,z)を求める手段についてメモ。
数学弱いので間違ってるかもしれません。
気づいた点があれば教えてください。

まず、カメラに写った像と、カメラの座標系での点の対応は、このHPにあるようになるらしい。

となると、求めたいワールド座標(x,y,z)を、ワールド座標系→カメラ座標系　変換行列(Aとおく)
に掛けて求めた座標((a,b,c)とする)から、k/c(a,b) = (r,s) = 画像上の点の座標
ということになるらしい。

ここで、拘束条件(から求められる物)は、ワールド座標系のz、変換行列Aとなる。

・・・が、変換行列Aを求められず悩み中。
座標を2軸回転させるのってどうやるんだろう・・・

オイラー角がどうのとかその辺・・・？

疲れたのでまた今度・・・