pesqdsp.c

pesq算法

/*****************************************************************************

Perceptual Evaluation of Speech Quality (PESQ)
ITU-T Draft Recommendation P.862.
Version 1.1 - 15 November 2000.

NOTICE

The Perceptual Evaluation of Speech Quality (PESQ) algorithm and the copyright
therein is the property of British Telecommunications plc and Royal KPN NV,
and is protected by UK, US and other patents.  Permission is granted to use
PESQ for the purpose of evaluation of ITU-T recommendation P.862.  Any other
use of this software or the PESQ algorithm requires a license, which may be
obtained from:

OPTICOM GmbH
Michael Keyhl, Am Weichselgarten 7, D- 91058 Erlangen, Germany
Phone: [phone]     Fax: [phone]     E-mail: [e-mail]

PsyTechnics Limited
Richard Reynolds, B54 Adastral Park, Ipswich IP5 3RE, UK
Phone: [phone] or [phone]    Fax: [phone]
E-mail: [e-mail]

Patent-only licences should be obtained from Opticom. PsyTechnics or Opticom
can provide licences, and further information, for other PESQ products.

Further information is also available from: www.pesq.org

By using this software you acknowledge that PESQ is protected by copyright
and by patents and is being made available to you for the purpose of
evaluation of ITU-T Recommendation P.862.

You must not use PESQ for any other purpose without first obtaining a written
license from British Telecommunications plc and Royal KPN NV, from their
agents listed above.

You must not disclose, reproduce or otherwise release PESQ to any third
party without the prior written permission of British Telecommunications plc
and Royal KPN NV.

Authors:
  Antony Rix (BT) <[e-mail]>
  Mike Hollier (BT)
  Andries Hekstra (KPN Research)
  John Beerends (KPN Research)

*****************************************************************************/

#include <math.h>
#include <stdio.h>
#include "pesq.h"
#include "dsp.h"

void DC_block( float * data, long Nsamples )
{
    float *p;
    long count;
    float facc = 0.0f;

    long ofs = SEARCHBUFFER * Downsample;

    p = data + ofs;
    for( count = (Nsamples - 2 * ofs); count > 0L; count-- )
        facc += *(p++);
    facc /= Nsamples;

    p = data + ofs;
    for( count = (Nsamples - 2 * ofs); count > 0L; count-- )
        *(p++) -= facc;

    p = data + ofs;
    for( count = 0L; count < Downsample; count++ )
       *(p++) *= (0.5f + count) / Downsample;

    p = data + Nsamples - ofs - 1L;
    for( count = 0L; count < Downsample; count++ )
       *(p--) *= (0.5f + count) / Downsample;
}

long InIIR_Nsos;
float *InIIR_Hsos;

void apply_filters( float * data, long Nsamples )
{
    IIRFilt( InIIR_Hsos, InIIR_Nsos, NULL,
             data, Nsamples + DATAPADDING_MSECS  * (Fs / 1000), NULL );
}

float interpolate (float    freq, 
                   double   filter_curve_db [][2],
                   int      number_of_points) {
    double  result;
    int     i;
    double  freqLow, freqHigh;
    double  curveLow, curveHigh;
    
    if (freq <= filter_curve_db [0][0]) {
        freqLow = filter_curve_db [0][0];
        curveLow = filter_curve_db [0][1];
        freqHigh = filter_curve_db [1][0];
        curveHigh = filter_curve_db [1][1];

        result = ((freq - freqLow) * curveHigh + (freqHigh - freq) * curveLow)/ (freqHigh - freqLow);
    
        return (float) result;
    }

    if (freq >= filter_curve_db [number_of_points-1][0]) {
        freqLow = filter_curve_db [number_of_points-2][0];
        curveLow = filter_curve_db [number_of_points-2][1];
        freqHigh = filter_curve_db [number_of_points-1][0];
        curveHigh = filter_curve_db [number_of_points-1][1];

        result = ((freq - freqLow) * curveHigh + (freqHigh - freq) * curveLow)/ (freqHigh - freqLow);
    
        return (float) result;
    }
        
    i = 1;
    freqHigh = filter_curve_db [i][0];
    while (freqHigh < freq) {
        i++;
        freqHigh = filter_curve_db [i][0];    
    }
    curveHigh = filter_curve_db [i][1];

    freqLow = filter_curve_db [i-1][0];
    curveLow = filter_curve_db [i-1][1];

    result = ((freq - freqLow) * curveHigh + (freqHigh - freq) * curveLow)/ (freqHigh - freqLow);

    return (float) result;
}       


void apply_filter ( float * data, long maxNsamples, int number_of_points, double filter_curve_db [][2] )
{ 
    long    n           = maxNsamples - 2 * SEARCHBUFFER * Downsample + DATAPADDING_MSECS  * (Fs / 1000);
    long    pow_of_2    = nextpow2 (n);
    float    *x            = (float *) safe_malloc ((pow_of_2 + 2) * sizeof (float));

    float    factorDb, factor;
    
    float   overallGainFilter = interpolate ((float) 1000, filter_curve_db, number_of_points); 
    float   freq_resolution;
    int        i;
    
    for (i = 0; i < pow_of_2 + 2; i++) {
        x [i] = 0;
    }

    for (i = 0; i < n; i++) {
        x [i] = data [i + SEARCHBUFFER * Downsample];    
    }

    RealFFT (x, pow_of_2);
    
    freq_resolution = (float) Fs / (float) pow_of_2;


    for (i = 0; i <= pow_of_2/2; i++) { 
        factorDb = interpolate (i * freq_resolution, filter_curve_db, number_of_points) - overallGainFilter;
        factor = (float) pow ((float) 10, factorDb / (float) 20); 

        x [2 * i] *= factor;       
        x [2 * i + 1] *= factor;   
    }

    RealIFFT (x, pow_of_2);

    for (i = 0; i < n; i++) {
        data [i + SEARCHBUFFER * Downsample] = x[i];    
    }

    safe_free (x);
}

void apply_VAD( SIGNAL_INFO * pinfo, float * data, float * VAD, float * logVAD )
{
    float g;
    float LevelThresh;
    float LevelNoise;
    float StDNoise;
    float LevelSig;
    float LevelMin;
    long  count;
    long  iteration;
    long  length;
    long  start;
    long  finish;
    long  Nwindows = (*pinfo).Nsamples / Downsample;

    for( count = 0L; count < Nwindows; count++ )
    {
        VAD[count] = 0.0f;
        for( iteration = 0L; iteration < Downsample; iteration++ )
        {
            g = data[count * Downsample + iteration];
            VAD[count] += (g * g);
        }
        VAD[count] /= Downsample;
    }

    LevelThresh = 0.0f;
    for( count = 0L; count < Nwindows; count++ )
        LevelThresh += VAD[count];
    LevelThresh /= Nwindows;

    LevelMin = 0.0f;
    for( count = 0L; count < Nwindows; count++ )
        if( VAD[count] > LevelMin )
            LevelMin = VAD[count];
    if( LevelMin > 0.0f )
        LevelMin *= 1.0e-4f;
    else
        LevelMin = 1.0f;
    
    for( count = 0L; count < Nwindows; count++ )
        if( VAD[count] < LevelMin )
            VAD[count] = LevelMin;

    for( iteration = 0L; iteration < 12L; iteration++ )
    {
        LevelNoise = 0.0f;
        StDNoise = 0.0f;
        length = 0L;
        for( count = 0L; count < Nwindows; count++ )
            if( VAD[count] <= LevelThresh )
            {
                LevelNoise += VAD[count];
                length++;
            }
        if( length > 0L )
        {
            LevelNoise /= length;
            for( count = 0L; count < Nwindows; count++ )
                if( VAD[count] <= LevelThresh )
                {
                    g = VAD[count] - LevelNoise;
                    StDNoise += g * g;
                }
            StDNoise = (float)sqrt(StDNoise / length);
        }

        LevelThresh = 1.001f * (LevelNoise + 2.0f * StDNoise);
    }

    LevelNoise = 0.0f;
    LevelSig = 0.0f;
    length = 0L;
    for( count = 0L; count < Nwindows; count++ )
    {
        if( VAD[count] > LevelThresh )
        {
            LevelSig += VAD[count];
            length++;
        }
        else
            LevelNoise += VAD[count];
    }
    if( length > 0L )
        LevelSig /= length;
    else
        LevelThresh = -1.0f;
    if( length < Nwindows )
        LevelNoise /= (Nwindows - length);
    else
        LevelNoise = 1.0f;

    for( count = 0L; count < Nwindows; count++ )
        if( VAD[count] <= LevelThresh )
            VAD[count] = -VAD[count];

    VAD[0] = -LevelMin;
    VAD[Nwindows-1] = -LevelMin;

    start = 0L;
    finish = 0L;
    for( count = 1; count < Nwindows; count++ )
    {
        if( (VAD[count] > 0.0f) && (VAD[count-1] <= 0.0f) )
            start = count;
        if( (VAD[count] <= 0.0f) && (VAD[count-1] > 0.0f) )
        {
            finish = count;
            if( (finish - start) <= MINSPEECHLGTH )
                for( iteration = start; iteration < finish; iteration++ )
                    VAD[iteration] = -VAD[iteration];
        }
    }

    if( LevelSig >= (LevelNoise * 1000.0f) )
    {
        for( count = 1; count < Nwindows; count++ )
        {
            if( (VAD[count] > 0.0f) && (VAD[count-1] <= 0.0f) )
                start = count;
            if( (VAD[count] <= 0.0f) && (VAD[count-1] > 0.0f) )
            {
                finish = count;
                g = 0.0f;
                for( iteration = start; iteration < finish; iteration++ )
                    g += VAD[iteration];
                if( g < 3.0f * LevelThresh * (finish - start) )
                    for( iteration = start; iteration < finish; iteration++ )
                        VAD[iteration] = -VAD[iteration];
            }
        }
    }

    start = 0L;
    finish = 0L;
    for( count = 1; count < Nwindows; count++ )
    {
        if( (VAD[count] > 0.0f) && (VAD[count-1] <= 0.0f) )
        {
            start = count;
            if( (finish > 0L) && ((start - finish) <= JOINSPEECHLGTH) )
                for( iteration = finish; iteration < start; iteration++ )
                    VAD[iteration] = LevelMin;
        }
        if( (VAD[count] <= 0.0f) && (VAD[count-1] > 0.0f) )
            finish = count;
    }

    start = 0L;
    for( count = 1; count < Nwindows; count++ )
    {
        if( (VAD[count] > 0.0f) && (VAD[count-1] <= 0.0f) )
            start = count;
    }