V_ROTQRMEAN calculates the mean rotation of a quaternion array [y,s]=[q] Inputs: q(4,n) normalized real quaternion array Outputs: y(4,1) normalized mean quaternion s(1,n) sign vector such that y=q*s', y=y/sqrt(y.'*y) v average squared deviation from the mean quaternion Since quaternions represent a rotation only to within a sign ambiguity we need to select +1 or -1 for each one when calculating the mean. This routine selects the sign for each quaternion to maximize the norm of their sum or, equivalently, to minimize their variance. Copyright (C) Mike Brookes 2011-2012 Version: $Id: v_rotqrmean.m 10865 2018-09-21 17:22:45Z dmb $ VOICEBOX is a MATLAB toolbox for speech processing. Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You can obtain a copy of the GNU General Public License from http://www.gnu.org/copyleft/gpl.html or by writing to Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 function [y,s,v]=v_rotqrmean(q) 0002 %V_ROTQRMEAN calculates the mean rotation of a quaternion array [y,s]=[q] 0003 % 0004 % Inputs: q(4,n) normalized real quaternion array 0005 % 0006 % Outputs: y(4,1) normalized mean quaternion 0007 % s(1,n) sign vector such that y=q*s', y=y/sqrt(y.'*y) 0008 % v average squared deviation from the mean quaternion 0009 % 0010 % Since quaternions represent a rotation only to within a sign ambiguity 0011 % we need to select +1 or -1 for each one when calculating the mean. 0012 % This routine selects the sign for each quaternion to maximize the norm 0013 % of their sum or, equivalently, to minimize their variance. 0014 % 0015 % Copyright (C) Mike Brookes 2011-2012 0016 % Version: $Id: v_rotqrmean.m 10865 2018-09-21 17:22:45Z dmb $ 0017 % 0018 % VOICEBOX is a MATLAB toolbox for speech processing. 0019 % Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html 0020 % 0021 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0022 % This program is free software; you can redistribute it and/or modify 0023 % it under the terms of the GNU General Public License as published by 0024 % the Free Software Foundation; either version 2 of the License, or 0025 % (at your option) any later version. 0026 % 0027 % This program is distributed in the hope that it will be useful, 0028 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0029 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0030 % GNU General Public License for more details. 0031 % 0032 % You can obtain a copy of the GNU General Public License from 0033 % http://www.gnu.org/copyleft/gpl.html or by writing to 0034 % Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA. 0035 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0036 mmax=10; % number of n-best hypotheses to keep 0037 nq=size(q,2); 0038 mkx=zeros(nq,mmax); % save signs: 0=+, 1 = - 0039 mprev=ones(nq,mmax); % save back pointers 0040 msum=zeros(4,2*mmax); 0041 msum(:,1)=q(:,1); % current values of sum 0042 ix=1:mmax; 0043 jx=mmax+1:2*mmax; 0044 r=ones(1,mmax); 0045 for i=2:nq 0046 msum(:,jx)=msum(:,ix)-q(:,i(r)); 0047 msum(:,ix)=msum(:,ix)+q(:,i(r)); 0048 [vx,kx]=sort(sum(msum.^2,1),2,'descend'); 0049 mkx(i,:)=kx(ix); % negative is > mmax 0050 msum(:,ix)=msum(:,kx(ix)); % save mmax sums having highest norms 0051 end 0052 y=msum(:,1); % unnormalized mean 0053 y=y/sqrt(y.'*y); 0054 if nargout>1 % do traceback 0055 s=zeros(1,nq); 0056 k=1; 0057 for i=nq:-1:2 0058 s(i)=(mkx(i,k)>mmax); 0059 k=mkx(i,k)-mmax*s(i); 0060 end 0061 s=1-2*s; 0062 v=sum(mean((q-y*s).^2,2)); 0063 end 0064 0065