E-PnP results different between OpenCV and Matlab

Hello,
I’m trying to use the PnP implementation from this repository:

Someone before me was using the matlab implementation, and used the code below to test it. My task now is to use OpenCV in Python instead in our project (From what I’ve seen the PnP implementation in OpenCV is identical to the C++ one in this repository).
So I converted the test code to Python, and I tested that the input matrices `worldPoints, imagePoints, intrinsicMatrix` are identical so I expected the output to be the same for both cases but for some reason it’s not. Is it something to do with the coordinate systems, or maybe the order of the points? I’d appreciate if someone can help me figure out what I’m doing wrong.

Matlab test code:

``````% GOAL: estimate lat lon and heading
clear all

%useful constants
a_wgs84 = 6378137.0;
f_wgs84 = 1/298.257223563;
e_wgs84 = sqrt(2*f_wgs84-f_wgs84^2);

% camera parameters
fx = 1100.7622070312500;
fy = 1102.0703125000000;
cx = 494.43371582031250;
cy = 311.53576660156250;
s = 0;
%IntrinsicMatrix = [715.2699 0 0; 0 711.5281 0; 565.6995 355.3466 1];

IntrinsicMatrix = [fx 0 0; s fy 0; cx cy 1];
k2 = 0.15480099999999999;
k4 = -0.051244999999999999;
cameraParams = ...
cameraParameters('IntrinsicMatrix',IntrinsicMatrix...

% pose estimation
% FROM CAMERA: object detector must output 6 image points
p = [402.53841229193335493619088083506, 1042.8969270166453497949987649918;
739.0592563903950349413207732141, 737.46669248644434446759987622499
1064.4752130131680587510345503688, 436.04453911696316481538815423846
1445.3768396591788132354849949479, 768.95855925639011729799676686525
1853.2714949651433471444761380553, 373.06080557707161915459437295794
1497.8632842757549497036961838603, 20.651820294345468628969790859799];

%scaling in order to prevent ill conditioning
mu = mean(p(:,1));
su = std(p(:,1));
mv = mean(p(:,2));
sv = std(p(:,2));

Scale_uv = [sqrt(2)/su, 0 -sqrt(2)*mu/su;
0,          sqrt(2)/sv, -sqrt(2)*mv/sv;
0,0 ,1];

for i=1:length(p)
A  = Scale_uv*[p(i,:)';1];
p_s(i,:)= [A(1,1),A(2,1)];
end

imagePoints = p_s;

% FROM AERIAL IMAGERY:  world points must be converted to ECEF
[x(1,1),y(1,1),z(1,1)] = lla2ecef(31.5944594*pi/180,34.6125853*pi/180,83.1465369);
[x(2,1),y(2,1),z(2,1)] = lla2ecef(31.5946889*pi/180,34.6125481*pi/180,82.574187);
[x(3,1),y(3,1),z(3,1)] = lla2ecef(31.5945089*pi/180,34.6123806*pi/180,83.1020015);
[x(4,1),y(4,1),z(4,1)] = lla2ecef(31.5945264*pi/180,34.6123929*pi/180,83.0722942);
[x(5,1),y(5,1),z(5,1)] = lla2ecef(31.5945082*pi/180,34.6121296*pi/180,82.6599009);
[x(6,1),y(6,1),z(6,1)] = lla2ecef(31.5944436*pi/180,34.611917*pi/180,82.4766086);

%scaling in order to prevent ill conditioning
mx = mean(x);
sx =std(x);
my = mean(y);
sy =std(y);
mz = mean(z);
sz =std(z);

Scale_xyz= [ sqrt(3)/sx, 0, 0, -sqrt(3)*mx/sx;
0, sqrt(3)/sy,0,-sqrt(3)*my/sy;
0, 0, sqrt(3)/sz,-sqrt(3)*mz/sz;
0,0,0, 1];

for i =1:length(x)
A =Scale_xyz*[x(i,1);y(i,1);z(i,1); 1];
Lambda_s(i,:)= [A(1,1),A(2,1),A(3,1)];

end
worldPoints =Lambda_s;

[R,T,Xc,best_solution]=efficient_pnp(worldPoints,imagePoints,cameraParams.IntrinsicMatrix);
disp(R)
disp('')
disp(T)
``````

Python test code:

``````import cv2
import numpy as np

def lla2ecef(lat,lon,alt):
# %WGS84 ellipsoid constants:
a = 6378137
e = 8.1819190842622e-2

# intermediate calculation
# (prime vertical radius of curvature)
N = np.divide(a , np.sqrt(1- np.multiply(e**2, np.power(np.sin(lat), 2))))

# results:
x = np.multiply((N+alt) , np.multiply(np.cos(lat), np.cos(lon)))
y = np.multiply((N+alt), np.multiply(np.cos(lat), np.sin(lon)))
z = np.multiply(np.multiply((1-e**2), N) + alt, np.sin(lat))

return x, y, z

def efficient_PnP():
# camera parameters
fx = 1100.7622070312500
fy = 1102.0703125000000
cx = 494.43371582031250
cy = 311.53576660156250
s = 0

IntrinsicMatrix = np.asanyarray([(fx, 0, 0), (s, fy, 0), (cx, cy, 1)])

p = np.asanyarray(
[(402.53841229193335493619088083506, 1042.8969270166453497949987649918),
(739.0592563903950349413207732141, 737.46669248644434446759987622499),
(1064.4752130131680587510345503688, 436.04453911696316481538815423846),
(1445.3768396591788132354849949479, 768.95855925639011729799676686525),
(1853.2714949651433471444761380553, 373.06080557707161915459437295794),
(1497.8632842757549497036961838603, 20.651820294345468628969790859799)]
)

# scaling in order to prevent ill conditioning
mu = np.mean(p[:,0])
su = np.std(p[:,0], ddof=1)
mv = np.mean(p[:,1])
sv = np.std(p[:,1], ddof=1)

Scale_uv = np.asanyarray([(np.sqrt(2)/su, 0, -np.sqrt(2) * mu/su),
(0, np.sqrt(2)/sv, -np.sqrt(2)*mv/sv),
(0, 0 ,1)])

p_s = np.zeros((p.shape[0], p.shape[1]))
for i in range(len(p)):
A = np.matmul(Scale_uv, np.append(p[i,:].conj().T, 1))
p_s[i,:]= np.asanyarray([A[0], A[1]])

imagePoints = p_s

x, y, z = np.zeros(6), np.zeros(6), np.zeros(6)
# FROM AERIAL IMAGERY:  world points must be converted to ECEF
x[0],y[0],z[0] = lla2ecef(31.5944594*np.pi/180,34.6125853*np.pi/180,83.1465369)
x[1],y[1],z[1] = lla2ecef(31.5946889*np.pi/180,34.6125481*np.pi/180,82.574187)
x[2],y[2],z[2] = lla2ecef(31.5945089*np.pi/180,34.6123806*np.pi/180,83.1020015)
x[3],y[3],z[3] = lla2ecef(31.5945264*np.pi/180,34.6123929*np.pi/180,83.0722942)
x[4],y[4],z[4] = lla2ecef(31.5945082*np.pi/180,34.6121296*np.pi/180,82.6599009)
x[5],y[5],z[5] = lla2ecef(31.5944436*np.pi/180,34.611917*np.pi/180,82.4766086)

# scaling in order to prevent ill conditioning
mx = np.mean(x)
sx = np.std(x, ddof=1)
my = np.mean(y)
sy = np.std(y, ddof=1)
mz = np.mean(z)
sz = np.std(z, ddof=1)

Scale_xyz= np.asanyarray(
[(np.sqrt(3)/sx, 0, 0, -np.sqrt(3)*mx/sx),
(0, np.sqrt(3)/sy,0, -np.sqrt(3)*my/sy),
(0, 0, np.sqrt(3)/sz, -np.sqrt(3)*mz/sz),
(0,0,0, 1)]
)

Lambda_s = np.zeros((x.shape[0], 3))
for i in range(len(x)):
A = np.matmul(Scale_xyz, np.asanyarray([x[i], y[i], z[i], 1]))
Lambda_s[i,:] = np.asanyarray([A[0], A[1], A[2]])

worldPoints = Lambda_s

success, Rvec, Tvec = cv2.solvePnP(worldPoints, imagePoints, IntrinsicMatrix, None, flags=cv2.SOLVEPNP_EPNP)
# print(success)
Rt, _ = cv2.Rodrigues(Rvec)
R = Rt.transpose()

print(R)
print('')
print(Tvec)

efficient_PnP()

``````

Matlab output:

``````   0.576090   0.765969  -0.285326
0.479800  -0.034283   0.876708
0.661750  -0.641962  -0.387262

-2.4656e-03
2.1059e-03
1.5815e+00
``````

Python output:

``````[[ 0.96843659  0.01555782 -0.24877406]
[-0.08614055  0.957447   -0.27545426]
[ 0.23390251  0.28818952  0.92856686]]

[[ 1.29799416e-03]
[-1.60481850e-03]
[ 7.20212047e+03]]
``````