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 |
|
---|