asterisk/codecs/ilbc/iCBSearch.c

504 lines
14 KiB
C

/******************************************************************
iLBC Speech Coder ANSI-C Source Code
iCBSearch.c
Copyright (C) The Internet Society (2004).
All Rights Reserved.
******************************************************************/
#include <math.h>
#include <string.h>
#include "iLBC_define.h"
#include "gainquant.h"
#include "createCB.h"
#include "filter.h"
#include "constants.h"
/*----------------------------------------------------------------*
* Search routine for codebook encoding and gain quantization.
*---------------------------------------------------------------*/
void iCBSearch(
iLBC_Enc_Inst_t *iLBCenc_inst,
/* (i) the encoder state structure */
int *index, /* (o) Codebook indices */
int *gain_index,/* (o) Gain quantization indices */
float *intarget,/* (i) Target vector for encoding */
float *mem, /* (i) Buffer for codebook construction */
int lMem, /* (i) Length of buffer */
int lTarget, /* (i) Length of vector */
int nStages, /* (i) Number of codebook stages */
float *weightDenum, /* (i) weighting filter coefficients */
float *weightState, /* (i) weighting filter state */
int block /* (i) the sub-block number */
){
int i, j, icount, stage, best_index, range, counter;
float max_measure, gain, measure, crossDot, ftmp;
float gains[CB_NSTAGES];
float target[SUBL];
int base_index, sInd, eInd, base_size;
int sIndAug=0, eIndAug=0;
float buf[CB_MEML+SUBL+2*LPC_FILTERORDER];
float invenergy[CB_EXPAND*128], energy[CB_EXPAND*128];
float *pp, *ppi=0, *ppo=0, *ppe=0;
float cbvectors[CB_MEML];
float tene, cene, cvec[SUBL];
float aug_vec[SUBL];
memset(cvec,0,SUBL*sizeof(float));
/* Determine size of codebook sections */
base_size=lMem-lTarget+1;
if (lTarget==SUBL) {
base_size=lMem-lTarget+1+lTarget/2;
}
/* setup buffer for weighting */
memcpy(buf,weightState,sizeof(float)*LPC_FILTERORDER);
memcpy(buf+LPC_FILTERORDER,mem,lMem*sizeof(float));
memcpy(buf+LPC_FILTERORDER+lMem,intarget,lTarget*sizeof(float));
/* weighting */
AllPoleFilter(buf+LPC_FILTERORDER, weightDenum,
lMem+lTarget, LPC_FILTERORDER);
/* Construct the codebook and target needed */
memcpy(target, buf+LPC_FILTERORDER+lMem, lTarget*sizeof(float));
tene=0.0;
for (i=0; i<lTarget; i++) {
tene+=target[i]*target[i];
}
/* Prepare search over one more codebook section. This section
is created by filtering the original buffer with a filter. */
filteredCBvecs(cbvectors, buf+LPC_FILTERORDER, lMem);
/* The Main Loop over stages */
for (stage=0; stage<nStages; stage++) {
range = search_rangeTbl[block][stage];
/* initialize search measure */
max_measure = (float)-10000000.0;
gain = (float)0.0;
best_index = 0;
/* Compute cross dot product between the target
and the CB memory */
crossDot=0.0;
pp=buf+LPC_FILTERORDER+lMem-lTarget;
for (j=0; j<lTarget; j++) {
crossDot += target[j]*(*pp++);
}
if (stage==0) {
/* Calculate energy in the first block of
'lTarget' samples. */
ppe = energy;
ppi = buf+LPC_FILTERORDER+lMem-lTarget-1;
ppo = buf+LPC_FILTERORDER+lMem-1;
*ppe=0.0;
pp=buf+LPC_FILTERORDER+lMem-lTarget;
for (j=0; j<lTarget; j++, pp++) {
*ppe+=(*pp)*(*pp);
}
if (*ppe>0.0) {
invenergy[0] = (float) 1.0 / (*ppe + EPS);
} else {
invenergy[0] = (float) 0.0;
}
ppe++;
measure=(float)-10000000.0;
if (crossDot > 0.0) {
measure = crossDot*crossDot*invenergy[0];
}
}
else {
measure = crossDot*crossDot*invenergy[0];
}
/* check if measure is better */
ftmp = crossDot*invenergy[0];
if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)) {
best_index = 0;
max_measure = measure;
gain = ftmp;
}
/* loop over the main first codebook section,
full search */
for (icount=1; icount<range; icount++) {
/* calculate measure */
crossDot=0.0;
pp = buf+LPC_FILTERORDER+lMem-lTarget-icount;
for (j=0; j<lTarget; j++) {
crossDot += target[j]*(*pp++);
}
if (stage==0) {
*ppe++ = energy[icount-1] + (*ppi)*(*ppi) -
(*ppo)*(*ppo);
ppo--;
ppi--;
if (energy[icount]>0.0) {
invenergy[icount] =
(float)1.0/(energy[icount]+EPS);
} else {
invenergy[icount] = (float) 0.0;
}
measure=(float)-10000000.0;
if (crossDot > 0.0) {
measure = crossDot*crossDot*invenergy[icount];
}
}
else {
measure = crossDot*crossDot*invenergy[icount];
}
/* check if measure is better */
ftmp = crossDot*invenergy[icount];
if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)) {
best_index = icount;
max_measure = measure;
gain = ftmp;
}
}
/* Loop over augmented part in the first codebook
* section, full search.
* The vectors are interpolated.
*/
if (lTarget==SUBL) {
/* Search for best possible cb vector and
compute the CB-vectors' energy. */
searchAugmentedCB(20, 39, stage, base_size-lTarget/2,
target, buf+LPC_FILTERORDER+lMem,
&max_measure, &best_index, &gain, energy,
invenergy);
}
/* set search range for following codebook sections */
base_index=best_index;
/* unrestricted search */
if (CB_RESRANGE == -1) {
sInd=0;
eInd=range-1;
sIndAug=20;
eIndAug=39;
}
/* restricted search around best index from first
codebook section */
else {
/* Initialize search indices */
sIndAug=0;
eIndAug=0;
sInd=base_index-CB_RESRANGE/2;
eInd=sInd+CB_RESRANGE;
if (lTarget==SUBL) {
if (sInd<0) {
sIndAug = 40 + sInd;
eIndAug = 39;
sInd=0;
} else if ( base_index < (base_size-20) ) {
if (eInd > range) {
sInd -= (eInd-range);
eInd = range;
}
} else { /* base_index >= (base_size-20) */
if (sInd < (base_size-20)) {
sIndAug = 20;
sInd = 0;
eInd = 0;
eIndAug = 19 + CB_RESRANGE;
if(eIndAug > 39) {
eInd = eIndAug-39;
eIndAug = 39;
}
} else {
sIndAug = 20 + sInd - (base_size-20);
eIndAug = 39;
sInd = 0;
eInd = CB_RESRANGE - (eIndAug-sIndAug+1);
}
}
} else { /* lTarget = 22 or 23 */
if (sInd < 0) {
eInd -= sInd;
sInd = 0;
}
if(eInd > range) {
sInd -= (eInd - range);
eInd = range;
}
}
}
/* search of higher codebook section */
/* index search range */
counter = sInd;
sInd += base_size;
eInd += base_size;
if (stage==0) {
ppe = energy+base_size;
*ppe=0.0;
pp=cbvectors+lMem-lTarget;
for (j=0; j<lTarget; j++, pp++) {
*ppe+=(*pp)*(*pp);
}
ppi = cbvectors + lMem - 1 - lTarget;
ppo = cbvectors + lMem - 1;
for (j=0; j<(range-1); j++) {
*(ppe+1) = *ppe + (*ppi)*(*ppi) - (*ppo)*(*ppo);
ppo--;
ppi--;
ppe++;
}
}
/* loop over search range */
for (icount=sInd; icount<eInd; icount++) {
/* calculate measure */
crossDot=0.0;
pp=cbvectors + lMem - (counter++) - lTarget;
for (j=0;j<lTarget;j++) {
crossDot += target[j]*(*pp++);
}
if (energy[icount]>0.0) {
invenergy[icount] =(float)1.0/(energy[icount]+EPS);
} else {
invenergy[icount] =(float)0.0;
}
if (stage==0) {
measure=(float)-10000000.0;
if (crossDot > 0.0) {
measure = crossDot*crossDot*
invenergy[icount];
}
}
else {
measure = crossDot*crossDot*invenergy[icount];
}
/* check if measure is better */
ftmp = crossDot*invenergy[icount];
if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)) {
best_index = icount;
max_measure = measure;
gain = ftmp;
}
}
/* Search the augmented CB inside the limited range. */
if ((lTarget==SUBL)&&(sIndAug!=0)) {
searchAugmentedCB(sIndAug, eIndAug, stage,
2*base_size-20, target, cbvectors+lMem,
&max_measure, &best_index, &gain, energy,
invenergy);
}
/* record best index */
index[stage] = best_index;
/* gain quantization */
if (stage==0){
if (gain<0.0){
gain = 0.0;
}
if (gain>CB_MAXGAIN) {
gain = (float)CB_MAXGAIN;
}
gain = gainquant(gain, 1.0, 32, &gain_index[stage]);
}
else {
if (stage==1) {
gain = gainquant(gain, (float)fabs(gains[stage-1]),
16, &gain_index[stage]);
} else {
gain = gainquant(gain, (float)fabs(gains[stage-1]),
8, &gain_index[stage]);
}
}
/* Extract the best (according to measure)
codebook vector */
if (lTarget==(STATE_LEN-iLBCenc_inst->state_short_len)) {
if (index[stage]<base_size) {
pp=buf+LPC_FILTERORDER+lMem-lTarget-index[stage];
} else {
pp=cbvectors+lMem-lTarget-
index[stage]+base_size;
}
} else {
if (index[stage]<base_size) {
if (index[stage]<(base_size-20)) {
pp=buf+LPC_FILTERORDER+lMem-
lTarget-index[stage];
} else {
createAugmentedVec(index[stage]-base_size+40,
buf+LPC_FILTERORDER+lMem,aug_vec);
pp=aug_vec;
}
} else {
int filterno, position;
filterno=index[stage]/base_size;
position=index[stage]-filterno*base_size;
if (position<(base_size-20)) {
pp=cbvectors+filterno*lMem-lTarget-
index[stage]+filterno*base_size;
} else {
createAugmentedVec(
index[stage]-(filterno+1)*base_size+40,
cbvectors+filterno*lMem,aug_vec);
pp=aug_vec;
}
}
}
/* Subtract the best codebook vector, according
to measure, from the target vector */
for (j=0;j<lTarget;j++) {
cvec[j] += gain*(*pp);
target[j] -= gain*(*pp++);
}
/* record quantized gain */
gains[stage]=gain;
}/* end of Main Loop. for (stage=0;... */
/* Gain adjustment for energy matching */
cene=0.0;
for (i=0; i<lTarget; i++) {
cene+=cvec[i]*cvec[i];
}
j=gain_index[0];
for (i=gain_index[0]; i<32; i++) {
ftmp=cene*gain_sq5Tbl[i]*gain_sq5Tbl[i];
if ((ftmp<(tene*gains[0]*gains[0])) &&
(gain_sq5Tbl[j]<(2.0*gains[0]))) {
j=i;
}
}
gain_index[0]=j;
}