Merge branch 'beamformingRevision' into 'master'

Beamforming revision

Fertig mit Ueberarbeiten von beamforming Kram
- keine Mex files mehr (bringt wohl nicht so viel)
- saubere Implementierung
- Cross-Spectral Matrix (fehlte vorher ganz)

See merge request !1

applications/SignalProcessing/Beamforming/focuss.m

+function x = focuss(A,b,x,tol,maxIter,silentFlag)
+% solve linear system of equations using FOCUSS (sparse solution)
+
+% Author: MMT -- Email: markus.mueller-trapet@rwth-aachen.de
+% Created:  02-Nov-2016
+
+lastx = x.*(1 + 10*tol);
+nIter = 0;
+
+%% regularized FOCUSS
+p = 0;
+while norm(lastx-x)/norm(x) > tol && nIter < maxIter
+    nIter = nIter + 1;
+    lastx = x;
+    Wp = diag(abs(x).^(1-p/2));
+    AW = A*Wp;
+    AWinv = (AW'*AW + 1e-6.*norm(AW).*eye(size(A)))\AW';
+    x = Wp*(AWinv*b);
+    x = max(0,x);
+%     x(x < 1e-3.*max(x)) = 0; %limit dynamic range
+end
+
+%% check for convergence
+if ~silentFlag
+    if nIter == maxIter
+        disp(['FOCUSS: convergence not achieved after ' num2str(nIter) ' iterations']);
+        x(:) = 0;
+    else
+        disp(['FOCUSS: converged after ' num2str(nIter) ' iterations']);
+    end
+end
+
+end % function
\ No newline at end of file

applications/SignalProcessing/Beamforming/gauss_seidel.m

+function x = gauss_seidel(A,b,x,tol,maxIter,silentFlag)
+% solve linear system of equations using Gauss-Seidel method in matrix form
+
+% Author: MMT -- Email: markus.mueller-trapet@rwth-aachen.de
+% Created:  02-Nov-2016
+
+lastx = x.*(1 + 10*tol);
+nIter = 0;
+
+%% Gauss-Seidel (with LU decomposition)
+% U = triu(A,1);
+% L = tril(A,0);
+% T = -L\U;
+% C = L\b;
+
+N_scan = numel(b);
+
+while norm(lastx-x)/norm(x) > tol && nIter < maxIter
+    nIter = nIter + 1;
+    lastx = x;    
+    % from Brooks and Humphreys Paper
+    for n = 1:N_scan
+        x(n) = max(0,b(n) - (A(n,1:(n-1))*x(1:(n-1)) + A(n,(n+1):N_scan)*x((n+1):N_scan)));
+    end
+    for n = N_scan:-1:1
+        x(n) = max(0,b(n) - (A(n,1:(n-1))*x(1:(n-1)) + A(n,(n+1):N_scan)*x((n+1):N_scan)));
+    end
+    % LU method
+%     x = max(0,T*x + C);
+    x(x < 1e-5.*max(x)) = 0; %limit dynamic range
+end
+
+%% check for convergence
+if ~silentFlag
+    if nIter == maxIter
+        disp(['GaussSeidel: convergence not achieved after ' num2str(nIter) ' iterations']);
+        x(:) = 0;
+    else
+        disp(['GaussSeidel: converged after ' num2str(nIter) ' iterations']);
+    end
+end
+
+end % function
\ No newline at end of file

applications/SignalProcessing/Beamforming/get_distances.m

+function d = get_distances(x,y)
+% calculate euclidean distances quickly
+
+%% check input data
+sz_x = size(x);
+sz_y = size(y);
+
+if numel(sz_x) > 2 || numel(sz_y) > 2
+    error('only matrix or vector input allowed')
+end
+
+if sz_x(2) ~= 3
+    if sz_x(1) == 3 % transform
+        x = x.';
+    else
+        error('wrong dimensions for x input');
+    end
+end
+
+if sz_y(2) ~= 3
+    if sz_y(1) == 3 % transform
+        y = y.';
+    else
+        error('wrong dimensions for y input');
+    end
+end
+
+%% calculate norm(x-y)
+d = sqrt(bsxfun(@plus,sum(abs(x).^2,2),sum(abs(y).^2,2).') - 2.*x*y.');
+
+end % function
\ No newline at end of file

applications/SignalProcessing/Beamforming/ita_beam_beampattern.m

@@ -41,13 +41,12 @@ function varargout = ita_beam_beampattern(varargin)
 % Modified: 07-Jan-2014 ('plotRange', 'plotCoord' - tumbraegel)
 
 %% Initialization and Input Parsing
-sArgs = struct('pos1_array','itaMicArray','pos2_f','numeric','pos3_steering_th','numeric','pos4_steering_phi','numeric','plotPlane','none','plotType','mag','plotRange',[], 'plotCoord', 'polar', 'wavetype',ita_beam_evaluatePreferences('ManifoldType'),'lineStyle','-');
+sArgs = struct('pos1_array','itaMicArray','pos2_f','numeric','pos3_steering_th','numeric','pos4_steering_phi','numeric','plotPlane','none','plotType','mag','plotRange',[], 'plotCoord', 'polar', 'wavetype',ita_beam_evaluatePreferences('SteeringType'),'lineStyle','-');
 [array,f,steering_th,steering_phi,sArgs] = ita_parse_arguments(sArgs,varargin);
     
 %% do the calculation
 % positions of array microphones
-arrayPositions = array.cart.';
-weights = array.w(:);
+arrayPositions = array.cart;
 
 % make a matrix with spherical coordinates for the unit sphere with
 % given angular resolution
@@ -56,19 +55,20 @@ R = 1;
 scanGrid = ita_generateSampling_equiangular(resolution,resolution);
 theta = unique(scanGrid.theta);
 phi = unique(scanGrid.phi);
-scanPositions = R.*scanGrid.cart.';
+scanPositions = R.*scanGrid.cart;
 
 % create steering vector with given steering angles
-steer_vec = [sin((0+steering_th)*pi/180)*cos((0+steering_phi)*pi/180);...
-    sin((0+steering_th)*pi/180)*sin((0+steering_phi)*pi/180);...
+steer_vec = [sin((0+steering_th)*pi/180)*cos((0+steering_phi)*pi/180),...
+    sin((0+steering_th)*pi/180)*sin((0+steering_phi)*pi/180),...
     cos((0+steering_th)*pi/180)];
 
 k = 2*pi*f/double(ita_constants('c'));
 % create manifold vector for the spherical grid ...
-v = manifoldVector(k,arrayPositions,scanPositions,sArgs.wavetype);
+v = squeeze(ita_beam_steeringVector(k,arrayPositions,scanPositions,sArgs.wavetype));
 % ... and multiply with the manifold vector for the steering
 % direction to get the beampattern
-v = v'*(weights(:).*manifoldVector(k,arrayPositions,steer_vec,sArgs.wavetype));
+v_steer = ita_beam_steeringVector(k,arrayPositions,steer_vec,sArgs.wavetype).';
+v = v'*v_steer./sum(abs(v_steer).^2);
 
 B = reshape(v,numel(theta),numel(phi));
 B = B./max(abs(B(:))); % normalize to maximum

applications/SignalProcessing/Beamforming/ita_beam_manifoldVectorMex.cpp

-#include "mex.h"
-#include "math.h"
-#include "stdlib.h"
-#include "vector"
-using namespace std;
-
-#define NDIMS 2
-
-/*
- * ita_beam_manifoldVectorMex
- * (mex version of the manifold vector function)
- *
- * call: v = ita_beam_manifoldVectorMex(k,arrayPos,scanPos,d_scan,type);
- *
- * 
- */
-
-/* create the complex vector */
-// v = exp(1i*k.*d) split into real and imag
-// vr = cos(k.*d), vi = sin(k.*d);
-void manifold_vector(double *vr, double *vi, double k, double *array,
-                       double *scan, double *d_scan, int type,
-                       mwSize *dims) {
-    
-  mwSize i,j,count=0;
-  double d_tmp=0,d=0;
-  
-      // distance between scan point and microphone
-      for (i=0; i<*(dims+1); i++) {
-          for (j=0; j<*(dims); j++) {
-              // convert to linear index
-              count = j+i* *(dims);
-              switch (type) {
-                  case 1:
-                      d_tmp = (*(array+3*j)* *(scan+3*i))+
-                              (*(array+3*j+1)* *(scan+3*i+1))+
-                              (*(array+3*j+2)* *(scan+3*i+2));
-                      d = d_tmp/ *(d_scan+i);
-                      *(vr+count) = cos(k*d);
-                      *(vi+count) = sin(k*d);
-                      break;
-                  case 2:
-                      d_tmp = pow(pow(*(array+3*j)-*(scan+3*i), 2)+
-                              pow(*(array+3*j+1)-*(scan+3*i+1), 2)+
-                              pow(*(array+3*j+2)-*(scan+3*i+2), 2), 0.5);
-                      d = *(d_scan+i) - d_tmp;
-                      *(vr+count) = cos(k*d)* *(d_scan+i)/d_tmp;
-                      *(vi+count) = sin(k*d)* *(d_scan+i)/d_tmp;
-                      break;
-                  case 3:
-                      d_tmp = pow(pow(*(array+3*j)-*(scan+3*i), 2)+
-                              pow(*(array+3*j+1)-*(scan+3*i+1), 2)+
-                              pow(*(array+3*j+2)-*(scan+3*i+2), 2), 0.5);
-                      d = *(d_scan+i) - d_tmp;
-                      *(vr+count) = cos(k*d);
-                      *(vi+count) = sin(k*d);
-                      break;
-                  default:
-                      d_tmp = pow(pow(*(array+3*j)-*(scan+3*i), 2)+
-                              pow(*(array+3*j+1)-*(scan+3*i+1), 2)+
-                              pow(*(array+3*j+2)-*(scan+3*i+2), 2), 0.5);
-                      d = *(d_scan+i) - d_tmp;
-                      *(vr+count) = cos(k*d);
-                      *(vi+count) = sin(k*d);
-              }
-          }
-      }
-}
-
-/* the gateway function */
-void mexFunction( int nlhs, mxArray *plhs[],
-                  int nrhs, const mxArray *prhs[]) {
-  double *arrayPos,*scanPos,*vr, *vi,*d_scan;
-  double k;
-  int type;
-  mwSize nFreq,nArray,nScan;
-  mwSize dims[NDIMS] = {0,0};
-  
-  /*  check for proper number of arguments */
-  if(nrhs!=5) {
-      mexErrMsgTxt("Four inputs required.");
-  }
-  if(nlhs!=1) {
-      mexErrMsgTxt("One output required.");
-  }
-  
-  /* check to make sure the first input argument is a scalar */
-  if( !mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]) ||
-      mxGetN(prhs[0])*mxGetM(prhs[0])!=1 ) {
-    mexErrMsgTxt("Input 'k' must be a scalar.");
-  }
-  
-  /* check to make sure the last input argument is a scalar */
-  if( !mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) ||
-      mxGetN(prhs[4])*mxGetM(prhs[4])!=1 ) {
-    mexErrMsgTxt("Input 'type' must be a scalar.");
-  }
-  
-  if ((mxGetM(prhs[1])!=3) || ((mxGetM(prhs[2])!=3))) {
-      mexErrMsgTxt("Only three-dimensional data.");
-  }
-  
-   /* get the dimensions of the input matrices */
-  nArray = mxGetN(prhs[1]);
-  nScan  = mxGetN(prhs[2]);
-  
-  dims[0] = nArray;
-  dims[1] = nScan;
-  
-  if (nScan != mxGetN(prhs[3])) {
-      mexErrMsgTxt("Length of the scan mesh and the scan distance vector must match.");
-  }
-  
-  /* create a pointer to the inputs */
-  k        = mxGetScalar(prhs[0]);
-  arrayPos = mxGetPr(prhs[1]);
-  scanPos  = mxGetPr(prhs[2]);
-  d_scan   = mxGetPr(prhs[3]);
-  type     = mxGetScalar(prhs[4]);
-  
-  /* set the output pointer to the output matrix */
-  plhs[0] = mxCreateNumericArray(NDIMS,dims,mxDOUBLE_CLASS,mxCOMPLEX);
-  
-  /* create a C pointer to a copy of the output matrix */
-  vr = mxGetPr(plhs[0]);
-  vi = mxGetPi(plhs[0]);
-  
-  /* call the C subroutine */
-  manifold_vector(vr,vi,k,arrayPos,scanPos,d_scan,type,dims);
-}

applications/SignalProcessing/Beamforming/ita_beam_simulate.m

@@ -33,13 +33,13 @@ function varargout = ita_beam_simulate(varargin)
 thisFuncStr  = [upper(mfilename) ':'];     %#ok<NASGU> % Use to show warnings or infos in this functions
 
 %% Initialization and Input Parsing
-sArgs        = struct('pos1_array','itaMicArray','source',itaMicArray([0.2,0.2,-1],'cart'),'type',ita_beam_evaluatePreferences('Method'),'SNR',Inf,'sigmaArray',0,'soundspeed',double(ita_constants('c')),'wavetype',ita_beam_evaluatePreferences('ManifoldType'));
+sArgs        = struct('pos1_array','itaMicArray','source',itaMicArray([0.2,0.2,-1],'cart'),'type',ita_beam_evaluatePreferences('Method'),'SNR',Inf,'sigmaArray',0,'soundspeed',double(ita_constants('c')),'wavetype',ita_beam_evaluatePreferences('SteeringType'));
 [array,sArgs] = ita_parse_arguments(sArgs,varargin);
 
 %% Body
 d = abs(mean(array.z)-mean(sArgs.source.z)); % distance between array and sources
 width_max = max(4*max(abs([sArgs.source.x(:).',sArgs.source.y(:).'])),0.1*ceil(10*tan(20*pi/180)*2*d)); % maximum scan width for 20 degree opening angle
-N = min(2601,((ceil(ceil(width_max/0.01)/2)*2)+1)^2); % number of scanpoints
+N = min(625,((ceil(ceil(width_max/0.01)/2)*2)+1)^2); % number of scanpoints
 resolution = 1e-3*round(1e3*width_max/(sqrt(N)-1)); % resolution in x,y directions
 % create a mesh of scanpoints
 Scanmesh = ita_beam_makeArray('grid','Nx',sqrt(N),'Ny',sqrt(N),'dx',resolution,'dy',resolution);
@@ -63,12 +63,12 @@ if numel(sArgs.source.w) > 1
     amplitudes = amplitudes.*exp(1i*2*pi.*rand(numel(sArgs.source.w),1));
     for i=1:nFreqs % for each frequency
         % create pressure vector as superposition of all sources
-        freqData(i,:) = sum(bsxfun(@times,amplitudes,manifoldVector(k(i),array.cart.',sArgs.source.cart.',2).'));
+        freqData(i,:) = sum(bsxfun(@times,amplitudes,squeeze(ita_beam_steeringVector(k(i),array.cart,sArgs.source.cart,2)).'));
     end
 else
     for i=1:nFreqs % for each frequency
         % create pressure vector for the single source
-        freqData(i,:) = amplitudes.*manifoldVector(k(i),array.cart.',sArgs.source.cart.',2).';
+        freqData(i,:) = amplitudes.*squeeze(ita_beam_steeringVector(k(i),array.cart,sArgs.source.cart,2)).';
     end
 end
 
@@ -107,14 +107,6 @@ p.userData = {'nodeN',array.ID(:)};
 %% array imperfections
 array.cart = array.cart + sArgs.sigmaArray.*randn(size(array.cart));
 
-%% use precomputed manifold vectors for sArgs.type 99
-if sArgs.type == 99
-    sArgs.type = zeros(p.nBins,array.nPoints,Scanmesh.nPoints);
-    for i = 1:p.nBins
-        sArgs.type(i,:,:) = manifoldVector(k(i),array.cart.',Scanmesh.cart.',sArgs.wavetype);
-    end
-end
-
 %% do the beamforming
 result = ita_beam_beamforming(array,p,Scanmesh,'type',sArgs.type,'wavetype',sArgs.wavetype);
 

applications/SignalProcessing/Beamforming/ita_beam_steeringVector.m

+function v = ita_beam_steeringVector(k,arrayPositions,scanPositions,waveType)
+
+% <ITA-Toolbox>
+% This file is part of the application Beamforming for the ITA-Toolbox. All rights reserved. 
+% You can find the license for this m-file in the application folder. 
+% </ITA-Toolbox>
+
+if nargin < 4
+    waveType = 2;
+end
+
+%% distance vectors only computed once
+d0 = get_distances(scanPositions,mean(arrayPositions,1));
+
+%% calculate plane wave distance differently for phase term
+if waveType == 1
+    scanPositions = bsxfun(@minus,scanPositions,mean(arrayPositions,1));
+    arrayPositions = bsxfun(@minus,arrayPositions,mean(arrayPositions,1));
+    di = bsxfun(@rdivide,scanPositions*arrayPositions.',d0);
+else
+    di = get_distances(scanPositions,arrayPositions);
+end
+
+%% across frequency
+v = zeros(numel(k),size(arrayPositions,1),size(scanPositions,1));
+for iScan = 1:size(scanPositions,1)
+    % calculate manifold vector
+    v(:,:,iScan) = steering_vector_sub(k,di(iScan,:),d0(iScan),waveType);
+end
+v = bsxfun(@rdivide,v,sum(abs(v).^2,2));
+
+end
+
+%% subfunctions
+function v = steering_vector_sub(k,di,d0,type)
+% calculate manifold vector for all wave types
+v = exp(-1i*bsxfun(@times,k,di));
+switch type
+    case 1 % plane waves
+        v = 1./v;
+    case 2 % spherical waves
+        v = bsxfun(@rdivide,v,di);
+    case 3 % spherical waves, relative to array center, d0 term for level cancels out during normalization
+        v = bsxfun(@rdivide,v,bsxfun(@times,exp(-1i*bsxfun(@times,k,d0)),di));
+    case 4 % spherical waves relative to array center w/o 1/r
+        v = bsxfun(@rdivide,v,exp(-1i*bsxfun(@times,k,d0)));
+    otherwise
+        error([upper(mfilename) ':type of manifold vector not valid']);
+end
+end % function
\ No newline at end of file

applications/SignalProcessing/Beamforming/ita_preferences_beamforming.m

@@ -11,8 +11,8 @@ if nargin < 1
         ita_preferences_gui_tabs(eval(mfilename), {mfilename}, true);
     else
         res = { 'Beamforming_Settings','ita_preferences_beamforming','simple_button','App: Beamforming','',4;...
-            'beamforming_ManifoldType','Finite Distance Focus (spherical waves)','popup_char','Manifold Vector Type','Type for the manifold vector.[Infinite Distance Focus (plane waves)|Finite Distance Focus (spherical waves)]',0;...
-            'beamforming_Method','Delay-and-Sum','popup_char','Your favorite method','Method to calculate beamforming.[Delay-and-Sum|Delay-and-Sum w/o Autospectra|Cross-Spectral Imaging|Cross-Spectral Imaging w/o Autospectra|MVDR|Eigenanalysis|CLEAN-SC]',0;...
+            'beamforming_SteeringType','Finite Distance Focus (spherical waves)','popup_char','Steering Vector Type','Type for the steering vector.[Infinite Distance Focus (plane waves)|Finite Distance Focus (spherical waves)]',0;...
+            'beamforming_Method','Delay-and-Sum','popup_char','Your favorite method','Method to calculate beamforming.[Delay-and-Sum|MVDR|MUSIC|Subspace|Functional|CLEAN|CLEAN-SC|DAMAS|SparseDAMAS]',0;...
             };
     end
 else

applications/SignalProcessing/Beamforming/manifoldVector.m

-function v = manifoldVector(k,arrayPos,scanPos,type)
-
-% <ITA-Toolbox>
-% This file is part of the application Beamforming for the ITA-Toolbox. All rights reserved. 
-% You can find the license for this m-file in the application folder. 
-% </ITA-Toolbox>
-
-if nargin < 4
-    type = 2;
-end
-nMics = size(arrayPos,2);
-nScanPoints = size(scanPos,2);
-% get centroid of array and shift array and scanmesh accordingly
-r_0      = (round((10^4).*mean(arrayPos,2))./10^4);
-scanPos  = scanPos - r_0(:,ones(1,nScanPoints));
-arrayPos = arrayPos - r_0(:,ones(1,nMics));
-d_scan = sqrt(sum(abs(scanPos).^2));
-% % create manifold vector
-try
-    v = ita_beam_manifoldVectorMex(k,arrayPos,scanPos,d_scan,type);
-catch %#ok<CTCH>
-    switch type
-        case 1 % plane waves
-            d = ((arrayPos.'*scanPos))./d_scan(ones(nMics,1),:);
-            v = exp(1i*k.*d);
-        case 2 % spherical waves
-            d = zeros(nMics,nScanPoints);
-            for i=1:nMics
-                d(i,:) = d_scan-sqrt(sum(abs(repmat(arrayPos(:,i),1,nScanPoints) - scanPos).^2));
-            end
-            v = exp(1i*k.*d).*d_scan(ones(nMics,1),:)./(d_scan(ones(nMics,1),:)-d);
-        case 3 % spherical w/o 1/r
-            d = zeros(nMics,nScanPoints);
-            for i=1:nMics
-                d(i,:) = d_scan-sqrt(sum(abs(repmat(arrayPos(:,i),1,nScanPoints) - scanPos).^2));
-            end
-            v = exp(1i*k.*d);
-        otherwise
-            error([upper(mfilename) ':type of manifold vector not valid']);
-    end
-end
-end
\ No newline at end of file

applications/SignalProcessing/Beamforming/test_ita_beamforming.m

@@ -10,29 +10,22 @@ function test_ita_beamforming()
 %
 %
 
-%% test for the external mex file
-if exist(['ita_beam_manifoldVectorMex.' mexext],'file') ~= 3
-    comeFrom = pwd;
-    cd([fileparts(which(mfilename)) filesep]);
-    mex ita_beam_manifoldVectorMex.cpp;
-    cd(comeFrom);
-end
-
 %% test array function
-array = ita_beam_makeArray('spiral','N',36,'d',0.11);
+array = ita_beam_makeArray('spiral','N',20,'d',0.1);
 array = ita_beam_makeArray(array,'weightType','taylor');
 
 %% test the parameter computation
-f = ita_ANSI_center_frequencies([200,5000],1);
+f = ita_ANSI_center_frequencies([1000,5000],1);
 params = ita_beam_computeParameters(array,f);
 ita_plot_freq(params,'ylim',[-50 60]);
 
 %% test the actual beamforming
 plotFreq = 2000;
-source = itaMicArray([-1 0.5 -10; 2 0.5 -10],'cart');
+source = itaMicArray([-1 0.5 2; 2 0.5 2],'cart');
 source.w(:) = [1; 1]; % linear sound pressure
-[B1,p1] = ita_beam_simulate(array,'source',source,'type',3,'wavetype',1);
-B2 = ita_beam_beamforming(array,p1,B1.channelCoordinates,'type',2,'wavetype',1);
+[B1,p1] = ita_beam_simulate(array,'source',source,'type',1,'wavetype',4);
+[B1,CSM] = ita_beam_beamforming(array,p1,B1.channelCoordinates,'type',1,'wavetype',4);
+B2 = ita_beam_beamforming(array,p1,B1.channelCoordinates,'type',5,'wavetype',1,'CSM',CSM);
 figure;
 subplot(1,2,1);
 ita_plot_2D(B1,plotFreq,'newFigure',false);
@@ -51,14 +44,15 @@ figure;
 plot(B1.channelCoordinates.x(goalIndex),20.*log10(abs(tmp)));
 hold all
 plot(B2.channelCoordinates.x(goalIndex),20.*log10(abs(tmp2)));
+ylim([-100 0]);
 
 %% also for strange scanning grids
 f = (100:100:5000).';
-p = itaResult(5.*randn(numel(f),36),f,'freq');
+p = itaResult(5.*randn(numel(f),array.nPoints),f,'freq');
 p.userData = {'nodeN',array.ID};
 mesh = array;
 mesh.z = 5;
-B = ita_beam_beamforming(array,p,mesh,'type',2);
+B = ita_beam_beamforming(array,p,mesh,'type',1);
 ita_plot_freq(mean(B));
 
 %%

kernel/PlotRoutines/ita_plot_2D.m

@@ -135,7 +135,7 @@ switch sArgs.plotType
             tmp = 10*round(0.1*magMax)+10;
             sArgs.plotRange = [tmp-sArgs.plotRange tmp];
         end
-        plotData(plotData < sArgs.plotRange(1)) = -inf;
+        plotData(plotData < sArgs.plotRange(1)) = tmp-500;
         % colorbar limits
         lim1 = sArgs.plotRange(1);
         lim2 = sArgs.plotRange(2);