/
Fundamental_Matrix.m
39 lines (35 loc) · 1.14 KB
/
Fundamental_Matrix.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
% A = [0 0 0; -0.09 0.15 -0.07; -1.29 -0.7 -0.01; -1.24 -0.8 -0.01; -0.69 -0.95 -0.06; -0.54 -0.85 -0.09];
% B = [559 181 666 240; 550 213 609 230; 1007 243 487 522; 1012 222 529 531; 920 136 686 489; 869 135 701 455];
% F = (((B' * B)^(-1)) * B' * A)';
% D1 = [539; 134; 644; 168];
% P1 = F * D1;
% D2 = [729; 421; 649; 623];
% P2 = F * D2;
% P3 = [-73.5; 14.5; 10.5];
% stage_XYZ = P3 - P2 + P1
% pix1 = [82; 477];
% pix2 = [428; 43];
% pix1 = [595; 354];
% pix2 = [546; 369];
% U = [0 -1 pix1(2); 1 0 -pix1(1); -pix1(2) pix1(1) 0];
% V = [0 -1 pix2(2); 1 0 -pix2(1); -pix2(2) pix2(1) 0];
% P1 = [312 0 960 0; 0 312 540 0; 0 0 1 0];
% P2 = [-258.569 -713.8 665.264 -253.627; 41.2195 -188.439 593.073 -156.752; -0.399789 -0.462581 0.79132 0.0125556];
% Img1 = U * P1;
% Img2 = V * P2;
% [U1, S1, V1] = svd(Img1);
% [U2, S2, V2] = svd(Img2);
% [R1, Rb1] = rref(Img1);
% [R2, Rb2] = rref(Img2);
% Img1_red = R1(Rb1, :);
% Img2_red = R2(Rb2, :);
% Img = [Img1_red; Img2_red];
% [U, S, V] = svd(Img);
% vect = V(:, 4)/V(4,4);
% X = vect(1);
% Y = vect(2);
% Z = vect(3);
pip_pix_l = [629; 286];
pip_pix_r = [515; 303];
inj_lef = [213; 453];
inj_rig = [300; 30];