Paul B Mahol
5 years ago
parent
aaac48fb50
commit
93414ce831
6 changed files with 415 additions and 1 deletions
@ -0,0 +1,378 @@ |
||||
/*
|
||||
* Copyright (c) 2019 Paul B Mahol |
||||
* |
||||
* This file is part of FFmpeg. |
||||
* |
||||
* FFmpeg is free software; you can redistribute it and/or |
||||
* modify it under the terms of the GNU Lesser General Public |
||||
* License as published by the Free Software Foundation; either |
||||
* version 2.1 of the License, or (at your option) any later version. |
||||
* |
||||
* FFmpeg is distributed in the hope that it will be useful, |
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
||||
* Lesser General Public License for more details. |
||||
* |
||||
* You should have received a copy of the GNU Lesser General Public |
||||
* License along with FFmpeg; if not, write to the Free Software |
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA |
||||
*/ |
||||
|
||||
#include "libavutil/avassert.h" |
||||
#include "libavutil/audio_fifo.h" |
||||
#include "libavutil/channel_layout.h" |
||||
#include "libavutil/common.h" |
||||
#include "libavutil/opt.h" |
||||
|
||||
#include "audio.h" |
||||
#include "avfilter.h" |
||||
#include "formats.h" |
||||
#include "filters.h" |
||||
#include "internal.h" |
||||
|
||||
typedef struct AudioXCorrelateContext { |
||||
const AVClass *class; |
||||
|
||||
int size; |
||||
int algo; |
||||
int64_t pts; |
||||
|
||||
AVAudioFifo *fifo[2]; |
||||
AVFrame *cache[2]; |
||||
AVFrame *mean_sum[2]; |
||||
AVFrame *num_sum; |
||||
AVFrame *den_sum[2]; |
||||
int used; |
||||
|
||||
int (*xcorrelate)(AVFilterContext *ctx, AVFrame *out); |
||||
} AudioXCorrelateContext; |
||||
|
||||
static int query_formats(AVFilterContext *ctx) |
||||
{ |
||||
AVFilterFormats *formats; |
||||
AVFilterChannelLayouts *layouts; |
||||
static const enum AVSampleFormat sample_fmts[] = { |
||||
AV_SAMPLE_FMT_FLTP, |
||||
AV_SAMPLE_FMT_NONE |
||||
}; |
||||
int ret; |
||||
|
||||
layouts = ff_all_channel_counts(); |
||||
if (!layouts) |
||||
return AVERROR(ENOMEM); |
||||
ret = ff_set_common_channel_layouts(ctx, layouts); |
||||
if (ret < 0) |
||||
return ret; |
||||
|
||||
formats = ff_make_format_list(sample_fmts); |
||||
if (!formats) |
||||
return AVERROR(ENOMEM); |
||||
ret = ff_set_common_formats(ctx, formats); |
||||
if (ret < 0) |
||||
return ret; |
||||
|
||||
formats = ff_all_samplerates(); |
||||
if (!formats) |
||||
return AVERROR(ENOMEM); |
||||
return ff_set_common_samplerates(ctx, formats); |
||||
} |
||||
|
||||
static float mean_sum(const float *in, int size) |
||||
{ |
||||
float mean_sum = 0.f; |
||||
|
||||
for (int i = 0; i < size; i++) |
||||
mean_sum += in[i]; |
||||
|
||||
return mean_sum; |
||||
} |
||||
|
||||
static float square_sum(const float *x, const float *y, int size) |
||||
{ |
||||
float square_sum = 0.f; |
||||
|
||||
for (int i = 0; i < size; i++) |
||||
square_sum += x[i] * y[i]; |
||||
|
||||
return square_sum; |
||||
} |
||||
|
||||
static float xcorrelate(const float *x, const float *y, float sumx, float sumy, int size) |
||||
{ |
||||
const float xm = sumx / size, ym = sumy / size; |
||||
float num = 0.f, den, den0 = 0.f, den1 = 0.f; |
||||
|
||||
for (int i = 0; i < size; i++) { |
||||
float xd = x[i] - xm; |
||||
float yd = y[i] - ym; |
||||
|
||||
num += xd * yd; |
||||
den0 += xd * xd; |
||||
den1 += yd * yd; |
||||
} |
||||
|
||||
num /= size; |
||||
den = sqrtf((den0 * den1) / (size * size)); |
||||
|
||||
return den <= 1e-6f ? 0.f : num / den; |
||||
} |
||||
|
||||
static int xcorrelate_slow(AVFilterContext *ctx, AVFrame *out) |
||||
{ |
||||
AudioXCorrelateContext *s = ctx->priv; |
||||
const int size = s->size; |
||||
int used; |
||||
|
||||
for (int ch = 0; ch < out->channels; ch++) { |
||||
const float *x = (const float *)s->cache[0]->extended_data[ch]; |
||||
const float *y = (const float *)s->cache[1]->extended_data[ch]; |
||||
float *sumx = (float *)s->mean_sum[0]->extended_data[ch]; |
||||
float *sumy = (float *)s->mean_sum[1]->extended_data[ch]; |
||||
float *dst = (float *)out->extended_data[ch]; |
||||
|
||||
used = s->used; |
||||
if (!used) { |
||||
sumx[0] = mean_sum(x, size); |
||||
sumy[0] = mean_sum(y, size); |
||||
used = 1; |
||||
} |
||||
|
||||
for (int n = 0; n < out->nb_samples; n++) { |
||||
dst[n] = xcorrelate(x + n, y + n, sumx[0], sumy[0], size); |
||||
|
||||
sumx[0] -= x[n]; |
||||
sumx[0] += x[n + size]; |
||||
sumy[0] -= y[n]; |
||||
sumy[0] += y[n + size]; |
||||
} |
||||
} |
||||
|
||||
return used; |
||||
} |
||||
|
||||
static int xcorrelate_fast(AVFilterContext *ctx, AVFrame *out) |
||||
{ |
||||
AudioXCorrelateContext *s = ctx->priv; |
||||
const int size = s->size; |
||||
int used; |
||||
|
||||
for (int ch = 0; ch < out->channels; ch++) { |
||||
const float *x = (const float *)s->cache[0]->extended_data[ch]; |
||||
const float *y = (const float *)s->cache[1]->extended_data[ch]; |
||||
float *num_sum = (float *)s->num_sum->extended_data[ch]; |
||||
float *den_sumx = (float *)s->den_sum[0]->extended_data[ch]; |
||||
float *den_sumy = (float *)s->den_sum[1]->extended_data[ch]; |
||||
float *dst = (float *)out->extended_data[ch]; |
||||
|
||||
used = s->used; |
||||
if (!used) { |
||||
num_sum[0] = square_sum(x, y, size); |
||||
den_sumx[0] = square_sum(x, x, size); |
||||
den_sumy[0] = square_sum(y, y, size); |
||||
used = 1; |
||||
} |
||||
|
||||
for (int n = 0; n < out->nb_samples; n++) { |
||||
float num, den; |
||||
|
||||
num = num_sum[0] / size; |
||||
den = sqrtf((den_sumx[0] * den_sumy[0]) / (size * size)); |
||||
|
||||
dst[n] = den <= 1e-6f ? 0.f : num / den; |
||||
|
||||
num_sum[0] -= x[n] * y[n]; |
||||
num_sum[0] += x[n + size] * y[n + size]; |
||||
den_sumx[0] -= x[n] * x[n]; |
||||
den_sumx[0] = FFMAX(den_sumx[0], 0.f); |
||||
den_sumx[0] += x[n + size] * x[n + size]; |
||||
den_sumy[0] -= y[n] * y[n]; |
||||
den_sumy[0] = FFMAX(den_sumy[0], 0.f); |
||||
den_sumy[0] += y[n + size] * y[n + size]; |
||||
} |
||||
} |
||||
|
||||
return used; |
||||
} |
||||
|
||||
static int activate(AVFilterContext *ctx) |
||||
{ |
||||
AudioXCorrelateContext *s = ctx->priv; |
||||
AVFrame *frame = NULL; |
||||
int ret, status; |
||||
int available; |
||||
int64_t pts; |
||||
|
||||
FF_FILTER_FORWARD_STATUS_BACK_ALL(ctx->outputs[0], ctx); |
||||
|
||||
for (int i = 0; i < 2; i++) { |
||||
ret = ff_inlink_consume_frame(ctx->inputs[i], &frame); |
||||
if (ret > 0) { |
||||
if (s->pts == AV_NOPTS_VALUE) |
||||
s->pts = frame->pts; |
||||
ret = av_audio_fifo_write(s->fifo[i], (void **)frame->extended_data, |
||||
frame->nb_samples); |
||||
av_frame_free(&frame); |
||||
if (ret < 0) |
||||
return ret; |
||||
} |
||||
} |
||||
|
||||
available = FFMIN(av_audio_fifo_size(s->fifo[0]), av_audio_fifo_size(s->fifo[1])); |
||||
if (available > s->size) { |
||||
const int out_samples = available - s->size; |
||||
AVFrame *out; |
||||
|
||||
if (!s->cache[0] || s->cache[0]->nb_samples < available) { |
||||
av_frame_free(&s->cache[0]); |
||||
s->cache[0] = ff_get_audio_buffer(ctx->outputs[0], available); |
||||
if (!s->cache[0]) |
||||
return AVERROR(ENOMEM); |
||||
} |
||||
|
||||
if (!s->cache[1] || s->cache[1]->nb_samples < available) { |
||||
av_frame_free(&s->cache[1]); |
||||
s->cache[1] = ff_get_audio_buffer(ctx->outputs[0], available); |
||||
if (!s->cache[1]) |
||||
return AVERROR(ENOMEM); |
||||
} |
||||
|
||||
ret = av_audio_fifo_peek(s->fifo[0], (void **)s->cache[0]->extended_data, available); |
||||
if (ret < 0) |
||||
return ret;; |
||||
|
||||
ret = av_audio_fifo_peek(s->fifo[1], (void **)s->cache[1]->extended_data, available); |
||||
if (ret < 0) |
||||
return ret;; |
||||
|
||||
out = ff_get_audio_buffer(ctx->outputs[0], out_samples); |
||||
if (!out) |
||||
return AVERROR(ENOMEM); |
||||
|
||||
s->used = s->xcorrelate(ctx, out); |
||||
|
||||
out->pts = s->pts; |
||||
s->pts += out_samples; |
||||
|
||||
av_audio_fifo_drain(s->fifo[0], out_samples); |
||||
av_audio_fifo_drain(s->fifo[1], out_samples); |
||||
|
||||
return ff_filter_frame(ctx->outputs[0], out); |
||||
} |
||||
|
||||
if (av_audio_fifo_size(s->fifo[0]) > s->size && |
||||
av_audio_fifo_size(s->fifo[1]) > s->size) { |
||||
ff_filter_set_ready(ctx, 10); |
||||
return 0; |
||||
} |
||||
|
||||
for (int i = 0; i < 2; i++) { |
||||
if (ff_inlink_acknowledge_status(ctx->inputs[i], &status, &pts)) { |
||||
ff_outlink_set_status(ctx->outputs[0], status, pts); |
||||
return 0; |
||||
} |
||||
} |
||||
|
||||
if (ff_outlink_frame_wanted(ctx->outputs[0])) { |
||||
for (int i = 0; i < 2; i++) { |
||||
if (av_audio_fifo_size(s->fifo[i]) > s->size) |
||||
continue; |
||||
ff_inlink_request_frame(ctx->inputs[i]); |
||||
return 0; |
||||
} |
||||
} |
||||
|
||||
return FFERROR_NOT_READY; |
||||
} |
||||
|
||||
static int config_output(AVFilterLink *outlink) |
||||
{ |
||||
AVFilterContext *ctx = outlink->src; |
||||
AVFilterLink *inlink = ctx->inputs[0]; |
||||
AudioXCorrelateContext *s = ctx->priv; |
||||
|
||||
s->pts = AV_NOPTS_VALUE; |
||||
|
||||
outlink->format = inlink->format; |
||||
outlink->channels = inlink->channels; |
||||
s->fifo[0] = av_audio_fifo_alloc(outlink->format, outlink->channels, s->size); |
||||
s->fifo[1] = av_audio_fifo_alloc(outlink->format, outlink->channels, s->size); |
||||
if (!s->fifo[0] || !s->fifo[1]) |
||||
return AVERROR(ENOMEM); |
||||
|
||||
s->mean_sum[0] = ff_get_audio_buffer(outlink, 1); |
||||
s->mean_sum[1] = ff_get_audio_buffer(outlink, 1); |
||||
s->num_sum = ff_get_audio_buffer(outlink, 1); |
||||
s->den_sum[0] = ff_get_audio_buffer(outlink, 1); |
||||
s->den_sum[1] = ff_get_audio_buffer(outlink, 1); |
||||
if (!s->mean_sum[0] || !s->mean_sum[1] || !s->num_sum || |
||||
!s->den_sum[0] || !s->den_sum[1]) |
||||
return AVERROR(ENOMEM); |
||||
|
||||
switch (s->algo) { |
||||
case 0: s->xcorrelate = xcorrelate_slow; break; |
||||
case 1: s->xcorrelate = xcorrelate_fast; break; |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
static av_cold void uninit(AVFilterContext *ctx) |
||||
{ |
||||
AudioXCorrelateContext *s = ctx->priv; |
||||
|
||||
av_audio_fifo_free(s->fifo[0]); |
||||
av_audio_fifo_free(s->fifo[1]); |
||||
av_frame_free(&s->cache[0]); |
||||
av_frame_free(&s->cache[1]); |
||||
av_frame_free(&s->mean_sum[0]); |
||||
av_frame_free(&s->mean_sum[1]); |
||||
av_frame_free(&s->num_sum); |
||||
av_frame_free(&s->den_sum[0]); |
||||
av_frame_free(&s->den_sum[1]); |
||||
} |
||||
|
||||
static const AVFilterPad inputs[] = { |
||||
{ |
||||
.name = "axcorrelate0", |
||||
.type = AVMEDIA_TYPE_AUDIO, |
||||
}, |
||||
{ |
||||
.name = "axcorrelate1", |
||||
.type = AVMEDIA_TYPE_AUDIO, |
||||
}, |
||||
{ NULL } |
||||
}; |
||||
|
||||
static const AVFilterPad outputs[] = { |
||||
{ |
||||
.name = "default", |
||||
.type = AVMEDIA_TYPE_AUDIO, |
||||
.config_props = config_output, |
||||
}, |
||||
{ NULL } |
||||
}; |
||||
|
||||
#define AF AV_OPT_FLAG_AUDIO_PARAM|AV_OPT_FLAG_FILTERING_PARAM |
||||
#define OFFSET(x) offsetof(AudioXCorrelateContext, x) |
||||
|
||||
static const AVOption axcorrelate_options[] = { |
||||
{ "size", "set segment size", OFFSET(size), AV_OPT_TYPE_INT, {.i64=256}, 2, 131072, AF }, |
||||
{ "algo", "set alghorithm", OFFSET(algo), AV_OPT_TYPE_INT, {.i64=0}, 0, 1, AF, "algo" }, |
||||
{ "slow", "slow algorithm", 0, AV_OPT_TYPE_CONST, {.i64=0}, 0, 0, AF, "algo" }, |
||||
{ "fast", "fast algorithm", 0, AV_OPT_TYPE_CONST, {.i64=1}, 0, 0, AF, "algo" }, |
||||
{ NULL } |
||||
}; |
||||
|
||||
AVFILTER_DEFINE_CLASS(axcorrelate); |
||||
|
||||
AVFilter ff_af_axcorrelate = { |
||||
.name = "axcorrelate", |
||||
.description = NULL_IF_CONFIG_SMALL("Cross-correlate two audio streams."), |
||||
.priv_size = sizeof(AudioXCorrelateContext), |
||||
.priv_class = &axcorrelate_class, |
||||
.query_formats = query_formats, |
||||
.activate = activate, |
||||
.uninit = uninit, |
||||
.inputs = inputs, |
||||
.outputs = outputs, |
||||
}; |
||||
Loading…
Reference in new issue