sugr_get_Euclidean_Point_3D

% Get Euclidean coordinates of 3D point with CovM

 [e,Cee] = sugr_get_Euclidean_Point_3D(x)

 if finite point:
      e   = 3x1 Euclidean coordinates
      Cee = 3x3 CovM of e

 else zeros

 Wolfgang Förstner 1/2011
 wfoerstn@uni-bonn.de

 See also sugr_Point_3D

0001 %% Get Euclidean coordinates of 3D point with CovM
0002 %
0003 % [e,Cee] = sugr_get_Euclidean_Point_3D(x)
0004 %
0005 % if finite point:
0006 %      e   = 3x1 Euclidean coordinates
0007 %      Cee = 3x3 CovM of e
0008 %
0009 % else zeros
0010 %
0011 % Wolfgang Förstner 1/2011
0012 % wfoerstn@uni-bonn.de
0013 %
0014 % See also sugr_Point_3D
0015 
0016 function [e,Cee] = sugr_get_Euclidean_Point_3D(x)
0017 
0018 switch sugr_get_isfinite_Point_3D(x)
0019     case 0  % point at infinity
0020         e   = zeros(3,1);
0021         Cee = zeros(3);
0022     case 1  % finite point
0023         e   = x.h(1:3)/x.h(4);
0024         J   =  1/x.h(4) * [eye(3), -x.h(1:3)/x.h(4)] * null(x.h');
0025         Cee = J * squeeze(x.Crr)  * J';
0026 end