This homework is split into two weeks.
[N1, N2, N3] = p3p_distances( d12, d23, d31, c12, c23, c31 )  N1, N2, N3 = hw04a.p3p_distances( d12, d23, d31, c12, c23, c31 ) 
d12 , d23 , d31 .. distances between three points in the space (scalars) 

c12 , c23 , c31 .. appropriate cosines (scalars) 
Create the function p3p_distances
for computing distances of three spatial points from a center of calibrated camera. The function must return the distances η_i in N1
, N2
, N3
of the three points. Implement only the case 'A' of the computation. If there are more solutions, the returned variables are row vectors (matlab) or lists (python). If there is no solution by the case 'A', return empty vector/list ([]
]). For constructing the fourth order polynomial, there is the p3p_polynom
function in the tools repository, that can be used in your code.
The function p3p_distances
should return only such solutions, that are consistent with input arguments, i.e., that pass verification using cosine law (see below). Use (relative) threshold 1e4
.
e = p3p_dverify( N1, N2, N3, d12, d23, d31, c12, c23, c31 )  e = hw04a.p3p_dverify( N1, N2, N3, d12, d23, d31, c12, c23, c31 ) 
Create the function p3p_dverify
for verification of computed cameratopoint distances using the cosine law. Use this function in p3p_distances
. The function returns vector of three errors, one for each equation. Each computed error should be distance (not squared), relative to particular $d_{jk}$, i.e.
$e(1) = \frac{\sqrt{\eta_1^2 + \eta_2^2  2\eta_1\eta_2c_{12}}  d_{12}}{d_{12}}$
[R C] = p3p_RC( N, u, X, K )  R, C = hw03b.p3p_RC( N, u, X, K ) 
Create the function p3p_RC
for computing calibrated camera centre C
and orientation R
from three scenetoimage correspondences (X
,u
), using already computed distances N = [η_1, η_2, η_3]
.
The function takes one configuration of η_i and returns a single R
and C
. Note that R must be orthonormal with determinant equal to +1.
p3p_distances
using the following known values:
C=[1,2,3]^{T}
, f=1
, K = R = I
(3×3 identity). Project the 3D points X1=[0,0,0]^{T}
, X2=[1,0,0]^{T}
, X3=[0,1,0]^{T}
by the P
and compute the cosines c12
, c23
and c31
for the projected image points. Using the 3D points and the cosines, compute the camerapoints distances η and compare with correct known values (C  X_{i}).
X1=[1,0,0]^{T}
, X2=[0,2,0]^{T}
, X3=[0,0,3]^{T}
, c12 = 0.9037378393
, c23 = 0.8269612542
, c31 = 0.9090648231
. There should be two solutions, N1
is [4.984934779184205, 4.123105628753765]
, N2
is [5.172995343542000, 5.099019520925607]
, N3
is [2.147077467616821, 6.403124241485243]
.
K
from the A.E. Input data. Plot the distances in a single graph; use red for N1, blue for N2, green for N3. Since there can be more than one solution for a single triplet, horizontal axis shows just total numerical order of solutions. Export as 04_distances.pdf
.
Note: The matrix Q
obtained in HW02 is neither related to this task nor is used in any way.
P
where C = [1,2,3]^{T}
, f=1
, K = R = I
. Project the 3D points X1 = [0, 0, 0]^{T}
, X2 = [1, 0, 0]^{T}
, X3 = [0, 1, 0]^{T}
by the P
. Compute the distances η
and camera pose using your p3p_RC
for all solutions. Compare with correct known values of R
, C
.
K
from the A.E. Input data. (There can be more than one solutions for each triplet.)
R
, C
, compose camera matrix and compute the reprojection errors on all 109 points and find their maximum.
R
, C
, and point_sel
(indices [i1, i2, i3]
of the three points used for computing the optimal R
, C
) as 04_p3p.mat
.
daliborka_01
) and draw u
as blue dots, highlight the three points used for computing the best R
, C
by drawing them as yellow dots, and draw the displacements of reprojected points x
multiplied 100 times as red lines. Export as 04_RC_projections_errors.pdf.
04_RC_maxerr.pdf
. Plot the errors as points, not lines, in this case.
R
, C
on all 109 points as the function of point index and export as 04_RC_pointerr.pdf
.
R
, C
, draw the 3D scene points (blue), and draw centers (red) of all cameras you have tested. Export as 04_scene.pdf
.
save( '04_p3p.mat', 'R', 'C', 'point_sel' );  sio.savemat( '04_p3p.mat', { 'R':R, 'C':C, 'point_sel': point_sel } ) 
Note: Use your p3p_distances
and plot_csystem
from the previous HW.
Note: The matrix P
obtained in HW02 is neither related to this task nor is not used in any way.
Upload two archives containing the following files.
The first part (04a):
matlab  python 

p3p_distances.m  
p3p_dverify.m  
04_distances.pdf 

hw04a.m  hw04a.py containing the required functions 
any other files required by your solution 
The second part (04b):
matlab  python 

p3p_RC.m  
04_RC_projections_errors.pdf , 04_RC_maxerr.pdf , 04_RC_pointerr.pdf , 04_scene.pdf 

04_p3p.mat 

hw04b.m  hw04b.py containing p3p_RC function 
any other your files required by your solution 
The input entry point scripts hw04a
, hw04b
should make all required figures, output files and prints without manual intervention.
Note: The required files must be in the root directory of the archive.