sugr_cov_matr_X

% determine CovM for triangulated point (cov_matr_X)

 determine CovM from estimated directions and 3D point 
 following PCV (13.268)
 directions are assumed to follwo epipolar constraint

 X_est = sugr_cov_matr_X(X,sigma,est_u,est_w,rs)

 X           = 4x1 homgeneous vector, spherically normaized
 sigma        = standard deviation of directions, isotropic
 est_u,       = estimated directions in first camera system
 est_w        = est_v in first camera system, w=R'*v
 rs           = 2x1 vector of relative distances
 
 X_est        3D struct of point
 *     .h     = Xs
 *     .Crr   = CovM of reduced spherically homogeneous point coordinates
                
 Wolfgang Förstner 
 wfoerstn@uni-bonn.de

0001 %% determine CovM for triangulated point (cov_matr_X)
0002 %
0003 % determine CovM from estimated directions and 3D point
0004 % following PCV (13.268)
0005 % directions are assumed to follwo epipolar constraint
0006 %
0007 % X_est = sugr_cov_matr_X(X,sigma,est_u,est_w,rs)
0008 %
0009 % X           = 4x1 homgeneous vector, spherically normaized
0010 % sigma        = standard deviation of directions, isotropic
0011 % est_u,       = estimated directions in first camera system
0012 % est_w        = est_v in first camera system, w=R'*v
0013 % rs           = 2x1 vector of relative distances
0014 %
0015 % X_est        3D struct of point
0016 % *     .h     = Xs
0017 % *     .Crr   = CovM of reduced spherically homogeneous point coordinates
0018 %
0019 % Wolfgang Förstner
0020 % wfoerstn@uni-bonn.de
0021 %
0022 
0023 function X_est = sugr_cov_matr_X(X,sigma,est_u,est_w,rs)
0024 
0025 % determine basis
0026 nv = calc_S(est_u)*est_w;
0027 nv = nv/norm(nv);
0028 rv = calc_S(nv)*est_u;
0029 sv = calc_S(nv)*est_w;
0030 C =  [inv(rv*rv'/rs(1)^2+sv*sv'/rs(2)^2+ ...
0031        nv*nv'*(rs(1)^2+rs(2)^2)/(rs(1)^2*rs(2)^2)+eye(3)*10^(-12)...
0032        )*sigma^2 ...
0033        zeros(3,1); [0,0,0,0]];                                              %#ok<MINV>
0034 X_est = sugr_Point_3D(X,C);