octave_packages/vrml-1.0.13/best_dir.m

   1 ## Copyright (C) 2002 Etienne Grossmann <etienne@egdn.net>
   2 ##
   3 ## This program is free software; you can redistribute it and/or modify it under
   4 ## the terms of the GNU General Public License as published by the Free Software
   5 ## Foundation; either version 3 of the License, or (at your option) any later
   6 ## version.
   7 ##
   8 ## This program is distributed in the hope that it will be useful, but WITHOUT
   9 ## ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  10 ## FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
  11 ## details.
  12 ##
  13 ## You should have received a copy of the GNU General Public License along with
  14 ## this program; if not, see <http://www.gnu.org/licenses/>.
  15
  16 ##       [d,w,rx,cv,wx] = best_dir( x, [a , sx ] )
  17 ##
  18 ## Some points  x,  are observed and one assumes that they belong to
  19 ## parallel planes. There is an unknown direction  d  s.t. for each
  20 ## point  x(i,:), one has :
  21 ##
  22 ##         x(i,:)*d == w(j(i)) + noise
  23 ##
  24 ## where j is known(given by the matrix  a ), but  w  is unknown.
  25 ##
  26 ## Under the assumption that the error on  x  are i.i.d. gaussian,
  27 ## best_dir() returns the maximum likelihood estimate of  d  and  w.
  28 ##
  29 ## This function is slower when cv is returned.
  30 ##
  31 ## INPUT :
  32 ## -------
  33 ## x  : D x P    P points. Each one is the sum of a point that belongs
  34 ##               to a plane and a noise term.
  35 ##
  36 ## a  : P x W    0-1 matrix describing association of points (rows of
  37 ##               x) to planes :
  38 ##
  39 ##           a(p,i) == 1 iff point x(p,:) belongs to the i'th plane.
  40 ##
  41 ##                                                Default is ones(P,1)
  42 ##
  43 ## sx : P x 1    Covariance of x(i,:) is sx(i)*eye(D).
  44 ##                                                Default is ones(P,1)
  45 ## OUTPUT :
  46 ## --------
  47 ## d  : D x 1    All the planes have the same normal, d. d has unit
  48 ##               norm.
  49 ##
  50 ## w  : W x 1    The i'th plane is { y | y*d = w(i) }.
  51 ##
  52 ## rx : P x 1    Residuals of projection of points to corresponding plane.
  53 ##
  54 ##
  55 ##               Assuming that the covariance of  x  (i.e. sx) was known
  56 ##               only up to a scale factor, an estimate of the
  57 ##               covariance of  x  and  [w;d]  are
  58 ##
  59 ##                 sx * mean(rx.^2)/mean(sx)       and
  60 ##                 cv * mean(rx.^2)/mean(sx),  respectively.
  61 ##
  62 ## cv : (D+W)x(D+W)
  63 ##               Covariance of the estimator at [d,w] ( assuming that
  64 ##               diag(covariance(vec(x))) == sx ).
  65 ##
  66 ## wx : (D+W)x(D*P)
  67 ##               Derivatives of [w;d] wrt to x.
  68 ##
  69 ## Author  : Etienne Grossmann <etienne@egdn.net>
  70 ## Created : March 2000
  71 ##
  72 function [d,w,rx,cv,wx] = best_dir( x, a, sx )
  73
  74 [D,P] = size(x) ;
  75 ## Check dimension of args
  76 if nargin<2,
  77   a = ones(P,1) ;
  78 elseif size(a,1) != P,
  79   error ("best_dir : size(a,1)==%d != size(x,2)==%d\n",size(a,1),P);
  80   ## keyboard
  81 end
  82 if isempty (a)
  83   error ("best_dir : a is empty. This will not do!\n");
  84   ## keyboard
  85 end
  86
  87 W = size(a,2) ;
  88 if nargin<3,
  89   sx = ones(P,1) ;
  90 else
  91   if prod(size(sx)) != P,
  92     error ("best_dir : sx has %d elements, rather than P=%d\n",
  93            prod(size(sx)),P);
  94     ## keyboard
  95   end
  96   if !all(sx)>0,
  97     error ("best_dir : sx has non positive element\n");
  98     ## keyboard
  99   end
 100 end
 101 sx = sx(:);
 102
 103
 104 ## If not all points belong to a plane, clean  a.
 105
 106 keep = 0 ;
 107 if ! all(sum([a';a'])),         # trick for single-column  a
 108
 109   ## if verbose, printf ("best_dir : Cleaning up useless rows of 'a'\n"); end
 110   keep = find(sum([a';a'])) ;
 111   ## [d,w,cv] = best_dir(x(keep,:),a(keep,:),sx(keep)) ;
 112   ## return ;
 113   x = x(:,keep);
 114   a = a(keep,:);
 115   sx = sx(keep);
 116   P_orig = P ;
 117   P = prod(size(keep));
 118 end
 119
 120 ## If not all planes are used, remove some rows of a.
 121 if !all(sum(a)),
 122   keep = find(sum(a)) ;
 123   if nargout >= 4,
 124     [d,ww,rx,cv2] = best_dir(x,a(:,keep),sx) ;
 125     cv = zeros(W+D,W+D) ;
 126     cv([1:3,3+keep],[1:3,3+keep]) = cv2 ;
 127   else
 128     [d,ww,rx]    = best_dir(x,a(:,keep),sx) ;
 129   end
 130   w = zeros(W,1);
 131   w(keep) = ww ;
 132   return
 133 end
 134 ## Now,  a  has rank  W  for sure.
 135
 136 ## tmp = diag(1./sx) ;
 137 tmp = (1./sx)*ones(1,W) ;
 138 tmp2 = inv(a'*(tmp.*a))*(a.*tmp)' ; ;
 139 tmp = x*(eye(P) - tmp2'*a') ;
 140 ## tmp = tmp*diag(1./sx)*tmp' ;
 141 tmp = (tmp.*(ones(D,1)*(1./sx)'))*tmp' ;
 142
 143 [u,S,v] = svd(tmp) ;
 144 d = v(:,D) ;
 145 w = tmp2*x'*d ;
 146
 147 rx = (x'*d - a*w) ;
 148
 149 if nargout >= 4,
 150   wd = [w;d];
 151   ## shuffle = ( ones(D,1)*[0:P-1]+[1:P:P*D]'*ones(1,P) )(:) ;
 152   ## [cv,wx] = best_dir_cov(x',a,sx,wd) ;
 153   ## ## wx = wx(:,shuffle) ;
 154
 155   [cv,wx] = best_dir_cov(x,a,sx,wd) ;
 156
 157   ##  [cv2,wx2] = best_dir_cov2(x,a,sx,wd) ;
 158   ##    if any(abs(cv2(:)-cv(:))>eps),
 159   ##      printf("test cov : bug 1\n") ;
 160   ##      keyboard
 161   ##    end
 162   ##    if any(abs(wx2(:)-wx(:))>eps),
 163   ##      printf("test cov : bug 2\n") ;
 164   ##      keyboard
 165   ##    end
 166
 167 end
 168
 169
 170 if keep,
 171   tmp = zeros(P_orig,1) ;
 172   tmp(keep) = rx ;
 173   rx = tmp ;
 174   if nargout >= 5,
 175     k1 = zeros(1,P_orig) ; k1(keep) = 1 ; k1 = kron(ones(1,D),k1) ;
 176     tmp = zeros(D+W,P_orig*D) ;
 177     tmp(:,k1) = wx ;
 178     wx = tmp ;
 179   end
 180 end
 181
 182 endfunction
 183
 184