Hooyah,
ML2 OS version: 1.10.0
MLSDK version: 1.10.0
Host OS: Win
The PNP reprojection error seems too high, especially at the image borders when using the intrinsics provided by MLCameraResultExtras/MLCameraIntrinsicCalibrationParameters.
Below is a clone of my implementation in python that is based on [1]. Could you please quickly double check the 'undist_magic_leap()' function. Maybe i made a silly mistake 
The script outputs around 0.3 pixels RMS error for the small marker. I think that is way too high.
####### Magic Leap intrinsics
Pose in camera frame:
[[ 0.00707712 0.99896557 -0.04491877]
[ 0.98881859 -0.01368233 -0.14849442]
[-0.1489554 -0.0433656 -0.98789256]]
[-210.09145112 286.64426796 662.64285839]
Residuals (pixels)
[ 0.27064139 -0.24666436 -0.28740943 0.30538637 0.23132418 -0.31972344
-0.21509027 0.26071783]
RMS error (pixels)
0.26924121538579704
Python code:
import numpy as np
from scipy.spatial.transform import Rotation as R
from scipy.optimize import least_squares
# Measurements of four corner points of a 45x45mm square
# obtained from the luminance channel (MLCameraOutputFormat_YUV_420_888)
# ..
# cv::Mat image(output->planes[0].height, output->planes[0].width, CV_8UC1, output->planes[0].data);
# ..
image_points = np.array(
[[1128.440, 3056.529],
[1135.872, 2827.960],
[922.817, 2827.048],
[914.213, 3053.972]])
s = 199.6 / 200.0
object_points = s*np.array(
[[22.5,22.5,0],
[-22.5,22.5,0],
[-22.5,-22.5,0],
[22.5,-22.5,0]])
# MLCameraIntrinsicCalibrationParameters obtained from MLCameraResultExtras
focal_length = np.array([3229.906982422, 3229.008300781])
principal_point = np.array([2041.873, 1554.210])
distortion = np.array([0.102010615170, -0.384192407131, 0.000052323157, 0.000182186413, 0.405351102352])
width = 4096
height = 3072
# https://developer-docs.magicleap.cloud/docs/guides/unity/camera/ml-camera-metadata/
def undist_magic_leap(image_points, width, height, focal_length, principal_point, distortion):
image_points_undist = np.zeros(image_points.shape)
K = np.array([distortion[0],distortion[1],distortion[4]])
P = np.array([distortion[2],distortion[3]])
pixel_to_normalized = 1.0 / np.linalg.norm(np.array([width, height]))
for i in range(image_points.shape[0]):
x = pixel_to_normalized * (image_points[i] - principal_point)
r2 = np.linalg.norm(x)**2
r4 = r2*r2
r6 = r2*r2*r2
s = x + x * K.dot(np.array([r2, r4, r6]))
s[0] += P[0] * (r2+2*x[0]**2) + 2.0*P[1]*x[0]*x[1]
s[1] += P[1] * (r2+2*x[1]**2) + 2.0*P[0]*x[0]*x[1]
#print(s)
image_points_undist[i] = s / pixel_to_normalized
image_points_undist[i] /= focal_length
return image_points_undist
# PNP objective function, for testing purposes
def pnp_error(x, f, obj_points, img_points):
r = R.from_rotvec(np.array([x[0],x[1],x[2]])).as_matrix()
t = np.array([x[3],x[4],x[5]])
residuals = np.zeros(obj_points.shape[0]*2)
for i in range(obj_points.shape[0]):
point_camera = r@obj_points[i]+t
residuals[2*i] = f[0] * point_camera[0]/point_camera[2] - img_points[i,0]
residuals[2*i+1] = f[1] * point_camera[1]/point_camera[2] - img_points[i,1]
return residuals
image_points_undist_leap = undist_magic_leap(image_points, width, height, focal_length, principal_point, distortion)
pose_guess = np.array([ -2.2, -2.2, -0.17, -211, 283, 677])
print("####### Magic Leap intrinsics")
res = least_squares(pnp_error, pose_guess, args=(np.array([1,1]), object_points, image_points_undist_leap))
print("Pose in camera frame:")
x = res["x"]
sol_r = R.from_rotvec(np.array([x[0],x[1],x[2]])).as_matrix()
print(sol_r)
print(x[3:6])
print("Residuals (pixels)")
residuals = pnp_error(res["x"], np.array([1,1]), object_points, image_points_undist_leap)
residuals_pixels = residuals * np.repeat(focal_length,object_points.shape[0])
print(residuals_pixels)
print("RMS error (pixels)")
print(np.sqrt(np.sum(residuals_pixels**2)/(2*object_points.shape[0])))
[1] # Intrinsic/Extrinsic Parameters | MagicLeap Developer Documentation