1 | % By Philip Torr 2002
|
---|
2 | % copyright Microsoft Corp.
|
---|
3 | %
|
---|
4 | % %designed for the good of the world by Philip Torr
|
---|
5 | % copyright Philip Torr and Microsoft Corp 2002
|
---|
6 | % linear estimation of H
|
---|
7 |
|
---|
8 |
|
---|
9 | %this uses a costly but accurrate 1st order approximation to the optimal reprojection error
|
---|
10 | %as described in
|
---|
11 | %
|
---|
12 | %
|
---|
13 | % @article{Torr99c,
|
---|
14 | % author = "Torr, P. H. S. and Zisserman, A",
|
---|
15 | % title ="MLESAC: A New Robust Estimator with Application to Estimating Image Geometry ",
|
---|
16 | % journal = "CVIU",
|
---|
17 | % Volume = {78},
|
---|
18 | % number = 1,
|
---|
19 | % pages = {138-156},
|
---|
20 | % year = 2000}
|
---|
21 | %
|
---|
22 | % %MAPSAC is the Bayesian version of MLESAC, and it is easier to pronounce!
|
---|
23 | % it is described in:
|
---|
24 | %
|
---|
25 | % @article{Torr02d,
|
---|
26 | % author = "Torr, P. H. S.",
|
---|
27 | % title ="Bayesian Model Estimation and Selection for Epipolar Geometry and
|
---|
28 | % Generic Manifold Fitting",
|
---|
29 | % journal = "IJCV",
|
---|
30 | % Volume = {?},
|
---|
31 | % number = ?,
|
---|
32 | % pages = {?},
|
---|
33 | % url = "http://research.microsoft.com/~philtorr/",
|
---|
34 | % year = 2002}
|
---|
35 | %
|
---|
36 |
|
---|
37 | function e = torr_errh(h, x1,y1,x2,y2, no_matches,m3)
|
---|
38 | %disp('estimating error on h')
|
---|
39 | %this is transfer error from nx1 y1 to nx2 y2
|
---|
40 |
|
---|
41 |
|
---|
42 | A(:,1) = x1(:) .*m3;
|
---|
43 | A(:,2) = y1(:) .*m3;
|
---|
44 | A(:,3) = m3 *m3;
|
---|
45 |
|
---|
46 | A(:,4) = 0;
|
---|
47 | A(:,5) = 0;
|
---|
48 | A(:,6) = 0;
|
---|
49 |
|
---|
50 | A(:,7) = x1(:) .* x2(:);
|
---|
51 | A(:,8) = y1(:) .* x2(:);
|
---|
52 | A(:,9) = m3 .* x2(:);
|
---|
53 |
|
---|
54 |
|
---|
55 |
|
---|
56 | B(:,4) = x1(:) .*m3;
|
---|
57 | B(:,5) = y1(:) .*m3;
|
---|
58 | B(:,6) = m3 *m3;
|
---|
59 |
|
---|
60 | B(:,1) = 0;
|
---|
61 | B(:,2) = 0;
|
---|
62 | B(:,3) = 0;
|
---|
63 |
|
---|
64 | B(:,7) = x1(:) .* y2(:);
|
---|
65 | B(:,8) = y1(:) .* y2(:);
|
---|
66 | B(:,9) = m3 .* y2(:);
|
---|
67 |
|
---|
68 |
|
---|
69 | r1 = A * h;
|
---|
70 | r2 = B * h;
|
---|
71 |
|
---|
72 |
|
---|
73 | %dx
|
---|
74 | dA1 = h(1)* m3 + h(7) * x2(:);
|
---|
75 | %dy
|
---|
76 | dA2 = h(2) * m3 + h(8) * x2(:);
|
---|
77 | %dx2
|
---|
78 | dA3 = h(7) * x1(:) + h(8) * y1(:) + h(9) * m3;
|
---|
79 | %dy2
|
---|
80 | dA4 = 0;
|
---|
81 |
|
---|
82 |
|
---|
83 | %x1
|
---|
84 | dB1 = h(4)*m3 + h(7)*y2(:);
|
---|
85 | %y1
|
---|
86 | dB2 = h(5)*m3 + h(8)*y2(:);
|
---|
87 | %x2
|
---|
88 | dB3 = 0.0;
|
---|
89 | %y2
|
---|
90 | dB4 = h(7)*x1(:)+h(8)*y1(:)+h(9) * m3;
|
---|
91 |
|
---|
92 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
93 | %%%%%%%%%should compare this to one below %%%%%%%%%%
|
---|
94 | magG1 = sqrt(dA1.^2 + dA2.^2 + dA3.^2 + dA4.^2);
|
---|
95 | magG2 = sqrt(dB1.^2 + dB2.^2 + dB3.^2 + dB4.^2);
|
---|
96 | magG1G2 = dA1 .* dB1 + dA2 .* dB2 + dA3 .* dB3 + dA4 .* dB4;
|
---|
97 |
|
---|
98 | angle = acos(magG1G2 ./ (magG1 .* magG2));
|
---|
99 |
|
---|
100 | % r1
|
---|
101 | % magG1
|
---|
102 | D1 = r1 ./ magG1;
|
---|
103 | D2 = r2 ./ magG2;
|
---|
104 |
|
---|
105 | sina = sin(angle);
|
---|
106 | %remove divide by zero
|
---|
107 | sina(find(sina == 0)) = 0.00000001;
|
---|
108 |
|
---|
109 | r = (D1 .* D1 + D2 .* D2 - 2 * D1 .* D2 .* cos (angle)) ./ sina;
|
---|
110 | e = r;
|
---|
111 | % this is squared residual?
|
---|
112 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
113 | %%%%%%%%%should compare this to one below %%%%%%%%%%
|
---|
114 |
|
---|
115 |
|
---|
116 | % C = [dA; dB];
|
---|
117 | % G = C * C';
|
---|
118 | % H = G^-1; %Inverse Hessian to get covariance matrix
|
---|
119 | % rv = [ r1 r2];
|
---|
120 | % r = rv * H * rv';
|
---|
121 | % e(i) = sqrt(r);
|
---|
122 |
|
---|
123 |
|
---|
124 | %e(i) = sqrt((tx2(i) - x2(i))^2 + (ty2(i) - y2(i))^2);
|
---|
125 | %sse = norm(e);
|
---|
126 | %disp('finished estimating error on f')
|
---|
127 |
|
---|
128 |
|
---|
129 |
|
---|
130 |
|
---|
131 |
|
---|