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
addpath('EPnp_matlab\EPnP')
%useful constants
a_wgs84 = 6378137.0;
f_wgs84 = 1/298.257223563;
e_wgs84 = sqrt(2*f_wgs84-f_wgs84^2);
w_ie=7.2921151467e-5; %rotation_rate rad/s
% 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;
%radialDistortion = [-0.3361 0.0921];
radialDistortion = [k2 k4];
cameraParams = ...
cameraParameters('IntrinsicMatrix',IntrinsicMatrix...
,'RadialDistortion',radialDistortion);
% 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]]
Thank you in advance.