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