1 | % By Philip Torr 2002
|
---|
2 | % copyright Microsoft Corp.
|
---|
3 | %this function calculates the set of 3D points given matches and two projection matrices
|
---|
4 | %for this method to work best the matches must be corrected to lie on the epipolar lines
|
---|
5 |
|
---|
6 | %P-i ith row of the P matrix then
|
---|
7 | %constraints are of the form x p_3^t - p_1 & y p_3^t - p_2
|
---|
8 |
|
---|
9 | function X = torr_triangulate(matches, m3, P1, P2)
|
---|
10 |
|
---|
11 | %first establish the 4 x 4 matrix J such that J^X = 0
|
---|
12 | [nr no_matches] = size(matches);
|
---|
13 |
|
---|
14 | x1 = matches(:,1)/m3;
|
---|
15 | y1 = matches(:,2)/m3;
|
---|
16 | x2 = matches(:,3)/m3;
|
---|
17 | y2 = matches(:,4)/m3;
|
---|
18 |
|
---|
19 | for i = 1:nr
|
---|
20 |
|
---|
21 | J(1,1) = P1(3,1) * x1(i) - P1(1,1);
|
---|
22 | J(1,2) = P1(3,2) * x1(i) - P1(1,2);
|
---|
23 | J(1,3) = P1(3,3) * x1(i) - P1(1,3);
|
---|
24 | J(1,4) = P1(3,4) * x1(i) - P1(1,4);
|
---|
25 |
|
---|
26 | J(2,1) = P1(3,1) * y1(i) - P1(2,1);
|
---|
27 | J(2,2) = P1(3,2) * y1(i) - P1(2,2);
|
---|
28 | J(2,3) = P1(3,3) * y1(i) - P1(2,3);
|
---|
29 | J(2,4) = P1(3,4) * y1(i) - P1(2,4);
|
---|
30 |
|
---|
31 | J(3,1) = P2(3,1) * x2(i) - P2(1,1);
|
---|
32 | J(3,2) = P2(3,2) * x2(i) - P2(1,2);
|
---|
33 | J(3,3) = P2(3,3) * x2(i) - P2(1,3);
|
---|
34 | J(3,4) = P2(3,4) * x2(i) - P2(1,4);
|
---|
35 |
|
---|
36 | J(4,1) = P2(3,1) * y2(i) - P2(2,1);
|
---|
37 | J(4,2) = P2(3,2) * y2(i) - P2(2,2);
|
---|
38 | J(4,3) = P2(3,3) * y2(i) - P2(2,3);
|
---|
39 | J(4,4) = P2(3,4) * y2(i) - P2(2,4);
|
---|
40 |
|
---|
41 | X(:,i) = torr_ls(J);
|
---|
42 | end
|
---|
43 |
|
---|
44 | %at the moment this is unnormalized so that chierality can be determined |
---|