1 ## Copyright (C) 2002 Etienne Grossmann <etienne@egdn.net>
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
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
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/>.
16 ## [d,w,rx,cv,wx] = best_dir( x, [a , sx ] )
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 :
22 ## x(i,:)*d == w(j(i)) + noise
24 ## where j is known(given by the matrix a ), but w is unknown.
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.
29 ## This function is slower when cv is returned.
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.
36 ## a : P x W 0-1 matrix describing association of points (rows of
39 ## a(p,i) == 1 iff point x(p,:) belongs to the i'th plane.
41 ## Default is ones(P,1)
43 ## sx : P x 1 Covariance of x(i,:) is sx(i)*eye(D).
44 ## Default is ones(P,1)
47 ## d : D x 1 All the planes have the same normal, d. d has unit
50 ## w : W x 1 The i'th plane is { y | y*d = w(i) }.
52 ## rx : P x 1 Residuals of projection of points to corresponding plane.
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
59 ## sx * mean(rx.^2)/mean(sx) and
60 ## cv * mean(rx.^2)/mean(sx), respectively.
63 ## Covariance of the estimator at [d,w] ( assuming that
64 ## diag(covariance(vec(x))) == sx ).
67 ## Derivatives of [w;d] wrt to x.
69 ## Author : Etienne Grossmann <etienne@egdn.net>
70 ## Created : March 2000
72 function [d,w,rx,cv,wx] = best_dir( x, a, sx )
75 ## Check dimension of args
78 elseif size(a,1) != P,
79 error ("best_dir : size(a,1)==%d != size(x,2)==%d\n",size(a,1),P);
83 error ("best_dir : a is empty. This will not do!\n");
91 if prod(size(sx)) != P,
92 error ("best_dir : sx has %d elements, rather than P=%d\n",
97 error ("best_dir : sx has non positive element\n");
104 ## If not all points belong to a plane, clean a.
107 if ! all(sum([a';a'])), # trick for single-column a
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)) ;
117 P = prod(size(keep));
120 ## If not all planes are used, remove some rows of a.
122 keep = find(sum(a)) ;
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 ;
128 [d,ww,rx] = best_dir(x,a(:,keep),sx) ;
134 ## Now, a has rank W for sure.
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' ;
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) ;
155 [cv,wx] = best_dir_cov(x,a,sx,wd) ;
157 ## [cv2,wx2] = best_dir_cov2(x,a,sx,wd) ;
158 ## if any(abs(cv2(:)-cv(:))>eps),
159 ## printf("test cov : bug 1\n") ;
162 ## if any(abs(wx2(:)-wx(:))>eps),
163 ## printf("test cov : bug 2\n") ;
171 tmp = zeros(P_orig,1) ;
175 k1 = zeros(1,P_orig) ; k1(keep) = 1 ; k1 = kron(ones(1,D),k1) ;
176 tmp = zeros(D+W,P_orig*D) ;