0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025 function [X_est, est_sigma_0, est_u, est_v, f] = ...
0026 sugr_triangulate_two_spherical_cameras(...
0027 b, Rot, u, v, sigma, Tol, max_iter, k, print_option)
0028
0029
0030 E = calc_S(b)*Rot';
0031
0032
0033 sigma_c= sigma*sqrt(u'*(E*E')*u+v'*(E'*E)*v);
0034 crit_E = k*sigma_c;
0035
0036 cgl = u'*E*v;
0037 if print_option > 0
0038 cgl=cgl
0039 end
0040
0041 if abs(cgl) > crit_E
0042 X_est.h=zeros(4,1);
0043 X_est.Crr=zeros(3);
0044 X_est.type=3;
0045 est_sigma_0 = 0;
0046 f=2;
0047 est_u=u;
0048 est_v=v;
0049 return
0050 end
0051
0052
0053 f = 0;
0054 ua = u;
0055 va = v;
0056 est_sigma_0=0;
0057
0058
0059 for nu = 1:max_iter
0060 g = ua'*E*va;
0061 if print_option > 0
0062 nu=nu
0063 g=g
0064 end
0065
0066 if abs(g) < Tol*sigma
0067 break
0068 end
0069 J1 = null(ua');
0070 J2 = null(va');
0071 l = [J1'*u;J2'*v];
0072 B = [J1'*E*va;J2'*E'*ua];
0073 cg = -cgl;
0074 n = B'*B;
0075 Deltal = l + cg/n*B;
0076 ua = ua+J1*Deltal(1:2);
0077 va = va+J2*Deltal(3:4);
0078 ua = ua/norm(ua);
0079 va = va/norm(va);
0080 est_sigma_0 = abs(cgl)/sigma_c;
0081 end
0082
0083 est_u = ua;
0084 est_v = va;
0085 est_w = Rot'*est_v;
0086 if print_option > 0
0087 uvw= [est_u', est_v', est_w']
0088 end
0089 m = calc_S(calc_S(b)*est_u)*b;
0090 m = m /norm(m);
0091 D = det([b, m, calc_S(est_u)*est_w]);
0092 rs = m' * [est_w, est_u];
0093 X = [rs(1)* est_u; D];
0094
0095
0096
0097 X_est = sugr_cov_matr_X(X,sigma,est_u,est_w,rs);
0098
0099
0100
0101
0102
0103 sigma_D = 2*sigma;
0104 crit_D = k*sigma_D;
0105 if abs(D) < eps
0106 if print_option > 0
0107 disp('point at infinity')
0108 end
0109 return
0110 end
0111 d=rs/D;
0112
0113 if sign(d(1)) < 0 || sign(d(2)) < 0
0114 if abs(D) < crit_D
0115
0116 if print_option > 0
0117 disp('point at infinity')
0118 end
0119
0120 return
0121 else
0122
0123 f=1;
0124 X_est.h=zeros(4,1);
0125 X_est.Crr=zeros(3);
0126 X_est.type=3;
0127 if print_option > 0
0128 disp('backward point')
0129 end
0130
0131 return
0132 end
0133 else
0134 if print_option > 0
0135 disp('signs ok')
0136 end
0137 end
0138
0139
0140