1 | function [H,rms] = vgg_H_from_x_nonlin(H_initial,p1,p2)
|
---|
2 |
|
---|
3 | % [H,rms] = vgg_H_from_x_nonlin(H_initial,xs1,xs2)
|
---|
4 | %
|
---|
5 | % Compute H using non-linear method which minimizes Sampson's approx to
|
---|
6 | % geometric reprojection error (see Hartley & Zisserman Alg 3.3 page 98 in
|
---|
7 | % 1st edition, Alg 4.3 page 114 in 2nd edition).
|
---|
8 |
|
---|
9 | % An initial estimate of
|
---|
10 | % H is required, which would usually be obtained using
|
---|
11 | % vgg_H_from_x_linear. It is not necessary to precondition the
|
---|
12 | % supplied points.
|
---|
13 | %
|
---|
14 | % The format of the xs is
|
---|
15 | % [x1 x2 x3 ... xn ;
|
---|
16 | % y1 y2 y3 ... yn ;
|
---|
17 | % w1 w2 w3 ... wn]
|
---|
18 |
|
---|
19 | [r,c] = size(p1);
|
---|
20 |
|
---|
21 | if (size(p1) ~= size(p2))
|
---|
22 | error ('Input point sets are different sizes!')
|
---|
23 | end
|
---|
24 |
|
---|
25 | global gp1 gp2 C1 C2;
|
---|
26 |
|
---|
27 | gp1 = p1;
|
---|
28 | gp2 = p2;
|
---|
29 |
|
---|
30 | % Make conditioners
|
---|
31 | C1 = vgg_conditioner_from_pts(p1);
|
---|
32 | C2 = vgg_conditioner_from_pts(p2);
|
---|
33 |
|
---|
34 | H_cond = C2 * H_initial * inv(C1);
|
---|
35 |
|
---|
36 | opt = optimset( optimset('lsqnonlin') , 'LargeScale','off', 'Diagnostics','off', 'Display','off');
|
---|
37 |
|
---|
38 | H_cond = lsqnonlin(@lsq_func,H_cond,[],[],opt);
|
---|
39 |
|
---|
40 | H = inv(C2) * H_cond * C1;
|
---|
41 | rms = sqrt(vgg_H_sampson_distance_sqr(H,gp1,gp2));
|
---|
42 |
|
---|
43 | clear gp1 gp2 C1 C2;
|
---|
44 |
|
---|
45 |
|
---|
46 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
47 |
|
---|
48 | function r = lsq_func(H)
|
---|
49 | global gp1 gp2 C1 C2;
|
---|
50 |
|
---|
51 | H_decond = inv(C2) * H * C1;
|
---|
52 |
|
---|
53 | d = vgg_H_sampson_distance_sqr(H_decond,gp1,gp2);
|
---|
54 |
|
---|
55 | r = sqrt(d);
|
---|
56 |
|
---|