波束形成（BF）从算法仿真到工程源码实现-第十一节-非线性波束形成算法工程化
非线性波束形成算法工程化
·
一、概述
本节我们对非线性波束形成算法进行工程化，运行在respeaker core v2平台上，算法实时率在0.046左右。更多资料和代码可以进入https://t.zsxq.com/qgmoN ，同时欢迎大家提出宝贵的建议，以共同探讨学习。
二、算法实现
2.1 代码结构
abf
├── CMakeLists.txt
├── bin
│ ├── CMakeLists.txt
│ └── test-api.c
├── build
│ ├── CMakeCache.txt
│ ├── CMakeFiles
│ │ ├── 3.22.1
│ │ │ ├── CMakeCCompiler.cmake
│ │ │ ├── CMakeCXXCompiler.cmake
│ │ │ ├── CMakeDetermineCompilerABI_C.bin
│ │ │ ├── CMakeDetermineCompilerABI_CXX.bin
│ │ │ ├── CMakeSystem.cmake
│ │ │ ├── CompilerIdC
│ │ │ │ ├── CMakeCCompilerId.c
│ │ │ │ ├── a.out
│ │ │ │ └── tmp
│ │ │ └── CompilerIdCXX
│ │ │ ├── CMakeCXXCompilerId.cpp
│ │ │ ├── a.out
│ │ │ └── tmp
│ │ ├── CMakeDirectoryInformation.cmake
│ │ ├── CMakeOutput.log
│ │ ├── CMakeTmp
│ │ ├── Makefile.cmake
│ │ ├── Makefile2
│ │ ├── TargetDirectories.txt
│ │ ├── cmake.check_cache
│ │ └── progress.marks
│ ├── Makefile
│ ├── bin
│ │ ├── CMakeFiles
│ │ │ ├── CMakeDirectoryInformation.cmake
│ │ │ ├── progress.marks
│ │ │ └── test-api.dir
│ │ │ ├── DependInfo.cmake
│ │ │ ├── build.make
│ │ │ ├── cmake_clean.cmake
│ │ │ ├── compiler_depend.make
│ │ │ ├── compiler_depend.ts
│ │ │ ├── depend.make
│ │ │ ├── flags.make
│ │ │ ├── link.txt
│ │ │ ├── progress.make
│ │ │ ├── test-api.c.o
│ │ │ └── test-api.c.o.d
│ │ ├── Makefile
│ │ ├── cmake_install.cmake
│ │ └── test-api
│ ├── cmake_install.cmake
│ └── src
│ ├── CMakeFiles
│ │ ├── CMakeDirectoryInformation.cmake
│ │ ├── mouse-abf.dir
│ │ │ ├── DependInfo.cmake
│ │ │ ├── bessel0.c.o
│ │ │ ├── bessel0.c.o.d
│ │ │ ├── bf-api.c.o
│ │ │ ├── bf-api.c.o.d
│ │ │ ├── bf-blocker.c.o
│ │ │ ├── bf-blocker.c.o.d
│ │ │ ├── bf-complex.c.o
│ │ │ ├── bf-complex.c.o.d
│ │ │ ├── bf-fft.c.o
│ │ │ ├── bf-fft.c.o.d
│ │ │ ├── bf-matrix.c.o
│ │ │ ├── bf-matrix.c.o.d
│ │ │ ├── bf-nonlinear.c.o
│ │ │ ├── bf-nonlinear.c.o.d
│ │ │ ├── build.make
│ │ │ ├── cmake_clean.cmake
│ │ │ ├── cmake_clean_target.cmake
│ │ │ ├── compiler_depend.make
│ │ │ ├── compiler_depend.ts
│ │ │ ├── depend.make
│ │ │ ├── fft4g.c.o
│ │ │ ├── fft4g.c.o.d
│ │ │ ├── flags.make
│ │ │ ├── link.txt
│ │ │ ├── progress.make
│ │ │ ├── ring_buffer.c.o
│ │ │ └── ring_buffer.c.o.d
│ │ └── progress.marks
│ ├── Makefile
│ ├── cmake_install.cmake
│ └── libmouse-abf.a
├── src
│ ├── CMakeLists.txt
│ ├── bessel0.c
│ ├── bessel0.h
│ ├── bf-api.c
│ ├── bf-api.h
│ ├── bf-blocker.c
│ ├── bf-blocker.h
│ ├── bf-complex.c
│ ├── bf-complex.h
│ ├── bf-fft.c
│ ├── bf-fft.h
│ ├── bf-matrix.c
│ ├── bf-matrix.h
│ ├── bf-nonlinear.c
│ ├── bf-nonlinear.h
│ ├── bf-types.h
│ ├── fft4g.c
│ ├── fft4g.h
│ ├── ring_buffer.c
│ └── ring_buffer.h
└── test
├── simulate_role1_0_t60_0.2_role2_180_t60_0.2.pcm
├── simulate_role1_0_t60_0.2_role2_180_t60_0.2.wav
├── simulate_role1_0_t60_0.2_role2_180_t60_0.2_nonlinear_bf_0du.pcm
└── simulate_role1_0_t60_0.2_role2_180_t60_0.2_nonlinear_bf_180du.pcm
2.2 核心代码实现
（1）、src/bf-api.h
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-api.h
* */
#ifndef __BF_API_H__
#define __BF_API_H__
#ifdef __cplusplus
extern "C"
{
#endif
void *api_create_bf_handle(const char *mic_positions, int mic_num, float angle);
void api_destroy_bf_handle(void *handle);
int api_start_bf(void *handle);
int api_process_bf(void *handle, float *input_data, float *output_data, int chunk_size);
int api_end_bf(void *handle);
#ifdef __cplusplus
}
#endif
#endif
（2）、src/bf-api.c
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-api.c
* */
#include "bf-api.h"
#include "bf-blocker.h"
#include "bf-fft.h"
#include "bf-types.h"
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <assert.h>
static int MouseBF_GetPoint(const char *in, MouseBF_point *array_geometry) {
char input[128];
strcpy(input, in);
int mic_num = 0;
int mic_idx = 0;
int tt = 0;
float value = 0;
char *p = strtok(input, " ");
if (p) {
//printf("%s\n", p);
value = atof(p);
mic_num = tt / 3;
mic_idx = tt % 3;
array_geometry[mic_num].c[mic_idx] = value;
tt++;
}
while (p = strtok(NULL, " ")) {
//printf("%s\n", p);
value = atof(p);
mic_num = tt / 3;
mic_idx = tt % 3;
array_geometry[mic_num].c[mic_idx] = value;
tt++;
}
//printf("mic_num:%d\n", mic_num + 1);
return mic_num + 1;
}
static void MouseBF_KaiserBesselDerived(float alpha, size_t length, float* window) {
const size_t half = (length + 1) / 2;
float sum = 0.0f;
for (size_t i = 0; i <= half; ++i) {
MouseBF_complex_f r, t;
r.real_ = (4.0f * i) / length - 1.0f;
r.imag_ = 0.0;
t.real_ = M_PI * alpha * sqrt(1.0f - r.real_ * r.real_);
t.imag_ = 0.0;
sum += MouseBF_I0(t).real_;
window[i] = sum;
}
for (size_t i = length - 1; i >= half; --i) {
window[length - i - 1] = sqrtf(window[length - i - 1] / sum);
window[i] = window[length - i - 1];
}
if (length % 2 == 1) {
window[half - 1] = sqrtf(window[half - 1] / sum);
}
}
static float MouseBF_FloatS16ToFloat(float v) {
static const float kMaxInt16Inverse = 1.0 / 32767;
static const float kMinInt16Inverse = 1.0 / -32768;
return v * (v > 0 ? kMaxInt16Inverse : -kMinInt16Inverse);
}
static float MouseBF_FloatToFloatS16(float v) {
return v * (v > 0 ? 32767.0 : 32768.0);
}
typedef struct MouseBF_BeamformerHandle_ {
MouseBF_blocker *blocker_;
float **in_buf_;
float *out_buf_;
int mic_num_;
MouseBF_nonlinear *bf_inst_;
} MouseBF_BeamformerHandle;
void *api_create_bf_handle(const char *mic_positions, int mic_num, float angle) {
MouseBF_point array_geometry[20];
int mic_num_ = MouseBF_GetPoint(mic_positions, array_geometry);
assert(mic_num_ == mic_num);
MouseBF_spherical_pointf target_direction;
target_direction.s[0] = angle * M_PI / 180.0;
target_direction.s[1] = 0;
target_direction.s[2] = 1;
MouseBF_nonlinear *bf_inst = MouseBF_nonlinear_instance(kSampleRate,
array_geometry,
mic_num,
target_direction);
float *window = (float*)malloc(sizeof(float)*256);
MouseBF_KaiserBesselDerived(1.5f, 256, window);
MouseBF_blocker *blocker = MouseBF_blocker_instance(160,
256,
mic_num,
1,
window,
128,
MouseBF_nonlinear_process_block,
bf_inst);
free(window);
MouseBF_BeamformerHandle *handle = (MouseBF_BeamformerHandle*)malloc(sizeof(MouseBF_BeamformerHandle));
handle->blocker_ = blocker;
handle->bf_inst_ = bf_inst;
handle->in_buf_ = (float**)malloc(sizeof(float*) * mic_num);
handle->mic_num_ = mic_num;
for (int i = 0; i < mic_num; i++) {
handle->in_buf_[i] = (float*)malloc(sizeof(float) * 160);
}
handle->out_buf_ = (float*)malloc(sizeof(float) * 160);
return (void*)handle;
}
void api_destroy_bf_handle(void *handle) {
MouseBF_BeamformerHandle *handle_ = (MouseBF_BeamformerHandle *)handle;
MouseBF_nonlinear_destroy(handle_->bf_inst_);
MouseBF_blocker_destroy(handle_->blocker_);
for (int i = 0; i < handle_->mic_num_; i++) {
free(handle_->in_buf_[i]);
}
free(handle_->in_buf_);
free(handle_->out_buf_);
free(handle_);
}
int api_start_bf(void *handle) {
return 0;
}
int api_process_bf(void *handle, float *input_data, float *output_data, int chunk_size) {
MouseBF_BeamformerHandle *handle_ = (MouseBF_BeamformerHandle *)handle;
for (int j = 0; j < chunk_size; j++) {
for (int i = 0; i < handle_->mic_num_; i++) {
handle_->in_buf_[i][j] = MouseBF_FloatS16ToFloat(input_data[j*handle_->mic_num_ + i]);
}
}
MouseBF_blocker_process_chunk(handle_->blocker_,
handle_->in_buf_,
chunk_size,
handle_->mic_num_,
1,
&(handle_->out_buf_));
for (int i = 0; i < chunk_size; i++) {
output_data[i] = MouseBF_FloatToFloatS16(handle_->out_buf_[i]);
}
return 0;
}
int api_end_bf(void *handle) {
//MouseBF_BeamformerHandle *handle_ = (MouseBF_BeamformerHandle *)handle;
return 0;
}
（3）、src/bf-blocker.h
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-blocker.h
* */
#ifndef __BF_BLOCKER_H__
#define __BF_BLOCKER_H__
#include <stddef.h>
#include "ring_buffer.h"
#include "bf-fft.h"
#include "bf-nonlinear.h"
typedef struct MouseBF_blocker_ {
size_t chunk_size_;
size_t block_size_;
int num_input_channels_;
int num_output_channels_;
// The number of frames of delay to add at the beginning of the first chunk.
size_t initial_delay_;
size_t frame_offset_;
// Both contain |initial delay| + |chunk_size| frames. The input is a fairly
// standard FIFO, but due to the overlap-add it's harder to use an
// AudioRingBuffer for the output.
RingBuffer **input_buffer_;
float **output_buffer_;
// Space for the input block (can't wrap because of windowing).
float **input_block_;
// Space for the output block (can't wrap because of overlap/add).
float **output_block_;
float *window_;
// The amount of frames between the start of contiguous blocks. For example,
// |shift_amount_| = |block_size_| / 2 for a Hann window.
size_t shift_amount_;
MouseBF_fft *fft_inst_;
MouseBF_complex_f **fft_input_block_;
MouseBF_complex_f **fft_output_block_;
//bf
MouseBF_nonlinear *bf_inst_;
//callback
MouseBF_Callback callback_;
} MouseBF_blocker;
// create a blocker instance.
MouseBF_blocker *MouseBF_blocker_instance(size_t chunk_size,
size_t block_size,
int num_input_channels,
int num_output_channels,
const float* window,
size_t shift_amount,
MouseBF_Callback callback,
MouseBF_nonlinear *bf_inst
);
// destroy a blocker.
void MouseBF_blocker_destroy(MouseBF_blocker *inst);
//process a chunk data.
int MouseBF_blocker_process_chunk(MouseBF_blocker *inst,
float** input,
size_t chunk_size,
int num_input_channels,
int num_output_channels,
float** output);
#endif
（4）、src/bf-blocker.c
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-blocker.c
* */
#include "bf-blocker.h"
#include "bf-nonlinear.h"
#include <string.h>
#include <stdlib.h>
#include <assert.h>
// Adds |a| and |b| frame by frame into |result| (basically matrix addition).
static void MouseBF_AddFrames(float** a,
size_t a_start_index,
float** b,
int b_start_index,
size_t num_frames,
int num_channels,
float** result,
size_t result_start_index) {
for (int i = 0; i < num_channels; ++i) {
for (size_t j = 0; j < num_frames; ++j) {
result[i][j + result_start_index] =
a[i][j + a_start_index] + b[i][j + b_start_index];
}
}
}
// Copies |src| into |dst| channel by channel.
static void MouseBF_CopyFrames(float** src,
size_t src_start_index,
size_t num_frames,
int num_channels,
float** dst,
size_t dst_start_index) {
for (int i = 0; i < num_channels; ++i) {
memcpy(&dst[i][dst_start_index],
&src[i][src_start_index],
num_frames * sizeof(dst[i][dst_start_index]));
}
}
// Moves |src| into |dst| channel by channel.
static void MouseBF_MoveFrames(float** src,
size_t src_start_index,
size_t num_frames,
int num_channels,
float** dst,
size_t dst_start_index) {
for (int i = 0; i < num_channels; ++i) {
memmove(&dst[i][dst_start_index],
&src[i][src_start_index],
num_frames * sizeof(dst[i][dst_start_index]));
}
}
static void MouseBF_ZeroOut(float** buffer,
size_t starting_idx,
size_t num_frames,
int num_channels) {
for (int i = 0; i < num_channels; ++i) {
memset(&buffer[i][starting_idx], 0,
num_frames * sizeof(buffer[i][starting_idx]));
}
}
// Pointwise multiplies each channel of |frames| with |window|. Results are
// stored in |frames|.
static void MouseBF_ApplyWindow(const float* window,
size_t num_frames,
int num_channels,
float* const* frames) {
for (int i = 0; i < num_channels; ++i) {
for (size_t j = 0; j < num_frames; ++j) {
frames[i][j] = frames[i][j] * window[j];
}
}
}
static size_t MouseBF_gcd(size_t a, size_t b) {
size_t tmp;
while (b) {
tmp = a;
a = b;
b = tmp % b;
}
return a;
}
MouseBF_blocker *MouseBF_blocker_instance(size_t chunk_size,
size_t block_size,
int num_input_channels,
int num_output_channels,
const float* window,
size_t shift_amount,
MouseBF_Callback callback,
MouseBF_nonlinear *bf_inst
) {
MouseBF_blocker *inst = (MouseBF_blocker*)malloc(sizeof(MouseBF_blocker));
inst->chunk_size_ = chunk_size;
inst->block_size_ = block_size;
inst->num_input_channels_ = num_input_channels;
inst->num_output_channels_ = num_output_channels;
inst->initial_delay_ = block_size - MouseBF_gcd(chunk_size, shift_amount);
inst->frame_offset_ = 0;
inst->input_buffer_ = (RingBuffer**)malloc(sizeof(RingBuffer*) * num_input_channels);
for (int i = 0; i < num_input_channels; i++) {
inst->input_buffer_[i] = WebRtc_CreateBuffer(inst->chunk_size_ + inst->initial_delay_, sizeof(float));
}
inst->output_buffer_ = (float**)malloc(sizeof(float*) * num_output_channels);
for (int i = 0; i < num_output_channels; i++) {
inst->output_buffer_[i] = (float*)malloc(sizeof(float) * (inst->chunk_size_ + inst->initial_delay_));
memset(inst->output_buffer_[i], 0, sizeof(float) * (inst->chunk_size_ + inst->initial_delay_));
}
inst->input_block_ = (float**)malloc(sizeof(float*) * num_input_channels);
for (int i = 0; i < num_input_channels; i++) {
inst->input_block_[i] = (float*)malloc(sizeof(float) * block_size);
}
inst->output_block_ = (float**)malloc(sizeof(float*) * num_output_channels);
for (int i = 0; i < num_output_channels; i++) {
inst->output_block_[i] = (float*)malloc(sizeof(float) * block_size);
}
inst->window_ = (float*)malloc(sizeof(float) * block_size);
inst->shift_amount_ = shift_amount;
memcpy(inst->window_, window, block_size * sizeof(float));
for (int i = 0; i < num_input_channels; i++) {
int moved = WebRtc_MoveReadPtr(inst->input_buffer_[i], -inst->initial_delay_);
assert(moved == -inst->initial_delay_);
}
inst->fft_inst_ = MouseBF_fft_instance(inst->block_size_);
inst->fft_input_block_ = (MouseBF_complex_f**)malloc(sizeof(MouseBF_complex_f*) * num_input_channels);
for (int i = 0; i < num_input_channels; i++) {
inst->fft_input_block_[i] = (MouseBF_complex_f*)malloc(sizeof(MouseBF_complex_f) * inst->fft_inst_->complex_length_);
}
inst->fft_output_block_ = (MouseBF_complex_f**)malloc(sizeof(MouseBF_complex_f*) * num_output_channels);
for (int i = 0; i < num_output_channels; i++) {
inst->fft_output_block_[i] = (MouseBF_complex_f*)malloc(sizeof(MouseBF_complex_f) * inst->fft_inst_->complex_length_);
}
inst->callback_ = callback;
inst->bf_inst_ = bf_inst;
return inst;
}
// destroy a blocker.
void MouseBF_blocker_destroy(MouseBF_blocker *inst) {
for (int i = 0; i < inst->num_input_channels_; i++) {
WebRtc_FreeBuffer(inst->input_buffer_[i]);
}
free(inst->input_buffer_);
for (int i = 0; i < inst->num_output_channels_; i++) {
free(inst->output_buffer_[i]);
}
free(inst->output_buffer_);
for (int i = 0; i < inst->num_input_channels_; i++) {
free(inst->input_block_[i]);
}
free(inst->input_block_);
for (int i = 0; i < inst->num_output_channels_; i++) {
free(inst->output_block_[i]);
}
free(inst->output_block_);
free(inst->window_);
MouseBF_fft_destroy(inst->fft_inst_);
for (int i = 0; i < inst->num_input_channels_; i++) {
free(inst->fft_input_block_[i]);
}
free(inst->fft_input_block_);
for (int i = 0; i < inst->num_output_channels_; i++) {
free(inst->fft_output_block_[i]);
}
free(inst->fft_output_block_);
free(inst);
}
//process a chunk data.
int MouseBF_blocker_process_chunk(MouseBF_blocker *inst,
float** input,
size_t chunk_size,
int num_input_channels,
int num_output_channels,
float** output) {
for (size_t i = 0; i < num_input_channels; ++i) {
WebRtc_WriteBuffer(inst->input_buffer_[i], input[i], chunk_size);
}
size_t first_frame_in_block = inst->frame_offset_;
// Loop through blocks.
while (first_frame_in_block < inst->chunk_size_) {
for (int i = 0; i < inst->num_input_channels_; i++) {
WebRtc_ReadBuffer(inst->input_buffer_[i], NULL, inst->input_block_[i], inst->block_size_);
WebRtc_MoveReadPtr(inst->input_buffer_[i], -(inst->block_size_ - inst->shift_amount_));
}
MouseBF_ApplyWindow(inst->window_, inst->block_size_, inst->num_input_channels_, inst->input_block_);
for (int i = 0; i < inst->num_input_channels_; i++) {
MouseBF_fft_forward(inst->fft_inst_, inst->input_block_[i], inst->fft_input_block_[i]);
}
inst->callback_(inst->bf_inst_, inst->fft_input_block_,
inst->num_input_channels_, kNumFreqBins,
inst->num_output_channels_,
inst->fft_output_block_);
for (int i = 0; i < inst->num_output_channels_; i++) {
MouseBF_fft_invert(inst->fft_inst_, inst->fft_output_block_[i], inst->output_block_[i]);
}
MouseBF_ApplyWindow(inst->window_, inst->block_size_, inst->num_output_channels_, inst->output_block_);
MouseBF_AddFrames(inst->output_buffer_, first_frame_in_block, inst->output_block_, 0, inst->block_size_,
inst->num_output_channels_, inst->output_buffer_, first_frame_in_block);
first_frame_in_block += inst->shift_amount_;
}
// Copy output buffer to output
MouseBF_CopyFrames(inst->output_buffer_, 0, inst->chunk_size_, inst->num_output_channels_, output, 0);
// Copy output buffer [chunk_size_, chunk_size_ + initial_delay]
// to output buffer [0, initial_delay], zero the rest.
MouseBF_MoveFrames(inst->output_buffer_, chunk_size, inst->initial_delay_, inst->num_output_channels_,
inst->output_buffer_, 0);
MouseBF_ZeroOut(inst->output_buffer_, inst->initial_delay_, inst->chunk_size_, inst->num_output_channels_);
// Calculate new starting frames.
inst->frame_offset_ = first_frame_in_block - inst->chunk_size_;
return 0;
}
（5）、src/bf-complex.h
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-complex.h
* */
#ifndef __BF_COMPLEX_H__
#define __BF_COMPLEX_H__
typedef struct MouseBF_complex_f_ {
float real_;
float imag_;
} MouseBF_complex_f;
MouseBF_complex_f MouseBF_complex_mul(MouseBF_complex_f a, MouseBF_complex_f b);
MouseBF_complex_f MouseBF_complex_add(MouseBF_complex_f a, MouseBF_complex_f b);
MouseBF_complex_f MouseBF_complex_conj(MouseBF_complex_f a);
#endif
（6）、src/bf-complex.c
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-complex.c
* */
#include "bf-complex.h"
MouseBF_complex_f MouseBF_complex_mul(MouseBF_complex_f a, MouseBF_complex_f b) {
MouseBF_complex_f c;
c.real_ = a.real_ * b.real_ - a.imag_ * b.imag_;
c.imag_ = a.real_ * b.imag_ + a.imag_ * b.real_;
return c;
}
MouseBF_complex_f MouseBF_complex_add(MouseBF_complex_f a, MouseBF_complex_f b) {
MouseBF_complex_f c;
c.real_ = a.real_ + b.real_;
c.imag_ = a.imag_ + b.imag_;
return c;
}
MouseBF_complex_f MouseBF_complex_conj(MouseBF_complex_f a) {
MouseBF_complex_f ret;
ret.real_ = a.real_;
ret.imag_ = -1 * a.imag_;
return ret;
}
（7）、src/bf-fft.h
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-fft.h
* */
#ifndef __BF_FFT_H__
#define __BF_FFT_H__
#include <stddef.h>
#include "bf-complex.h"
typedef struct MouseBF_fft_ {
size_t *work_ip_;
float *work_w_;
int order_;
size_t length_;
size_t complex_length_;
} MouseBF_fft;
// Modified Bessel function of order 0 for complex inputs.
MouseBF_complex_f MouseBF_I0(MouseBF_complex_f x);
//input: len is fft len
//return: fft handle
MouseBF_fft *MouseBF_fft_instance(int len);
//destroy
void MouseBF_fft_destroy(MouseBF_fft *inst);
//fft transform
int MouseBF_fft_forward(MouseBF_fft *inst, float *src, MouseBF_complex_f *des);
//fft invert
int MouseBF_fft_invert(MouseBF_fft *inst, MouseBF_complex_f *src, float *des);
#endif
（8）、src/bf-fft.c
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-fft.c
* */
#include "bf-fft.h"
#include "fft4g.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#ifndef int16_t
#define int16_t short
#endif
static void MouseBF_Conjugate(MouseBF_complex_f* array, size_t complex_length) {
for (int i = 0; i < complex_length; i++) {
array[i].imag_ *= -1;
}
}
static size_t MouseBF_ComputeWorkIpSize(size_t fft_length) {
float len = fft_length;
size_t ret = 2 + ceil(sqrt(len));
return ret;
}
static int16_t MouseBF_WebRtcSpl_GetSizeInBits(uint32_t n) {
int16_t bits;
if (0xFFFF0000 & n) {
bits = 16;
} else {
bits = 0;
}
if (0x0000FF00 & (n >> bits)) bits += 8;
if (0x000000F0 & (n >> bits)) bits += 4;
if (0x0000000C & (n >> bits)) bits += 2;
if (0x00000002 & (n >> bits)) bits += 1;
if (0x00000001 & (n >> bits)) bits += 1;
return bits;
}
static size_t MouseBF_FftLength(int order) {
size_t ret = 1 << order;
return ret;
}
static size_t MouseBF_ComplexLength(int order) {
size_t ret = MouseBF_FftLength(order) / 2 + 1;
return ret;
}
MouseBF_fft *MouseBF_fft_instance(int len) {
MouseBF_fft *inst = (MouseBF_fft*)malloc(sizeof(MouseBF_fft));
inst->order_ = MouseBF_WebRtcSpl_GetSizeInBits(len - 1);
inst->length_ = MouseBF_FftLength(inst->order_);
inst->complex_length_ = MouseBF_ComplexLength(inst->order_);
size_t ip_len = MouseBF_ComputeWorkIpSize(len);
inst->work_ip_ = (size_t*)malloc(sizeof(size_t) * ip_len);
inst->work_ip_[0] = 0;
inst->work_w_ = (float*)malloc(sizeof(float) * inst->complex_length_);
return inst;
}
void MouseBF_fft_destroy(MouseBF_fft *inst) {
free(inst->work_ip_);
free(inst->work_w_);
free(inst);
}
int MouseBF_fft_forward(MouseBF_fft *inst, float *src, MouseBF_complex_f *des) {
float *des_float = (float*)des;
memcpy(des_float, src, sizeof(float) * inst->length_);
WebRtc_rdft(inst->length_, 1, des_float, inst->work_ip_, inst->work_w_);
des[inst->complex_length_ - 1].real_ = des[0].imag_;
des[inst->complex_length_ - 1].imag_ = 0.0;
des[0].imag_ = 0.0;
MouseBF_Conjugate(des, inst->complex_length_);
return 0;
}
int MouseBF_fft_invert(MouseBF_fft *inst, MouseBF_complex_f *src, float *des) {
float *src_float = (float*)src;
MouseBF_complex_f *des_complex = (MouseBF_complex_f*)des;
memcpy(des, src_float, sizeof(float) * inst->length_);
MouseBF_Conjugate(des_complex, inst->complex_length_-1);
des_complex[0].imag_ = src[inst->complex_length_-1].real_;
WebRtc_rdft(inst->length_, -1, des, inst->work_ip_, inst->work_w_);
for (int i = 0; i < inst->length_; i++) {
des[i] *= 2.0/inst->length_;
}
return 0;
}
// Modified Bessel function of order 0 for complex inputs.
MouseBF_complex_f MouseBF_I0(MouseBF_complex_f x) {
MouseBF_complex_f a, y, c;
a.real_ = 1 / 3.75;
a.imag_ = 0.0;
y = MouseBF_complex_mul(x, a);
y = MouseBF_complex_mul(y, y);
a.real_ = 0.45813e-2;
a.imag_ = 0.0;
c = MouseBF_complex_mul(y, a);
a.real_ = 0.360768e-1;
a.imag_ = 0.0;
c = MouseBF_complex_add(c, a);
c = MouseBF_complex_mul(y, c);
a.real_ = 0.2659732;
a.imag_ = 0.0;
c = MouseBF_complex_add(c, a);
c = MouseBF_complex_mul(y, c);
a.real_ = 1.2067492;
a.imag_ = 0.0;
c = MouseBF_complex_add(c, a);
c = MouseBF_complex_mul(y, c);
a.real_ = 3.0899424;
a.imag_ = 0.0;
c = MouseBF_complex_add(c, a);
c = MouseBF_complex_mul(y, c);
a.real_ = 3.5156229;
a.imag_ = 0.0;
c = MouseBF_complex_add(c, a);
c = MouseBF_complex_mul(y, c);
a.real_ = 1.0;
a.imag_ = 0.0;
c = MouseBF_complex_add(c, a);
return c;
}
（9）、src/bf-matrix.h
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-matrix.h
* */
#ifndef __BF_MATRIX_H__
#define __BF_MATRIX_H__
#include "bf-complex.h"
#include <stddef.h>
typedef struct MouseBF_complex_matrix_f_ {
size_t num_rows_;
size_t num_cols_;
MouseBF_complex_f* data_;
MouseBF_complex_f** elements_;
} MouseBF_complex_matrix_f;
void MouseBF_complex_matrix_init(MouseBF_complex_matrix_f *mat, size_t row, size_t col);
void MouseBF_complex_matrix_free(MouseBF_complex_matrix_f *mat);
void MouseBF_complex_matrix_reset(MouseBF_complex_matrix_f *mat, size_t row, size_t col);
void MouseBF_complex_matrix_copy_from_column(MouseBF_complex_matrix_f *mat, size_t column_index, MouseBF_complex_f **src);
void MouseBF_complex_matrix_copy(MouseBF_complex_matrix_f *mat, MouseBF_complex_matrix_f *src);
float MouseBF_SumAbs(MouseBF_complex_matrix_f *mat);
float MouseBF_SumSquares(MouseBF_complex_matrix_f *mat);
MouseBF_complex_f MouseBF_ConjugateDotProduct(MouseBF_complex_matrix_f *lhs, MouseBF_complex_matrix_f *rhs);
float MouseBF_Norm(MouseBF_complex_matrix_f *mat, MouseBF_complex_matrix_f *norm_mat);
void MouseBF_complex_matrix_scale(MouseBF_complex_matrix_f *mat, MouseBF_complex_f scale);
void MouseBF_TransposedConjugatedProduct(MouseBF_complex_matrix_f *in, MouseBF_complex_matrix_f *out);
MouseBF_complex_f MouseBF_Trace(MouseBF_complex_matrix_f *mat);
void MouseBF_Transpose(MouseBF_complex_matrix_f *in, MouseBF_complex_matrix_f *out);
void MouseBF_PointwiseConjugate(MouseBF_complex_matrix_f *mat);
void MouseBF_ComplexMatrixMultiply(MouseBF_complex_matrix_f *interf_cov_vector_transposed, MouseBF_complex_matrix_f *interf_cov_vector, MouseBF_complex_matrix_f *mat);
void MouseBF_ComplexMatrixAdd(MouseBF_complex_matrix_f *src1, MouseBF_complex_matrix_f *src2, MouseBF_complex_matrix_f *mat);
#endif
（10）、src/bf-matrix.c
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-matrix.c
* */
#include "bf-matrix.h"
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
void MouseBF_complex_matrix_init(MouseBF_complex_matrix_f *mat, size_t row, size_t col) {
mat->num_rows_ = row;
mat->num_cols_ = col;
mat->data_ = (MouseBF_complex_f*)malloc(row * col * sizeof(MouseBF_complex_f));
memset(mat->data_, 0, row * col * sizeof(MouseBF_complex_f));
mat->elements_ = (MouseBF_complex_f**)malloc(row * sizeof(MouseBF_complex_f*));
for(int i = 0; i < row; i++) {
mat->elements_[i] = &mat->data_[i * col];
}
}
void MouseBF_complex_matrix_free(MouseBF_complex_matrix_f *mat) {
if (mat->data_) {
free(mat->data_);
free(mat->elements_);
}
}
void MouseBF_complex_matrix_reset(MouseBF_complex_matrix_f *mat, size_t row, size_t col) {
if (mat->data_ != NULL) {
free(mat->data_);
free(mat->elements_);
}
MouseBF_complex_matrix_init(mat, row, col);
}
void MouseBF_complex_matrix_copy_from_column(MouseBF_complex_matrix_f *mat, size_t column_index, MouseBF_complex_f **src) {
for (int i = 0; i < mat->num_cols_; ++i) {
mat->data_[i] = src[i][column_index];
}
}
void MouseBF_complex_matrix_copy(MouseBF_complex_matrix_f *mat, MouseBF_complex_matrix_f *src) {
MouseBF_complex_matrix_reset(mat, src->num_rows_, src->num_cols_);
for (int i = 0; i < mat->num_cols_* mat->num_rows_; ++i) {
mat->data_[i] = src->data_[i];
}
}
float MouseBF_SumAbs(MouseBF_complex_matrix_f *mat) {
float sum_abs = 0.0;
MouseBF_complex_f **mat_els = mat->elements_;
for (int i = 0; i < mat->num_rows_; ++i) {
for (int j = 0; j < mat->num_cols_; ++j) {
sum_abs += sqrt(mat_els[i][j].real_ * mat_els[i][j].real_ + mat_els[i][j].imag_ * mat_els[i][j].imag_);
}
}
return sum_abs;
}
float MouseBF_SumSquares(MouseBF_complex_matrix_f *mat) {
float sum_squares = 0.f;
for (int i = 0; i < mat->num_rows_ * mat->num_cols_; i++) {
sum_squares += mat->data_[i].real_ * mat->data_[i].real_ + mat->data_[i].imag_ * mat->data_[i].imag_;
}
return sum_squares;
}
MouseBF_complex_f MouseBF_ConjugateDotProduct(MouseBF_complex_matrix_f *lhs, MouseBF_complex_matrix_f *rhs) {
assert(lhs->num_rows_ == 1);
assert(rhs->num_rows_ == 1);
assert(rhs->num_cols_ == lhs->num_cols_);
MouseBF_complex_f result;
result.real_ = 0.0;
result.imag_ = 0.0;
MouseBF_complex_f **lhs_elements = lhs->elements_;
MouseBF_complex_f **rhs_elements = rhs->elements_;
for (int i = 0; i < lhs->num_cols_; i++) {
MouseBF_complex_f tmp = MouseBF_complex_mul(MouseBF_complex_conj(lhs_elements[0][i]), rhs_elements[0][i]);
result = MouseBF_complex_add(result, tmp);
}
return result;
}
float MouseBF_Norm(MouseBF_complex_matrix_f *mat, MouseBF_complex_matrix_f *norm_mat) {
MouseBF_complex_f first_product, second_product;
first_product.real_ = 0.0;
first_product.imag_ = 0.0;
second_product.real_ = 0.0;
second_product.imag_ = 0.0;
MouseBF_complex_f **mat_els = mat->elements_;
MouseBF_complex_f **norm_mat_els = norm_mat->elements_;
for (int i = 0; i < norm_mat->num_cols_; i++) {
for (int j = 0; j < norm_mat->num_cols_; j++) {
first_product = MouseBF_complex_add(first_product, MouseBF_complex_mul(MouseBF_complex_conj(norm_mat_els[0][j]), mat_els[j][i]));
}
second_product = MouseBF_complex_add(second_product, MouseBF_complex_mul(first_product, norm_mat_els[0][i]));
first_product.real_ = 0.0;
first_product.imag_ = 0.0;
}
float ret = second_product.real_;
if (ret < 0.f) {
ret = 0.f;
}
return ret;
}
void MouseBF_complex_matrix_scale(MouseBF_complex_matrix_f *mat, MouseBF_complex_f scale) {
int size = mat->num_cols_ * mat->num_rows_;
for (int i = 0; i < size; i++) {
mat->data_[i] = MouseBF_complex_mul(mat->data_[i], scale);
}
}
void MouseBF_TransposedConjugatedProduct(MouseBF_complex_matrix_f *in, MouseBF_complex_matrix_f *out) {
assert(in->num_rows_ == 1);
assert(out->num_rows_ == in->num_cols_);
assert(out->num_cols_ == in->num_cols_);
MouseBF_complex_f* in_elements = in->elements_[0];
MouseBF_complex_f** out_elements = out->elements_;
for (int i = 0; i < out->num_rows_; ++i) {
for (int j = 0; j < out->num_cols_; ++j) {
out_elements[i][j] = MouseBF_complex_mul(in_elements[i], MouseBF_complex_conj(in_elements[j]));
}
}
}
MouseBF_complex_f MouseBF_Trace(MouseBF_complex_matrix_f *mat) {
assert(mat->num_rows_ == mat->num_cols_);
MouseBF_complex_f trace;
trace.real_ = 0.0;
trace.imag_ = 0.0;
for (int i = 0; i < mat->num_rows_; ++i) {
trace = MouseBF_complex_add(trace, mat->elements_[i][i]);
}
return trace;
}
void MouseBF_Transpose(MouseBF_complex_matrix_f *in, MouseBF_complex_matrix_f *out) {
for (int i = 0; i < out->num_rows_; ++i) {
for (int j = 0; j < out->num_cols_; ++j) {
out->elements_[i][j] = in->elements_[j][i];
}
}
}
void MouseBF_PointwiseConjugate(MouseBF_complex_matrix_f *mat) {
for (int i = 0; i < mat->num_rows_; ++i) {
for (int j = 0; j < mat->num_cols_; ++j) {
mat->elements_[i][j].imag_ *= -1;
}
}
}
void MouseBF_ComplexMatrixMultiply(MouseBF_complex_matrix_f *interf_cov_vector_transposed, MouseBF_complex_matrix_f *interf_cov_vector, MouseBF_complex_matrix_f *mat) {
for (int row = 0; row < interf_cov_vector_transposed->num_rows_; ++row) {
for (int col = 0; col < interf_cov_vector->num_cols_; ++col) {
MouseBF_complex_f cur_element;
cur_element.real_ = 0.0;
cur_element.imag_ = 0.0;
for (int i = 0; i < interf_cov_vector->num_rows_; ++i) {
cur_element = MouseBF_complex_add(cur_element, MouseBF_complex_mul(interf_cov_vector_transposed->elements_[row][i], interf_cov_vector->elements_[i][col]));
}
mat->elements_[row][col] = cur_element;
}
}
}
void MouseBF_ComplexMatrixAdd(MouseBF_complex_matrix_f *src1, MouseBF_complex_matrix_f *src2, MouseBF_complex_matrix_f *mat) {
for (size_t i = 0; i < src1->num_cols_ * src1->num_rows_; ++i) {
mat->data_[i] = MouseBF_complex_add(src1->data_[i], src2->data_[i]);
}
}
（11）、src/bf-nonlinear.h
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-nonlinear.h
* */
#ifndef __BF_NONLINEAR_H__
#define __BF_NONLINEAR_H__
#include "bf-types.h"
#include "bf-matrix.h"
#include "bf-complex.h"
#include <stddef.h>
typedef struct MouseBF_nonlinear_ {
int sample_rate_hz_;
int num_input_channels_;
MouseBF_point *array_geometry_;
// The normal direction of the array if it has one and it is in the xy-plane.
MouseBF_point *array_normal_;
// Minimum spacing between microphone pairs.
float min_mic_spacing_;
// Calculated based on user-input and constants
size_t low_mean_start_bin_;
size_t low_mean_end_bin_;
size_t high_mean_start_bin_;
size_t high_mean_end_bin_;
// Quickly varying mask updated every block.
float new_mask_[kNumFreqBins];
// Time smoothed mask.
float time_smooth_mask_[kNumFreqBins];
// Time and frequency smoothed mask.
float final_mask_[kNumFreqBins];
float target_angle_radians_;
// Angles of the interferer scenarios.
float interf_angles_radians_[kInterfNum];
// The angle between the target and the interferer scenarios.
float away_radians_;
// Array of length |kNumFreqBins|, Matrix of size |1| x |num_channels_|.
MouseBF_complex_matrix_f delay_sum_masks_[kNumFreqBins];
//MouseBF_complex_matrix_f normalized_delay_sum_masks_[kNumFreqBins];
// Arrays of length |kNumFreqBins|, Matrix of size |num_input_channels_| x
// |num_input_channels_|.
MouseBF_complex_matrix_f target_cov_mats_[kNumFreqBins];
MouseBF_complex_matrix_f uniform_cov_mat_[kNumFreqBins];
// Array of length |kNumFreqBins|, Matrix of size |num_input_channels_| x
// |num_input_channels_|. ScopedVector has a size equal to the number of
// interferer scenarios.
MouseBF_complex_matrix_f interf_cov_mats_[kNumFreqBins][kInterfNum];
// Of length |kNumFreqBins|.
float mask_thresholds_[kNumFreqBins];
float wave_numbers_[kNumFreqBins];
// Of length |kNumFreqBins|.
float rxiws_[kNumFreqBins];
// The vector has a size equal to the number of interferer scenarios.
float rpsiws_[kNumFreqBins][kInterfNum];
// The microphone normalization factor.
MouseBF_complex_matrix_f eig_m_;
// For processing the high-frequency input signal.
float high_pass_postfilter_mask_;
// True when the target signal is present.
int is_target_present_;
// Number of blocks after which the data is considered interference if the
// mask does not pass |kMaskSignalThreshold|.
size_t hold_target_blocks_;
// Number of blocks since the last mask that passed |kMaskSignalThreshold|.
size_t interference_blocks_count_;
} MouseBF_nonlinear;
MouseBF_nonlinear *MouseBF_nonlinear_instance(int sample_rate_hz,
MouseBF_point *array_geometry,
int mic_num,
MouseBF_spherical_pointf target_direction);
void MouseBF_nonlinear_destroy(MouseBF_nonlinear *inst);
void MouseBF_nonlinear_reset(MouseBF_nonlinear *inst,
MouseBF_spherical_pointf target_direction);
void MouseBF_nonlinear_process_block(MouseBF_nonlinear *inst,
MouseBF_complex_f** input,
int num_input_channels,
size_t num_freq_bins,
int num_output_channels,
MouseBF_complex_f** output);
typedef void (*MouseBF_Callback)(MouseBF_nonlinear*,
MouseBF_complex_f**,
int,
size_t,
int,
MouseBF_complex_f**);
#endif
（12）、src/bf-nonlinear.c
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-nonlinear.c
* */
#include "bf-nonlinear.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include "bessel0.h"
static float MouseBF_DotProduct(MouseBF_point a, MouseBF_point b) {
return a.c[0] * b.c[0] + a.c[1] * b.c[1] + a.c[2] * b.c[2];
}
static MouseBF_point MouseBF_PairDirection(MouseBF_point a, MouseBF_point b) {
MouseBF_point ret;
ret.c[0] = b.c[0] - a.c[0];
ret.c[1] = b.c[1] - a.c[1];
ret.c[2] = b.c[2] - a.c[2];
return ret;
}
static MouseBF_point MouseBF_CrossProduct(MouseBF_point a, MouseBF_point b) {
MouseBF_point ret;
ret.c[0] = a.c[1] * b.c[2] - a.c[2] * b.c[1];
ret.c[1] = a.c[2] * b.c[0] - a.c[0] * b.c[2];
ret.c[2] = a.c[0] * b.c[1] - a.c[1] * b.c[0];
return ret;
}
static int MouseBF_AreParallel(MouseBF_point a, MouseBF_point b) {
MouseBF_point cross_product = MouseBF_CrossProduct(a, b);
return MouseBF_DotProduct(cross_product, cross_product) < kMaxDotProduct;
}
static MouseBF_point *MouseBF_GetDirectionIfLinear(MouseBF_point *array_geometry, int mic_num) {
MouseBF_point first_pair_direction = MouseBF_PairDirection(array_geometry[0], array_geometry[1]);
for (size_t i = 2u; i < mic_num; ++i) {
MouseBF_point pair_direction = MouseBF_PairDirection(array_geometry[i - 1], array_geometry[i]);
if (!MouseBF_AreParallel(first_pair_direction, pair_direction)) {
return NULL;
}
}
MouseBF_point *ret = (MouseBF_point*)malloc(sizeof(MouseBF_point));
*ret = first_pair_direction;
return ret;
}
static int MouseBF_ArePerpendicular(MouseBF_point a, MouseBF_point b) {
return abs(MouseBF_DotProduct(a, b)) < kMaxDotProduct;
}
static MouseBF_point *MouseBF_GetNormalIfPlanar(MouseBF_point *array_geometry, int mic_num) {
MouseBF_point first_pair_direction = MouseBF_PairDirection(array_geometry[0], array_geometry[1]);
MouseBF_point pair_direction;
pair_direction.c[0] = 0.f;
pair_direction.c[1] = 0.f;
pair_direction.c[2] = 0.f;
size_t i = 2;
int is_linear = 1;
for (; i < mic_num && is_linear; ++i) {
pair_direction = MouseBF_PairDirection(array_geometry[i - 1], array_geometry[i]);
if (!MouseBF_AreParallel(first_pair_direction, pair_direction)) {
is_linear = 0;
}
}
if (is_linear) {
return NULL;
}
MouseBF_point normal_direction = MouseBF_CrossProduct(first_pair_direction, pair_direction);
for (; i < mic_num; ++i) {
pair_direction = MouseBF_PairDirection(array_geometry[i - 1], array_geometry[i]);
if (!MouseBF_ArePerpendicular(normal_direction, pair_direction)) {
return NULL;
}
}
MouseBF_point *ret = (MouseBF_point*)malloc(sizeof(MouseBF_point));
*ret = normal_direction;
return ret;
}
MouseBF_point MouseBF_AzimuthToPoint(float azimuth) {
MouseBF_point ret;
ret.c[0] = cos(azimuth);
ret.c[1] = sin(azimuth);
ret.c[2] = 0.f;
return ret;
}
static MouseBF_point *MouseBF_GetArrayNormalIfExists(MouseBF_point *array_geometry, int mic_num) {
MouseBF_point *direction = MouseBF_GetDirectionIfLinear(array_geometry, mic_num);
if (direction) {
MouseBF_point ret = *direction;
(*direction).c[0] = ret.c[1];
(*direction).c[1] = -ret.c[0];
(*direction).c[2] = 0.f;
return direction;
}
MouseBF_point *normal = MouseBF_GetNormalIfPlanar(array_geometry, mic_num);
if (normal && normal->c[2] < kMaxDotProduct) {
return normal;
}
return NULL;
}
static void MouseBF_GetCenteredArray(MouseBF_point *array_geometry_src, int mic_num, MouseBF_point *array_geometry_des) {
for (int dim = 0; dim < 3; ++dim) {
float center = 0.f;
for (size_t i = 0; i < mic_num; ++i) {
center += array_geometry_src[i].c[dim];
}
center /= mic_num;
for (size_t i = 0; i < mic_num; ++i) {
array_geometry_des[i].c[dim] = array_geometry_src[i].c[dim] - center;
}
}
}
static float MouseBF_Distance(MouseBF_point a, MouseBF_point b) {
return sqrt((a.c[0] - b.c[0]) * (a.c[0] - b.c[0]) +
(a.c[1] - b.c[1]) * (a.c[1] - b.c[1]) +
(a.c[2] - b.c[2]) * (a.c[2] - b.c[2]));
}
static float MouseBF_GetMinimumSpacing(MouseBF_point *array_geometry, int mic_num) {
float mic_spacing = 10000000.0;
for (size_t i = 0; i < mic_num - 1; ++i) {
for (size_t j = i + 1; j < mic_num; ++j) {
float tmp = MouseBF_Distance(array_geometry[i], array_geometry[j]);
if (tmp < mic_spacing) {
mic_spacing = tmp;
}
}
}
return mic_spacing;
}
static void MouseBF_InitLowFrequencyCorrectionRanges(MouseBF_nonlinear *inst) {
inst->low_mean_start_bin_ = (size_t)floor(kLowMeanStartHz * kFftSize / inst->sample_rate_hz_ + 0.5);
inst->low_mean_end_bin_ = (size_t)floor(kLowMeanEndHz * kFftSize / inst->sample_rate_hz_ + 0.5);
}
static void MouseBF_UniformCovarianceMatrix(float wave_number,
MouseBF_point *geometry, int mic_num,
MouseBF_complex_matrix_f* mat) {
assert(mic_num == mat->num_rows_);
assert(mic_num == mat->num_cols_);
MouseBF_complex_f **mat_els = mat->elements_;
for (size_t i = 0; i < mic_num; ++i) {
for (size_t j = 0; j < mic_num; ++j) {
if (wave_number > 0.f) {
//mat_els[i][j].real_ = j0(wave_number * MouseBF_Distance(geometry[i], geometry[j]));
mat_els[i][j].real_ = bessel0(wave_number * MouseBF_Distance(geometry[i], geometry[j]));
} else {
mat_els[i][j].real_ = i == j ? 1.f : 0.f;
}
mat_els[i][j].imag_ = 0.0;
}
}
}
static void MouseBF_InitDiffuseCovMats(MouseBF_nonlinear *inst) {
for (size_t i = 0; i < kNumFreqBins; ++i) {
MouseBF_complex_matrix_reset(&(inst->uniform_cov_mat_[i]), inst->num_input_channels_, inst->num_input_channels_);
MouseBF_UniformCovarianceMatrix(inst->wave_numbers_[i], inst->array_geometry_, inst->num_input_channels_, &(inst->uniform_cov_mat_[i]));
MouseBF_complex_f trace = MouseBF_Trace(&(inst->uniform_cov_mat_[i]));
MouseBF_complex_f normalization_factor;
normalization_factor.real_ = trace.real_ / (trace.real_*trace.real_ + trace.imag_*trace.imag_);
normalization_factor.imag_ = -trace.imag_ / (trace.real_*trace.real_ + trace.imag_*trace.imag_);
MouseBF_complex_matrix_scale(&(inst->uniform_cov_mat_[i]), normalization_factor);
trace.real_ = 1 - kBalance;
trace.imag_ = 0.0;
MouseBF_complex_matrix_scale(&(inst->uniform_cov_mat_[i]), trace);
}
}
static void MouseBF_InitHighFrequencyCorrectionRanges(MouseBF_nonlinear *inst) {
//float kAliasingFreqHz = kSpeedOfSoundMeterSeconds / (inst->min_mic_spacing_ *
// (1.f + abs(cos(inst->target_angle_radians_))));
float kHighMeanStartHz = 2000;
float kHighMeanEndHz = 3000;
inst->high_mean_start_bin_ = (size_t)floor(kHighMeanStartHz * kFftSize / inst->sample_rate_hz_ + 0.5);
inst->high_mean_end_bin_ = (size_t)floor(kHighMeanEndHz * kFftSize / inst->sample_rate_hz_ + 0.5);
}
static void MouseBF_InitInterfAngles(MouseBF_nonlinear *inst) {
MouseBF_point target_direction = MouseBF_AzimuthToPoint(inst->target_angle_radians_);
MouseBF_point clockwise_interf_direction = MouseBF_AzimuthToPoint(inst->target_angle_radians_ - inst->away_radians_);
if (!inst->array_normal_ || MouseBF_DotProduct(*(inst->array_normal_), target_direction) *
MouseBF_DotProduct(*(inst->array_normal_), clockwise_interf_direction) >= 0.f) {
// The target and clockwise interferer are in the same half-plane defined
// by the array.
inst->interf_angles_radians_[0] = inst->target_angle_radians_ - inst->away_radians_;
} else {
// Otherwise, the interferer will begin reflecting back at the target.
// Instead rotate it away 180 degrees.
inst->interf_angles_radians_[0] = inst->target_angle_radians_ - inst->away_radians_ + M_PI;
}
MouseBF_point counterclock_interf_direction = MouseBF_AzimuthToPoint(inst->target_angle_radians_ + inst->away_radians_);
if (!inst->array_normal_ || MouseBF_DotProduct(*(inst->array_normal_), target_direction) *
MouseBF_DotProduct(*(inst->array_normal_), counterclock_interf_direction) >= 0.f) {
// The target and counter-clockwise interferer are in the same half-plane
// defined by the array.
inst->interf_angles_radians_[1] = inst->target_angle_radians_ + inst->away_radians_;
} else {
// Otherwise, the interferer will begin reflecting back at the target.
// Instead rotate it away 180 degrees.
inst->interf_angles_radians_[1] = inst->target_angle_radians_ + inst->away_radians_ - M_PI;
}
//wj add
clockwise_interf_direction = MouseBF_AzimuthToPoint(inst->target_angle_radians_ - M_PI / 2.0);
if (!inst->array_normal_ || MouseBF_DotProduct(*(inst->array_normal_), target_direction) *
MouseBF_DotProduct(*(inst->array_normal_), clockwise_interf_direction) >= 0.f) {
// The target and clockwise interferer are in the same half-plane defined
// by the array.
inst->interf_angles_radians_[2] = inst->target_angle_radians_ - M_PI / 2.0;
} else {
// Otherwise, the interferer will begin reflecting back at the target.
// Instead rotate it away 180 degrees.
inst->interf_angles_radians_[2] = inst->target_angle_radians_ - M_PI / 2.0 + M_PI;
}
counterclock_interf_direction = MouseBF_AzimuthToPoint(inst->target_angle_radians_ + M_PI / 2.0);
if (!inst->array_normal_ || MouseBF_DotProduct(*(inst->array_normal_), target_direction) *
MouseBF_DotProduct(*(inst->array_normal_), counterclock_interf_direction) >= 0.f) {
// The target and counter-clockwise interferer are in the same half-plane
// defined by the array.
inst->interf_angles_radians_[3] = inst->target_angle_radians_ + M_PI / 2.0;
} else {
// Otherwise, the interferer will begin reflecting back at the target.
// Instead rotate it away 180 degrees.
inst->interf_angles_radians_[3] = inst->target_angle_radians_ + M_PI / 2.0 - M_PI;
}
}
static void MouseBF_PhaseAlignmentMasks(size_t frequency_bin,
size_t fft_size,
int sample_rate,
float sound_speed,
MouseBF_point *geometry,
int num_mic,
float angle,
MouseBF_complex_matrix_f* mat) {
assert(1 == mat->num_rows_);
assert(num_mic == mat->num_cols_);
float freq_in_hertz = (float)(frequency_bin) / fft_size * sample_rate;
MouseBF_complex_f **mat_els = mat->elements_;
for (size_t c_ix = 0; c_ix < num_mic; ++c_ix) {
float distance = cos(angle) * geometry[c_ix].c[0] +
sin(angle) * geometry[c_ix].c[1];
float phase_shift = 2.f * M_PI * distance * freq_in_hertz / sound_speed;
// Euler's formula for mat[0][c_ix] = e^(j * phase_shift).
mat_els[0][c_ix].real_ = cos(phase_shift);
mat_els[0][c_ix].imag_ = sin(phase_shift);
}
}
static void MouseBF_InitDelaySumMasks(MouseBF_nonlinear *inst) {
for (size_t f_ix = 0; f_ix < kNumFreqBins; ++f_ix) {
MouseBF_complex_matrix_reset(&(inst->delay_sum_masks_[f_ix]), 1, inst->num_input_channels_);
//MouseBF_complex_matrix_reset(&(inst->normalized_delay_sum_masks_[f_ix]), 1, inst->num_input_channels_);
MouseBF_PhaseAlignmentMasks(f_ix, kFftSize, inst->sample_rate_hz_, kSpeedOfSoundMeterSeconds,
inst->array_geometry_, inst->num_input_channels_,
inst->target_angle_radians_, &(inst->delay_sum_masks_[f_ix]));
//_// zoro_ditch
//_// MouseBF_complex_f norm_factor = MouseBF_ConjugateDotProduct(&(inst->delay_sum_masks_[f_ix]), &(inst->delay_sum_masks_[f_ix]));
MouseBF_complex_f scale;
//_// norm_factor.real_ = sqrt(norm_factor.real_);
//_// norm_factor.imag_ = 0.0;//sqrt(norm_factor.imag_);
//_// scale.real_ = norm_factor.real_ / (norm_factor.real_ * norm_factor.real_ + norm_factor.imag_ * norm_factor.imag_);
//_// zoro_update
scale.real_ = 1/sqrt(inst->num_input_channels_);
scale.imag_ = 0.0;
MouseBF_complex_matrix_scale(&(inst->delay_sum_masks_[f_ix]), scale);
/*MouseBF_complex_matrix_copy(&(inst->normalized_delay_sum_masks_[f_ix]), &(inst->delay_sum_masks_[f_ix]));
norm_factor.real = 1.0 / MouseBF_SumAbs(&(inst->normalized_delay_sum_masks_[f_ix]));
norm_factor.imag_ = 0.0;
MouseBF_complex_matrix_scale(&(inst->normalized_delay_sum_masks_[f_ix]), norm_factor);*/
}
}
static void MouseBF_InitTargetCovMats(MouseBF_nonlinear *inst) {
for (size_t i = 0; i < kNumFreqBins; ++i) {
MouseBF_complex_matrix_reset(&(inst->target_cov_mats_[i]), inst->num_input_channels_, inst->num_input_channels_);
MouseBF_TransposedConjugatedProduct(&(inst->delay_sum_masks_[i]), &(inst->target_cov_mats_[i]));
//_// zoro_ditch
//_// MouseBF_complex_f trace = MouseBF_Trace(&(inst->target_cov_mats_[i]));
//_// MouseBF_complex_f norm_factor;
//_// norm_factor.real_ = trace.real_ / (trace.real_ * trace.real_ + trace.imag_ * trace.imag_);
//_// norm_factor.imag_ = -trace.imag_ / (trace.real_ * trace.real_ + trace.imag_ * trace.imag_);
//_// MouseBF_complex_matrix_scale(&(inst->target_cov_mats_[i]), norm_factor);
}
}
static void MouseBF_AngledCovarianceMatrix(float sound_speed, float angle, size_t frequency_bin,
size_t fft_size, size_t num_freq_bins, int sample_rate,
MouseBF_point *geometry, int num_mic,
MouseBF_complex_matrix_f *mat) {
assert(num_mic == mat->num_rows_);
assert(num_mic == mat->num_cols_);
MouseBF_complex_matrix_f interf_cov_vector, interf_cov_vector_transposed;
MouseBF_complex_matrix_init(&interf_cov_vector, 1, num_mic);
MouseBF_complex_matrix_init(&interf_cov_vector_transposed, num_mic, 1);
MouseBF_PhaseAlignmentMasks(frequency_bin, fft_size, sample_rate,
sound_speed, geometry, num_mic, angle,
&interf_cov_vector);
MouseBF_Transpose(&interf_cov_vector, &interf_cov_vector_transposed);
MouseBF_PointwiseConjugate(&interf_cov_vector);
MouseBF_ComplexMatrixMultiply(&interf_cov_vector_transposed, &interf_cov_vector, mat);
MouseBF_complex_matrix_free(&interf_cov_vector);
MouseBF_complex_matrix_free(&interf_cov_vector_transposed);
}
static void MouseBF_InitInterfCovMats(MouseBF_nonlinear *inst) {
for (size_t i = 0; i < kNumFreqBins; ++i) {
for (size_t j = 0; j < kInterfNum; ++j) {
MouseBF_complex_matrix_reset(&(inst->interf_cov_mats_[i][j]), inst->num_input_channels_, inst->num_input_channels_);
MouseBF_complex_matrix_f angled_cov_mat;
MouseBF_complex_matrix_init(&(angled_cov_mat), inst->num_input_channels_, inst->num_input_channels_);
MouseBF_AngledCovarianceMatrix(kSpeedOfSoundMeterSeconds, inst->interf_angles_radians_[j],
i, kFftSize, kNumFreqBins, inst->sample_rate_hz_,
inst->array_geometry_, inst->num_input_channels_, &angled_cov_mat);
// Normalize matrices before averaging them.
//complex_f normalization_factor = angled_cov_mat.elements()[0][0];
//_// MouseBF_complex_f trace = MouseBF_Trace(&angled_cov_mat);
//_// zoro_ditch
//_//
MouseBF_complex_f norm_factor;
//_// norm_factor.real_ = trace.real_ / (trace.real_ * trace.real_ + trace.imag_ * trace.imag_);
//_// norm_factor.imag_ = -trace.imag_ / (trace.real_ * trace.real_ + trace.imag_ * trace.imag_);
//_// zoro_update
norm_factor.real_ = 1/inst->num_input_channels_;
norm_factor.imag_ = 0;
MouseBF_complex_matrix_scale(&(angled_cov_mat), norm_factor);
MouseBF_complex_f trace;
trace.real_ = kBalance;
trace.imag_ = 0.0;
MouseBF_complex_matrix_scale(&(angled_cov_mat), trace);
MouseBF_ComplexMatrixAdd(&(inst->uniform_cov_mat_[i]), &(angled_cov_mat), &(inst->interf_cov_mats_[i][j]));
MouseBF_complex_matrix_free(&(angled_cov_mat));
}
}
}
static void MouseBF_NormalizeCovMats(MouseBF_nonlinear *inst) {
for (size_t i = 0; i < kNumFreqBins; ++i) {
//zoro_ditch equal 1
//_// inst->rxiws_[i] = MouseBF_Norm(&(inst->target_cov_mats_[i]), &(inst->delay_sum_masks_[i]));
for (size_t j = 0; j < kInterfNum; ++j) {
inst->rpsiws_[i][j] = MouseBF_Norm(&(inst->interf_cov_mats_[i][j]), &(inst->delay_sum_masks_[i]));
}
}
}
static float MouseBF_CalculatePostfilterMask(MouseBF_nonlinear *inst,
MouseBF_complex_matrix_f *interf_cov_mat,
float rpsiw,
float ratio_rxiw_rxim,
float rmw_r, float mask_threshold) {
const float kMaskMinimum = 0.01f;
float rpsim = MouseBF_Norm(interf_cov_mat, &(inst->eig_m_));
// Find lambda.
float ratio = 0.f;
if (rpsim > 0.f) {
ratio = rpsiw / rpsim;
}
float numerator = rmw_r - ratio;
float denominator = ratio_rxiw_rxim - ratio;
float mask = 1.f;
if (denominator > mask_threshold) {
float lambda = numerator / denominator;
mask = lambda * ratio_rxiw_rxim / rmw_r;
if (mask < kMaskMinimum) {
mask = kMaskMinimum;
}
}
return mask;
}
static void MouseBF_ApplyMaskTimeSmoothing(MouseBF_nonlinear *inst) {
for (size_t i = inst->low_mean_start_bin_; i <= inst->high_mean_end_bin_; ++i) {
inst->time_smooth_mask_[i] = kMaskTimeSmoothAlpha * inst->new_mask_[i] +
(1 - kMaskTimeSmoothAlpha) * inst->time_smooth_mask_[i];
}
}
static void MouseBF_ApplyLowFrequencyCorrection(MouseBF_nonlinear *inst) {
float low_frequency_mask = 0;
for (int i = inst->low_mean_start_bin_; i < inst->low_mean_end_bin_ + 1; i++) {
low_frequency_mask += inst->time_smooth_mask_[i];
}
low_frequency_mask = low_frequency_mask / (inst->low_mean_end_bin_ - inst->low_mean_start_bin_ + 1);
for (int i = 0; i < inst->low_mean_start_bin_; i++) {
inst->time_smooth_mask_[i] = low_frequency_mask;
}
}
static void MouseBF_ApplyHighFrequencyCorrection(MouseBF_nonlinear *inst) {
inst->high_pass_postfilter_mask_ = 0;
for (int i = inst->high_mean_start_bin_; i < inst->high_mean_end_bin_ + 1; i++) {
inst->high_pass_postfilter_mask_ += inst->time_smooth_mask_[i];
}
inst->high_pass_postfilter_mask_ = inst->high_pass_postfilter_mask_ / (inst->high_mean_end_bin_ - inst->high_mean_start_bin_ + 1);
for (int i = inst->high_mean_end_bin_+1; i < kNumFreqBins; i++) {
inst->time_smooth_mask_[i] = inst->high_pass_postfilter_mask_;
}
}
static void MouseBF_ApplyMaskFrequencySmoothing(MouseBF_nonlinear *inst) {
memcpy(inst->final_mask_, inst->time_smooth_mask_, kNumFreqBins * sizeof(float));
for (size_t i = inst->low_mean_start_bin_; i < kNumFreqBins; ++i) {
inst->final_mask_[i] = kMaskFrequencySmoothAlpha * inst->final_mask_[i] +
(1 - kMaskFrequencySmoothAlpha) * inst->final_mask_[i - 1];
}
for (size_t i = inst->high_mean_end_bin_ + 1; i > 0; --i) {
inst->final_mask_[i - 1] = kMaskFrequencySmoothAlpha * inst->final_mask_[i - 1] +
(1 - kMaskFrequencySmoothAlpha) * inst->final_mask_[i];
}
}
static void MouseBF_ApplyMasks(MouseBF_nonlinear *inst, MouseBF_complex_f **input, MouseBF_complex_f **output) {
MouseBF_complex_f* output_channel = output[0];
for (size_t f_ix = 0; f_ix < kNumFreqBins; ++f_ix) {
output_channel[f_ix].real_ = 0.0;
output_channel[f_ix].imag_ = 0.0;
MouseBF_complex_f* delay_sum_mask_els = inst->delay_sum_masks_[f_ix].elements_[0];
for (int c_ix = 0; c_ix < inst->num_input_channels_; ++c_ix) {
output_channel[f_ix] = MouseBF_complex_add(output_channel[f_ix], MouseBF_complex_mul(input[c_ix][f_ix], MouseBF_complex_conj(delay_sum_mask_els[c_ix])));
}
MouseBF_complex_f mask;
mask.real_ = inst->final_mask_[f_ix] * kCompensationGain;
mask.imag_ = 0;
output_channel[f_ix] = MouseBF_complex_mul(output_channel[f_ix], mask);
}
for (size_t f_ix = 0; f_ix < 4; ++f_ix) {
output_channel[f_ix].real_ = 0;
output_channel[f_ix].imag_ = 0;
}
}
MouseBF_nonlinear *MouseBF_nonlinear_instance(int sample_rate_hz,
MouseBF_point *array_geometry,
int mic_num,
MouseBF_spherical_pointf target_direction) {
MouseBF_nonlinear *inst = (MouseBF_nonlinear*)malloc(sizeof(MouseBF_nonlinear));
inst->sample_rate_hz_ = sample_rate_hz;
inst->num_input_channels_ = mic_num;
inst->array_geometry_ = (MouseBF_point*)malloc(sizeof(MouseBF_point) * mic_num);
MouseBF_GetCenteredArray(array_geometry, mic_num, inst->array_geometry_);
inst->array_normal_ = MouseBF_GetArrayNormalIfExists(array_geometry, mic_num);
inst->min_mic_spacing_ = MouseBF_GetMinimumSpacing(array_geometry, mic_num);
inst->target_angle_radians_ = target_direction.s[0];
inst->away_radians_ = M_PI / 4.0;
inst->high_pass_postfilter_mask_ = 1.0;
for (size_t i = 0; i < kNumFreqBins; ++i) {
inst->time_smooth_mask_[i] = 1.f;
inst->final_mask_[i] = 1.f;
float freq_hz = ((float)i / kFftSize) * inst->sample_rate_hz_;
inst->wave_numbers_[i] = 2 * M_PI * freq_hz / kSpeedOfSoundMeterSeconds;
inst->mask_thresholds_[i] = inst->num_input_channels_ * inst->num_input_channels_ *
kBeamwidthConstant * inst->wave_numbers_[i] *
inst->wave_numbers_[i];
}
for (size_t i = 0; i < kNumFreqBins; ++i) {
memset(&(inst->delay_sum_masks_[i]), 0, sizeof(MouseBF_complex_matrix_f));
//memset(&(inst->normalized_delay_sum_masks_[i]), 0, sizeof(MouseBF_complex_matrix_f));
memset(&(inst->target_cov_mats_[i]), 0, sizeof(MouseBF_complex_matrix_f));
memset(&(inst->uniform_cov_mat_[i]), 0, sizeof(MouseBF_complex_matrix_f));
for (size_t j = 0; j < kInterfNum; ++j) {
memset(&(inst->interf_cov_mats_[i][j]), 0, sizeof(MouseBF_complex_matrix_f));
}
}
memset(&(inst->eig_m_), 0, sizeof(MouseBF_complex_matrix_f));
MouseBF_complex_matrix_init(&(inst->eig_m_), 1, inst->num_input_channels_);
MouseBF_InitLowFrequencyCorrectionRanges(inst);
MouseBF_InitDiffuseCovMats(inst);
target_direction.s[1] = 0.f;
target_direction.s[2] = 1.f;
MouseBF_nonlinear_reset(inst, target_direction);
return inst;
}
void MouseBF_nonlinear_reset(MouseBF_nonlinear *inst,
MouseBF_spherical_pointf target_direction) {
inst->target_angle_radians_ = target_direction.s[0];
MouseBF_InitHighFrequencyCorrectionRanges(inst);
MouseBF_InitInterfAngles(inst);
MouseBF_InitDelaySumMasks(inst);
MouseBF_InitTargetCovMats(inst);
MouseBF_InitInterfCovMats(inst);
MouseBF_NormalizeCovMats(inst);
}
void MouseBF_nonlinear_process_block(MouseBF_nonlinear *inst,
MouseBF_complex_f** input,
int num_input_channels,
size_t num_freq_bins,
int num_output_channels,
MouseBF_complex_f** output) {
#ifdef _debug_
FILE *fp = fopen("input.txt", "a");
fprintf(fp, "[");
for (int i = 0; i < num_input_channels; i++) {
for (int j = 0; j < num_freq_bins; j++) {
char flag[2];
if (input[i][j].imag_ > 0) {
strcpy(flag,"+");
} else {
strcpy(flag,"");
}
if (i == 0 && j == 0) {
fprintf(fp, "%f%s%fi", input[i][j].real_, flag, input[i][j].imag_);
} else {
fprintf(fp, " %f%s%fi", input[i][j].real_, flag, input[i][j].imag_);
}
}
}
fprintf(fp, "]\n");
fclose(fp);
#endif
// Calculating the post-filter masks. Note that we need two for each
// frequency bin to account for the positive and negative interferer
// angle.
for (size_t i = inst->low_mean_start_bin_; i <= inst->high_mean_end_bin_; ++i) {
MouseBF_complex_matrix_copy_from_column(&(inst->eig_m_), i, input);
float eig_m_norm_factor = sqrt(MouseBF_SumSquares(&(inst->eig_m_)));
if (eig_m_norm_factor != 0.f) {
MouseBF_complex_f a;
a.real_ = 1.0 / eig_m_norm_factor;
a.imag_ = 0.0;
MouseBF_complex_matrix_scale(&(inst->eig_m_), a);
}
//_// zoro_ditch
//_// float rxim = MouseBF_Norm(&(inst->target_cov_mats_[i]), &(inst->eig_m_));
//_// float ratio_rxiw_rxim = 0.f;
//_// if (rxim > 0.f) {
//_// ratio_rxiw_rxim = inst->rxiws_[i] / rxim;
//_// }
//_// MouseBF_complex_f rmw = MouseBF_ConjugateDotProduct(&(inst->delay_sum_masks_[i]), &(inst->eig_m_));
//_// //abs
//_// rmw.real_ = sqrt(rmw.real_ * rmw.real_ + rmw.imag_ * rmw.imag_);
//_// rmw.imag_ = 0.0;
//_// rmw = MouseBF_complex_mul(rmw, rmw);
//_// float rmw_r = rmw.real_;
//_// inst->new_mask_[i] = MouseBF_CalculatePostfilterMask(inst, &(inst->interf_cov_mats_[i][0]),
//_// inst->rpsiws_[i][0],
//_// ratio_rxiw_rxim,
//_// rmw_r,
//_// inst->mask_thresholds_[i]);
//
//zoro_update
const float kMaskMinimum = 0.01f;
float rxim = MouseBF_Norm(&(inst->target_cov_mats_[i]), &(inst->eig_m_)); // P_x_target
float rpsim = MouseBF_Norm(&(inst->interf_cov_mats_[i][0]), &(inst->eig_m_)); // P_x_gamma
float ratio_rpsim_rpsiwm = rpsim/inst->rpsiws_[i][0];
float mask = 1;
if(ratio_rpsim_rpsiwm-rxim > inst->mask_thresholds_[i]*rxim*ratio_rpsim_rpsiwm){
mask = (1-rxim*ratio_rpsim_rpsiwm)/(rxim*rxim-rxim*ratio_rpsim_rpsiwm);
}
if(mask < kMaskMinimum){
mask = kMaskMinimum;
}
inst->new_mask_[i] = mask;
for (int j = 1; j < kInterfNum; j++) {
//_// zoro_update
ratio_rpsim_rpsiwm = rpsim/inst->rpsiws_[i][j];
mask = 1;
if(ratio_rpsim_rpsiwm-rxim > inst->mask_thresholds_[i]*rxim*ratio_rpsim_rpsiwm){
mask = (1-rxim*ratio_rpsim_rpsiwm)/(rxim*rxim-rxim*ratio_rpsim_rpsiwm);
}
if(mask < kMaskMinimum){
mask = kMaskMinimum;
}
inst->new_mask_[i] *= mask;
//_// inst->new_mask_[i] *= MouseBF_CalculatePostfilterMask(inst, &(inst->interf_cov_mats_[i][j]),
//_// inst->rpsiws_[i][j],
//_// ratio_rxiw_rxim,
//_// rmw_r,
//_// inst->mask_thresholds_[i]);
}
}
MouseBF_ApplyMaskTimeSmoothing(inst);
//MouseBF_EstimateTargetPresence(inst);
MouseBF_ApplyLowFrequencyCorrection(inst);
MouseBF_ApplyHighFrequencyCorrection(inst);
MouseBF_ApplyMaskFrequencySmoothing(inst);
MouseBF_ApplyMasks(inst, input, output);
}
void MouseBF_nonlinear_destroy(MouseBF_nonlinear *inst) {
free(inst->array_geometry_);
free(inst->array_normal_);
for (size_t i = 0; i < kNumFreqBins; ++i) {
MouseBF_complex_matrix_free(&(inst->delay_sum_masks_[i]));
//MouseBF_complex_matrix_free(&(inst->normalized_delay_sum_masks_[i]));
MouseBF_complex_matrix_free(&(inst->target_cov_mats_[i]));
MouseBF_complex_matrix_free(&(inst->uniform_cov_mat_[i]));
for (size_t j = 0; j < kInterfNum; ++j) {
MouseBF_complex_matrix_free(&(inst->interf_cov_mats_[i][j]));
}
}
MouseBF_complex_matrix_free(&(inst->eig_m_));
free(inst);
}
（13）、src/bf-types.h
/*
*@author : aflyingwolf
*@date : 2025.4.10
*@file : bf-types.h
* */
#ifndef __BF_TYPES_H__
#define __BF_TYPES_H__
#define M_PI 3.14159265358979323846264338327950288
#define kFftSize 256
#define kNumFreqBins (kFftSize / 2 + 1)
#define kMaskTimeSmoothAlpha 0.2
#define kMaskFrequencySmoothAlpha 0.6
#define kCompensationGain 0.2
#define kSpeedOfSoundMeterSeconds 343.0
#define kBalance 0.90
#define kBeamwidthConstant 0.00002
#define kInterfNum 4
#define kMaxDotProduct 1e-6f
#define kLowMeanStartHz 125
#define kLowMeanEndHz 400
#define kSampleRate 16000
typedef struct MouseBF_spherical_pointf_ {
float s[3]; //azimuth, elevation, radius
} MouseBF_spherical_pointf;
typedef struct MouseBF_point_ {
float c[3]; // x,y,z
} MouseBF_point;
#endif
（14）、src/CMakeLists.txt
set(SOURCES
ring_buffer.c
fft4g.c
bf-fft.c
bf-blocker.c
bf-api.c
bf-complex.c
bf-matrix.c
bf-nonlinear.c
bessel0.c
)
add_library("${NAME}" STATIC ${SOURCES})
（15）、bin/test-api.c
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include "bf-api.h"
#define NUM_FRAMES 160
int main(int argc, char* argv[]) {
if (argc < 3) {
printf("please input in.pcm out.pcm mic_positions angles(40) mic_num(6)\n");
return -1;
}
const char *i = argv[1];
const char *o = argv[2];
const char *mic_positions = argv[3];
float angles = atof(argv[4]);
int mic_num = atoi(argv[5]);
void *handle = api_create_bf_handle(mic_positions, mic_num, angles);
FILE *fp_r = fopen(i, "rb");
FILE *fp_w = fopen(o, "wb");
api_start_bf(handle);
short *input_data = (short*)malloc(sizeof(short) * mic_num * NUM_FRAMES);
float *inp_float = (float*)malloc(sizeof(float) * mic_num * NUM_FRAMES);
short *out_data = (short*)malloc(sizeof(short) * NUM_FRAMES);
float *out_float = (float*)malloc(sizeof(float) * NUM_FRAMES);
while (fread(input_data, sizeof(short), mic_num*NUM_FRAMES, fp_r) == mic_num*NUM_FRAMES) {
for (int i = 0; i < mic_num*NUM_FRAMES; i++) {
inp_float[i] = input_data[i];
}
api_process_bf(handle, inp_float, out_float, NUM_FRAMES);
for (int i = 0; i < NUM_FRAMES; i++) {
out_data[i] = out_float[i];
}
fwrite(out_data, sizeof(short), NUM_FRAMES, fp_w);
}
api_end_bf(handle);
fclose(fp_r);
fclose(fp_w);
api_destroy_bf_handle(handle);
free(input_data);
free(inp_float);
free(out_data);
free(out_float);
return 0;
}
（16）、bin/CMakeLists.txt
add_executable(test-api test-api.c)
target_link_libraries(test-api ${NAME} -lm)
（17）、CMakeLists.txt
cmake_minimum_required (VERSION 2.6)
project (mouse-abf)
#SET(CMAKE_BUILD_TYPE "DEBUG")
SET(CMAKE_BUILD_TYPE "RELEASE")
SET(CMAKE_C_FLAGS_DEBUG "$ENV{CFLAGS} -g")
SET(CMAKE_C_FLAGS_RELEASE "$ENV{CFLAGS} -O3")
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
add_compile_options(-Wall)
add_compile_options(-Wno-sign-compare)
add_compile_options(-Wno-unused-local-typedefs)
add_compile_options(-Wno-deprecated-declarations)
add_compile_options(-fPIC)
add_compile_options(-Wno-deprecated)
add_compile_options(-Wwrite-strings)
add_compile_options(-Wno-sign-compare -Wno-unused-local-typedefs -Winit-self)
SET(BUILD_PATH ${CMAKE_BINARY_DIR})
SET(NAME "mouse-abf")
MESSAGE(STATUS "NAME:${NAME}")
link_directories(${LIB_PATH} ${CMAKE_BINARY_DIR}/src)
include_directories("${CMAKE_SOURCE_DIR}/src")
add_subdirectory(src)
add_subdirectory(bin)
2.4 demo
三、结果展示
3.1 0度方向波束结果
3.2 180度方向波束结果
四、总结
本节我们完成了非线性波束形成算法的工程实现，可以作为单独的波束形成系统来使用。完整代码可以进入https://t.zsxq.com/qgmoN 获取。
中国智能体开发者社区，聚焦智能体与大模型开发，提供前沿资讯、实用工具链、开源项目及行业案例。通过技术沙龙、开发者大赛等活动，促进经验交流与协作，助力开发者快速构建创新智能应用。
更多推荐
12
0- 0
已为社区贡献10条内容
所有评论(0)