問題設定
SLAMやSfMを使って1. 複数の画像毎のカメラパラメータと 2. グローバルな3Dモデル（点群やメッシュなど）がOpenCV座標系で得られている。
OpenGLベースのシステムで推定したカメラから3Dモデルをレンダリングしたい。
レンダリングした3Dモデルを実画像に重畳表示してエラーを確認する、といった用途が考えられる。
なにが問題？
まずOpenCVとOpenGLは座標系が異なる。
同じ右手座標系であるが、OpenCVの各軸の向きはX: 右, Y: 下, Z: 前に対しOpenGLはX: 右, Y: 上, Z: 後である。
OpenCV座標系とOpenGL座標系はX軸周りに180°回転させた関係にある。
OpenGLのカメラはOpenCVのカメラを上下反転させ後ろ向きにしたものなので無変換でインポートしただけではもちろん意図したとおりの絵は出ない。
参照：http://jibaravr.blog51.fc2.com/blog-entry-83.html?sp
また、3次元の世界座標からカメラから見た2次元の画像座標への変換は
- 世界座標をカメラ座標に持っていくRigid Transformと
- 3次元のカメラ座標から2次元の画像座標に投影するProjection
からなる。
数学的な概念は共通だが風習がOpenCVとOpenGLでは異なる。
OpenGLの文脈で登場するRigid Transformはworld->cameraであることが多い（View matrix）。
OpenGLではカメラ座標から-1~+1に正規化されたNormalized Device Coordinate（NDC）へのProjectionを行う。画像座標はスクリーンサイズにより決定される。
SLAMやSfMでは登場するRigid Transformがworld->cameraとは限らない。
世界座標の3Dモデルを作成する都合上、カメラの位置姿勢がcamera->worldとして取得されることも多い。
またOpenCVではカメラ座標から画像座標へのProjectionを行う。
Projectionはピクセルのアスペクト比や光軸中心のずれを考慮したモデルとなっている。
（これらの差異は固定サイズの実画像から3DモデルをインバースレンダリングするComputer Visionと可変サイズのスクリーンに3DモデルをレンダリングするComputer Graphicsの文化の違いからきているのだろう。）
Rigid Transform
やり方1. 3Dモデルとカメラを全てOpenGL座標系に変換する
OpenCV座標系のデータをX軸周りに180°回転させOpenGL座標系にする。
Rodriguesベクトル表記であれば
[\pi, 0, 0]
4x4の回転行列であれば
T_{x180} = \left(
\begin{array}{cccc}
1 & 0 & 0 & 0\\
0 & \cos(\pi) & -\sin(\pi) & 0\\
0 & \sin(\pi) & \cos(\pi) & 0 \\
0 & 0 & 0 & 1
\end{array}
\right)
= \left(
\begin{array}{cccc}
1 & 0 & 0 & 0\\
0 & -1 & 0 & 0\\
0 & 0 & -1 & 0\\
0 & 0 & 0 & 1\\
\end{array}
\right)
これを3Dモデルの頂点位置、カメラのcamera->worldに対して左からかければよい。
OpenCV座標系におけるcamera->worldを$ T^{cv}_{c2w} $とおく。
T^{cv}_{c2w} = \left(
\begin{array}{cccc}
R_{x0} & R_{y0} & R_{z0} & t_{x}\\
R_{x1} & R_{y1} & R_{z1} & t_{y}\\
R_{x2} & R_{y2} & R_{z2} & t_{z}\\
0 & 0 & 0 & 1
\end{array}
\right)
OpenGL座標系におけるcamera->worldの変換を$ T^{gl}_{c2w} $とおくと
T^{gl}_{c2w} = T_{x180} T^{cv}_{c2w}
= \left(
\begin{array}{cccc}
R_{x0} & R_{y0} & R_{z0} & t_{x}\\
-R_{x1} & -R_{y1} & -R_{z1} & -t_{y}\\
-R_{x2} & -R_{y2} & -R_{z2} & -t_{z}\\
0 & 0 & 0 & 1
\end{array}
\right)
となる。頂点位置はR成分がないのでyとzの符号を反転させればよいだけである。
なお、world->cameraがほしい場合には上記の逆行列をとればよく、
T^{gl}_{w2c} = (T^{gl}_{c2w})^{-1} = (T_{x180} T^{cv}_{c2w})^{-1} = (T^{cv}_{c2w})^{-1} (T_{x180})^{-1}=T^{cv}_{w2c}T_{x180}
OpenCV座標系におけるworld->cameraに右から$ T_{x180} $をかければよいことがわかる。
このやり方だと3Dモデルの座標が変わってしまうため、OpenGLで表示中に3Dモデルを編集して再度OpenCV座標系でエクスポート……などをやろうとすると煩雑になる。
やり方2. カメラのRだけをOpenGL座標系に変換する
実は3Dモデルとカメラ位置はOpenCV座標系で整合性がとれているのでOpenGL座標系に変換する必要はない。
問題になるのはカメラの姿勢、Rだけである。
カメラが上下反転して後ろ向きになることを直せば正しい絵がでる。
camera->worldのRのy軸とz軸の符号を反転させればよいので
T^{gl'}_{c2w}
= \left(
\begin{array}{cccc}
R_{x0} & -R_{y0} & -R_{z0} & t_{x}\\
R_{x1} & -R_{y1} & -R_{z1} & t_{y}\\
R_{x2} & -R_{y2} & -R_{z2} & t_{z}\\
0 & 0 & 0 & 1
\end{array}
\right)
とすればよい。これはRに右から$ T_{x180} $をかけることに相当する。
world->cameraがほしい場合には上記の逆行列をとればよい。
Projection
ピンホールカメラモデルを考える。
やり方1. 焦点距離からFoVに変換する
以下の式でピクセル単位の焦点距離からラジアン単位のFoV Yに変換できる。
FoVY_rad = 2 * atan(height * 0.5 / fy)
あとはこれを度単位に変換してgluPerspective()などに渡せばよい。
ただしfx, fyが同じで、cx, cyが画像中心である場合限定である。
コンピュータビジョン的にはそういうシチュエーションはあまりないだろう。
やり方2. 手でProjection matrixをつくる
これならfx, fyが異なり、かつcx, cyが画像中心でない場合も扱える。
基本的にはピンホールカメラモデルに基づく画像座標への投影をスケーリングして、-1~+1の範囲のNDC (Normalized Device Coordinate)にすればいいだけである。OpenCVとOpenGLで画像座標のY軸が上下反転していることには注意。
日本語の資料はこれが詳しい。英語の資料はググるといろいろある。
http://13mzawa2.hateblo.jp/entry/2016/06/12/202640
上記日本語資料ではデプスをPerspective projectionと同様に非線形変換をしているが、遠いデプスの精度劣化が気になる場合にはOrthogonal projectionと同様に線型変換してもいいだろう。Zのスケーリングに関係する(3, 3)成分と(3, 4)成分を挿げ替えればよい。
http://www.songho.ca/opengl/gl_projectionmatrix.html
補足. Distortionをかける
undistortionした世界で全てを扱うことが多いので、あえて3Dモデルをレンダリングしてからdistortionをかけたいことはあまりないと思うが一応。
行列に基づくMVP変換の範囲では非線形な変換であるdistortionは扱えない。
そこで画像ができてから2次元的な画像処理をかけることになる。
GPUで処理したいならcompute shader、CPUでいいならglReadPixels()してよしなにすればよい。なおOpenCVにはundistort()はあるが逆にdistortionをかける関数はないようなので手で書かないといけない。
バグってる場合のTips
絵が出なくて辛いことはよくある。
行列はrow-majorか？col-majorか？
row-major、col-majorには複数の意味がある。
https://qiita.com/suzuryo3893/items/9e543cdf8bc64dc7002a
OpenCVとOpenGLはどちらも右手系であり行列は左からかける習わしだが、唯一注意すべきはメモリ配置である。
OpenCVのメモリ配置はrow-majorであるのに対し、3D Computer VisionやGeometry Processingで使われることが多いEigen(デフォルト)やOpenGLはcol-majorである。
バグが起きる可能性があるのは行列の生配列にインデックスでアクセスする場合や生配列をmemcpy()やglLoadMatrix()などで渡す場合である。
コピー元とコピー先で整合性がとれていないと行列が転置されるので結果が完全におかしくなる。
なお、Eigen::Matrix::coeff(row, col)、cv::Mat::at(row, col)といったrow, colを指定するアクセサだけを使って行列データの受け渡しをしていればここに問題は生じない（メモリ配置は関係ないため）。
Rigid Transformはworld->cameraか？camera->worldか？
例えばcv::calibrateCamera()の返り値（Extrinsic Parameter）やOpenGLのView Matrixはworld->cameraである。
一方、SLAMなどで出力されるCamera Poseと呼ばれるものはcamera->worldであることが多い。
なお"View Matrix"という単語がworld->cameraを指す確率は100%だが、"Extrinsic Parameter"がworld->cameraを指す確率及び"Camera Pose"がcamera->worldを指す確率は100％ではなくライブラリによる（個人の感想です）。
自前で計算しているならRigid Transformに触っているところを一つ一つチェック、ライブラリを使用しているならドキュメントを読む。恐ろしいことにRとtが別々に格納されていてRがworld->cameraでtがcamera->worldであることすらある。
Blenderで確認したら変
BlenderはZ軸が上なので頑張って変換しましょう。