I’ve been trying to reproduce OpenCV calibrateCamera() method by hand for a uni assignment (which requires us to compare our implementation with OpenCV‘s one), and I can’t seem to get a decent K matrix in the first place.

I have got my image and object points manually measured from this .png.

Reference image

What I do to estimate the camera matrix is set up a system of equations and solve it using SVD, since it all comes up to a minimization problem (Ap=0).
The eq. system is the following:

A = np.zeros((points * 2, 12), dtype = np.float32)
X, Y, Z = 0, 1, 2
for i in range(points):
A[2 * i:] = np.array([
objp[i,X],
objp[i,Y],
objp[i,Z],
1,
0, 0, 0, 0,
-imgp[i,X] * objp[i,X],
-imgp[i,X] * objp[i,Y],
-imgp[i,X] * objp[i,Z],
-imgp[i,X]
])

A[2 * i + 1:] = np.array(
[
0, 0, 0, 0,
objp[i,X],
objp[i,Y],
objp[i,Z],
1,
-imgp[i,Y] * objp[i,X],
-imgp[i,Y] * objp[i,Y],
-imgp[i,Y] * objp[i,Z],
-imgp[i,Y]
]
)

U, S, Vt = np.linalg.svd(A)

P = Vt[np.argmin(S)]

return P.reshape(3, 4)

Then i use RQ factorization to obtain R (=K matrix, normalized in order to have 1 in K[2,2]) and Q (=R matrix).

K, R = linalg.rq(P[:3,:3])
K = K/K[2,2]

Yet when i to try calc the absolute errore between the 2 K, this is what i get:

K

[[ 1.1347135e+03 -1.5465388e+01  5.4754879e+01]
[ 0.0000000e+00  1.1388589e+03 -1.3496671e+02]
[ 0.0000000e+00  0.0000000e+00  1.0000000e+00]]

K OpenCV

[[ 1.04488512e+03  0.00000000e+00  3.67825286e+01]
[ 0.00000000e+00  1.06650239e+03 -5.04452860e+02]
[ 0.00000000e+00  0.00000000e+00  1.00000000e+00]]

Errore assoluto

[[ 89.82838194  15.4653883   17.97235037]
[  0.          72.35649686 369.48615424]
[  0.           0.           0.        ]]

Any suggestion? Tips? I followed our uni slides for these steps, but i dont know if i misunderstood something or else.

Found the solution by simply asking the teacher and having a 2 hours long Meet.

Full code on my github, the average MSE at the end now is much lower with N greater or equal 12 (at 24 is 30 and can be noticed in the projected points).