はじめに
先日、カメラ行列をRQ分解することがあったのだが、cv2を使うと思ったような結果が得られなかった。なので、そもそもRQ分解が何かというまとめと、自分で計算した結果とcv2の結果を比較して、諸々考察する。
参考資料
3D系でよく使う以下の本のAppendix 4.1
[3] Richard Hartley, Andrew Zisserman. "Multiple View Geometry in computer vision". cambridge.
実験で使った dataset
以下のoxford multi-view dataset
https://www.robots.ox.ac.uk/~vgg/data/mview/
のうち、最上部のdinosaur dataを使った。
RQ分解とは
行列を三角行列と直交行列との積に分解する、いわゆる QR 分解の１つ。
R(upper-triangular matrix, 上三角行列)は正方行列の中で以下のような左下成分が0となる行列。（言い換えるとrightが0でない）
R =
\begin{bmatrix}
r_{1,1} & r_{1,2} & r_{1,3} & \cdots & r_{1,n} \\
0 & r_{2,2} & r_{2,3} & \cdots & r_{2,n} \\
0 & \cdots & \ddots & \ddots & \vdots \\
0 & 0 & \cdots & r_{n-1,n-1} & r_{n-1,n} \\
0 & 0 & \cdots & 0 & r_{n,n}
\end{bmatrix} \tag{0.1}
上側が０でないという意味で名前の通り U と書くこともある。
一方で Q は直交行列。（一般的な直交行列をR(rotation)とする表記と異なるので注意が必要！）
なので、RQ分解とはある行列を三角行列と直交行列に分解すること。
以下では3D系やるときに関係ある、3x3行列 -> RQへの分解 の場合だけ考える。
RQ分解の基本的な方針
もし任意の 3x3 行列 A に直交行列 Q を右からかけて以下のように
R = A Q \tag{0.2}
と三角行列にできるなら、
A = R Q^{-1} = R Q^\top \tag{0.3}
とRQ分解できるだろう。ではそのようなQ, Rはあるのか？
いきなりそのようなものを考えると難しいので、Aに $Q_1$ をかけて３つの０成分のうちの１つが０となるものをまず考える。
それが可能なら、さらに $Q_2, Q_3$ をかけて 0 を増やしていくという方向でいく。
つまり以下の step。
１）任意の 3x3 行列 A に右から直交行列 $Q_1$ をかけて左下の成分の１つが 0 とする。
２）さらに右から 直交行列 $Q_2$ をかけて左下の成分の 0 を１つ増やす
３）さらに右から 直交行列 $Q_3$ をかけて左下の 3 成分全てを 0 とする
４）$A = R Q^\top_3 Q^\top_2 Q^\top_1$ として終了
RQ分解
成分１つを0とする
実際には直交行列として３つの回転行列を利用する。
\begin{eqnarray}
Q_x &=&
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\theta_x & -\sin\theta_x \\
0 & \sin\theta_x & \cos\theta_x\\
\end{bmatrix}, \tag{1.1}\\
Q_y &=&
\begin{bmatrix}
\cos\theta_y & 0 & \sin\theta_y \\
0 & 1 & 0 \\
-\sin\theta_y & 0 & \cos\theta_y \\
\end{bmatrix}, \tag{1.2}\\
Q_z &=&
\begin{bmatrix}
\cos\theta_z & -\sin\theta_z & 0 \\
\sin\theta_z & \cos\theta_z & 0 \\
0 & 0 & 1 \\
\end{bmatrix}, \tag{1.3}\\
\end{eqnarray}
任意の 3x3行列 A
A =
\begin{bmatrix}
a_{1,1} & a_{1,2} & a_{1,3} \\
a_{2,1} & a_{2,2} & a_{2,3} \\
a_{3,1} & a_{3,2} & a_{3,3} \\
\end{bmatrix}, \tag{1.4}\\
の右から $\theta_x$ を未知数とする $Q_x$ をかける
\begin{eqnarray}
B &=& A Q_x\\
&=&
\begin{bmatrix}
a_{1,1} & a_{1,2} & a_{1,3} \\
a_{2,1} & a_{2,2} & a_{2,3} \\
a_{3,1} & a_{3,2} & a_{3,3} \\
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\theta_x & -\sin\theta_x \\
0 & \sin\theta_x & \cos\theta_x \\
\end{bmatrix}, \\
&=&
\begin{bmatrix}
a_{1,1} & a_{1,2} \cos\theta_x + a_{1,3} \sin\theta_x & - a_{1,2} \sin\theta_x + a_{1,3} \cos\theta_x \\
a_{2,1} & a_{2,2} \cos\theta_x + a_{2,3} \sin\theta_x & - a_{2,2} \sin\theta_x + a_{2,3} \cos\theta_x \\
a_{3,1} & a_{3,2} \cos\theta_x + a_{3,3} \sin\theta_x & - a_{3,2} \sin\theta_x + a_{3,3} \cos\theta_x \\
\end{bmatrix} \tag{1.5}
\end{eqnarray}
ここで左辺 B の(3,2)成分を 0 にするよう、$\theta_x$ を決める。
\begin{eqnarray}
\left\{
\begin{aligned}
a_{3,2} \cos\theta_x + a_{3,3} \sin\theta_x = 0 \\
\cos^2\theta_x + \sin^2\theta_x = 1
\end{aligned} \tag{1.6}
\right.
\end{eqnarray}
$a_{3,2} \neq 0$ として
\begin{eqnarray}
\rightarrow \left\{
\begin{aligned}
\cos\theta_x = - \frac{a_{3,3}}{a_{3,2}} \sin\theta_x \\
\cos^2\theta_x + \sin^2\theta_x = 1
\end{aligned} \tag{1.7}
\right. \\
\\
\rightarrow \left\{
\begin{aligned}
\cos\theta_x = - \frac{a_{3,3}}{a_{3,2}} \sin\theta_x \\
\left( \frac{a_{3,3}^2 + a_{3,2}^2}{a_{3,2}^2} \right) \sin^2\theta_x = 1
\end{aligned}\tag{1.8}
\right.
\end{eqnarray} \\
さらに $a_{3,3} \neq 0$ として
\begin{eqnarray}
\rightarrow \left\{
\begin{aligned}
\cos\theta_x = - \frac{a_{3,3}}{a_{3,2}} \sin\theta_x \\
\sin^2\theta_x = \left( \frac{a_{3,2}^2}{a_{3,3}^2 + a_{3,2}^2} \right)
\end{aligned}\tag{1.9}
\right.
\end{eqnarray} \\
当面の目標は $\theta_x$ の解が１つ見つかればよいので、$\sin\theta > 0, a_{3,2} > 0$ のみ採用すると
\begin{eqnarray}
&\rightarrow& \left\{
\begin{aligned}
\cos\theta_x = - \frac{a_{3,3}}{a_{3,2}} \sin\theta_x \\
\sin\theta_x = \frac{a_{3,2}}{\left(a_{3,3}^2 + a_{3,2}^2 \right)^{\frac{1}{2}}}
\end{aligned}\tag{1.10}
\right. \\
\\
&\leftrightarrow& \left\{
\begin{aligned}
\cos\theta_x = - \frac{a_{3,3}}{\left(a_{3,3}^2 + a_{3,2}^2 \right)^{\frac{1}{2}}} \\
\sin\theta_x = \frac{a_{3,2}}{\left(a_{3,3}^2 + a_{3,2}^2 \right)^{\frac{1}{2}}}
\end{aligned}\tag{1.11}
\right.
\end{eqnarray} \\
これにより(1.5)式の関係は
\begin{eqnarray}
B &=& A Q_x\\
&=&
\begin{bmatrix}
a_{1,1} & a_{1,2} & a_{1,3} \\
a_{2,1} & a_{2,2} & a_{2,3} \\
a_{3,1} & a_{3,2} & a_{3,3} \\
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\theta_x & -\sin\theta_x \\
0 & \sin\theta_x & \cos\theta_x \\
\end{bmatrix}, \\
&=&
\begin{bmatrix}
a_{1,1} & a_{1,2}' & a_{1,3}' \\
a_{2,1} & a_{2,2}' & a_{2,3}' \\
a_{3,1} & 0 & a_{3,3}' \\
\end{bmatrix} \tag{1.12}
\end{eqnarray}
さらに成分の１つを０とする
次にこの B の右から $\theta_y$ を未知数とする $Q_y$ をかける。
\begin{eqnarray}
C &=& A Q_x Q_y\\
&=& B Q_y \\
&=&
\begin{bmatrix}
a_{1,1} & a_{1,2}' & a_{1,3}' \\
a_{2,1} & a_{2,2}' & a_{2,3}' \\
a_{3,1} & 0 & a_{3,3}' \\
\end{bmatrix}
\begin{bmatrix}
\cos\theta_y & 0 & \sin\theta_y \\
0 & 1 & 0 \\
-\sin\theta_y & 0 & \cos\theta_y \\
\end{bmatrix}, \\
&=&
\begin{bmatrix}
a_{1,1}\cos\theta_y - a_{1,3}'\sin\theta_y & a_{1,2}' & a_{1,1}\sin\theta_y + a_{1,3}' \cos\theta_y \\
a_{2,1}\cos\theta_y - a_{2,3}'\sin\theta_y & a_{2,2}' & a_{2,1}\sin\theta_y + a_{2,3}' \cos\theta_y \\
a_{3,1}\cos\theta_y - a_{3,3}'\sin\theta_y & 0 & a_{3,1}\sin\theta_y + a_{3,3}' \cos\theta_y \\
\end{bmatrix} \tag{2.1}
\end{eqnarray}
そうすると2列目の値は維持されるので、(3,2)成分の 0 は維持される。
また先と同様に $\theta_y$ を調整することで C の値１つをゼロとしたい。今回は（3,1)成分を0とする。
\begin{eqnarray}
\left\{
\begin{aligned}
a_{3,1} \cos\theta_y - a_{3,3}' \sin\theta_y = 0 \\
\cos^2\theta_y + \sin^2\theta_y = 1
\end{aligned} \tag{2.2}
\right.
\end{eqnarray}
同様にこれを解いて解を１つ求めると
\begin{eqnarray}
\rightarrow
\left\{
\begin{aligned}
\sin\theta_y &=& \frac{a_{3,1}}{\left( {a'}_{3,3}^2 + a_{3,1}^2 \right)^{\frac{1}{2}}} \\
\cos\theta_y &=& \frac{{a'}_{3,3}}{\left( {a'}_{3,3}^2 + a_{3,1}^2 \right)^{\frac{1}{2}}}
\end{aligned} \tag{2.3}
\right.
\end{eqnarray}
これにより(2,1)は
\begin{eqnarray}
C &=& A Q_x Q_y\\
&=& B Q_y \\
&=&
\begin{bmatrix}
a_{1,1} & a_{1,2}' & a_{1,3}' \\
a_{2,1} & a_{2,2}' & a_{2,3}' \\
a_{3,1} & 0 & a_{3,3}' \\
\end{bmatrix}
\begin{bmatrix}
\cos\theta_y & 0 & \sin\theta_y \\
0 & 1 & 0 \\
-\sin\theta_y & 0 & \cos\theta_y \\
\end{bmatrix}, \\
&=&
\begin{bmatrix}
a_{1,1}' & a_{1,2}' & a_{1,3}'' \\
a_{2,1}' & a_{2,2}' & a_{2,3}'' \\
0 & 0 & a_{3,3}'' \\
\end{bmatrix} \tag{2.4}
\end{eqnarray}
となる。あと(2,1)成分を0 とできれば三角行列ができあがる。
０を３つとして三角行列を完成させる
(2.4)式のCの右から $Q_z$ をかける。
\begin{eqnarray}
D &=& A Q_x Q_y Q_z\\
&=& C Q_z \\
&=&
\begin{bmatrix}
a_{1,1}' & a_{1,2}' & a_{1,3}'' \\
a_{2,1}' & a_{2,2}' & a_{2,3}'' \\
0 & 0 & a_{3,3}'' \\
\end{bmatrix}
\begin{bmatrix}
\cos\theta_z & -\sin\theta_z & 0 \\
\sin\theta_z & \cos\theta_z & 0 \\
0 & 0 & 1 \\
\end{bmatrix} \\
&=&
\begin{bmatrix}
a_{1,1}'\cos\theta_z + a_{1,2}' \sin\theta_z & -a_{1,1}'\sin\theta_z + a_{1,2}' \cos\theta_z & a_{1,3}'' \\
a_{2,1}'\cos\theta_z + a_{2,2}' \sin\theta_z & -a_{2,1}'\sin\theta_z + a_{2,2}' \cos\theta_z & a_{2,3}'' \\
0 & 0 & a_{3,3}'' \\
\end{bmatrix}
\tag{3.1}
\end{eqnarray}
と第３行が維持されるので、２つの０は維持される。
よって、(2,1)成分が０となるように $\theta_z$ を決めると三角行列が完成する。
\begin{eqnarray}
\left\{
\begin{aligned}
a_{2,1}'\cos\theta_z + a_{2,2}' \sin\theta_z = 0 \\
\cos^2\theta_z + \sin^2\theta_z = 1
\end{aligned} \tag{3.2}
\right.
\end{eqnarray}
同様にこれを解いて解を１つ求めると
\begin{eqnarray}
\rightarrow
\left\{
\begin{aligned}
\sin\theta_z &=& \frac{{a'}_{2,1}}{\left( {a'}_{2,2}^2 + {a'}_{2,1}^2 \right)^{\frac{1}{2}}} \\
\cos\theta_z &=& - \frac{{a'}_{2,2}}{\left( {a'}_{2,2}^2 + {a'}_{2,1}^2 \right)^{\frac{1}{2}}}
\end{aligned} \tag{3.3}
\right.
\end{eqnarray}
これにより(3,1)は
\begin{eqnarray}
D &=& A Q_x Q_y Q_z\\
&=& C Q_z \\
&=&
\begin{bmatrix}
a_{1,1}' & a_{1,2}' & a_{1,3}'' \\
a_{2,1}' & a_{2,2}' & a_{2,3}'' \\
0 & 0 & a_{3,3}'' \\
\end{bmatrix}
\begin{bmatrix}
\cos\theta_z & -\sin\theta_z & 0 \\
\sin\theta_z & \cos\theta_z & 0 \\
0 & 0 & 1 \\
\end{bmatrix} \\
&=&
\begin{bmatrix}
{a''}_{1,1} & {a''}_{1,2} & a_{1,3}'' \\
0 & {a''}_{2,2} & {a''}_{2,3}'' \\
0 & 0 & a_{3,3}'' \\
\end{bmatrix} \\
&=& R \tag{3.4}
\end{eqnarray}
となり三角行列が求まった。
RQ分解
これに対し(0.3)式のように
\begin{eqnarray}
R &=& A Q_x Q_y Q_z\\
\leftrightarrow R Q_z^\top Q_y^\top Q_x^\top &=& A \tag{4.1} \\
\end{eqnarray}
とすればRQが完成する。
分解の可能性について
論文とかちゃんと読んでないが、これらの操作を行うことにより、元の行列の左下成分が０となる等の例外の場合を除けば必ずRQ分解はできそう。
一方で元の行列の左下成分が０となる等であれば、三角行列はさらに簡単に求まりそうなので、結局「必ずRQ分解できるっぽい」
疑問
- この計算方法だと、途中で２次方程式を３回くらい解くので解は８パターンくらい存在しそう。またこの方法以外で求まる方法があるのなら更に解は増えそう。そうした場合に、例えばRをカメラ内部行列とするものはどうやって求まるのか。
-> 実際に numpy で上記計算を実施し、cv2.DecomposeProjectionMatrix() の結果と比較する。
実験：numpyとcv2.DecomposeProjectionMatrix()との比較
1) data setから射影行列Pを取得
dataset の射影行列Pをloadし、そのうちの n 番目のみ使う。
以下では射影行列をカメラ行列と回転行列に分解することを想定し、上三角行列を K、直交行列を R と表記する。
import numpy as np
import scipy.io
import cv2
# load projection matrix
mat = scipy.io.loadmat(os.path.join(data_dir, 'dino_Ps.mat'))
# use only idx=0
n = 0
P = mat['P'][0][n]
2) cv2.DecomposeProjectionMatrix() を使ってRQ分解する
# get K(Upper triangular matrix), R(orthogonal matrix) using cv2
out = cv2.decomposeProjectionMatrix(P)
print("K")
print(out[0])
print("R")
print(out[1])
上三角行列（K)と直交行列（R)は以下のようになる。
K
[[ 3.94551604e+01 -9.63979111e-01 -3.55473738e+00]
[ 0.00000000e+00 2.81127518e+01 1.31280929e+01]
[ 0.00000000e+00 0.00000000e+00 -1.22633291e-02]]
R
[[ 0.0100503 0.99916705 -0.03954999]
[-0.04685491 -0.03903798 -0.99813859]
[-0.99885114 0.0118847 0.04642353]]
3) numpyを使ってRQ分解し、結果を比較する
文字は出来るだけ上記の式と一致させた。
A = P[:, :3]
# 1) get B, Qx
sin_theta_x1 = A[2, 1] / (A[2, 2] ** 2 + A[2, 1] ** 2) ** 0.5
cos_theta_x1 = - A[2, 2] / A[2, 1] * sin_theta_x1
sin_theta_x2 = - sin_theta_x1
cos_theta_x2 = - cos_theta_x1
Qx1 = np.array([[1., 0., 0.],
[0., cos_theta_x1,-sin_theta_x1],
[0., sin_theta_x1, cos_theta_x1]], dtype=A.dtype)
Qx2 = np.array([[1., 0., 0.],
[0., cos_theta_x2, -sin_theta_x2],
[0., sin_theta_x2, cos_theta_x2]], dtype=A.dtype)
B1 = A.dot(Qx1)
B2 = A.dot(Qx2)
# 2) get C, Qy
sin_theta_y11 = B1[2, 0] / (B1[2, 2] ** 2 + B1[2, 0] ** 2) ** 0.5
cos_theta_y11 = B1[2, 2] / B1[2, 0] * sin_theta_y11
sin_theta_y12 = - sin_theta_y11
cos_theta_y12 = - cos_theta_y11
sin_theta_y21 = B2[2, 0] / (B2[2, 2] ** 2 + B2[2, 0] ** 2) ** 0.5
cos_theta_y21 = B2[2, 2] / B2[2, 0] * sin_theta_y21
sin_theta_y22 = - sin_theta_y21
cos_theta_y22 = - cos_theta_y21
Qy11 = np.array([[cos_theta_y11, 0., sin_theta_y11],
[0., 1., 0.],
[-sin_theta_y11, 0., cos_theta_y11]], dtype=A.dtype)
Qy12 = np.array([[cos_theta_y12, 0., sin_theta_y12],
[0., 1., 0.],
[-sin_theta_y12, 0., cos_theta_y12]], dtype=A.dtype)
Qy21 = np.array([[cos_theta_y21, 0., sin_theta_y21],
[0., 1., 0.],
[-sin_theta_y21, 0., cos_theta_y21]], dtype=A.dtype)
Qy22 = np.array([[cos_theta_y22, 0., sin_theta_y22],
[0., 1., 0.],
[-sin_theta_y22, 0., cos_theta_y22]], dtype=A.dtype)
C11 = B1.dot(Qy11)
C12 = B1.dot(Qy12)
C21 = B2.dot(Qy21)
C22 = B2.dot(Qy22)
# 3) get R, Qz
sin_theta_z111 = C11[1, 0] / (C11[1, 1] ** 2 + C11[1, 0] ** 2) ** 0.5
cos_theta_z111 = -C11[1, 1] / C11[1, 0] * sin_theta_z111
sin_theta_z112 = -sin_theta_z111
cos_theta_z112 = -cos_theta_z111
sin_theta_z121 = C12[1, 0] / (C12[1, 1] ** 2 + C12[1, 0] ** 2) ** 0.5
cos_theta_z121 = -C12[1, 1] / C12[1, 0] * sin_theta_z121
sin_theta_z122 = -sin_theta_z121
cos_theta_z122 = -cos_theta_z121
sin_theta_z211 = C21[1, 0] / (C21[1, 1] ** 2 + C21[1, 0] ** 2) ** 0.5
cos_theta_z211 = -C21[1, 1] / C21[1, 0] * sin_theta_z211
sin_theta_z212 = -sin_theta_z211
cos_theta_z212 = -cos_theta_z211
sin_theta_z221 = C22[1, 0] / (C22[1, 1] ** 2 + C22[1, 0] ** 2) ** 0.5
cos_theta_z221 = -C22[1, 1] / C22[1, 0] * sin_theta_z221
sin_theta_z222 = -sin_theta_z221
cos_theta_z222 = -cos_theta_z221
Qz111 = np.array([[cos_theta_z111, -sin_theta_z111, 0.],
[sin_theta_z111, cos_theta_z111, 0.],
[0., 0., 1.]], dtype=A.dtype)
Qz112 = np.array([[cos_theta_z112, -sin_theta_z112, 0.],
[sin_theta_z112, cos_theta_z112, 0.],
[0., 0., 1.]], dtype=A.dtype)
Qz121 = np.array([[cos_theta_z121, -sin_theta_z121, 0.],
[sin_theta_z121, cos_theta_z121, 0.],
[0., 0., 1.]], dtype=A.dtype)
Qz122 = np.array([[cos_theta_z122, -sin_theta_z122, 0.],
[sin_theta_z122, cos_theta_z122, 0.],
[0., 0., 1.]], dtype=A.dtype)
Qz211 = np.array([[cos_theta_z211, -sin_theta_z211, 0.],
[sin_theta_z211, cos_theta_z211, 0.],
[0., 0., 1.]], dtype=A.dtype)
Qz212 = np.array([[cos_theta_z212, -sin_theta_z212, 0.],
[sin_theta_z212, cos_theta_z212, 0.],
[0., 0., 1.]], dtype=A.dtype)
Qz221 = np.array([[cos_theta_z221, -sin_theta_z221, 0.],
[sin_theta_z221, cos_theta_z221, 0.],
[0., 0., 1.]], dtype=A.dtype)
Qz222 = np.array([[cos_theta_z222, -sin_theta_z222, 0.],
[sin_theta_z222, cos_theta_z222, 0.],
[0., 0., 1.]], dtype=A.dtype)
K111 = C11.dot(Qz111)
K112 = C11.dot(Qz112)
K121 = C12.dot(Qz121)
K122 = C12.dot(Qz122)
K211 = C21.dot(Qz211)
K212 = C21.dot(Qz212)
K221 = C22.dot(Qz221)
K222 = C22.dot(Qz222)
R111 = Qz111.T.dot(Qy11.T).dot(Qx1.T)
R112 = Qz112.T.dot(Qy11.T).dot(Qx1.T)
R121 = Qz121.T.dot(Qy12.T).dot(Qx1.T)
R122 = Qz122.T.dot(Qy12.T).dot(Qx1.T)
R211 = Qz211.T.dot(Qy21.T).dot(Qx2.T)
R212 = Qz212.T.dot(Qy21.T).dot(Qx2.T)
R221 = Qz221.T.dot(Qy22.T).dot(Qx2.T)
R222 = Qz222.T.dot(Qy22.T).dot(Qx2.T)
２次方程式を３回解くので解は８パターンできるが、実際はその中に同じものがある。この中にcv2の結果と一致するものがあるか、調べる。
K_R_Qx_Qy_Qz = {"solution1": {'K': K111, 'R': R111, 'Qx': Qx1, 'Qy': Qy11, 'Qz': Qz111},
"solution2": {'K': K112, 'R': R112, 'Qx': Qx1, 'Qy': Qy11, 'Qz': Qz112},
"solution3": {'K': K121, 'R': R121, 'Qx': Qx1, 'Qy': Qy12, 'Qz': Qz121},
"solution4": {'K': K122, 'R': R122, 'Qx': Qx1, 'Qy': Qy12, 'Qz': Qz122},
"solution5": {'K': K211, 'R': R211, 'Qx': Qx2, 'Qy': Qy21, 'Qz': Qz211},
"solution6": {'K': K212, 'R': R212, 'Qx': Qx2, 'Qy': Qy21, 'Qz': Qz212},
"solution7": {'K': K221, 'R': R221, 'Qx': Qx2, 'Qy': Qy22, 'Qz': Qz221},
"solution8": {'K': K222, 'R': R222, 'Qx': Qx2, 'Qy': Qy22, 'Qz': Qz222},
}
for k, v in K_R_Qx_Qy_Qz.items():
print("----------------------------- {} -----------------------------".format(k))
print("K")
print(v['K'])
print("R")
print(v['R'])
以下が結果。
----------------------------- solution1 -----------------------------
K
[[ 3.94551604e+01 9.63979112e-01 3.55473738e+00]
[-3.02876531e-16 -2.81127518e+01 -1.31280929e+01]
[ 2.43812135e-21 -2.50285060e-20 1.22633291e-02]]
R
[[ 0.0100503 0.99916705 -0.03954999]
[ 0.04685491 0.03903798 0.99813859]
[ 0.99885114 -0.0118847 -0.04642353]]
----------------------------- solution2 -----------------------------
K
[[-3.94551604e+01 -9.63979112e-01 3.55473738e+00]
[ 3.02876531e-16 2.81127518e+01 -1.31280929e+01]
[-2.43812135e-21 2.50285060e-20 1.22633291e-02]]
R
[[-0.0100503 -0.99916705 0.03954999]
[-0.04685491 -0.03903798 -0.99813859]
[ 0.99885114 -0.0118847 -0.04642353]]
----------------------------- solution3 ----------------------------
K
[[-3.94551604e+01 9.63979112e-01 -3.55473738e+00]
[ 3.02876531e-16 -2.81127518e+01 1.31280929e+01]
[-2.43812135e-21 -2.50285060e-20 -1.22633291e-02]]
R
[[-0.0100503 -0.99916705 0.03954999]
[ 0.04685491 0.03903798 0.99813859]
[-0.99885114 0.0118847 0.04642353]]
----------------------------- solution4 -----------------------------
K
[[ 3.94551604e+01 -9.63979112e-01 -3.55473738e+00]
[-3.02876531e-16 2.81127518e+01 1.31280929e+01]
[ 2.43812135e-21 2.50285060e-20 -1.22633291e-02]]
R
[[ 0.0100503 0.99916705 -0.03954999]
[-0.04685491 -0.03903798 -0.99813859]
[-0.99885114 0.0118847 0.04642353]]
----------------------------- solution5 -----------------------------
K
[[ 3.94551604e+01 9.63979112e-01 3.55473738e+00]
[-3.02876531e-16 -2.81127518e+01 -1.31280929e+01]
[ 2.43812135e-21 -2.50285060e-20 1.22633291e-02]]
R
[[ 0.0100503 0.99916705 -0.03954999]
[ 0.04685491 0.03903798 0.99813859]
[ 0.99885114 -0.0118847 -0.04642353]]
----------------------------- solution6 -----------------------------
K
[[-3.94551604e+01 -9.63979112e-01 3.55473738e+00]
[ 3.02876531e-16 2.81127518e+01 -1.31280929e+01]
[-2.43812135e-21 2.50285060e-20 1.22633291e-02]]
R
[[-0.0100503 -0.99916705 0.03954999]
[-0.04685491 -0.03903798 -0.99813859]
[ 0.99885114 -0.0118847 -0.04642353]]
----------------------------- solution7 -----------------------------
K
[[-3.94551604e+01 9.63979112e-01 -3.55473738e+00]
[ 3.02876531e-16 -2.81127518e+01 1.31280929e+01]
[-2.43812135e-21 -2.50285060e-20 -1.22633291e-02]]
R
[[-0.0100503 -0.99916705 0.03954999]
[ 0.04685491 0.03903798 0.99813859]
[-0.99885114 0.0118847 0.04642353]]
----------------------------- solution8 -----------------------------
K
[[ 3.94551604e+01 -9.63979112e-01 -3.55473738e+00]
[-3.02876531e-16 2.81127518e+01 1.31280929e+01]
[ 2.43812135e-21 2.50285060e-20 -1.22633291e-02]]
R
[[ 0.0100503 0.99916705 -0.03954999]
[-0.04685491 -0.03903798 -0.99813859]
[-0.99885114 0.0118847 0.04642353]]
４番目、もしくは８番目と一致しているようだ。念のため、cv2の結果からnumpyの結果を行列ベースで引いたものに対しフロベニウス・ノルムを求め、それが小さな値となるか調べる。
def check_solutions(K_from_cv2, R_from_cv2, My_K_R_Qx_Qy_Qz):
for k, v in My_K_R_Qx_Qy_Qz.items():
my_K = v['K']
my_R = v['R']
# calculate Frobenius norm of difference between cv2 and numpy
froNorm_K = np.linalg.norm(K_from_cv2 - my_K, ord='fro')
froNorm_R = np.linalg.norm(R_from_cv2 - my_R, ord='fro')
if froNorm_K < 0.0000001 and froNorm_R < 0.0000001 :
print("solution {} is matched!".format(k))
check_solutions(out[0], out[1], K_R_Qx_Qy_Qz)
以下が結果。
solution4 is matched!
solution8 is matched!
やはり4番目と８番目が一致。
８番目に関しては $\sin \theta$ に関する2次方程式を解く際に全てマイナスとした場合に相当する。cv2はそのような基準で１つを選定しているのだろうか。