iCBSearch.c 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503
  1. /******************************************************************
  2. iLBC Speech Coder ANSI-C Source Code
  3. iCBSearch.c
  4. Copyright (C) The Internet Society (2004).
  5. All Rights Reserved.
  6. ******************************************************************/
  7. #include <math.h>
  8. #include <string.h>
  9. #include "iLBC_define.h"
  10. #include "gainquant.h"
  11. #include "createCB.h"
  12. #include "filter.h"
  13. #include "constants.h"
  14. /*----------------------------------------------------------------*
  15. * Search routine for codebook encoding and gain quantization.
  16. *---------------------------------------------------------------*/
  17. void iCBSearch(
  18. iLBC_Enc_Inst_t *iLBCenc_inst,
  19. /* (i) the encoder state structure */
  20. int *index, /* (o) Codebook indices */
  21. int *gain_index,/* (o) Gain quantization indices */
  22. float *intarget,/* (i) Target vector for encoding */
  23. float *mem, /* (i) Buffer for codebook construction */
  24. int lMem, /* (i) Length of buffer */
  25. int lTarget, /* (i) Length of vector */
  26. int nStages, /* (i) Number of codebook stages */
  27. float *weightDenum, /* (i) weighting filter coefficients */
  28. float *weightState, /* (i) weighting filter state */
  29. int block /* (i) the sub-block number */
  30. ){
  31. int i, j, icount, stage, best_index, range, counter;
  32. float max_measure, gain, measure, crossDot, ftmp;
  33. float gains[CB_NSTAGES];
  34. float target[SUBL];
  35. int base_index, sInd, eInd, base_size;
  36. int sIndAug=0, eIndAug=0;
  37. float buf[CB_MEML+SUBL+2*LPC_FILTERORDER];
  38. float invenergy[CB_EXPAND*128], energy[CB_EXPAND*128];
  39. float *pp, *ppi=0, *ppo=0, *ppe=0;
  40. float cbvectors[CB_MEML];
  41. float tene, cene, cvec[SUBL];
  42. float aug_vec[SUBL];
  43. memset(cvec,0,SUBL*sizeof(float));
  44. /* Determine size of codebook sections */
  45. base_size=lMem-lTarget+1;
  46. if (lTarget==SUBL) {
  47. base_size=lMem-lTarget+1+lTarget/2;
  48. }
  49. /* setup buffer for weighting */
  50. memcpy(buf,weightState,sizeof(float)*LPC_FILTERORDER);
  51. memcpy(buf+LPC_FILTERORDER,mem,lMem*sizeof(float));
  52. memcpy(buf+LPC_FILTERORDER+lMem,intarget,lTarget*sizeof(float));
  53. /* weighting */
  54. AllPoleFilter(buf+LPC_FILTERORDER, weightDenum,
  55. lMem+lTarget, LPC_FILTERORDER);
  56. /* Construct the codebook and target needed */
  57. memcpy(target, buf+LPC_FILTERORDER+lMem, lTarget*sizeof(float));
  58. tene=0.0;
  59. for (i=0; i<lTarget; i++) {
  60. tene+=target[i]*target[i];
  61. }
  62. /* Prepare search over one more codebook section. This section
  63. is created by filtering the original buffer with a filter. */
  64. filteredCBvecs(cbvectors, buf+LPC_FILTERORDER, lMem);
  65. /* The Main Loop over stages */
  66. for (stage=0; stage<nStages; stage++) {
  67. range = search_rangeTbl[block][stage];
  68. /* initialize search measure */
  69. max_measure = (float)-10000000.0;
  70. gain = (float)0.0;
  71. best_index = 0;
  72. /* Compute cross dot product between the target
  73. and the CB memory */
  74. crossDot=0.0;
  75. pp=buf+LPC_FILTERORDER+lMem-lTarget;
  76. for (j=0; j<lTarget; j++) {
  77. crossDot += target[j]*(*pp++);
  78. }
  79. if (stage==0) {
  80. /* Calculate energy in the first block of
  81. 'lTarget' samples. */
  82. ppe = energy;
  83. ppi = buf+LPC_FILTERORDER+lMem-lTarget-1;
  84. ppo = buf+LPC_FILTERORDER+lMem-1;
  85. *ppe=0.0;
  86. pp=buf+LPC_FILTERORDER+lMem-lTarget;
  87. for (j=0; j<lTarget; j++, pp++) {
  88. *ppe+=(*pp)*(*pp);
  89. }
  90. if (*ppe>0.0) {
  91. invenergy[0] = (float) 1.0 / (*ppe + EPS);
  92. } else {
  93. invenergy[0] = (float) 0.0;
  94. }
  95. ppe++;
  96. measure=(float)-10000000.0;
  97. if (crossDot > 0.0) {
  98. measure = crossDot*crossDot*invenergy[0];
  99. }
  100. }
  101. else {
  102. measure = crossDot*crossDot*invenergy[0];
  103. }
  104. /* check if measure is better */
  105. ftmp = crossDot*invenergy[0];
  106. if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)) {
  107. best_index = 0;
  108. max_measure = measure;
  109. gain = ftmp;
  110. }
  111. /* loop over the main first codebook section,
  112. full search */
  113. for (icount=1; icount<range; icount++) {
  114. /* calculate measure */
  115. crossDot=0.0;
  116. pp = buf+LPC_FILTERORDER+lMem-lTarget-icount;
  117. for (j=0; j<lTarget; j++) {
  118. crossDot += target[j]*(*pp++);
  119. }
  120. if (stage==0) {
  121. *ppe++ = energy[icount-1] + (*ppi)*(*ppi) -
  122. (*ppo)*(*ppo);
  123. ppo--;
  124. ppi--;
  125. if (energy[icount]>0.0) {
  126. invenergy[icount] =
  127. (float)1.0/(energy[icount]+EPS);
  128. } else {
  129. invenergy[icount] = (float) 0.0;
  130. }
  131. measure=(float)-10000000.0;
  132. if (crossDot > 0.0) {
  133. measure = crossDot*crossDot*invenergy[icount];
  134. }
  135. }
  136. else {
  137. measure = crossDot*crossDot*invenergy[icount];
  138. }
  139. /* check if measure is better */
  140. ftmp = crossDot*invenergy[icount];
  141. if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)) {
  142. best_index = icount;
  143. max_measure = measure;
  144. gain = ftmp;
  145. }
  146. }
  147. /* Loop over augmented part in the first codebook
  148. * section, full search.
  149. * The vectors are interpolated.
  150. */
  151. if (lTarget==SUBL) {
  152. /* Search for best possible cb vector and
  153. compute the CB-vectors' energy. */
  154. searchAugmentedCB(20, 39, stage, base_size-lTarget/2,
  155. target, buf+LPC_FILTERORDER+lMem,
  156. &max_measure, &best_index, &gain, energy,
  157. invenergy);
  158. }
  159. /* set search range for following codebook sections */
  160. base_index=best_index;
  161. /* unrestricted search */
  162. if (CB_RESRANGE == -1) {
  163. sInd=0;
  164. eInd=range-1;
  165. sIndAug=20;
  166. eIndAug=39;
  167. }
  168. /* restricted search around best index from first
  169. codebook section */
  170. else {
  171. /* Initialize search indices */
  172. sIndAug=0;
  173. eIndAug=0;
  174. sInd=base_index-CB_RESRANGE/2;
  175. eInd=sInd+CB_RESRANGE;
  176. if (lTarget==SUBL) {
  177. if (sInd<0) {
  178. sIndAug = 40 + sInd;
  179. eIndAug = 39;
  180. sInd=0;
  181. } else if ( base_index < (base_size-20) ) {
  182. if (eInd > range) {
  183. sInd -= (eInd-range);
  184. eInd = range;
  185. }
  186. } else { /* base_index >= (base_size-20) */
  187. if (sInd < (base_size-20)) {
  188. sIndAug = 20;
  189. sInd = 0;
  190. eInd = 0;
  191. eIndAug = 19 + CB_RESRANGE;
  192. if(eIndAug > 39) {
  193. eInd = eIndAug-39;
  194. eIndAug = 39;
  195. }
  196. } else {
  197. sIndAug = 20 + sInd - (base_size-20);
  198. eIndAug = 39;
  199. sInd = 0;
  200. eInd = CB_RESRANGE - (eIndAug-sIndAug+1);
  201. }
  202. }
  203. } else { /* lTarget = 22 or 23 */
  204. if (sInd < 0) {
  205. eInd -= sInd;
  206. sInd = 0;
  207. }
  208. if(eInd > range) {
  209. sInd -= (eInd - range);
  210. eInd = range;
  211. }
  212. }
  213. }
  214. /* search of higher codebook section */
  215. /* index search range */
  216. counter = sInd;
  217. sInd += base_size;
  218. eInd += base_size;
  219. if (stage==0) {
  220. ppe = energy+base_size;
  221. *ppe=0.0;
  222. pp=cbvectors+lMem-lTarget;
  223. for (j=0; j<lTarget; j++, pp++) {
  224. *ppe+=(*pp)*(*pp);
  225. }
  226. ppi = cbvectors + lMem - 1 - lTarget;
  227. ppo = cbvectors + lMem - 1;
  228. for (j=0; j<(range-1); j++) {
  229. *(ppe+1) = *ppe + (*ppi)*(*ppi) - (*ppo)*(*ppo);
  230. ppo--;
  231. ppi--;
  232. ppe++;
  233. }
  234. }
  235. /* loop over search range */
  236. for (icount=sInd; icount<eInd; icount++) {
  237. /* calculate measure */
  238. crossDot=0.0;
  239. pp=cbvectors + lMem - (counter++) - lTarget;
  240. for (j=0;j<lTarget;j++) {
  241. crossDot += target[j]*(*pp++);
  242. }
  243. if (energy[icount]>0.0) {
  244. invenergy[icount] =(float)1.0/(energy[icount]+EPS);
  245. } else {
  246. invenergy[icount] =(float)0.0;
  247. }
  248. if (stage==0) {
  249. measure=(float)-10000000.0;
  250. if (crossDot > 0.0) {
  251. measure = crossDot*crossDot*
  252. invenergy[icount];
  253. }
  254. }
  255. else {
  256. measure = crossDot*crossDot*invenergy[icount];
  257. }
  258. /* check if measure is better */
  259. ftmp = crossDot*invenergy[icount];
  260. if ((measure>max_measure) && (fabs(ftmp)<CB_MAXGAIN)) {
  261. best_index = icount;
  262. max_measure = measure;
  263. gain = ftmp;
  264. }
  265. }
  266. /* Search the augmented CB inside the limited range. */
  267. if ((lTarget==SUBL)&&(sIndAug!=0)) {
  268. searchAugmentedCB(sIndAug, eIndAug, stage,
  269. 2*base_size-20, target, cbvectors+lMem,
  270. &max_measure, &best_index, &gain, energy,
  271. invenergy);
  272. }
  273. /* record best index */
  274. index[stage] = best_index;
  275. /* gain quantization */
  276. if (stage==0){
  277. if (gain<0.0){
  278. gain = 0.0;
  279. }
  280. if (gain>CB_MAXGAIN) {
  281. gain = (float)CB_MAXGAIN;
  282. }
  283. gain = gainquant(gain, 1.0, 32, &gain_index[stage]);
  284. }
  285. else {
  286. if (stage==1) {
  287. gain = gainquant(gain, (float)fabs(gains[stage-1]),
  288. 16, &gain_index[stage]);
  289. } else {
  290. gain = gainquant(gain, (float)fabs(gains[stage-1]),
  291. 8, &gain_index[stage]);
  292. }
  293. }
  294. /* Extract the best (according to measure)
  295. codebook vector */
  296. if (lTarget==(STATE_LEN-iLBCenc_inst->state_short_len)) {
  297. if (index[stage]<base_size) {
  298. pp=buf+LPC_FILTERORDER+lMem-lTarget-index[stage];
  299. } else {
  300. pp=cbvectors+lMem-lTarget-
  301. index[stage]+base_size;
  302. }
  303. } else {
  304. if (index[stage]<base_size) {
  305. if (index[stage]<(base_size-20)) {
  306. pp=buf+LPC_FILTERORDER+lMem-
  307. lTarget-index[stage];
  308. } else {
  309. createAugmentedVec(index[stage]-base_size+40,
  310. buf+LPC_FILTERORDER+lMem,aug_vec);
  311. pp=aug_vec;
  312. }
  313. } else {
  314. int filterno, position;
  315. filterno=index[stage]/base_size;
  316. position=index[stage]-filterno*base_size;
  317. if (position<(base_size-20)) {
  318. pp=cbvectors+filterno*lMem-lTarget-
  319. index[stage]+filterno*base_size;
  320. } else {
  321. createAugmentedVec(
  322. index[stage]-(filterno+1)*base_size+40,
  323. cbvectors+filterno*lMem,aug_vec);
  324. pp=aug_vec;
  325. }
  326. }
  327. }
  328. /* Subtract the best codebook vector, according
  329. to measure, from the target vector */
  330. for (j=0;j<lTarget;j++) {
  331. cvec[j] += gain*(*pp);
  332. target[j] -= gain*(*pp++);
  333. }
  334. /* record quantized gain */
  335. gains[stage]=gain;
  336. }/* end of Main Loop. for (stage=0;... */
  337. /* Gain adjustment for energy matching */
  338. cene=0.0;
  339. for (i=0; i<lTarget; i++) {
  340. cene+=cvec[i]*cvec[i];
  341. }
  342. j=gain_index[0];
  343. for (i=gain_index[0]; i<32; i++) {
  344. ftmp=cene*gain_sq5Tbl[i]*gain_sq5Tbl[i];
  345. if ((ftmp<(tene*gains[0]*gains[0])) &&
  346. (gain_sq5Tbl[j]<(2.0*gains[0]))) {
  347. j=i;
  348. }
  349. }
  350. gain_index[0]=j;
  351. }