[37] | 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 |
---|