[37] | 1 | function d = vgg_H_sampson_distance_sqr(H,X1,X2)
|
---|
| 2 |
|
---|
| 3 | % d = vgg_H_sampson_distance_sqr(H,X1,X2)
|
---|
| 4 | %
|
---|
| 5 | % Returns an approximation of the squared geometric distance from the
|
---|
| 6 | % 4d joint-space point [X1;X2] to the H manifold. See Torr CVIU 97.
|
---|
| 7 |
|
---|
| 8 | if (size(X1) ~= size(X2))
|
---|
| 9 | error('Point sets not same size!');
|
---|
| 10 | end
|
---|
| 11 |
|
---|
| 12 | p1 = X1 ./ repmat(X1(3,:),3,1);
|
---|
| 13 | p2 = X2 ./ repmat(X2(3,:),3,1);
|
---|
| 14 |
|
---|
| 15 | alg = vgg_H_algebraic_distance(H,p1,p2);
|
---|
| 16 |
|
---|
| 17 | N = size(X1,2);
|
---|
| 18 |
|
---|
| 19 | h = reshape(H',9,1);
|
---|
| 20 |
|
---|
| 21 | G1 = [ H(1,1) - p2(1,:) * H(3,1) ; ...
|
---|
| 22 | H(1,2) - p2(1,:) * H(3,2) ; ...
|
---|
| 23 | -p1(1,:) * H(3,1) - p1(2,:) * H(3,2) - H(3,3) ; ...
|
---|
| 24 | zeros(1,N) ];
|
---|
| 25 |
|
---|
| 26 | G2 = [ H(2,1) - p2(2,:) * H(3,1) ; ...
|
---|
| 27 | H(2,2) - p2(2,:) * H(3,2) ; ...
|
---|
| 28 | zeros(1,N) ; ...
|
---|
| 29 | -p1(1,:) * H(3,1) - p1(2,:) * H(3,2) - H(3,3) ];
|
---|
| 30 |
|
---|
| 31 | magG1 = sqrt(sum(G1 .* G1));
|
---|
| 32 | magG2 = sqrt(sum(G2 .* G2));
|
---|
| 33 | magG1G2 = sum(G1 .* G2);
|
---|
| 34 |
|
---|
| 35 | alpha = acos( magG1G2 ./ (magG1 .* magG2) );
|
---|
| 36 |
|
---|
| 37 | D1 = alg(1,:) ./ magG1;
|
---|
| 38 | D2 = alg(2,:) ./ magG2;
|
---|
| 39 |
|
---|
| 40 | d = (D1.*D1 + D2.*D2 - 2 * D1 .* D2 .* cos(alpha)) ./ sin(alpha);
|
---|
| 41 |
|
---|
| 42 |
|
---|