[SOLVED] Highly wrong K matrix with DLT Camera Calibration

Highly wrong K matrix with DLT Camera Calibration

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.

Solution

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).