I have been trying this for two days and for some reason I cannot get it to work. I have two cameras with different intrinsic camera matrices, they are setup with global coordinates in blender
Camera Left: -5, -5, 0 with 45 degrees rotation about Z-axis
Camera Right: 5, -5, 0 with -45 degrees rotation about Z-axis
I simulated points in blender and the positions on the cameras should be exact. I hard coded these into the code, but I am getting these results
Angles
Out[22]: (173.62487179673582, 165.61076366618005, 155.76859475230103)
Out[21]: (179.7648211135763, 168.02313442078392, -22.82952854817841)
Translation
Out[24]: array([ 0.04009013, 0.03941624, -0.99841832])
Out[23]: array([-0.04009013, -0.03941624, 0.99841832])
I should be getting, exactly:
Angles [0, -90, 0]
Translation [.707, 0, .707]
Scene setup for reference:
Here is my code
import cv2
import numpy as np
K_l = np.array([[1800.0, 0.0, 960.0], [0.0, 1800.0, 540.0], [0.0, 0.0, 1.0]])
K_r = np.array([[2100.0, 0.0, 960.0], [0.0, 2100.0, 540.0], [0.0, 0.0, 1.0]])
pts_l = np.array([[ 1041 , 540 ],
[ 925 , 465 ],
[ 786 , 458 ],
[ 1060 , 469 ],
[ 756 , 732 ],
[ 325 , 503 ],
[ 886 , 958 ],
[ 960 , 180 ],
[ 796 , 424 ],
[ 945 , 219 ],
[ 651 , 386 ],
[ 1731 , 676 ],
[ 572 , 590 ]])
pts_r = np.array([[ 1203 , 540 ],
[ 1001 , 453 ],
[ 825 , 458 ],
[ 1139 , 445 ],
[ 1072 , 752 ],
[ 418 , 516 ],
[ 410 , 886 ],
[ 1086 , 95 ],
[ 1151 , 405 ],
[ 1355 , 99 ],
[ 942 , 388 ],
[ 1445 , 883 ],
[ 994 , 589 ]])
F, mask = cv2.findFundamentalMat(pts_l.astype(float),pts_r.astype(float),cv2.FM_LMEDS)
E = np.dot(np.dot(np.transpose(K_r),F),K_l)
U, S, Vt = np.linalg.svd(E)
W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3)
R_1 = U.dot(W).dot(Vt)
angles1, _, _, _, _, _ = cv2.RQDecomp3x3(R_1)
R_2 = U.dot(W.T).dot(Vt)
angles2, _, _, _, _, _ = cv2.RQDecomp3x3(R_2)
T1 = U[:, 2]
T2 = -U[:, 2]