Commit 4c0c2647 authored by Marco Berzborn's avatar Marco Berzborn

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
parents a23421d7 1e6674b5
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
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
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
......@@ -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
......
#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);
}
......@@ -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);
......
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
......@@ -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
......
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
......@@ -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));
%%
......
......@@ -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);
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment