libcm is a C development framework with an emphasis on audio signal processing applications.
Du kan inte välja fler än 25 ämnen Ämnen måste starta med en bokstav eller siffra, kan innehålla bindestreck ('-') och vara max 35 tecken långa.

cmProc.c 133KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917291829192920292129222923292429252926292729282929293029312932293329342935293629372938293929402941294229432944294529462947294829492950295129522953295429552956295729582959296029612962296329642965296629672968296929702971297229732974297529762977297829792980298129822983298429852986298729882989299029912992299329942995299629972998299930003001300230033004300530063007300830093010301130123013301430153016301730183019302030213022302330243025302630273028302930303031303230333034303530363037303830393040304130423043304430453046304730483049305030513052305330543055305630573058305930603061306230633064306530663067306830693070307130723073307430753076307730783079308030813082308330843085308630873088308930903091309230933094309530963097309830993100310131023103310431053106310731083109311031113112311331143115311631173118311931203121312231233124312531263127312831293130313131323133313431353136313731383139314031413142314331443145314631473148314931503151315231533154315531563157315831593160316131623163316431653166316731683169317031713172317331743175317631773178317931803181318231833184318531863187318831893190319131923193319431953196319731983199320032013202320332043205320632073208320932103211321232133214321532163217321832193220322132223223322432253226322732283229323032313232323332343235323632373238323932403241324232433244324532463247324832493250325132523253325432553256325732583259326032613262326332643265326632673268326932703271327232733274327532763277327832793280328132823283328432853286328732883289329032913292329332943295329632973298329933003301330233033304330533063307330833093310331133123313331433153316331733183319332033213322332333243325332633273328332933303331333233333334333533363337333833393340334133423343334433453346334733483349335033513352335333543355335633573358335933603361336233633364336533663367336833693370337133723373337433753376337733783379338033813382338333843385338633873388338933903391339233933394339533963397339833993400340134023403340434053406340734083409341034113412341334143415341634173418341934203421342234233424342534263427342834293430343134323433343434353436343734383439344034413442344334443445344634473448344934503451345234533454345534563457345834593460346134623463346434653466346734683469347034713472347334743475347634773478347934803481348234833484348534863487348834893490349134923493349434953496349734983499350035013502350335043505350635073508350935103511351235133514351535163517351835193520352135223523352435253526352735283529353035313532353335343535353635373538353935403541354235433544354535463547354835493550355135523553355435553556355735583559356035613562356335643565356635673568356935703571357235733574357535763577357835793580358135823583358435853586358735883589359035913592359335943595359635973598359936003601360236033604360536063607360836093610361136123613361436153616361736183619362036213622362336243625362636273628362936303631363236333634363536363637363836393640364136423643364436453646364736483649365036513652365336543655365636573658365936603661366236633664366536663667366836693670367136723673367436753676367736783679368036813682368336843685368636873688368936903691369236933694369536963697369836993700370137023703370437053706370737083709371037113712371337143715371637173718371937203721372237233724372537263727372837293730373137323733373437353736373737383739374037413742374337443745374637473748374937503751375237533754375537563757375837593760376137623763376437653766376737683769377037713772377337743775377637773778377937803781378237833784378537863787378837893790379137923793379437953796379737983799380038013802380338043805380638073808380938103811381238133814381538163817381838193820382138223823382438253826382738283829383038313832383338343835383638373838383938403841384238433844384538463847384838493850385138523853385438553856385738583859386038613862386338643865386638673868386938703871387238733874387538763877387838793880388138823883388438853886388738883889389038913892389338943895389638973898389939003901390239033904390539063907390839093910391139123913391439153916391739183919392039213922392339243925392639273928392939303931393239333934393539363937393839393940394139423943394439453946394739483949395039513952395339543955395639573958395939603961396239633964396539663967396839693970397139723973397439753976397739783979398039813982398339843985398639873988398939903991399239933994399539963997399839994000400140024003400440054006400740084009401040114012401340144015401640174018401940204021402240234024402540264027402840294030403140324033403440354036403740384039404040414042404340444045404640474048404940504051405240534054405540564057405840594060406140624063406440654066406740684069407040714072407340744075407640774078407940804081408240834084408540864087408840894090409140924093409440954096409740984099410041014102410341044105410641074108410941104111411241134114411541164117411841194120412141224123412441254126412741284129413041314132413341344135413641374138413941404141414241434144414541464147414841494150415141524153415441554156415741584159416041614162416341644165416641674168416941704171417241734174417541764177417841794180418141824183418441854186418741884189419041914192419341944195419641974198419942004201420242034204420542064207420842094210421142124213421442154216421742184219422042214222422342244225422642274228422942304231423242334234423542364237423842394240424142424243424442454246424742484249425042514252425342544255425642574258425942604261426242634264426542664267426842694270427142724273427442754276427742784279428042814282428342844285428642874288428942904291429242934294429542964297429842994300430143024303430443054306430743084309431043114312431343144315431643174318431943204321432243234324432543264327432843294330433143324333433443354336433743384339434043414342434343444345434643474348434943504351435243534354435543564357435843594360436143624363436443654366436743684369437043714372437343744375437643774378437943804381438243834384438543864387438843894390439143924393439443954396439743984399440044014402440344044405440644074408440944104411441244134414441544164417441844194420442144224423442444254426442744284429443044314432443344344435443644374438443944404441444244434444444544464447444844494450445144524453445444554456445744584459446044614462446344644465446644674468446944704471447244734474447544764477447844794480448144824483448444854486448744884489449044914492449344944495449644974498449945004501450245034504450545064507450845094510451145124513451445154516451745184519452045214522452345244525452645274528452945304531453245334534453545364537453845394540454145424543454445454546454745484549455045514552455345544555455645574558455945604561456245634564456545664567456845694570457145724573457445754576457745784579458045814582458345844585458645874588458945904591459245934594459545964597459845994600460146024603460446054606460746084609461046114612461346144615461646174618461946204621462246234624462546264627462846294630463146324633463446354636463746384639464046414642464346444645464646474648464946504651465246534654465546564657465846594660466146624663466446654666466746684669467046714672467346744675467646774678467946804681468246834684468546864687468846894690469146924693469446954696469746984699470047014702470347044705470647074708470947104711471247134714471547164717471847194720472147224723472447254726472747284729473047314732473347344735473647374738473947404741474247434744474547464747474847494750475147524753475447554756475747584759476047614762476347644765476647674768476947704771477247734774477547764777477847794780478147824783478447854786478747884789479047914792479347944795479647974798479948004801480248034804480548064807480848094810481148124813481448154816481748184819482048214822482348244825482648274828482948304831483248334834483548364837483848394840484148424843484448454846484748484849485048514852485348544855485648574858485948604861486248634864486548664867486848694870487148724873487448754876487748784879488048814882488348844885488648874888488948904891489248934894489548964897489848994900490149024903490449054906490749084909491049114912491349144915491649174918491949204921492249234924492549264927492849294930493149324933493449354936493749384939494049414942494349444945494649474948494949504951495249534954495549564957495849594960496149624963496449654966496749684969497049714972497349744975497649774978497949804981498249834984498549864987498849894990499149924993499449954996499749984999500050015002500350045005500650075008500950105011501250135014501550165017501850195020502150225023502450255026502750285029503050315032503350345035503650375038503950405041504250435044504550465047504850495050505150525053505450555056505750585059506050615062506350645065506650675068506950705071507250735074507550765077507850795080508150825083508450855086508750885089509050915092509350945095509650975098509951005101510251035104510551065107510851095110511151125113511451155116511751185119512051215122512351245125512651275128512951305131513251335134513551365137513851395140514151425143514451455146514751485149515051515152515351545155515651575158515951605161516251635164516551665167516851695170517151725173517451755176517751785179518051815182518351845185518651875188518951905191519251935194519551965197519851995200520152025203520452055206520752085209521052115212521352145215521652175218521952205221522252235224522552265227522852295230523152325233523452355236523752385239524052415242
  1. #include "cmPrefix.h"
  2. #include "cmGlobal.h"
  3. #include "cmRpt.h"
  4. #include "cmErr.h"
  5. #include "cmCtx.h"
  6. #include "cmMem.h"
  7. #include "cmMallocDebug.h"
  8. #include "cmLinkedHeap.h"
  9. #include "cmSymTbl.h"
  10. #include "cmFloatTypes.h"
  11. #include "cmComplexTypes.h"
  12. #include "cmFileSys.h"
  13. #include "cmProcObj.h"
  14. #include "cmProcTemplate.h"
  15. #include "cmAudioFile.h"
  16. #include "cmMath.h"
  17. #include "cmProc.h"
  18. #include "cmVectOps.h"
  19. #include "cmKeyboard.h"
  20. #include "cmGnuPlot.h"
  21. #include <time.h> // time()
  22. //------------------------------------------------------------------------------------------------------------
  23. void cmFloatPointExceptHandler( int signo, siginfo_t* info, void* context )
  24. {
  25. char* cp = "<Type Unknown>";
  26. switch( info->si_code )
  27. {
  28. case FPE_INTDIV: cp = "integer divide by zero"; break;
  29. case FPE_INTOVF: cp = "integer overflow"; break;
  30. case FPE_FLTDIV: cp = "divide by zero"; break;
  31. case FPE_FLTUND: cp = "underflow"; break;
  32. case FPE_FLTRES: cp = "inexact result"; break;
  33. case FPE_FLTINV: cp = "invalid operation"; break;
  34. case FPE_FLTSUB: cp = "subscript range error"; break;
  35. }
  36. fprintf(stderr,"Floating point exception: Type: %s\n",cp);
  37. exit(1);
  38. }
  39. // set 'orgSaPtr' to NULL to discard the current signal action state
  40. void cmSetupFloatPointExceptHandler( struct sigaction* orgSaPtr )
  41. {
  42. struct sigaction sa;
  43. sa.sa_handler = SIG_DFL;
  44. sa.sa_flags = SA_SIGINFO;
  45. sa.sa_sigaction = cmFloatPointExceptHandler;
  46. sigemptyset(&sa.sa_mask);
  47. // set all FP except flags excetp: FE_INEXACT
  48. #ifdef OS_OSX
  49. // we don't yet support FP exceptions on OSX
  50. // for an example of how to make this work with the linux interface as below
  51. // see: http://www-personal.umich.edu/~williams/archive/computation/fe-handling-example.c
  52. assert(0);
  53. #else
  54. // int flags = FE_DIVBYZERO | FE_UNDERFLOW | FE_OVERFLOW | FE_INVALID;
  55. // feenableexcept(flags);
  56. assert(0);
  57. #endif
  58. sigaction( SIGFPE, &sa, orgSaPtr );
  59. }
  60. //------------------------------------------------------------------------------------------------------------
  61. cmAudioFileRd* cmAudioFileRdAlloc( cmCtx* c, cmAudioFileRd* p, unsigned procSmpCnt, const cmChar_t* fn, unsigned chIdx, unsigned begFrmIdx, unsigned endFrmIdx )
  62. {
  63. cmAudioFileRd* op = cmObjAlloc( cmAudioFileRd, c, p );
  64. if( fn != NULL )
  65. if( cmAudioFileRdOpen( op, procSmpCnt, fn, chIdx, begFrmIdx, endFrmIdx ) != cmOkRC )
  66. cmAudioFileRdFree(&op);
  67. return op;
  68. }
  69. cmRC_t cmAudioFileRdFree( cmAudioFileRd** pp )
  70. {
  71. cmRC_t rc = cmOkRC;
  72. if( pp != NULL && *pp != NULL )
  73. {
  74. cmAudioFileRd* p = *pp;
  75. if((rc = cmAudioFileRdClose(p)) == cmOkRC )
  76. {
  77. cmMemPtrFree(&p->outV);
  78. cmMemPtrFree(&p->fn);
  79. cmObjFree(pp);
  80. }
  81. }
  82. return rc;
  83. }
  84. cmRC_t cmAudioFileRdOpen( cmAudioFileRd* p, unsigned procSmpCnt, const cmChar_t* fn, unsigned chIdx, unsigned begFrmIdx, unsigned endFrmIdx )
  85. {
  86. cmRC_t rc;
  87. cmRC_t afRC;
  88. if((rc = cmAudioFileRdClose(p)) != cmOkRC )
  89. return rc;
  90. p->h = cmAudioFileNewOpen( fn, &p->info, &afRC, p->obj.err.rpt );
  91. if( afRC != kOkAfRC )
  92. return cmCtxRtCondition( &p->obj, afRC, "Unable to open the audio file:'%s'", fn );
  93. p->chIdx = chIdx;
  94. p->outN = endFrmIdx==cmInvalidIdx ? p->info.frameCnt : procSmpCnt;
  95. p->outV = cmMemResizeZ( cmSample_t, p->outV, p->outN );
  96. p->fn = cmMemResizeZ( cmChar_t, p->fn, strlen(fn)+1 );
  97. strcpy(p->fn,fn);
  98. //p->mfp = cmCtxAllocDebugFile( p->obj.ctx,"audioFile");
  99. p->lastReadFrmCnt = 0;
  100. p->eofFl = false;
  101. p->begFrmIdx = begFrmIdx;
  102. p->endFrmIdx = endFrmIdx==0 ? p->info.frameCnt : endFrmIdx;
  103. p->curFrmIdx = p->begFrmIdx;
  104. if( p->begFrmIdx > 0 )
  105. rc = cmAudioFileRdSeek(p,p->begFrmIdx);
  106. return rc;
  107. }
  108. cmRC_t cmAudioFileRdClose( cmAudioFileRd* p )
  109. {
  110. cmRC_t rc = cmOkRC;
  111. cmRC_t afRC;
  112. if( p == NULL )
  113. return cmOkRC;
  114. //cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  115. if( cmAudioFileIsOpen(p->h) == false )
  116. return cmOkRC;
  117. if((afRC = cmAudioFileDelete(&p->h)) != cmOkRC )
  118. rc = cmCtxRtCondition( &p->obj, afRC, "An attempt to close the audio file'%s' failed.", p->fn );
  119. return rc;
  120. }
  121. cmRC_t cmAudioFileRdRead( cmAudioFileRd* p )
  122. {
  123. cmRC_t rc = cmOkRC;
  124. cmRC_t afRC;
  125. if(p->eofFl || ((p->eofFl = cmAudioFileIsEOF(p->h)) == true) )
  126. return cmEofRC;
  127. unsigned n = p->endFrmIdx==cmInvalidIdx ? p->outN : cmMin( p->outN, p->endFrmIdx - p->curFrmIdx );
  128. if((afRC = cmAudioFileReadSample( p->h, n, p->chIdx, 1, &p->outV, &p->lastReadFrmCnt )) != kOkAfRC )
  129. rc = cmCtxRtCondition( &p->obj, afRC, "Audio file read failed on:'%s'.", p->fn);
  130. p->curFrmIdx += p->lastReadFrmCnt;
  131. if( n < p->outN )
  132. {
  133. cmVOS_Zero(p->outV + p->lastReadFrmCnt, p->outN - p->lastReadFrmCnt);
  134. p->eofFl = true;
  135. }
  136. if( p->mfp != NULL )
  137. cmMtxFileSmpExec( p->mfp, p->outV, p->outN );
  138. return rc;
  139. }
  140. cmRC_t cmAudioFileRdSeek( cmAudioFileRd* p, unsigned frmIdx )
  141. {
  142. cmRC_t rc = cmOkRC;
  143. cmRC_t afRC;
  144. if((afRC = cmAudioFileSeek( p->h, frmIdx )) != kOkAfRC )
  145. rc = cmCtxRtCondition( &p->obj, afRC, "Audio file read failed on:'%s'.", p->fn);
  146. return rc;
  147. }
  148. cmRC_t cmAudioFileRdMinMaxMean( cmAudioFileRd* p, unsigned chIdx, cmSample_t* minPtr, cmSample_t* maxPtr, cmSample_t* meanPtr )
  149. {
  150. cmRC_t rc = cmOkRC;
  151. cmRC_t afRC;
  152. if(( afRC = cmAudioFileMinMaxMean( p->h, chIdx, minPtr, maxPtr, meanPtr )) != kOkAfRC )
  153. rc = cmCtxRtCondition( &p->obj, afRC, "Audio file min, max, and mean calculation failed on '%s'", p->fn );
  154. return rc;
  155. }
  156. //------------------------------------------------------------------------------------------------------------
  157. cmShiftBuf* cmShiftBufAlloc( cmCtx* c, cmShiftBuf* p, unsigned procSmpCnt, unsigned wndSmpCnt, unsigned hopSmpCnt )
  158. {
  159. cmShiftBuf* op = cmObjAlloc( cmShiftBuf, c, p );
  160. if( procSmpCnt > 0 && wndSmpCnt > 0 && hopSmpCnt > 0 )
  161. if( cmShiftBufInit(op, procSmpCnt, wndSmpCnt, hopSmpCnt ) != cmOkRC)
  162. cmShiftBufFree(&op);
  163. return op;
  164. }
  165. cmRC_t cmShiftBufFree( cmShiftBuf** pp )
  166. {
  167. cmRC_t rc = cmOkRC;
  168. if( pp != NULL && *pp != NULL )
  169. {
  170. if((rc = cmShiftBufFinal(*pp)) == cmOkRC )
  171. {
  172. cmMemPtrFree(&(*pp)->bufV);
  173. cmObjFree(pp);
  174. }
  175. }
  176. return rc;
  177. }
  178. cmRC_t cmShiftBufInit( cmShiftBuf* p, unsigned procSmpCnt, unsigned wndSmpCnt, unsigned hopSmpCnt )
  179. {
  180. cmRC_t rc;
  181. if( hopSmpCnt > wndSmpCnt )
  182. return cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "The window sample count (%i) must be greater than or equal to the hop sample count (%i).", wndSmpCnt, hopSmpCnt );
  183. if((rc = cmShiftBufFinal(p)) != cmOkRC )
  184. return rc;
  185. // The worst case storage requirement is where there are wndSmpCnt-1 samples in outV[] and procSmpCnt new samples arrive.
  186. p->bufSmpCnt = wndSmpCnt + procSmpCnt;
  187. p->bufV = cmMemResizeZ( cmSample_t, p->outV, p->bufSmpCnt );
  188. p->outV = p->bufV;
  189. p->outN = wndSmpCnt;
  190. p->wndSmpCnt = wndSmpCnt;
  191. p->procSmpCnt = procSmpCnt;
  192. p->hopSmpCnt = hopSmpCnt;
  193. p->inPtr = p->outV;
  194. p->fl = false;
  195. return cmOkRC;
  196. }
  197. cmRC_t cmShiftBufFinal( cmShiftBuf* p )
  198. {
  199. return cmOkRC;
  200. }
  201. // This function should be called in a loop until it returns false.
  202. // Note that 'sp' and 'sn' are ignored except p->fl == false.
  203. bool cmShiftBufExec( cmShiftBuf* p, const cmSample_t* sp, unsigned sn )
  204. {
  205. assert( sn <= p->procSmpCnt );
  206. // The active samples are in outV[wndSmpCnt]
  207. // Stored samples are between outV + wndSmpCnt and inPtr.
  208. // if the previous call to this function returned true then the buffer must be
  209. // shifted by hopSmpCnt samples - AND sp[] is ignored.
  210. if( p->fl )
  211. {
  212. // shift the output buffer to the left to remove expired samples
  213. p->outV += p->hopSmpCnt;
  214. // if there are not wndSmpCnt samples left in the buffer
  215. if( p->inPtr - p->outV < p->wndSmpCnt )
  216. {
  217. // then copy the remaining active samples (between outV and inPtr)
  218. // to the base of the physicalbuffer
  219. unsigned n = p->inPtr - p->outV;
  220. memmove( p->bufV, p->outV, n * sizeof(cmSample_t));
  221. p->inPtr = p->bufV + n; // update the input and output positions
  222. p->outV = p->bufV;
  223. }
  224. }
  225. else
  226. {
  227. // if the previous call to this function returned false then sp[sn] should not be ignored
  228. assert( p->inPtr + sn <= p->outV + p->bufSmpCnt );
  229. // copy the incoming samples into the buffer
  230. cmVOS_Copy(p->inPtr,sn,sp);
  231. p->inPtr += sn;
  232. }
  233. // if there are at least wndSmpCnt available samples in outV[]
  234. p->fl = p->inPtr - p->outV >= p->wndSmpCnt;
  235. return p->fl;
  236. }
  237. void cmShiftBufTest( cmCtx* c )
  238. {
  239. unsigned smpCnt = 48;
  240. unsigned procSmpCnt = 5;
  241. unsigned hopSmpCnt = 6;
  242. unsigned wndSmpCnt = 7;
  243. unsigned i;
  244. cmShiftBuf* b = cmShiftBufAlloc(c,NULL,procSmpCnt,wndSmpCnt,hopSmpCnt );
  245. cmSample_t x[ smpCnt ];
  246. cmVOS_Seq(x,smpCnt,1,1);
  247. //cmVOS_Print( rptFuncPtr, 1, smpCnt, x );
  248. for(i=0; i<smpCnt; i+=procSmpCnt)
  249. {
  250. while( cmShiftBufExec( b, x + i, procSmpCnt ) )
  251. {
  252. cmVOS_Print( c->obj.err.rpt, 1, wndSmpCnt, b->outV );
  253. }
  254. }
  255. cmShiftBufFree(&b);
  256. }
  257. /*
  258. bool cmShiftBufExec( cmShiftBuf* p, const cmSample_t* sp, unsigned sn )
  259. {
  260. bool retFl = false;
  261. if( sn > p->procSmpCnt )
  262. {
  263. cmCtxRtAssertFailed( p->obj.ctx, cmArgAssertRC, "The input sample count (%i) must be less than or equal to the proc sample count (%i).", sn, p->procSmpCnt);
  264. return false;
  265. }
  266. assert( sn <= p->procSmpCnt );
  267. cmSample_t* dbp = p->outV;
  268. cmSample_t* dep = p->outV + (p->outN - sn);
  269. cmSample_t* sbp = p->outV + sn;
  270. // shift the last bufCnt-shiftCnt samples over the first shiftCnt samples
  271. while( dbp < dep )
  272. *dbp++ = *sbp++;
  273. // copy in the new samples
  274. dbp = dep;
  275. dep = dbp + sn;
  276. while( dbp < dep )
  277. *dbp++ = *sp++;
  278. // if any space remains at the end of the buffer then zero it
  279. dep = p->outV + p->outN;
  280. while( dbp < dep )
  281. *dbp++ = 0;
  282. if( p->firstPtr > p->outV )
  283. p->firstPtr = cmMax( p->outV, p->firstPtr - p->procSmpCnt);
  284. p->curHopSmpCnt += sn;
  285. if( p->curHopSmpCnt >= p->hopSmpCnt )
  286. {
  287. p->curHopSmpCnt -= p->hopSmpCnt;
  288. retFl = true;
  289. }
  290. if( p->mfp != NULL )
  291. cmMtxFileSmpExec(p->mfp,p->outV,p->outN);
  292. return retFl;
  293. }
  294. */
  295. //------------------------------------------------------------------------------------------------------------
  296. cmWndFunc* cmWndFuncAlloc( cmCtx* c, cmWndFunc* p, unsigned wndId, unsigned wndSmpCnt, double kaiserSideLobeRejectDb )
  297. {
  298. cmWndFunc* op = cmObjAlloc( cmWndFunc, c, p );
  299. if( wndId != kInvalidWndId )
  300. if( cmWndFuncInit(op,wndId,wndSmpCnt,kaiserSideLobeRejectDb ) != cmOkRC )
  301. cmWndFuncFree(&op);
  302. return op;
  303. }
  304. cmRC_t cmWndFuncFree( cmWndFunc** pp )
  305. {
  306. cmRC_t rc = cmOkRC;
  307. if( pp != NULL && *pp != NULL )
  308. {
  309. cmWndFunc* p = *pp;
  310. if((rc = cmWndFuncFinal(p)) == cmOkRC )
  311. {
  312. cmMemPtrFree(&p->wndV);
  313. cmMemPtrFree(&p->outV);
  314. cmObjFree(pp);
  315. }
  316. }
  317. return rc;
  318. }
  319. cmRC_t cmWndFuncInit( cmWndFunc* p, unsigned wndId, unsigned wndSmpCnt, double kslRejectDb )
  320. {
  321. cmRC_t rc;
  322. if( wndId == (p->wndId | p->flags) && wndSmpCnt == p->outN && kslRejectDb == p->kslRejectDb )
  323. return cmOkRC;
  324. if((rc = cmWndFuncFinal(p)) != cmOkRC )
  325. return rc;
  326. p->wndV = cmMemResize( cmSample_t, p->wndV, wndSmpCnt );
  327. p->outV = cmMemResize( cmSample_t, p->outV, wndSmpCnt );
  328. p->outN = wndSmpCnt;
  329. p->wndId = wndId;
  330. p->kslRejectDb = kslRejectDb;
  331. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"wndFunc");
  332. p->flags = wndId & (~kWndIdMask);
  333. switch( wndId & kWndIdMask )
  334. {
  335. case kHannWndId: cmVOS_Hann( p->wndV, p->outN ); break;
  336. case kHannMatlabWndId: cmVOS_HannMatlab( p->wndV, p->outN ); break;
  337. case kHammingWndId: cmVOS_Hamming( p->wndV, p->outN ); break;
  338. case kTriangleWndId: cmVOS_Triangle( p->wndV, p->outN ); break;
  339. case kUnityWndId: cmVOS_Fill( p->wndV, p->outN, 1.0 ); break;
  340. case kKaiserWndId:
  341. {
  342. double beta = cmVOS_KaiserBetaFromSidelobeReject(fabs(kslRejectDb));
  343. cmVOS_Kaiser( p->wndV,p->outN, beta);
  344. }
  345. break;
  346. case kInvalidWndId: break;
  347. default:
  348. { assert(0); }
  349. }
  350. cmSample_t den = 0;
  351. cmSample_t num = 1;
  352. if( cmIsFlag(p->flags,kNormBySumWndFl) )
  353. {
  354. den = cmVOS_Sum(p->wndV, p->outN);
  355. num = wndSmpCnt;
  356. }
  357. if( cmIsFlag(p->flags,kNormByLengthWndFl) )
  358. den += wndSmpCnt;
  359. if( den > 0 )
  360. {
  361. cmVOS_MultVS(p->wndV,p->outN,num);
  362. cmVOS_DivVS(p->wndV,p->outN,den);
  363. }
  364. return cmOkRC;
  365. }
  366. cmRC_t cmWndFuncFinal( cmWndFunc* p )
  367. {
  368. //if( p != NULL )
  369. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  370. return cmOkRC;
  371. }
  372. cmRC_t cmWndFuncExec( cmWndFunc* p, const cmSample_t* sp, unsigned sn )
  373. {
  374. if( sn > p->outN )
  375. return cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "The length of the input vector (%i) is greater thean the length of the window function (%i).", sn, p->outN );
  376. if( p->wndId != kInvalidWndId )
  377. cmVOS_MultVVV( p->outV, sn, sp, p->wndV );
  378. if( p->mfp != NULL )
  379. cmMtxFileSmpExec(p->mfp,p->outV,p->outN);
  380. return cmOkRC;
  381. }
  382. void cmWndFuncTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  383. {
  384. unsigned wndCnt = 5;
  385. double kaiserSideLobeRejectDb = 30;
  386. cmCtx c;
  387. cmCtxAlloc(&c,rpt,lhH,stH);
  388. cmWndFunc* p = cmWndFuncAlloc(&c,NULL,kHannWndId,wndCnt, 0 );
  389. cmVOS_Print( rpt, 1, wndCnt, p->wndV );
  390. cmWndFuncInit(p,kHammingWndId ,wndCnt, 0 );
  391. cmVOS_Print( rpt, 1, wndCnt, p->wndV );
  392. cmWndFuncInit(p,kTriangleWndId ,wndCnt, 0 );
  393. cmVOS_Print( rpt, 1, wndCnt, p->wndV );
  394. cmWndFuncInit(p,kKaiserWndId ,wndCnt, kaiserSideLobeRejectDb );
  395. cmVOS_Print( rpt, 1, wndCnt, p->wndV );
  396. cmSample_t wV[ wndCnt ];
  397. cmVOS_HannMatlab(wV,wndCnt);
  398. cmVOS_Print( rpt, 1, wndCnt, wV);
  399. cmWndFuncFree(&p);
  400. }
  401. //------------------------------------------------------------------------------------------------------------
  402. cmSpecDelay* cmSpecDelayAlloc( cmCtx* c, cmSpecDelay* ap, unsigned maxDelayCnt, unsigned binCnt )
  403. {
  404. cmSpecDelay* p = cmObjAlloc( cmSpecDelay, c, ap );
  405. if( maxDelayCnt > 0 && binCnt > 0 )
  406. if( cmSpecDelayInit(p,maxDelayCnt,binCnt) != cmOkRC )
  407. cmSpecDelayFree(&p);
  408. return p;
  409. }
  410. cmRC_t cmSpecDelayFree( cmSpecDelay** pp )
  411. {
  412. cmRC_t rc = cmOkRC;
  413. if( pp != NULL && *pp != NULL )
  414. {
  415. cmSpecDelay* p = *pp;
  416. if((rc=cmSpecDelayFinal(p)) == cmOkRC )
  417. {
  418. cmMemPtrFree(&p->bufPtr);
  419. cmObjFree(pp);
  420. }
  421. }
  422. return rc;
  423. }
  424. cmRC_t cmSpecDelayInit( cmSpecDelay* p, unsigned maxDelayCnt, unsigned binCnt )
  425. {
  426. cmRC_t rc;
  427. if((rc = cmSpecDelayFinal(p)) != cmOkRC )
  428. return rc;
  429. p->bufPtr = cmMemResizeZ( cmSample_t, p->bufPtr, binCnt * maxDelayCnt );
  430. p->maxDelayCnt = maxDelayCnt;
  431. p->outN = binCnt;
  432. p->inIdx = 0;
  433. return cmOkRC;
  434. }
  435. cmRC_t cmSpecDelayFinal(cmSpecDelay* p )
  436. { return cmOkRC; }
  437. cmRC_t cmSpecDelayExec( cmSpecDelay* p, const cmSample_t* sp, unsigned sn )
  438. {
  439. cmSample_t* dp = p->bufPtr + (p->inIdx * p->outN);
  440. cmVOS_Copy( dp, cmMin(sn,p->outN), sp);
  441. p->inIdx = (p->inIdx+1) % p->maxDelayCnt;
  442. return cmOkRC;
  443. }
  444. const cmSample_t* cmSpecDelayOutPtr( cmSpecDelay* p, unsigned delayCnt )
  445. {
  446. assert( delayCnt < p->maxDelayCnt );
  447. int i = p->inIdx - delayCnt;
  448. if( i < 0 )
  449. i = p->maxDelayCnt + i;
  450. return p->bufPtr + (i * p->outN);
  451. }
  452. //------------------------------------------------------------------------------------------------------------
  453. cmFilter* cmFilterAlloc( cmCtx* c, cmFilter* ap, const cmReal_t* b, unsigned bn, const cmReal_t* a, unsigned an, unsigned procSmpCnt, const cmReal_t* d )
  454. {
  455. cmRC_t rc;
  456. cmFilter* p = cmObjAlloc(cmFilter,c,ap);
  457. if( (bn > 0 || an > 0) && procSmpCnt > 0 )
  458. if( (rc = cmFilterInit( p, b, bn, a, an, procSmpCnt, d)) != cmOkRC )
  459. cmFilterFree(&p);
  460. return p;
  461. }
  462. cmFilter* cmFilterAllocEllip( cmCtx* c, cmFilter* ap, cmReal_t srate, cmReal_t passHz, cmReal_t stopHz, cmReal_t passDb, cmReal_t stopDb, unsigned procSmpCnt, const cmReal_t* d )
  463. {
  464. cmRC_t rc;
  465. cmFilter* p = cmObjAlloc(cmFilter,c,ap);
  466. if( srate > 0 && passHz > 0 && procSmpCnt > 0 )
  467. if( (rc = cmFilterInitEllip( p, srate, passHz, stopHz, passDb, stopDb, procSmpCnt, d)) != cmOkRC )
  468. cmFilterFree(&p);
  469. return p;
  470. }
  471. cmRC_t cmFilterFree( cmFilter** pp )
  472. {
  473. cmRC_t rc = cmOkRC;
  474. if( pp != NULL && *pp != NULL )
  475. {
  476. cmFilter* p = *pp;
  477. if((rc = cmFilterFinal(p)) == cmOkRC )
  478. {
  479. cmMemPtrFree(&p->a);
  480. cmMemPtrFree(&p->b);
  481. cmMemPtrFree(&p->d);
  482. cmMemPtrFree(&p->outSmpV);
  483. cmObjFree(pp);
  484. }
  485. }
  486. return rc;
  487. }
  488. cmRC_t cmFilterInit( cmFilter* p, const cmReal_t* b, unsigned bn, const cmReal_t* a, unsigned an, unsigned procSmpCnt, const cmReal_t* d )
  489. {
  490. assert( bn >= 1 );
  491. assert( an >= 1 && a[0] != 0 );
  492. cmRC_t rc;
  493. if((rc = cmFilterFinal(p)) != cmOkRC )
  494. return rc;
  495. int cn = cmMax(an,bn) - 1;
  496. // The output vector may be used as either cmReal_t or cmSample_t.
  497. // Find the larger of the two possible types.
  498. if( sizeof(cmReal_t) > sizeof(cmSample_t) )
  499. {
  500. p->outRealV = cmMemResizeZ( cmReal_t, p->outRealV, procSmpCnt );
  501. p->outSmpV = (cmSample_t*)p->outRealV;
  502. }
  503. else
  504. {
  505. p->outSmpV = cmMemResizeZ( cmSample_t, p->outSmpV, procSmpCnt );
  506. p->outRealV = (cmReal_t*)p->outRealV;
  507. }
  508. p->a = cmMemResizeZ( cmReal_t, p->a, cn );
  509. p->b = cmMemResizeZ( cmReal_t, p->b, cn );
  510. p->d = cmMemResizeZ( cmReal_t, p->d, cn+1 );
  511. //p->outV = cmMemResizeZ( cmSample_t, p->outV, procSmpCnt );
  512. p->outN = procSmpCnt;
  513. p->an = an;
  514. p->bn = bn;
  515. p->cn = cn;
  516. p->di = 0;
  517. p->b0 = b[0] / a[0];
  518. int i;
  519. for(i=0; i<an-1; ++i)
  520. p->a[i] = a[i+1] / a[0];
  521. for(i=0; i<bn-1; ++i)
  522. p->b[i] = b[i+1] / a[0];
  523. if( d != NULL )
  524. cmVOR_Copy(p->d,cn,d);
  525. return cmOkRC;
  526. }
  527. // initialize an elliptic lowpass filter with the given characteristics
  528. // ref: Parks & Burrus, Digital Filter Design, sec. 7.2.7 - 7.2.8
  529. cmRC_t cmFilterInitEllip( cmFilter* p, cmReal_t srate, cmReal_t passHz, cmReal_t stopHz, cmReal_t passDb, cmReal_t stopDb, unsigned procSmpCnt, const cmReal_t* d )
  530. {
  531. assert( srate > 0 );
  532. assert( passHz > 0 && stopHz > passHz && srate/2 > stopHz );
  533. cmReal_t Wp, Ws, ep, v0,
  534. k, kc, k1, k1c,
  535. K, Kc, K1, K1c,
  536. sn, cn, dn,
  537. sm, cm, dm,
  538. zr, zi, pr, pi;
  539. unsigned N, L, j;
  540. // prewarp Wp and Ws, calculate k
  541. Wp = 2 * srate * tan(M_PI * passHz / srate);
  542. Ws = 2 * srate * tan(M_PI * stopHz / srate);
  543. k = Wp / Ws;
  544. // calculate ep and k1 from passDb and stopDb
  545. ep = sqrt(pow(10, passDb/10) - 1);
  546. k1 = ep / sqrt(pow(10, stopDb/10) - 1);
  547. // calculate complimentary moduli
  548. kc = sqrt(1-k*k);
  549. k1c = sqrt(1-k1*k1);
  550. // calculate complete elliptic integrals
  551. K = cmEllipK( kc );
  552. Kc = cmEllipK( k );
  553. K1 = cmEllipK( k1c );
  554. K1c = cmEllipK( k1 );
  555. // calculate minimum integer filter order N
  556. N = ceil(K*K1c/Kc/K1);
  557. // recalculate k and kc from chosen N
  558. // Ws is minimized while other specs held constant
  559. k = cmEllipDeg( K1c/K1/N );
  560. kc = sqrt(1-k*k);
  561. K = cmEllipK( kc );
  562. Kc = cmEllipK( k );
  563. Ws = Wp / k;
  564. // initialize temporary coefficient arrays
  565. cmReal_t b[N+1], a[N+1];
  566. a[0] = b[0] = 1;
  567. memset(b+1, 0, N*sizeof(cmReal_t));
  568. memset(a+1, 0, N*sizeof(cmReal_t));
  569. // intermediate value needed for determining poles
  570. v0 = K/K1/N * cmEllipArcSc( 1/ep, k1 );
  571. cmEllipJ( v0, k, &sm, &cm, &dm );
  572. for( L=1-N%2; L<N; L+=2 )
  573. {
  574. // find the next pole and zero on s-plane
  575. cmEllipJ( K*L/N, kc, &sn, &cn, &dn );
  576. zr = 0;
  577. zi = L ? Ws/sn : 1E25;
  578. pr = -Wp*sm*cm*cn*dn/(1-pow(dn*sm,2));
  579. pi = Wp*dm*sn/(1-pow(dn*sm,2));
  580. // convert pole and zero to z-plane using bilinear transform
  581. cmBlt( 1, srate, &zr, &zi );
  582. cmBlt( 1, srate, &pr, &pi );
  583. if( L == 0 )
  584. {
  585. // first order section
  586. b[1] = -zr;
  587. a[1] = -pr;
  588. }
  589. else
  590. {
  591. // replace complex root and its conjugate with 2nd order section
  592. zi = zr*zr + zi*zi;
  593. zr *= -2;
  594. pi = pr*pr + pi*pi;
  595. pr *= -2;
  596. // combine with previous sections to obtain filter coefficients
  597. for( j = L+1; j >= 2; j-- )
  598. {
  599. b[j] = b[j] + zr*b[j-1] + zi*b[j-2];
  600. a[j] = a[j] + pr*a[j-1] + pi*a[j-2];
  601. }
  602. b[1] += zr;
  603. a[1] += pr;
  604. }
  605. }
  606. // scale b coefficients s.t. DC gain is 0 dB
  607. cmReal_t sumB = 0, sumA = 0;
  608. for( j = 0; j < N+1; j++ )
  609. {
  610. sumB += b[j];
  611. sumA += a[j];
  612. }
  613. sumA /= sumB;
  614. for( j = 0; j < N+1; j++ )
  615. b[j] *= sumA;
  616. return cmFilterInit( p, b, N+1, a, N+1, procSmpCnt, d );
  617. }
  618. cmRC_t cmFilterFinal( cmFilter* p )
  619. { return cmOkRC; }
  620. cmRC_t cmFilterExecS( cmFilter* p, const cmSample_t* x, unsigned xn, cmSample_t* yy, unsigned yn )
  621. {
  622. cmSample_t* y;
  623. if( yy == NULL || yn==0 )
  624. {
  625. y = p->outSmpV;
  626. yn = p->outN;
  627. }
  628. else
  629. {
  630. y = yy;
  631. }
  632. cmVOS_Filter( y, yn, x, xn, p->b0, p->b, p->a, p->d, p->cn );
  633. return cmOkRC;
  634. /*
  635. int i,j;
  636. cmSample_t y0 = 0;
  637. cmSample_t* y;
  638. unsigned n;
  639. if( yy == NULL || yn==0 )
  640. {
  641. n = cmMin(p->outN,xn);
  642. y = p->outSmpV;
  643. yn = p->outN;
  644. }
  645. else
  646. {
  647. n = cmMin(yn,xn);
  648. y = yy;
  649. }
  650. // This is a direct form II algorithm based on the MATLAB implmentation
  651. // http://www.mathworks.com/access/helpdesk/help/techdoc/ref/filter.html#f83-1015962
  652. for(i=0; i<n; ++i)
  653. {
  654. //cmSample_t x0 = x[i];
  655. y[i] = (x[i] * p->b0) + p->d[0];
  656. y0 = y[i];
  657. for(j=0; j<p->cn; ++j)
  658. p->d[j] = (p->b[j] * x[i]) - (p->a[j] * y0) + p->d[j+1];
  659. }
  660. // if fewer input samples than output samples - zero the end of the output buffer
  661. if( yn > xn )
  662. cmVOS_Fill(y+i,yn-i,0);
  663. return cmOkRC;
  664. */
  665. }
  666. cmRC_t cmFilterExecR( cmFilter* p, const cmReal_t* x, unsigned xn, cmReal_t* yy, unsigned yn )
  667. {
  668. cmReal_t* y;
  669. if( yy == NULL || yn==0 )
  670. {
  671. y = p->outRealV;
  672. yn = p->outN;
  673. }
  674. else
  675. {
  676. //n = cmMin(yn,xn);
  677. y = yy;
  678. }
  679. cmVOR_Filter( y, yn, x, xn, p->b0, p->b, p->a, p->d, p->cn );
  680. return cmOkRC;
  681. }
  682. cmRC_t cmFilterSignal( cmCtx* c, const cmReal_t b[], unsigned bn, const cmReal_t a[], unsigned an, const cmSample_t* x, unsigned xn, cmSample_t* y, unsigned yn )
  683. {
  684. int procSmpCnt = cmMin(1024,xn);
  685. cmFilter* p = cmFilterAlloc(c,NULL,b,bn,a,an,procSmpCnt,NULL);
  686. int i,n;
  687. for(i=0; i<xn && i<yn; i+=n)
  688. {
  689. n = cmMin(procSmpCnt,cmMin(yn-i,xn-i));
  690. cmFilterExecS(p,x+i,n,y+i,n);
  691. }
  692. if( i < yn )
  693. cmVOS_Fill(y+i,yn-i,0);
  694. cmFilterFree(&p);
  695. return cmOkRC;
  696. }
  697. cmRC_t cmFilterFilterS(cmCtx* c, const cmReal_t bb[], unsigned bn, const cmReal_t aa[], unsigned an, const cmSample_t* x, unsigned xn, cmSample_t* y, unsigned yn )
  698. {
  699. cmFilter* f = cmFilterAlloc(c,NULL,NULL,0,NULL,0,0,NULL);
  700. cmVOS_FilterFilter( f, cmFilterExecS, bb,bn,aa,an,x,xn,y,yn);
  701. cmFilterFree(&f);
  702. return cmOkRC;
  703. }
  704. cmRC_t cmFilterFilterR(cmCtx* c, const cmReal_t bb[], unsigned bn, const cmReal_t aa[], unsigned an, const cmReal_t* x, unsigned xn, cmReal_t* y, unsigned yn )
  705. {
  706. cmFilter* f = cmFilterAlloc(c,NULL,NULL,0,NULL,0,0,NULL);
  707. cmVOR_FilterFilter( f, cmFilterExecR, bb,bn,aa,an,x,xn,y,yn);
  708. cmFilterFree(&f);
  709. return cmOkRC;
  710. }
  711. void cmFilterTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  712. {
  713. cmCtx c;
  714. cmCtxAlloc(&c, rpt, lhH, stH );
  715. cmReal_t b[] = { 0.16, 0.32, 0.16 };
  716. unsigned bn = sizeof(b)/sizeof(cmReal_t);
  717. cmReal_t a[] = {1 , -.5949, .2348 };
  718. unsigned an = sizeof(a)/sizeof(cmReal_t);
  719. cmReal_t x[] = { 1,0,0,0,1,0,0,0 };
  720. unsigned xn = sizeof(x)/sizeof(cmReal_t);
  721. cmReal_t d[] = { .5, -.25};
  722. // 0.1600 0.4152 0.3694 0.1223 0.1460 0.3781 0.3507 0.1198
  723. // -0.0111 -0.0281
  724. cmFilter* p = cmFilterAlloc(&c,NULL,b,bn,a,an,xn,d);
  725. cmFilterExecR(p,x,xn,NULL,0);
  726. cmVOR_Print( rpt, 1, xn, p->outRealV );
  727. cmVOR_Print( rpt, 1, p->cn, p->d );
  728. cmFilterFree(&p);
  729. cmObjFreeStatic( cmCtxFree, cmCtx, c );
  730. /*
  731. cmReal_t b[] = { 0.16, 0.32, 0.16 };
  732. unsigned bn = sizeof(b)/sizeof(cmReal_t);
  733. cmReal_t a[] = { 1, -.5949, .2348};
  734. unsigned an = sizeof(a)/sizeof(cmReal_t);
  735. cmSample_t x[] = { 1,0,0,0,0,0,0,0,0,0 };
  736. unsigned xn = sizeof(x)/sizeof(cmSample_t);
  737. cmFilter* p = cmFilterAlloc(&c,NULL,b,bn,a,an,xn);
  738. cmFilterExec(&c,p,x,xn,NULL,0);
  739. cmVOS_Print( vReportFunc, 1, xn, p->outV );
  740. cmVOR_Print( vReportFunc, 1, p->cn, p->d );
  741. cmFilterExec(&c,p,x,xn,NULL,0);
  742. cmVOS_Print( vReportFunc, 1, xn, p->outV );
  743. cmFilterFree(&p);
  744. */
  745. }
  746. void cmFilterFilterTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  747. {
  748. cmCtx c;
  749. cmCtxAlloc(&c, rpt, lhH, stH );
  750. cmReal_t b[] = { 0.36, 0.32, 0.36 };
  751. unsigned bn = sizeof(b)/sizeof(cmReal_t);
  752. cmReal_t a[] = {1 , -.5949, .2348 };
  753. unsigned an = sizeof(a)/sizeof(cmReal_t);
  754. cmReal_t x[] = { 1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0 };
  755. unsigned xn = sizeof(x)/sizeof(cmReal_t);
  756. unsigned yn = xn;
  757. cmReal_t y[yn];
  758. memset(y,0,sizeof(y));
  759. cmFilterFilterR(&c, b,bn,a,an,x,xn,y,yn );
  760. cmVOR_Print( rpt, 1, yn, y );
  761. cmObjFreeStatic( cmCtxFree, cmCtx, c );
  762. }
  763. //------------------------------------------------------------------------------------------------------------
  764. cmComplexDetect* cmComplexDetectAlloc(cmCtx* c, cmComplexDetect* p, unsigned binCnt )
  765. {
  766. cmComplexDetect* op = cmObjAlloc( cmComplexDetect, c, p );
  767. cmSpecDelayAlloc(c,&op->phsDelay,0,0);
  768. cmSpecDelayAlloc(c,&op->magDelay,0,0);
  769. if( binCnt > 0 )
  770. if( cmComplexDetectInit(op,binCnt) != cmOkRC && p == NULL )
  771. cmComplexDetectFree(&op);
  772. return op;
  773. }
  774. cmRC_t cmComplexDetectFree( cmComplexDetect** pp )
  775. {
  776. cmRC_t rc;
  777. if( pp != NULL && *pp != NULL )
  778. {
  779. cmComplexDetect* p = *pp;
  780. if((rc = cmComplexDetectFinal(p)) == cmOkRC )
  781. {
  782. cmSpecDelay* sdp;
  783. sdp = &p->phsDelay;
  784. cmSpecDelayFree(&sdp);
  785. sdp = &p->magDelay;
  786. cmSpecDelayFree(&sdp);
  787. cmObjFree(pp);
  788. }
  789. }
  790. return cmOkRC;
  791. }
  792. cmRC_t cmComplexDetectInit( cmComplexDetect* p, unsigned binCnt )
  793. {
  794. cmRC_t rc;
  795. if((rc = cmComplexDetectFinal(p)) != cmOkRC )
  796. return rc;
  797. cmSpecDelayInit(&p->phsDelay,2,binCnt);
  798. cmSpecDelayInit(&p->magDelay,1,binCnt);
  799. p->binCnt = binCnt;
  800. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"complexDetect");
  801. //p->cdfSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kCDF_FId, 1 );
  802. return cmOkRC;
  803. }
  804. cmRC_t cmComplexDetectFinal( cmComplexDetect* p)
  805. {
  806. //if( p != NULL )
  807. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  808. return cmOkRC;
  809. }
  810. cmRC_t cmComplexDetectExec( cmComplexDetect* p, const cmSample_t* magV, const cmSample_t* phsV, unsigned binCnt )
  811. {
  812. p->out = cmVOS_ComplexDetect( magV, cmSpecDelayOutPtr(&p->magDelay,0), phsV, cmSpecDelayOutPtr(&p->phsDelay,1), cmSpecDelayOutPtr(&p->phsDelay,0), binCnt);
  813. p->out /= 10000000;
  814. cmSpecDelayExec(&p->magDelay,magV,binCnt);
  815. cmSpecDelayExec(&p->phsDelay,phsV,binCnt);
  816. //if( p->mfp != NULL )
  817. // cmMtxFileSmpExec( p->mfp, &p->out, 1 );
  818. return cmOkRC;
  819. }
  820. //------------------------------------------------------------------------------------------------------------
  821. cmSample_t _cmComplexOnsetMedian( const cmSample_t* sp, unsigned sn, void* userPtr )
  822. { return cmVOS_Median(sp,sn); }
  823. cmComplexOnset* cmComplexOnsetAlloc( cmCtx* c, cmComplexOnset* p, unsigned procSmpCnt, double srate, unsigned medFiltWndSmpCnt, double threshold, unsigned frameCnt )
  824. {
  825. cmComplexOnset* op = cmObjAlloc( cmComplexOnset, c, p );
  826. if( procSmpCnt > 0 && srate > 0 && medFiltWndSmpCnt > 0 )
  827. if( cmComplexOnsetInit( op, procSmpCnt, srate, medFiltWndSmpCnt, threshold, frameCnt ) != cmOkRC )
  828. cmComplexOnsetFree(&op);
  829. return op;
  830. }
  831. cmRC_t cmComplexOnsetFree( cmComplexOnset** pp)
  832. {
  833. cmRC_t rc = cmOkRC;
  834. cmComplexOnset* p = *pp;
  835. if( pp==NULL || *pp == NULL )
  836. return cmOkRC;
  837. if((rc = cmComplexOnsetFinal(*pp)) != cmOkRC )
  838. return rc;
  839. cmMemPtrFree(&p->df);
  840. cmMemPtrFree(&p->fdf);
  841. cmObjFree(pp);
  842. return cmOkRC;
  843. }
  844. cmRC_t cmComplexOnsetInit( cmComplexOnset* p, unsigned procSmpCnt, double srate, unsigned medFiltWndSmpCnt, double threshold, unsigned frameCnt )
  845. {
  846. cmRC_t rc;
  847. if(( rc = cmComplexOnsetFinal(p)) != cmOkRC )
  848. return rc;
  849. p->frmCnt = frameCnt;
  850. p->dfi = 0;
  851. p->df = cmMemResizeZ( cmSample_t, p->df, frameCnt );
  852. p->fdf = cmMemResizeZ( cmSample_t, p->fdf, frameCnt );
  853. p->onrate = 0;
  854. p->threshold = threshold;
  855. p->medSmpCnt = medFiltWndSmpCnt;
  856. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"complexOnset");
  857. return cmOkRC;
  858. }
  859. cmRC_t cmComplexOnsetFinal( cmComplexOnset* p)
  860. {
  861. //if( p != NULL )
  862. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  863. return cmOkRC;
  864. }
  865. cmRC_t cmComplexOnsetExec( cmComplexOnset* p, cmSample_t cdf )
  866. {
  867. p->df[p->dfi++] = cdf;
  868. return cmOkRC;
  869. }
  870. cmRC_t cmComplexOnsetCalc( cmComplexOnset* p )
  871. {
  872. // df -= mean(df)
  873. cmVOS_SubVS(p->df,p->frmCnt,cmVOS_Mean(p->df,p->frmCnt));
  874. // low pass forward/backward filter df[] into fdf[]
  875. double d = 2 + sqrt(2);
  876. cmReal_t b[] = {1/d, 2/d, 1/d};
  877. unsigned bn = sizeof(b)/sizeof(b[0]);
  878. cmReal_t a[] = {1, 0, 7-2*d};
  879. unsigned an = sizeof(a)/sizeof(a[0]);
  880. cmFilterFilterS(p->obj.ctx,b,bn,a,an,p->df,p->frmCnt,p->fdf,p->frmCnt);
  881. // median filter to low-passed filtered fdf[] into df[]
  882. cmVOS_FnThresh(p->fdf,p->frmCnt,p->medSmpCnt,p->df,1,NULL);
  883. // subtract med filtered signal from the low passed signal.
  884. // fdf[] -= df[];
  885. cmVOS_SubVV(p->fdf,p->frmCnt,p->df);
  886. cmVOS_SubVS(p->fdf,p->frmCnt,p->threshold);
  887. cmSample_t *bp = p->df,
  888. *ep = bp + p->frmCnt - 1,
  889. *dp = p->fdf + 1;
  890. *bp++ = *ep = 0;
  891. for( ; bp<ep; bp++,dp++)
  892. {
  893. *bp = (*dp > *(dp-1) && *dp > *(dp+1) && *dp > 0) ? 1 : 0;
  894. p->onrate += *bp;
  895. }
  896. p->onrate /= p->frmCnt;
  897. /*
  898. if( p->mfp != NULL )
  899. {
  900. bp = p->df;
  901. ep = bp + p->frmCnt;
  902. while( bp < ep )
  903. cmMtxFileSmpExec( p->mfp, bp++, 1 );
  904. }
  905. */
  906. return cmOkRC;
  907. }
  908. //------------------------------------------------------------------------------------------------------------
  909. cmMfcc* cmMfccAlloc( cmCtx* c, cmMfcc* ap, double srate, unsigned melBandCnt, unsigned dctCoeffCnt, unsigned binCnt )
  910. {
  911. cmMfcc* p = cmObjAlloc( cmMfcc, c, ap );
  912. if( melBandCnt > 0 && binCnt > 0 && dctCoeffCnt > 0 )
  913. if( cmMfccInit( p, srate, melBandCnt, dctCoeffCnt, binCnt ) != cmOkRC )
  914. cmMfccFree(&p);
  915. return p;
  916. }
  917. cmRC_t cmMfccFree( cmMfcc** pp )
  918. {
  919. cmRC_t rc = cmOkRC;
  920. if( pp==NULL || *pp == NULL )
  921. return cmOkRC;
  922. cmMfcc* p = *pp;
  923. if( (rc = cmMfccFinal(p)) != cmOkRC )
  924. return rc;
  925. cmMemPtrFree(&p->melM);
  926. cmMemPtrFree(&p->dctM);
  927. cmMemPtrFree(&p->outV);
  928. cmObjFree(pp);
  929. return rc;
  930. }
  931. cmRC_t cmMfccInit( cmMfcc* p, double srate, unsigned melBandCnt, unsigned dctCoeffCnt, unsigned binCnt )
  932. {
  933. cmRC_t rc;
  934. if((rc = cmMfccFinal(p)) != cmOkRC )
  935. return rc;
  936. p->melM = cmMemResize( cmReal_t, p->melM, melBandCnt * binCnt );
  937. p->dctM = cmMemResize( cmReal_t, p->dctM, dctCoeffCnt * melBandCnt );
  938. p->outV = cmMemResize( cmReal_t, p->outV, dctCoeffCnt );
  939. // each row of the matrix melp contains a mask
  940. cmVOR_MelMask( p->melM, melBandCnt, binCnt, srate, kShiftMelFl );
  941. // each row contains melBandCnt elements
  942. cmVOR_DctMatrix(p->dctM, dctCoeffCnt, melBandCnt );
  943. p->melBandCnt = melBandCnt;
  944. p->dctCoeffCnt = dctCoeffCnt;
  945. p->binCnt = binCnt;
  946. p->outN = dctCoeffCnt;
  947. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"mfcc");
  948. //if( p->obj.ctx->statsProcPtr != NULL )
  949. // p->mfccSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kMFCC_FId, p->outN );
  950. return cmOkRC;
  951. }
  952. cmRC_t cmMfccFinal( cmMfcc* p )
  953. {
  954. //if( p != NULL )
  955. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  956. return cmOkRC;
  957. }
  958. cmRC_t cmMfccExecPower( cmMfcc* p, const cmReal_t* magPowV, unsigned binCnt )
  959. {
  960. assert( binCnt == p->binCnt );
  961. cmReal_t t[ p->melBandCnt ];
  962. // apply the mel filter mask to the power spectrum
  963. cmVOR_MultVMV( t, p->melBandCnt, p->melM, binCnt, magPowV );
  964. // convert mel bands to dB
  965. cmVOR_PowerToDb( t, p->melBandCnt, t );
  966. // decorellate the bands with a DCT
  967. cmVOR_MultVMV( p->outV, p->dctCoeffCnt, p->dctM, p->melBandCnt, t );
  968. /*
  969. cmPlotSelectSubPlot(0,0);
  970. cmPlotClear();
  971. //cmPlotLineS( "power", NULL, magPowV, NULL, 35, NULL, kSolidPlotLineId );
  972. cmPlotLineS( "mel", NULL, t0, NULL, p->melBandCnt, NULL, kSolidPlotLineId );
  973. cmPlotSelectSubPlot(1,0);
  974. cmPlotClear();
  975. //cmPlotLineS( "meldb", NULL, t1, NULL, p->melBandCnt, NULL, kSolidPlotLineId );
  976. cmPlotLineS( "mfcc", NULL, p->outV+1, NULL, p->dctCoeffCnt-1, NULL, kSolidPlotLineId );
  977. */
  978. if( p->mfp != NULL )
  979. cmMtxFileRealExec(p->mfp,p->outV, p->outN);
  980. return cmOkRC;
  981. }
  982. cmRC_t cmMfccExecAmplitude( cmMfcc* p, const cmReal_t* magAmpV, unsigned binCnt )
  983. {
  984. cmReal_t t[ binCnt ];
  985. cmVOR_MultVVV( t,binCnt, magAmpV, magAmpV );
  986. cmMfccExecPower(p,t,binCnt);
  987. if( p->mfp != NULL )
  988. cmMtxFileRealExec(p->mfp,p->outV, p->outN);
  989. return cmOkRC;
  990. }
  991. //------------------------------------------------------------------------------------------------------------
  992. enum { cmSonesEqlConBinCnt = kEqualLoudBandCnt, cmSonesEqlConCnt=13 };
  993. cmSones* cmSonesAlloc( cmCtx* c, cmSones* ap, double srate, unsigned barkBandCnt, unsigned binCnt, unsigned flags )
  994. {
  995. cmSones* p = cmObjAlloc( cmSones, c, ap );
  996. if( srate > 0 && barkBandCnt > 0 && binCnt > 0 )
  997. if( cmSonesInit(p,srate,barkBandCnt,binCnt,flags) != cmOkRC )
  998. cmSonesFree(&p);
  999. return p;
  1000. }
  1001. cmRC_t cmSonesFree( cmSones** pp )
  1002. {
  1003. cmRC_t rc = cmOkRC;
  1004. cmSones* p = *pp;
  1005. if( pp==NULL || *pp==NULL)
  1006. return cmOkRC;
  1007. if((rc = cmSonesFinal(p)) != cmOkRC )
  1008. return rc;
  1009. cmMemPtrFree(&p->ttmV);
  1010. cmMemPtrFree(&p->sfM);
  1011. cmMemPtrFree(&p->barkIdxV);
  1012. cmMemPtrFree(&p->barkCntV);
  1013. cmMemPtrFree(&p->outV);
  1014. cmObjFree(pp);
  1015. return rc;
  1016. }
  1017. cmRC_t cmSonesInit( cmSones* p, double srate, unsigned barkBandCnt, unsigned binCnt, unsigned flags )
  1018. {
  1019. p->ttmV = cmMemResize( cmReal_t, p->ttmV, binCnt);
  1020. p->sfM = cmMemResize( cmReal_t, p->sfM, binCnt*barkBandCnt);
  1021. p->barkIdxV = cmMemResize( unsigned, p->barkIdxV, barkBandCnt);
  1022. p->barkCntV = cmMemResize( unsigned, p->barkCntV, barkBandCnt);
  1023. p->outV = cmMemResize( cmReal_t, p->outV, barkBandCnt);
  1024. // calc outer ear filter
  1025. cmVOR_TerhardtThresholdMask( p->ttmV, binCnt, srate, kNoTtmFlags );
  1026. // calc shroeder spreading function
  1027. cmVOR_ShroederSpreadingFunc(p->sfM, barkBandCnt, srate);
  1028. // calc the bin to bark maps
  1029. p->barkBandCnt = cmVOR_BarkMap( p->barkIdxV, p->barkCntV, barkBandCnt, binCnt, srate );
  1030. //unsigned i = 0;
  1031. //for(; i<barkBandCnt; ++i)
  1032. // printf("%i %i %i\n", i+1, barkIdxV[i]+1, barkCntV[i]);
  1033. bool elFl = cmIsFlag(p->flags, kUseEqlLoudSonesFl);
  1034. p->binCnt = binCnt;
  1035. p->outN = elFl ? cmSonesEqlConCnt : p->barkBandCnt;
  1036. p->overallLoudness = 0;
  1037. p->flags = flags;
  1038. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"sones");
  1039. //p->sonesSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSones_FId, p->outN );
  1040. //p->loudSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kLoud_FId, 1 );
  1041. return cmOkRC;
  1042. }
  1043. cmRC_t cmSonesFinal( cmSones* p )
  1044. {
  1045. //if( p != NULL )
  1046. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1047. return cmOkRC;
  1048. }
  1049. cmRC_t cmSonesExec( cmSones* p, const cmReal_t* magPowV, unsigned binCnt )
  1050. {
  1051. assert( binCnt == p->binCnt );
  1052. // Equal-loudness and phon map from: Y. Wonho, 1999, EMBSD: an objective speech quality measure based on audible distortion,
  1053. // equal-loudness contours
  1054. double eqlcon[cmSonesEqlConCnt][cmSonesEqlConBinCnt] =
  1055. {
  1056. {12,7,4,1,0,0,0,-0.5,-2,-3,-7,-8,-8.5,-8.5,-8.5},
  1057. {20,17,14,12,10,9.5,9,8.5,7.5,6.5,4,3,2.5,2,2.5},
  1058. {29,26,23,21,20,19.5,19.5,19,18,17,15,14,13.5,13,13.5},
  1059. {36,34,32,30,29,28.5,28.5,28.5,28,27.5,26,25,24.5,24,24.5},
  1060. {45,43,41,40,40,40,40,40,40,39.5,38,37,36.5,36,36.5},
  1061. {53,51,50,49,48.5,48.5,49,49,49,49,48,47,46.5,45.5,46},
  1062. {62,60,59,58,58,58.5,59,59,59,59,58,57.5,57,56,56},
  1063. {70,69,68,67.5,67.5,68,68,68,68,68,67,66,65.5,64.5,64.5},
  1064. {79,79,79,79,79,79,79,79,78,77.5,76,75,74.5,73,73},
  1065. {89,89,89,89.5,90,90,90,89.5,89,88.5,87,86,85.5,84,83.5},
  1066. {100,100,100,100,100,99.5,99,99,98.5,98,96,95,94.5,93.5,93},
  1067. {112,112,112,112,111,110.5,109.5,109,108.5,108,106,105,104.5,103,102.5},
  1068. {122,122,121,121,120.5,120,119,118,117,116.5,114.5,113.5,113,111, 110.5}
  1069. };
  1070. // loudness levels (phone scales)
  1071. double phons[cmSonesEqlConCnt]= {0.0,10.0,20.0,30.0,40.0,50.0,60.0,70.0,80.0,90.0,100.0,110.0,120.0};
  1072. unsigned i,j;
  1073. cmReal_t t0[ binCnt ];
  1074. cmReal_t t1[ p->barkBandCnt ];
  1075. cmReal_t t2[ p->barkBandCnt ];
  1076. unsigned* idxV = p->barkIdxV;
  1077. unsigned* cntV = p->barkCntV;
  1078. cmReal_t* sfM = p->sfM;
  1079. // apply the outer ear filter
  1080. cmVOR_MultVVV( t0, binCnt, magPowV, p->ttmV);
  1081. // apply the bark frequency warping
  1082. for(i=0; i<p->barkBandCnt; ++i)
  1083. {
  1084. if( cntV[i] == 0 )
  1085. t1[i] = 0;
  1086. else
  1087. {
  1088. t1[i] = t0[ idxV[i] ];
  1089. for(j=1; j<cntV[i]; ++j)
  1090. t1[i] += t0[ idxV[i] + j ];
  1091. }
  1092. }
  1093. // apply the spreading filters
  1094. cmVOR_MultVMV( t2, p->barkBandCnt, sfM, p->barkBandCnt, t1 );
  1095. bool elFl = cmIsFlag(p->flags, kUseEqlLoudSonesFl);
  1096. unsigned bandCnt = elFl ? cmMin(p->barkBandCnt,cmSonesEqlConBinCnt) : p->barkBandCnt;
  1097. //p->outN = elFl ? cmSonesEqlConCnt : p->barkBandCnt;
  1098. p->overallLoudness = 0;
  1099. for( i = 0; i <bandCnt; i++ )
  1100. {
  1101. // if using the equal-loudness contours begin with the third bark band
  1102. // and end with the 18th bark band
  1103. unsigned k = elFl ? i+3 : i;
  1104. if( k < p->barkBandCnt )
  1105. {
  1106. double v = t2[k];
  1107. // convert to db
  1108. v = 10*log10( v<1 ? 1 : v );
  1109. if( elFl )
  1110. {
  1111. // db to phons
  1112. // see: Y. Wonho, 1999, EMBSD: an objective speech quality measure based on audible distortion,
  1113. j = 0;
  1114. // find the equal loudness curve for this frequency and db level
  1115. while( v >= eqlcon[j][i] )
  1116. ++j;
  1117. if( j == cmSonesEqlConCnt )
  1118. {
  1119. cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "Bark band %i is out of range during equal-loudness mapping.",j );
  1120. continue;
  1121. }
  1122. // convert db to phons
  1123. if( j == 0 )
  1124. v = phons[0];
  1125. else
  1126. {
  1127. double t1 = ( v - eqlcon[j-1][i] ) / ( eqlcon[j][i] - eqlcon[j-1][i] );
  1128. v = phons[j-1] + t1 * (phons[j] - phons[j-1]);
  1129. }
  1130. }
  1131. // convert to sones
  1132. // bladon and lindblom, 1981, JASA, modelling the judment of vowel quality differences
  1133. if( v >= 40 )
  1134. p->outV[i] = pow(2,(v-40)/10);
  1135. else
  1136. p->outV[i] = pow(v/40,2.642);
  1137. p->overallLoudness += p->outV[i];
  1138. }
  1139. }
  1140. if( p->mfp != NULL )
  1141. cmMtxFileRealExec( p->mfp, p->outV, p->outN );
  1142. return cmOkRC;
  1143. }
  1144. void cmSonesTest()
  1145. {
  1146. cmKbRecd kb;
  1147. double srate = 44100;
  1148. unsigned bandCnt = 23;
  1149. unsigned binCnt = 513;
  1150. cmSample_t tv[ binCnt ];
  1151. cmSample_t sm[ bandCnt * bandCnt ];
  1152. cmSample_t t[ bandCnt * bandCnt ];
  1153. unsigned binIdxV[ bandCnt ];
  1154. unsigned cntV[ bandCnt ];
  1155. unsigned i;
  1156. cmPlotSetup("Sones",1,1);
  1157. cmVOS_TerhardtThresholdMask(tv,binCnt,srate, kModifiedTtmFl );
  1158. cmVOS_ShroederSpreadingFunc(sm, bandCnt, srate);
  1159. cmVOS_Transpose( t, sm, bandCnt, bandCnt );
  1160. bandCnt = cmVOS_BarkMap(binIdxV,cntV, bandCnt, binCnt, srate );
  1161. for(i=0; i<bandCnt; ++i)
  1162. printf("%i %i %i\n", i, binIdxV[i], cntV[i] );
  1163. for(i=0; i<bandCnt; ++i )
  1164. {
  1165. cmPlotLineS( NULL, NULL, t+(i*bandCnt), NULL, bandCnt, NULL, kSolidPlotLineId );
  1166. }
  1167. //cmPlotLineS( NULL, NULL, tv, NULL, binCnt, NULL, kSolidPlotLineId );
  1168. cmPlotDraw();
  1169. cmKeyPress(&kb);
  1170. }
  1171. //------------------------------------------------------------------------------------------------------------
  1172. cmAudioOffsetScale* cmAudioOffsetScaleAlloc( cmCtx* c, cmAudioOffsetScale* ap, unsigned procSmpCnt, double srate, cmSample_t offset, double rmsWndSecs, double dBref, unsigned flags )
  1173. {
  1174. cmAudioOffsetScale* p = cmObjAlloc( cmAudioOffsetScale, c, ap );
  1175. if( procSmpCnt > 0 && srate > 0 )
  1176. if( cmAudioOffsetScaleInit( p, procSmpCnt, srate, offset, rmsWndSecs, dBref, flags ) != cmOkRC )
  1177. cmAudioOffsetScaleFree(&p);
  1178. return p;
  1179. }
  1180. cmRC_t cmAudioOffsetScaleFree( cmAudioOffsetScale** pp )
  1181. {
  1182. cmRC_t rc = cmOkRC;
  1183. cmAudioOffsetScale* p = *pp;
  1184. if( pp == NULL || *pp == NULL )
  1185. return cmOkRC;
  1186. if((rc = cmAudioOffsetScaleFinal(p)) != cmOkRC )
  1187. return rc;
  1188. cmMemPtrFree(&p->cBufPtr);
  1189. cmMemPtrFree(&p->cCntPtr);
  1190. cmMemPtrFree(&p->outV);
  1191. cmObjFree(pp);
  1192. return rc;
  1193. }
  1194. cmRC_t cmAudioOffsetScaleInit( cmAudioOffsetScale* p, unsigned procSmpCnt, double srate, cmSample_t offset, double rmsWndSecs, double dBref, unsigned flags )
  1195. {
  1196. assert( procSmpCnt > 0 && srate > 0);
  1197. cmRC_t rc;
  1198. if((rc = cmAudioOffsetScaleFinal(p)) != cmOkRC )
  1199. return rc;
  1200. p->cBufCnt = 0;
  1201. if( cmIsFlag(flags, kRmsAudioScaleFl) )
  1202. {
  1203. if( rmsWndSecs > 0 )
  1204. {
  1205. unsigned rmsSmpCnt = srate * rmsWndSecs;
  1206. p->cBufCnt = (unsigned)ceil( rmsSmpCnt / procSmpCnt );
  1207. if( p->cBufCnt > 0 )
  1208. {
  1209. p->cBufPtr = cmMemResizeZ( cmReal_t, p->cBufPtr, p->cBufCnt );
  1210. p->cCntPtr = cmMemResizeZ( unsigned, p->cCntPtr, p->cBufCnt );
  1211. }
  1212. }
  1213. else
  1214. {
  1215. p->cBufCnt = 0;
  1216. p->cBufPtr = NULL;
  1217. p->cCntPtr = NULL;
  1218. }
  1219. }
  1220. p->cBufIdx = 0;
  1221. p->cBufCurCnt = 0;
  1222. p->cBufSum = 0;
  1223. p->cCntSum = 0;
  1224. p->outV = cmMemResize( cmSample_t, p->outV, procSmpCnt );
  1225. p->outN = procSmpCnt;
  1226. p->offset = offset;
  1227. p->dBref = dBref;
  1228. p->flags = flags;
  1229. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"audioOffsetScale");
  1230. return cmOkRC;
  1231. }
  1232. cmRC_t cmAudioOffsetScaleFinal( cmAudioOffsetScale* p )
  1233. {
  1234. //if( p != NULL)
  1235. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1236. return cmOkRC;
  1237. }
  1238. cmRC_t cmAudioOffsetScaleExec( cmAudioOffsetScale* p, const cmSample_t* sp, unsigned sn )
  1239. {
  1240. double Pref = 20.0 / 1000000; // 20 micro Pascals
  1241. cmSample_t* dbp = p->outV;
  1242. const cmSample_t* dep = dbp + sn;
  1243. double scale = 0;
  1244. // if no scaling was requested then add offset only
  1245. if( cmIsFlag(p->flags, kNoAudioScaleFl) )
  1246. {
  1247. while( dbp < dep )
  1248. *dbp++ = *sp++ + p->offset;
  1249. goto doneLabel;
  1250. }
  1251. // if fixed scaling
  1252. if( cmIsFlag(p->flags, kFixedAudioScaleFl) )
  1253. {
  1254. if( scale == 0 )
  1255. scale = pow(10,p->dBref/20);
  1256. while( dbp < dep )
  1257. *dbp++ = (*sp++ + p->offset) * scale;
  1258. }
  1259. else // if RMS scaling
  1260. {
  1261. double sum = 0;
  1262. double rms = 0;
  1263. while( dbp < dep )
  1264. {
  1265. double v = (*sp++ + p->offset) / Pref;
  1266. sum += v*v;
  1267. *dbp++ = v;
  1268. }
  1269. // if there is no RMS buffer calc RMS on procSmpCnt samles
  1270. if( p->cBufCnt == 0 )
  1271. rms = sqrt( sum / sn );
  1272. else
  1273. {
  1274. p->cBufSum -= p->cBufPtr[ p->cBufIdx ];
  1275. p->cBufSum += sum;
  1276. p->cCntSum -= p->cCntPtr[ p->cBufIdx ];
  1277. p->cCntSum += sn;
  1278. p->cBufIdx = (p->cBufIdx+1) % p->cBufCnt;
  1279. p->cBufCurCnt = cmMin( p->cBufCurCnt+1, p->cBufCnt );
  1280. assert( p->cCntSum > 0 );
  1281. rms = sqrt( p->cBufSum / p->cCntSum );
  1282. }
  1283. double sigSPL = 20*log10(rms);
  1284. scale = pow(10,(p->dBref - sigSPL)/20);
  1285. dbp = p->outV;
  1286. while( dbp < dep )
  1287. *dbp++ *= scale;
  1288. }
  1289. doneLabel:
  1290. dbp = p->outV + sn;
  1291. dep = p->outV + p->outN;
  1292. while( dbp < dep )
  1293. *dbp++ = 0;
  1294. if( p->mfp != NULL )
  1295. cmMtxFileSmpExec(p->mfp,p->outV,p->outN);
  1296. return cmOkRC;
  1297. }
  1298. //------------------------------------------------------------------------------------------------------------
  1299. cmSpecMeas* cmSpecMeasAlloc( cmCtx* c, cmSpecMeas* ap, double srate, unsigned binCnt, unsigned wndFrmCnt, unsigned flags )
  1300. {
  1301. cmSpecMeas* p = cmObjAlloc( cmSpecMeas, c, ap );
  1302. if( srate > 0 && binCnt > 0 )
  1303. if( cmSpecMeasInit( p, srate, binCnt, wndFrmCnt, flags ) != cmOkRC )
  1304. cmSpecMeasFree(&p);
  1305. return p;
  1306. }
  1307. cmRC_t cmSpecMeasFree( cmSpecMeas** pp )
  1308. {
  1309. cmRC_t rc = cmOkRC;
  1310. cmSpecMeas* p = *pp;
  1311. if( pp == NULL || *pp == NULL )
  1312. return cmOkRC;
  1313. if((rc = cmSpecMeasFinal(p)) != cmOkRC )
  1314. return rc;
  1315. cmMemPtrFree(&p->rmsV);
  1316. cmMemPtrFree(&p->hfcV);
  1317. cmMemPtrFree(&p->scnV);
  1318. cmObjFree(pp);
  1319. return rc;
  1320. }
  1321. cmRC_t cmSpecMeasInit( cmSpecMeas* p, double srate, unsigned binCnt, unsigned wndFrmCnt, unsigned flags )
  1322. {
  1323. cmRC_t rc;
  1324. if((rc = cmSpecMeasFinal(p)) != cmOkRC )
  1325. return rc;
  1326. if( cmIsFlag(flags, kUseWndSpecMeasFl) )
  1327. {
  1328. p->rmsV = cmMemResizeZ( cmReal_t, p->rmsV, wndFrmCnt );
  1329. p->hfcV = cmMemResizeZ( cmReal_t, p->hfcV, wndFrmCnt );
  1330. p->scnV = cmMemResizeZ( cmReal_t, p->scnV, wndFrmCnt );
  1331. }
  1332. p->rmsSum = 0;
  1333. p->hfcSum = 0;
  1334. p->scnSum = 0;
  1335. p->binCnt = binCnt;
  1336. p->flags = flags;
  1337. p->wndFrmCnt = wndFrmCnt;
  1338. p->frameCnt = 0;
  1339. p->frameIdx = 0;
  1340. p->binHz = srate / ((binCnt-1) * 2);
  1341. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"specMeas");
  1342. //p->rmsSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kRMS_FId, 1);
  1343. //p->hfcSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kHFC_FId, 1);
  1344. //p->scSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSC_FId, 1);
  1345. //p->ssSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSS_FId, 1);
  1346. return cmOkRC;
  1347. }
  1348. cmRC_t cmSpecMeasFinal( cmSpecMeas* p )
  1349. {
  1350. //if( p != NULL )
  1351. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1352. return cmOkRC;
  1353. }
  1354. cmRC_t cmSpecMeasExec( cmSpecMeas* p, const cmReal_t* magPowV, unsigned binCnt )
  1355. {
  1356. assert( binCnt == p->binCnt );
  1357. unsigned i = 0;
  1358. const cmReal_t* sbp = magPowV;
  1359. const cmReal_t* sep = sbp + binCnt;
  1360. cmReal_t rmsSum = 0;
  1361. cmReal_t hfcSum = 0;
  1362. cmReal_t scnSum = 0;
  1363. cmReal_t ssSum = 0;
  1364. for(i=0; sbp < sep; ++i, ++sbp )
  1365. {
  1366. rmsSum += *sbp;
  1367. hfcSum += *sbp * i;
  1368. scnSum += *sbp * i * p->binHz;
  1369. }
  1370. p->frameCnt++;
  1371. if( cmIsFlag(p->flags, kUseWndSpecMeasFl) )
  1372. {
  1373. p->frameCnt = cmMin( p->frameCnt, p->wndFrmCnt );
  1374. cmReal_t* rmsV = p->rmsV + p->frameIdx;
  1375. cmReal_t* hfcV = p->hfcV + p->frameIdx;
  1376. cmReal_t* scnV = p->scnV + p->frameIdx;
  1377. p->rmsSum -= *rmsV;
  1378. p->hfcSum -= *hfcV;
  1379. p->scnSum -= *scnV;
  1380. *rmsV = rmsSum;
  1381. *hfcV = hfcSum;
  1382. *scnV = scnSum;
  1383. p->frameIdx = (p->frameIdx+1) % p->frameCnt;
  1384. }
  1385. p->rmsSum += rmsSum;
  1386. p->hfcSum += hfcSum;
  1387. p->scnSum += scnSum;
  1388. p->rms = sqrt(p->rmsSum / (p->binCnt * p->frameCnt) );
  1389. p->hfc = p->hfcSum / ( p->binCnt * p->frameCnt );
  1390. p->sc = p->scnSum / cmMax( cmReal_EPSILON, p->rmsSum );
  1391. sbp = magPowV;
  1392. for(i=0; sbp < sep; ++i )
  1393. {
  1394. cmReal_t t = (i*p->binHz) - p->sc;
  1395. ssSum += *sbp++ * (t*t);
  1396. }
  1397. p->ss = sqrt(ssSum / cmMax( cmReal_EPSILON, p->rmsSum ));
  1398. if( p->mfp != NULL )
  1399. {
  1400. cmReal_t a[4] = { p->rms, p->hfc, p->sc, p->ss };
  1401. cmMtxFileRealExec( p->mfp, a, 4 );
  1402. }
  1403. return cmOkRC;
  1404. }
  1405. //------------------------------------------------------------------------------------------------------------
  1406. cmSigMeas* cmSigMeasAlloc( cmCtx* c, cmSigMeas* ap, double srate, unsigned procSmpCnt, unsigned measSmpCnt )
  1407. {
  1408. cmSigMeas* p = cmObjAlloc( cmSigMeas, c, ap );
  1409. p->sbp = cmShiftBufAlloc(c,&p->shiftBuf,0,0,0);
  1410. if( srate > 0 && procSmpCnt > 0 && measSmpCnt > 0 )
  1411. if( cmSigMeasInit( p, srate, procSmpCnt, measSmpCnt ) != cmOkRC )
  1412. cmSigMeasFree(&p);
  1413. return p;
  1414. }
  1415. cmRC_t cmSigMeasFree( cmSigMeas** pp )
  1416. {
  1417. cmRC_t rc = cmOkRC;
  1418. cmSigMeas* p = *pp;
  1419. if( pp==NULL || *pp==NULL)
  1420. return cmOkRC;
  1421. if((rc = cmSigMeasFinal(p)) != cmOkRC )
  1422. return rc;
  1423. cmShiftBufFree(&p->sbp);
  1424. cmObjFree(pp);
  1425. return rc;
  1426. }
  1427. cmRC_t cmSigMeasInit( cmSigMeas* p, double srate, unsigned procSmpCnt, unsigned measSmpCnt )
  1428. {
  1429. cmRC_t rc;
  1430. if((rc = cmSigMeasFinal(p)) != cmOkRC )
  1431. return rc;
  1432. if( procSmpCnt != measSmpCnt )
  1433. cmShiftBufInit( p->sbp, procSmpCnt, measSmpCnt, procSmpCnt );
  1434. p->zcrDelay = 0;
  1435. p->srate = srate;
  1436. p->measSmpCnt = measSmpCnt;
  1437. p->procSmpCnt = procSmpCnt;
  1438. //p->zcrSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kZCR_FId, 1 );
  1439. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"sigMeas");
  1440. return cmOkRC;
  1441. }
  1442. cmRC_t cmSigMeasFinal( cmSigMeas* p )
  1443. {
  1444. //if( p != NULL )
  1445. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1446. return cmOkRC;
  1447. }
  1448. cmRC_t cmSigMeasExec( cmSigMeas* p, const cmSample_t* sp, unsigned sn )
  1449. {
  1450. if( p->procSmpCnt != p->measSmpCnt )
  1451. {
  1452. cmShiftBufExec( p->sbp, sp, sn );
  1453. sp = p->sbp->outV;
  1454. sn = p->sbp->wndSmpCnt;
  1455. assert( p->sbp->wndSmpCnt == p->measSmpCnt );
  1456. }
  1457. unsigned zcn = cmVOS_ZeroCrossCount( sp, sn, NULL );
  1458. p->zcr = (cmReal_t)zcn * p->srate / p->measSmpCnt;
  1459. if( p->mfp != NULL )
  1460. cmMtxFileRealExec( p->mfp, &p->zcr, 1 );
  1461. return cmOkRC;
  1462. }
  1463. //------------------------------------------------------------------------------------------------------------
  1464. cmSRC* cmSRCAlloc( cmCtx* c, cmSRC* ap, double srate, unsigned procSmpCnt, unsigned upFact, unsigned dnFact )
  1465. {
  1466. cmSRC* p = cmObjAlloc( cmSRC, c,ap );
  1467. cmFilterAlloc(c,&p->filt,NULL,0,NULL,0,0,NULL);
  1468. if( srate > 0 && procSmpCnt > 0 )
  1469. if( cmSRCInit( p, srate, procSmpCnt, upFact, dnFact ) != cmOkRC )
  1470. cmSRCFree(&p);
  1471. return p;
  1472. }
  1473. cmRC_t cmSRCFree( cmSRC** pp )
  1474. {
  1475. cmRC_t rc;
  1476. if( pp != NULL && *pp != NULL )
  1477. {
  1478. cmSRC* p = *pp;
  1479. if((rc = cmSRCFinal( p )) == cmOkRC )
  1480. {
  1481. cmFilter* fp = &p->filt;
  1482. cmFilterFree(&fp);
  1483. cmMemPtrFree(&p->outV);
  1484. cmObjFree(pp);
  1485. }
  1486. }
  1487. return cmOkRC;
  1488. }
  1489. cmRC_t cmSRCInit( cmSRC* p, double srate, unsigned procSmpCnt, unsigned upFact, unsigned dnFact )
  1490. {
  1491. cmRC_t rc;
  1492. if((rc = cmSRCFinal(p)) != cmOkRC )
  1493. return rc;
  1494. double hiRate = upFact * srate;
  1495. double loRate = hiRate / dnFact;
  1496. double minRate= cmMin( loRate, srate );
  1497. double fcHz = minRate/2;
  1498. double dHz = (fcHz * .1); // transition band is 5% of min sample rate
  1499. double passHz = fcHz-dHz;
  1500. double stopHz = fcHz;
  1501. double passDb = 0.001;
  1502. double stopDb = 20;
  1503. cmFilterInitEllip( &p->filt, hiRate, passHz, stopHz, passDb, stopDb, procSmpCnt, NULL );
  1504. //printf("CoeffCnt:%i dHz:%f passHz:%f stopHz:%f passDb:%f stopDb:%f\n", p->fir.coeffCnt, dHz, passHz, stopHz, passDb, stopDb );
  1505. p->outN = (unsigned)ceil(procSmpCnt * upFact / dnFact);
  1506. p->outV = cmMemResize( cmSample_t, p->outV, p->outN );
  1507. p->upi = 0;
  1508. p->dni = 0;
  1509. p->upFact = upFact;
  1510. p->dnFact = dnFact;
  1511. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"src");
  1512. return cmOkRC;
  1513. }
  1514. cmRC_t cmSRCFinal( cmSRC* p )
  1515. {
  1516. //if( p != NULL )
  1517. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1518. return cmOkRC;
  1519. }
  1520. cmRC_t cmSRCExec( cmSRC* p, const cmSample_t* sp, unsigned sn )
  1521. {
  1522. const cmSample_t* sep = sp + sn;
  1523. cmSample_t* op = p->outV;
  1524. const cmSample_t* oep = op + p->outN;
  1525. unsigned iN = sn * p->upFact;
  1526. unsigned i,j;
  1527. // run the filter at the upsampled rate ...
  1528. for(i=0; i<iN; ++i)
  1529. {
  1530. assert( p->upi!=0 || sp<sep );
  1531. cmSample_t x0 = p->upi==0 ? *sp++ : 0,
  1532. y0 = x0 * p->filt.b0 + p->filt.d[0];
  1533. // ... but output at the down sampled rate
  1534. if( p->dni==0 )
  1535. {
  1536. assert( op < oep );
  1537. *op++ = y0;
  1538. }
  1539. // advance the filter delay line
  1540. for(j=0; j<p->filt.cn; ++j)
  1541. p->filt.d[j] = p->filt.b[j]*x0 - p->filt.a[j]*y0 + p->filt.d[j+1];
  1542. // update the input/output clocks
  1543. p->upi = (p->upi + 1) % p->upFact;
  1544. p->dni = (p->dni + 1) % p->dnFact;
  1545. }
  1546. p->outN = op - p->outV;
  1547. if( p->mfp != NULL )
  1548. cmMtxFileSmpExec(p->mfp,p->outV,p->outN );
  1549. return cmOkRC;
  1550. }
  1551. //------------------------------------------------------------------------------------------------------------
  1552. cmConstQ* cmConstQAlloc( cmCtx* c, cmConstQ* ap, double srate, unsigned minMidiPitch, unsigned maxMidiPitch, unsigned binsPerOctave, double thresh )
  1553. {
  1554. cmConstQ* p = cmObjAlloc( cmConstQ, c, ap );
  1555. if( srate >0 )
  1556. if( cmConstQInit(p,srate,minMidiPitch,maxMidiPitch,binsPerOctave,thresh) != cmOkRC )
  1557. cmConstQFree(&p);
  1558. return p;
  1559. }
  1560. cmRC_t cmConstQFree( cmConstQ** pp )
  1561. {
  1562. cmRC_t rc;
  1563. cmConstQ* p = *pp;
  1564. if( pp==NULL || *pp==NULL)
  1565. return cmOkRC;
  1566. if((rc = cmConstQFinal(p)) != cmOkRC )
  1567. return rc;
  1568. cmMemPtrFree(&p->fiV);
  1569. cmMemPtrFree(&p->foV);
  1570. cmMemPtrFree(&p->skM);
  1571. cmMemPtrFree(&p->outV);
  1572. cmMemPtrFree(&p->magV);
  1573. cmMemPtrFree(&p->skBegV);
  1574. cmMemPtrFree(&p->skEndV);
  1575. cmObjFree(pp);
  1576. return cmOkRC;
  1577. }
  1578. cmRC_t cmConstQInit( cmConstQ* p, double srate, unsigned minMidiPitch, unsigned maxMidiPitch, unsigned binsPerOctave, double thresh )
  1579. {
  1580. cmRC_t rc;
  1581. if((rc = cmConstQFinal(p)) != cmOkRC )
  1582. return rc;
  1583. cmReal_t minHz = cmMidiToHz(minMidiPitch);
  1584. cmReal_t maxHz = cmMidiToHz(maxMidiPitch);
  1585. cmReal_t Q = 1.0/(pow(2,(double)1.0/binsPerOctave)-1);
  1586. unsigned K = (unsigned)ceil( binsPerOctave * log2(maxHz/minHz) );
  1587. unsigned fftN = cmNextPowerOfTwo( (unsigned)ceil(Q*srate/minHz) );
  1588. unsigned k = 0;
  1589. p->fiV = cmMemResize(cmComplexR_t, p->fiV, fftN);
  1590. p->foV = cmMemResize(cmComplexR_t, p->foV, fftN);
  1591. cmFftPlanR_t plan = cmFft1dPlanAllocR(fftN, p->fiV, p->foV, FFTW_FORWARD, FFTW_ESTIMATE );
  1592. p->wndSmpCnt = fftN;
  1593. p->constQBinCnt = K;
  1594. p->binsPerOctave= binsPerOctave;
  1595. p->skM = cmMemResizeZ( cmComplexR_t, p->skM, p->wndSmpCnt * p->constQBinCnt);
  1596. p->outV = cmMemResizeZ( cmComplexR_t, p->outV, p->constQBinCnt);
  1597. p->magV = cmMemResizeZ( cmReal_t, p->magV, p->constQBinCnt);
  1598. p->skBegV = cmMemResizeZ( unsigned, p->skBegV, p->constQBinCnt);
  1599. p->skEndV = cmMemResizeZ( unsigned, p->skEndV, p->constQBinCnt);
  1600. //p->mfp = cmCtxAllocDebugFile( p->obj.ctx, "constQ");
  1601. //printf("hz:%f %f bpo:%i sr:%f thresh:%f Q:%f K%i (cols) fftN:%i (rows)\n", minHz,maxHz,binsPerOctave,srate,thresh,Q,K,fftN);
  1602. double* hamm = NULL;
  1603. // note that the bands are created in reverse order
  1604. for(k=0; k<K; ++k)
  1605. {
  1606. unsigned iN = ceil( Q * srate / (minHz * pow(2,(double)k/binsPerOctave)));
  1607. unsigned start = fftN/2;
  1608. //double hamm[ iN ];
  1609. hamm = cmMemResizeZ(double,hamm,iN);
  1610. memset( p->fiV, 0, fftN * sizeof(cmComplexR_t));
  1611. memset( p->foV, 0, fftN * sizeof(cmComplexR_t));
  1612. cmVOD_Hamming( hamm, iN );
  1613. cmVOD_DivVS( hamm, iN, iN );
  1614. if( cmIsEvenU(iN) )
  1615. start -= iN/2;
  1616. else
  1617. start -= (iN+1)/2;
  1618. //printf("k:%i iN:%i start:%i %i\n",k,iN,start,start+iN-1);
  1619. unsigned i = start;
  1620. for(; i<=start+iN-1; ++i)
  1621. {
  1622. double arg = 2.0*M_PI*Q*(i-start)/iN;
  1623. double mag = hamm[i-start];
  1624. p->fiV[i-1] = (mag * cos(arg)) + (mag * I * sin(arg));
  1625. }
  1626. cmFftExecuteR(plan);
  1627. // since the bands are created in reverse order they are also stored in reverse order
  1628. // (i.e column k-1 is stored first and column 0 is stored last)
  1629. i=0;
  1630. unsigned minIdx = -1;
  1631. unsigned maxIdx = 0;
  1632. for(; i<fftN; ++i)
  1633. {
  1634. bool fl = cabs(p->foV[i]) <= thresh;
  1635. p->skM[ (k*p->wndSmpCnt) + i ] = fl ? 0 : p->foV[i]/fftN;
  1636. if( fl==false && minIdx == -1 )
  1637. minIdx = i;
  1638. if( fl==false && i>maxIdx )
  1639. maxIdx = i;
  1640. }
  1641. p->skBegV[k] = minIdx;
  1642. p->skEndV[k] = maxIdx;
  1643. }
  1644. cmMemPtrFree(&hamm);
  1645. cmFftPlanFreeR(plan);
  1646. return cmOkRC;
  1647. }
  1648. cmRC_t cmConstQFinal( cmConstQ* p )
  1649. {
  1650. //if( p != NULL )
  1651. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1652. return cmOkRC;
  1653. }
  1654. cmRC_t cmConstQExec( cmConstQ* p, const cmComplexR_t* ftV, unsigned srcBinCnt )
  1655. {
  1656. //acVORC_MultVVM( p->outV, p->constQBinCnt,ftV,p->wndSmpCnt, p->skM );
  1657. cmReal_t* mbp = p->magV;
  1658. cmComplexR_t* dbp = p->outV;
  1659. const cmComplexR_t* dep = p->outV + p->constQBinCnt;
  1660. unsigned i = 0;
  1661. for(; dbp < dep; ++dbp,++i,++mbp )
  1662. {
  1663. const cmComplexR_t* sbp = ftV + p->skBegV[i];
  1664. const cmComplexR_t* kp = p->skM + (i*p->wndSmpCnt) + p->skBegV[i];
  1665. const cmComplexR_t* ep = kp + (p->skEndV[i] - p->skBegV[i]) + 1;
  1666. *dbp = 0;
  1667. while( kp < ep )
  1668. *dbp += *sbp++ * *kp++;
  1669. *mbp = cmCabsR(*dbp);
  1670. }
  1671. if( p->mfp != NULL )
  1672. cmMtxFileComplexExec( p->mfp, p->outV, p->constQBinCnt, 1 );
  1673. return cmOkRC;
  1674. }
  1675. void cmConstQTest( cmConstQ* p )
  1676. {
  1677. cmKbRecd kb;
  1678. unsigned i,j;
  1679. cmSample_t* t = cmMemAlloc( cmSample_t, p->wndSmpCnt );
  1680. for(i=0; i<p->constQBinCnt; ++i)
  1681. {
  1682. for(j=0; j<p->wndSmpCnt; ++j)
  1683. t[j] = cabs( p->skM[ (i*p->wndSmpCnt) + j ]);
  1684. //cmPlotClear();
  1685. cmPlotLineS( NULL, NULL, t, NULL, 500, NULL, kSolidPlotLineId );
  1686. }
  1687. cmPlotDraw();
  1688. cmKeyPress(&kb);
  1689. cmMemPtrFree(&t);
  1690. }
  1691. //------------------------------------------------------------------------------------------------------------
  1692. cmHpcp* cmTunedHpcpAlloc( cmCtx* c, cmHpcp* ap, unsigned binsPerOctave, unsigned constQBinCnt, unsigned cqMinMidiPitch, unsigned frameCnt, unsigned medFiltOrder )
  1693. {
  1694. cmHpcp* p = cmObjAlloc( cmHpcp, c, ap );
  1695. if( binsPerOctave > 0 && constQBinCnt >> 0 )
  1696. if( cmTunedHpcpInit( p, binsPerOctave, constQBinCnt, cqMinMidiPitch, frameCnt, medFiltOrder ) != cmOkRC)
  1697. cmTunedHpcpFree(&p);
  1698. return p;
  1699. }
  1700. cmRC_t cmTunedHpcpFree( cmHpcp** pp )
  1701. {
  1702. cmRC_t rc = cmOkRC;
  1703. cmHpcp* p = *pp;
  1704. if(pp==NULL || *pp==NULL)
  1705. return cmOkRC;
  1706. if((rc = cmTunedHpcpFinal(p)) != cmOkRC )
  1707. return rc;
  1708. cmMemPtrFree(&p->hpcpM);
  1709. cmMemPtrFree(&p->fhpcpM);
  1710. cmMemPtrFree(&p->histV);
  1711. cmMemPtrFree(&p->outM);
  1712. cmMemPtrFree(&p->meanV);
  1713. cmMemPtrFree(&p->varV);
  1714. cmObjFree(pp);
  1715. return cmOkRC;
  1716. }
  1717. cmRC_t cmTunedHpcpInit( cmHpcp* p, unsigned binsPerOctave, unsigned constQBinCnt, unsigned cqMinMidiPitch, unsigned frameCnt, unsigned medFiltOrder )
  1718. {
  1719. assert( binsPerOctave % kStPerOctave == 0 );
  1720. assert( cmIsOddU( binsPerOctave / kStPerOctave ) );
  1721. cmRC_t rc;
  1722. if((rc = cmTunedHpcpFinal(p)) != cmOkRC )
  1723. return rc;
  1724. p->histN = binsPerOctave/kStPerOctave;
  1725. p->hpcpM = cmMemResizeZ( cmReal_t, p->hpcpM, frameCnt*binsPerOctave );
  1726. p->fhpcpM = cmMemResizeZ( cmReal_t, p->fhpcpM, binsPerOctave*frameCnt );
  1727. p->histV = cmMemResizeZ( unsigned, p->histV, p->histN );
  1728. p->outM = cmMemResizeZ( cmReal_t, p->outM, kStPerOctave * frameCnt );
  1729. p->meanV = cmMemResizeZ( cmReal_t, p->meanV, kStPerOctave );
  1730. p->varV = cmMemResizeZ( cmReal_t, p->varV, kStPerOctave );
  1731. p->constQBinCnt = constQBinCnt;
  1732. p->binsPerOctave = binsPerOctave;
  1733. p->frameCnt = frameCnt;
  1734. p->frameIdx = 0;
  1735. p->cqMinMidiPitch= cqMinMidiPitch;
  1736. p->medFiltOrder = medFiltOrder;
  1737. //p->mf0p = cmCtxAllocDebugFile(p->obj.ctx,"hpcp");
  1738. //p->mf1p = cmCtxAllocDebugFile(p->obj.ctx,"fhpcp");
  1739. //p->mf2p = cmCtxAllocDebugFile(p->obj.ctx,"chroma");
  1740. return cmOkRC;
  1741. }
  1742. cmRC_t cmTunedHpcpFinal( cmHpcp* p )
  1743. {
  1744. /*
  1745. if( p != NULL )
  1746. {
  1747. cmCtxFreeDebugFile(p->obj.ctx,&p->mf0p);
  1748. cmCtxFreeDebugFile(p->obj.ctx,&p->mf1p);
  1749. cmCtxFreeDebugFile(p->obj.ctx,&p->mf2p);
  1750. }
  1751. */
  1752. return cmOkRC;
  1753. }
  1754. cmRC_t cmTunedHpcpExec( cmHpcp* p, const cmComplexR_t* cqp, unsigned cqn )
  1755. {
  1756. assert( cqn == p->constQBinCnt );
  1757. // if there is no space to store the output then do nothing
  1758. if( p->frameIdx >= p->frameCnt )
  1759. return cmOkRC;
  1760. unsigned octCnt = (unsigned)floor(p->constQBinCnt / p->binsPerOctave);
  1761. unsigned i;
  1762. cmReal_t hpcpV[ p->binsPerOctave + 2 ];
  1763. unsigned idxV[ p->binsPerOctave ];
  1764. unsigned binsPerSt = p->binsPerOctave / kStPerOctave;
  1765. // Notice that the first and last elements of p->hpcp are reserved for
  1766. // use in producing the appeareance of circularity for the peak picking
  1767. // algorithm. The cmtual hpcp[] data begins on the index 1 (not 0) and
  1768. // ends on p->binsPerOctave (not p->binsPerOctave-1).
  1769. // sum the constQBinCnt constant Q bins into binsPerOctave bins to form the HPCP
  1770. for(i=0; i<p->binsPerOctave; ++i)
  1771. {
  1772. cmReal_t sum = 0;
  1773. const cmComplexR_t* sbp = cqp + i;
  1774. const cmComplexR_t* sep = cqp + (octCnt * p->binsPerOctave);
  1775. for(; sbp < sep; sbp += p->binsPerOctave)
  1776. sum += cmCabsR(*sbp);
  1777. hpcpV[i+1] = sum;
  1778. }
  1779. // shift the lowest ST center bin to (binsPerSt+1)/2 such that an equal number of
  1780. // flat and sharp bins are above an below it
  1781. int rotateCnt = ((binsPerSt+1)/2) - 1;
  1782. // shift pitch class C to the lowest semitone boundary
  1783. rotateCnt -= ( 48-(int)p->cqMinMidiPitch) * binsPerSt;
  1784. // perform the shift
  1785. cmVOR_Rotate(hpcpV+1, p->binsPerOctave, rotateCnt);
  1786. // duplicate the first and last bin to produce circularity in the hpcp
  1787. hpcpV[0] = hpcpV[ p->binsPerOctave ];
  1788. hpcpV[ p->binsPerOctave+1 ] = hpcpV[1];
  1789. // locate the indexes of the positive peaks in the hpcp
  1790. unsigned pkN = cmVOR_PeakIndexes( idxV, p->binsPerOctave, hpcpV, p->binsPerOctave, 0 );
  1791. // Convert the peak indexes to values in the range 0 to binsPerSet-1
  1792. // If stPerBin == 3 : 0=flat 1=in tune 2=sharp
  1793. cmVOU_Mod( idxV, pkN, binsPerSt );
  1794. // Form a histogram to keep count of the number of flat,in-tune and sharp peaks
  1795. cmVOU_Hist( p->histV, binsPerSt, idxV, pkN );
  1796. // store the hpcpV[] to the row p->hpcpM[p->frameIdx,:]
  1797. cmVOR_CopyN( p->hpcpM + p->frameIdx, p->binsPerOctave, p->frameCnt, hpcpV+1, 1 );
  1798. // write the hpcp debug file
  1799. if( p->mf0p != NULL )
  1800. cmMtxFileRealExecN( p->mf0p, p->hpcpM + p->frameIdx, p->binsPerOctave, p->frameCnt );
  1801. p->frameIdx++;
  1802. return cmOkRC;
  1803. }
  1804. cmRC_t cmTunedHpcpTuneAndFilter( cmHpcp* p)
  1805. {
  1806. // note: p->frameIdx now holds the cmtual count of frames in p->hpcpA[].
  1807. // p->frameCnt holds the allocated count of frames in p->hpcpA[].
  1808. unsigned i,j;
  1809. // filter each column of hpcpA[] into each row of fhpcpA[]
  1810. for(i=0; i<p->binsPerOctave; ++i)
  1811. {
  1812. cmVOR_MedianFilt( p->hpcpM + (i * p->frameCnt), p->frameIdx, p->medFiltOrder, p->fhpcpM + i, p->binsPerOctave );
  1813. // write the fhpcp[i,:] to the debug file
  1814. if( p->mf1p != NULL )
  1815. cmMtxFileRealExecN( p->mf1p, p->fhpcpM + i, p->frameIdx, p->binsPerOctave );
  1816. }
  1817. unsigned binsPerSt = p->histN;
  1818. assert( (binsPerSt > 0) && (cmIsOddU(binsPerSt)) );
  1819. unsigned maxIdx = cmVOU_MaxIndex(p->histV,binsPerSt,1);
  1820. int tuneShift = -(maxIdx - ((binsPerSt+1)/2));
  1821. cmReal_t gaussWndV[ binsPerSt ];
  1822. // generate a gaussian window
  1823. cmVOR_GaussWin( gaussWndV, binsPerSt, 2.5 );
  1824. // Rotate the window to apply tuning via the weighted sum operation below
  1825. // (the result will be equivalent to rotating p->fhpcpM[] prior to reducing the )
  1826. cmVOR_Rotate(gaussWndV, binsPerSt, tuneShift);
  1827. // zero the meanV[] before summing into it
  1828. cmVOR_Fill(p->meanV,kStPerOctave,0);
  1829. // for each frame
  1830. for(i=0; i<p->frameIdx; ++i)
  1831. {
  1832. // for each semitone
  1833. for(j=0; j<kStPerOctave; ++j)
  1834. {
  1835. // reduce each semitone to a single value by forming a weighted sum of all the assoc'd bins
  1836. cmReal_t sum = cmVOR_MultSumVV( gaussWndV, p->fhpcpM + (i*p->binsPerOctave) + (j*binsPerSt), binsPerSt );
  1837. // store time-series output to the ith column
  1838. p->outM[ (i*kStPerOctave) + j ] = sum;
  1839. // calc the sum of all chroma values in bin j.
  1840. p->meanV[ j ] += sum;
  1841. }
  1842. // write the chroma debug file
  1843. if( p->mf2p != NULL )
  1844. cmMtxFileRealExec( p->mf2p, p->outM + (i*kStPerOctave), kStPerOctave );
  1845. }
  1846. // form the chroma mean from the sum calc'd above
  1847. cmVOR_DivVS( p->meanV, kStPerOctave, p->frameIdx );
  1848. // variance
  1849. for(j=0; j<kStPerOctave; ++j)
  1850. p->varV[j] = cmVOR_VarianceN( p->outM + j, p->frameIdx, kStPerOctave, p->meanV + j );
  1851. return cmOkRC;
  1852. }
  1853. //------------------------------------------------------------------------------------------------------------
  1854. /*
  1855. cmStatsProc* cmStatsProcAlloc( cmCtx* c, cmStatsProc* p, unsigned wndEleCnt, unsigned flags )
  1856. {
  1857. cmStatsProc* op = cmObjAlloc(cmStatsProc,c,p);
  1858. if( wndEleCnt > 0 )
  1859. if( cmStatsProcInit(op, wndEleCnt, flags ) != cmOkRC )
  1860. cmStatsProcFree(&op);
  1861. if( op != NULL )
  1862. cmCtxSetStatsProc(c,op);
  1863. return op;
  1864. }
  1865. cmRC_t cmStatsProcFree( cmStatsProc** pp )
  1866. {
  1867. cmRC_t rc = cmOkRC;
  1868. cmStatsProc* p = *pp;
  1869. if( pp == NULL || *pp == NULL )
  1870. return cmOkRC;
  1871. if((rc = cmStatsProcFinal(p) ) != cmOkRC )
  1872. return rc;
  1873. cmCtxSetStatsProc(p->obj.ctx,NULL); // complement to cmSetStatsProc() in cmStatsProcAlloc()
  1874. cmMemPtrFree(&p->regArrayV);
  1875. cmMemPtrFree(&p->m);
  1876. cmMemPtrFree(&p->sumV);
  1877. cmMemPtrFree(&p->meanV);
  1878. cmMemPtrFree(&p->varV);
  1879. cmObjFree(pp);
  1880. return cmOkRC;
  1881. }
  1882. cmRC_t cmStatsProcInit( cmStatsProc* p, unsigned wndEleCnt, unsigned flags )
  1883. {
  1884. cmRC_t rc;
  1885. if((rc = cmStatsProcFinal(p)) != cmOkRC )
  1886. return rc;
  1887. p->wndEleCnt = wndEleCnt;
  1888. p->dimCnt = 0;
  1889. p->curIdx = 0;
  1890. p->curCnt = 1;
  1891. p->flags = flags;
  1892. p->execCnt = 0;
  1893. p->regArrayN = 0;
  1894. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"statsProc");
  1895. return rc;
  1896. }
  1897. cmRC_t cmStatsProcFinal( cmStatsProc* p )
  1898. {
  1899. if( p != NULL )
  1900. cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1901. return cmOkRC;
  1902. }
  1903. unsigned cmStatsProcReg( cmStatsProc* p, unsigned featId, unsigned featEleCnt )
  1904. {
  1905. cmStatsProcRecd r;
  1906. r.featId = featId;
  1907. r.idx = p->dimCnt;
  1908. r.cnt = featEleCnt;
  1909. p->dimCnt += featEleCnt;
  1910. //unsigned i;
  1911. // printf("B:\n");
  1912. //for(i=0; i<p->regArrayN; ++i)
  1913. // printf("fid:%i idx:%i cnt:%i : fid:%i cnt:%i\n", p->regArrayV[i].featId,p->regArrayV[i].idx,p->regArrayV[i].cnt,featId,featEleCnt);
  1914. //printf("\nA:\n");
  1915. p->regArrayV = cmMemResizePZ( cmStatsProcRecd, p->regArrayV, p->regArrayN+1 );
  1916. p->regArrayV[ p->regArrayN ] = r;
  1917. p->regArrayN++;
  1918. //for(i=0; i<p->regArrayN; ++i)
  1919. // printf("fid:%i idx:%i cnt:%i : fid:%i cnt:%i\n", p->regArrayV[i].featId,p->regArrayV[i].idx,p->regArrayV[i].cnt,featId,featEleCnt);
  1920. //printf("\n");
  1921. return p->regArrayN-1; // return the index of the new reg recd
  1922. }
  1923. const cmStatsProcRecd* cmStatsProcRecdPtr( cmStatsProc* p, unsigned regId )
  1924. {
  1925. assert( regId < p->regArrayN );
  1926. return p->regArrayV + regId;
  1927. }
  1928. cmRC_t cmStatsProcExecD( cmStatsProc* p, unsigned regId, const double v[], unsigned vCnt )
  1929. {
  1930. cmRC_t rc = cmOkRC;
  1931. // on the first pass allcoate the storage buffer (m) and vectors (sumV,meanV and varV)
  1932. if( p->execCnt == 0 )
  1933. {
  1934. p->sumV = cmMemResizeZ( double, p->sumV, p->dimCnt);
  1935. p->meanV = cmMemResizeZ( double, p->meanV, p->dimCnt );
  1936. p->varV = cmMemResizeZ( double, p->varV, p->dimCnt );
  1937. p->m = cmMemResizeZ( double, p->m, p->dimCnt * p->wndEleCnt );
  1938. }
  1939. // if the storage matrix is full
  1940. if( p->curIdx == p->wndEleCnt )
  1941. return rc;
  1942. // get the pointer to this data source reg recd
  1943. assert( regId < p->regArrayN);
  1944. cmStatsProcRecd* r = p->regArrayV + regId;
  1945. // the dimensionality of the incoming data must be <= the registered dimensionality
  1946. assert( r->cnt <= vCnt );
  1947. unsigned dimIdx = r->idx;
  1948. bool updateFl = cmIsFlag(p->flags,kUpdateOnExecStatProcFl);
  1949. double* sbp = p->sumV + dimIdx; // sum base ptr
  1950. // mbp point to a segment (mbp[vCnt]) in column p->curIdx
  1951. double* mbp = p->m + (p->curIdx * p->dimCnt) + dimIdx; // mem col base ptr
  1952. const double* mep = p->m + p->dimCnt * p->wndEleCnt;
  1953. // decr the current col segment from the sum
  1954. if( updateFl )
  1955. cmVOD_SubVV( sbp, vCnt, mbp );
  1956. assert( p->m <= mbp && mbp < mep && p->m <= mbp+vCnt && mbp+vCnt <= mep );
  1957. // copy in the incoming values to mem col segment
  1958. cmVOD_Copy( mbp, vCnt, v );
  1959. if( updateFl )
  1960. {
  1961. // incr the sum from the incoming value
  1962. cmVOD_AddVV( sbp, vCnt, mbp );
  1963. // use the new sum to compute new mean values
  1964. cmVOD_DivVVS( p->meanV + dimIdx, vCnt, sbp, p->curCnt );
  1965. // update the variance - cmross each row
  1966. unsigned di;
  1967. for(di=dimIdx; di<dimIdx+vCnt; ++di )
  1968. p->varV[di] = cmVOD_VarianceN( p->m + dimIdx, p->curCnt, p->dimCnt, p->meanV + dimIdx );
  1969. }
  1970. ++p->execCnt;
  1971. return cmOkRC;
  1972. }
  1973. cmRC_t cmStatsProcExecF( cmStatsProc* p, unsigned regId, const float v[], unsigned vCnt )
  1974. {
  1975. double dv[ vCnt ];
  1976. cmVOD_CopyF(dv,vCnt,v);
  1977. cmStatsProcExecD(p,regId,dv,vCnt);
  1978. return cmOkRC;
  1979. }
  1980. cmRC_t cmStatsProcCalc(cmStatsProc* p )
  1981. {
  1982. unsigned colCnt = cmMin(p->curCnt,p->wndEleCnt);
  1983. unsigned i = 0;
  1984. cmVOD_Fill(p->sumV,p->dimCnt,0);
  1985. // sum the ith col of p->m[] into p->sumV[i]
  1986. for(; i<colCnt; ++i)
  1987. {
  1988. cmVOD_AddVV( p->sumV, p->dimCnt, p->m + (i * p->dimCnt) );
  1989. if( p->mfp != NULL )
  1990. cmMtxFileDoubleExec( p->mfp, p->sumV, p->dimCnt, 1 );
  1991. }
  1992. // calc the mean of each row
  1993. cmVOD_DivVVS( p->meanV, p->dimCnt, p->sumV, colCnt );
  1994. // calc the variance cmross each row
  1995. for(i=0; i<p->dimCnt; ++i)
  1996. p->varV[i] = cmVOD_VarianceN(p->m + i, colCnt, p->dimCnt, p->meanV + i );
  1997. return cmOkRC;
  1998. }
  1999. cmRC_t cmStatsProcAdvance( cmStatsProc* p )
  2000. {
  2001. ++p->curIdx;
  2002. if( p->curIdx > p->wndEleCnt )
  2003. p->curIdx = 0;
  2004. p->curCnt = cmMin(p->curCnt+1,p->wndEleCnt);
  2005. return cmOkRC;
  2006. }
  2007. void cmStatsProcTest( cmVReportFuncPtr_t vReportFunc )
  2008. {
  2009. enum
  2010. {
  2011. wndEleCnt = 7,
  2012. dDimCnt = 3,
  2013. fDimCnt = 2,
  2014. dimCnt = dDimCnt + fDimCnt,
  2015. kTypeId0 = 0,
  2016. kTypeId1 = 1
  2017. };
  2018. unsigned flags = 0;
  2019. unsigned i;
  2020. double dd[ dDimCnt * wndEleCnt ] =
  2021. {
  2022. 0, 1, 2,
  2023. 3, 4, 5,
  2024. 6, 7, 8,
  2025. 9, 10, 11,
  2026. 12, 13, 14,
  2027. 15, 16, 17,
  2028. 18, 19, 20
  2029. };
  2030. float fd[ 14 ] =
  2031. {
  2032. 0, 1,
  2033. 2, 3,
  2034. 4, 5,
  2035. 6, 7,
  2036. 8, 9,
  2037. 10, 11,
  2038. 12, 13
  2039. };
  2040. cmCtx c;
  2041. cmCtxInit(&c, vReportFunc, vReportFunc, NULL );
  2042. cmStatsProc* p = cmStatsProcAlloc( &c, NULL, wndEleCnt, flags );
  2043. unsigned regId0 = cmStatsProcReg( p, kTypeId0, dDimCnt );
  2044. unsigned regId1 = cmStatsProcReg( p, kTypeId1, fDimCnt );
  2045. for(i=0; i<wndEleCnt; ++i)
  2046. {
  2047. cmStatsProcExecD( p, regId0, dd + (i*dDimCnt), dDimCnt );
  2048. cmStatsProcExecF( p, regId1, fd + (i*fDimCnt), fDimCnt );
  2049. cmStatsProcAdvance(p);
  2050. }
  2051. cmStatsProcCalc( p);
  2052. cmVOD_PrintE( vReportFunc, 1, p->dimCnt, p->meanV );
  2053. cmVOD_PrintE( vReportFunc, 1, p->dimCnt, p->varV );
  2054. cmStatsProcFree(&p);
  2055. }
  2056. */
  2057. //------------------------------------------------------------------------------------------------------------
  2058. cmBeatHist* cmBeatHistAlloc( cmCtx* c, cmBeatHist* ap, unsigned frmCnt )
  2059. {
  2060. cmBeatHist* p = cmObjAlloc(cmBeatHist,c,ap);
  2061. p->fft = cmFftAllocRR(c,NULL,NULL,0,kToPolarFftFl);
  2062. p->ifft = cmIFftAllocRR(c,NULL,0);
  2063. if( frmCnt > 0 )
  2064. if( cmBeatHistInit(p,frmCnt) != cmOkRC )
  2065. cmBeatHistFree(&p);
  2066. return p;
  2067. }
  2068. cmRC_t cmBeatHistFree( cmBeatHist** pp )
  2069. {
  2070. cmRC_t rc = cmOkRC;
  2071. cmBeatHist* p = *pp;
  2072. if( pp == NULL || *pp == NULL )
  2073. return cmOkRC;
  2074. if((rc = cmBeatHistFinal(p)) != cmOkRC )
  2075. return rc;
  2076. cmMemPtrFree(&p->m);
  2077. cmMemPtrFree(&p->H);
  2078. cmMemPtrFree(&p->df);
  2079. cmMemPtrFree(&p->fdf);
  2080. cmMemPtrFree(&p->histV);
  2081. cmFftFreeRR(&p->fft);
  2082. cmIFftFreeRR(&p->ifft);
  2083. cmObjFree(&p);
  2084. return rc;
  2085. }
  2086. void _cmBeatHistInitH( cmReal_t* H, unsigned hrn, unsigned hcn, unsigned ri, unsigned c0, unsigned c1 )
  2087. {
  2088. unsigned ci;
  2089. for(ci=c0; ci<=c1; ++ci)
  2090. H[ (ci*hrn) + ri ] = 1;
  2091. }
  2092. cmRC_t cmBeatHistInit( cmBeatHist* p, unsigned frmCnt )
  2093. {
  2094. cmRC_t rc;
  2095. unsigned i,j,k;
  2096. enum { kLagFact = 4, kHistBinCnt=15, kHColCnt=128 };
  2097. if((rc = cmBeatHistFinal(p)) != cmOkRC )
  2098. return rc;
  2099. p->frmCnt = frmCnt;
  2100. p->maxLagCnt = (unsigned)floor(p->frmCnt / kLagFact);
  2101. p->histBinCnt= kHistBinCnt;
  2102. p->hColCnt = kHColCnt;
  2103. p->dfi = 0;
  2104. unsigned cfbMemN = p->frmCnt * p->maxLagCnt;
  2105. unsigned hMemN = p->histBinCnt * kHColCnt;
  2106. //cmArrayResizeVZ(p->obj.ctx,&p->cfbMem, cmReal_t, &p->m, cfbMemN, &p->H, hMemN, NULL);
  2107. p->m = cmMemResizeZ( cmReal_t, p->m, cfbMemN );
  2108. p->H = cmMemResizeZ( cmReal_t, p->H, hMemN );
  2109. //p->df = cmArrayResizeZ(c,&p->dfMem, 2*p->frmCnt + kHistBinCnt, cmReal_t);
  2110. //p->fdf = p->df + p->frmCnt;
  2111. //p->histV = p->fdf + p->frmCnt;
  2112. //cmArrayResizeVZ(p->obj.ctx, &p->dfMem, cmReal_t, &p->df, p->frmCnt, &p->fdf, p->frmCnt, &p->histV, kHistBinCnt, NULL );
  2113. p->df = cmMemResizeZ( cmReal_t, p->df, p->frmCnt );
  2114. p->fdf = cmMemResizeZ( cmReal_t, p->fdf, p->frmCnt );
  2115. p->histV = cmMemResizeZ( cmReal_t, p->histV, kHistBinCnt );
  2116. cmFftInitRR( p->fft,NULL,cmNextPowerOfTwo(2*frmCnt),kToPolarFftFl);
  2117. cmIFftInitRR(p->ifft,p->fft->binCnt);
  2118. // initialize H
  2119. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 0, 103, 127 );
  2120. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 1, 86, 102 );
  2121. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 2, 73, 85 );
  2122. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 3, 64, 72 );
  2123. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 4, 57, 63 );
  2124. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 5, 51, 56 );
  2125. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 6, 46, 50 );
  2126. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 7, 43, 45 );
  2127. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 8, 39, 42 );
  2128. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 9, 36, 38 );
  2129. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 10,32, 35 );
  2130. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 11,28, 31 );
  2131. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 12,25, 27 );
  2132. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 13,21, 24 );
  2133. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 14,11, 20 );
  2134. // for each column
  2135. for(i=0; i<p->maxLagCnt; ++i)
  2136. {
  2137. // for each lag group
  2138. for(j=0; j<kLagFact; ++j)
  2139. {
  2140. for(k=0; k<=2*j; ++k)
  2141. {
  2142. unsigned idx = (i*p->frmCnt) + (i*j) + i + k;
  2143. if( idx < cfbMemN )
  2144. p->m[ idx ] = 1.0/(2*j+1);
  2145. }
  2146. }
  2147. }
  2148. double g[ p->maxLagCnt ];
  2149. double g_max = 0;
  2150. double b = 43;
  2151. b = b*b;
  2152. for(i=0; i<p->maxLagCnt; ++i)
  2153. {
  2154. double n = i+1;
  2155. g[i] = n/b * exp(-(n*n) / (2*b));
  2156. if( g[i] > g_max )
  2157. g_max = g[i];
  2158. }
  2159. // normalize g[]
  2160. cmVOD_DivVS( g, p->maxLagCnt, g_max );
  2161. // for each column of p->m[]
  2162. for(i=0; i<p->maxLagCnt; ++i)
  2163. {
  2164. double gg = g[i];
  2165. k = i*p->frmCnt;
  2166. for(j=0; j<p->frmCnt; ++j)
  2167. p->m[ k + j ] *= gg;
  2168. }
  2169. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"beatHist");
  2170. return cmOkRC;
  2171. }
  2172. cmRC_t cmBeatHistFinal( cmBeatHist* p )
  2173. {
  2174. //if( p != NULL )
  2175. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  2176. return cmOkRC;
  2177. }
  2178. cmRC_t cmBeatHistExec( cmBeatHist* p, cmSample_t df )
  2179. {
  2180. if( p->dfi < p->frmCnt )
  2181. p->df[p->dfi++] = df;
  2182. return cmOkRC;
  2183. }
  2184. cmRC_t cmBeatHistCalc( cmBeatHist* p )
  2185. {
  2186. unsigned i;
  2187. // df -= mean(df)
  2188. cmVOR_SubVS(p->df,p->frmCnt,cmVOR_Mean(p->df,p->frmCnt));
  2189. //cmPlotLineR( "dfm", NULL, p->df, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2190. // take alpha norm of df
  2191. double alpha = 9;
  2192. cmVOR_DivVS(p->df,p->frmCnt, cmVOR_AlphaNorm(p->df,p->frmCnt,alpha));
  2193. //cmPlotLineS( "dfd", NULL, p->df, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2194. // low pass forward/backward filter df[] into fdf[]
  2195. cmReal_t b[] = {0.1600, 0.3200, 0.1600};
  2196. unsigned bn = sizeof(b)/sizeof(b[0]);
  2197. cmReal_t a[] = {1.0000, -0.5949, 0.2348};
  2198. unsigned an = sizeof(a)/sizeof(a[0]);
  2199. cmFilterFilterR(p->obj.ctx,b,bn,a,an,p->df,p->frmCnt,p->fdf,p->frmCnt);
  2200. //cmPlotLineS( "fdf", NULL, p->fdf, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2201. // median filter to low-passed filtered fdf[] into df[]
  2202. cmVOR_FnThresh(p->fdf,p->frmCnt,16,p->df,1,NULL);
  2203. // subtract med filtered signal from the low pa1ssed signal.
  2204. // fdf[] -= df[];
  2205. cmVOR_SubVV(p->fdf,p->frmCnt,p->df);
  2206. // half-wave rectify fdf[] = set all negative values in fdf[] to zero.
  2207. cmVOR_HalfWaveRectify(p->fdf,p->frmCnt,p->fdf);
  2208. //cmPlotLineS( "meddf", NULL, p->fdf, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2209. // take FT of fdf[]
  2210. cmFftExecRR(p->fft,p->fdf,p->frmCnt);
  2211. // square FT magn.
  2212. cmVOR_PowVS(p->fft->magV,p->fft->binCnt,2);
  2213. //cmPlotLineS( "mag", NULL, p->fft->magV, NULL, p->fft->binCnt, NULL, kSolidPlotLineId );
  2214. // take the IFFT of the squared magnitude vector.
  2215. cmVOR_Fill(p->fft->phsV,p->fft->binCnt,0);
  2216. cmIFftExecRectRR(p->ifft,p->fft->magV,p->fft->phsV);
  2217. // Matlab automatically provides this scaling as part of the IFFT.
  2218. cmVOR_DivVS(p->ifft->outV,p->ifft->outN,p->ifft->outN);
  2219. // remove bias for short periods from CMF
  2220. for(i=0; i<p->frmCnt; ++i)
  2221. p->ifft->outV[i] /= (p->frmCnt - i);
  2222. //cmPlotLineS( "cm", NULL, p->ifft->outV, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2223. // apply comb filter to the CMF and store result in df[maxLagCnt]
  2224. cmVOR_MultVMtV(p->df,p->maxLagCnt,p->m,p->frmCnt,p->ifft->outV);
  2225. //acPlotLineS( "cfb", NULL, p->df, NULL, p->maxLagCnt, NULL, kSolidPlotLineId );
  2226. //acVOR_Print(p->obj.err.rpt,1,p->maxLagCnt,p->df);
  2227. assert( p->maxLagCnt == p->hColCnt );
  2228. cmVOR_MultVMV(p->histV,p->histBinCnt,p->H,p->hColCnt,p->df);
  2229. cmReal_t bins[] = { 25, 17, 13, 9, 7, 6, 5, 3, 4, 3, 4, 4, 3, 4, 10};
  2230. cmVOR_DivVV( p->histV, p->histBinCnt, bins );
  2231. //cmPlotLineS( "cfb", NULL, p->histV, NULL, p->histBinCnt, NULL, kSolidPlotLineId );
  2232. if( p->mfp != NULL )
  2233. cmMtxFileRealExec( p->mfp, p->histV, p->histBinCnt );
  2234. return cmOkRC;
  2235. }
  2236. //------------------------------------------------------------------------------------------------------------
  2237. cmGmm_t* cmGmmAlloc( cmCtx* c, cmGmm_t* ap, unsigned K, unsigned D, const cmReal_t* gM, const cmReal_t* uM, const cmReal_t* sMM, unsigned uflags )
  2238. {
  2239. cmGmm_t* p = cmObjAlloc( cmGmm_t, c, ap );
  2240. if( K > 0 && D > 0 )
  2241. if( cmGmmInit(p,K,D,gM,uM,sMM,uflags) != cmOkRC )
  2242. cmGmmFree(&p);
  2243. return p;
  2244. }
  2245. cmRC_t cmGmmFree( cmGmm_t** pp )
  2246. {
  2247. cmRC_t rc = cmOkRC;
  2248. cmGmm_t* p = *pp;
  2249. if( pp == NULL || *pp == NULL )
  2250. return cmOkRC;
  2251. if((rc = cmGmmFinal(p)) != cmOkRC )
  2252. return rc;
  2253. cmMemPtrFree(&p->gV);
  2254. cmMemPtrFree(&p->uM);
  2255. cmMemPtrFree(&p->sMM);
  2256. cmMemPtrFree(&p->isMM);
  2257. cmMemPtrFree(&p->uMM);
  2258. cmMemPtrFree(&p->logDetV);
  2259. cmMemPtrFree(&p->t);
  2260. cmObjFree(pp);
  2261. return rc;
  2262. }
  2263. cmRC_t _cmGmmUpdateCovar( cmGmm_t* p, const cmReal_t* sMM )
  2264. {
  2265. unsigned i;
  2266. if( sMM == NULL )
  2267. return cmOkRC;
  2268. unsigned De2 = p->D*p->D;
  2269. unsigned KDe2 = p->K*De2;
  2270. cmVOR_Copy(p->sMM, KDe2, sMM);
  2271. cmVOR_Copy(p->isMM,KDe2, sMM);
  2272. cmVOR_Copy(p->uMM, KDe2, sMM);
  2273. //if( cmIsFlag(p->uflags,cmGmmCovarNoProcFl) )
  2274. // return cmOkRC;
  2275. // for each component
  2276. for(i=0; i<p->K; ++i)
  2277. {
  2278. cmReal_t* is = p->isMM + i*De2;
  2279. cmReal_t* u = p->uMM + i*De2;
  2280. cmReal_t* r;
  2281. // if the covariance matrix is diagnal
  2282. if( cmIsFlag(p->uflags,cmGmmDiagFl))
  2283. {
  2284. p->logDetV[i] = cmVOR_LogDetDiagM(is,p->D); // calc the det. of diag. covar. mtx
  2285. r = cmVOR_InvDiagM(is,p->D); // calc the inverse in place
  2286. }
  2287. else
  2288. {
  2289. p->logDetV[i] = cmVOR_LogDetM(is,p->D); // calc the det. of covar mtx
  2290. r = cmVOR_InvM(is,p->D); // calc the inverse in place
  2291. }
  2292. if( fabs(p->logDetV[i]) < 1e-20 )
  2293. {
  2294. cmCtxPrint(p->obj.ctx,"%i\n",i);
  2295. cmVOR_PrintLE("DANGER SM:\n",p->obj.err.rpt,p->D,p->D,p->sMM);
  2296. }
  2297. if( cmVOR_CholZ(u,p->D) == NULL )
  2298. {
  2299. return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (Cholesky factorization failed.) was encountered in _cmGmmUpdateCovar().");
  2300. }
  2301. if( p->logDetV[i] == 0 )
  2302. {
  2303. cmGmmPrint(p,true);
  2304. return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (det==0) was encountered in _cmGmmUpdateCovar().");
  2305. }
  2306. if( r == NULL )
  2307. {
  2308. //cmCtxPrint(c,"%i\n",i);
  2309. //cmVOR_PrintLE("DANGER SM:\n",p->obj.err.rpt,p->D,p->D,p->sMM);
  2310. return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (inversion failed) was encountered in _cmGmmUpdateCovar().");
  2311. }
  2312. }
  2313. return cmOkRC;
  2314. }
  2315. cmRC_t cmGmmInit( cmGmm_t* p, unsigned K, unsigned D, const cmReal_t* gV, const cmReal_t* uM, const cmReal_t* sMM, unsigned uflags )
  2316. {
  2317. cmRC_t rc;
  2318. if((rc = cmGmmFinal(p)) != cmOkRC )
  2319. return rc;
  2320. // gM[K] uM[DK] sMM[DDK] isMM[DDK]+ uMM[DDK] logDetV[K] t[DD] fact[K]
  2321. /*
  2322. unsigned n = K + (D*K) + (D*D*K) + (D*D*K) + (D*D*K) + K + (D*D) + K;
  2323. p->gV = cmArrayResizeZ(c,&p->memA, n, cmReal_t );
  2324. p->uM = p->gV + K;
  2325. p->sMM = p->uM + (D*K);
  2326. p->isMM = p->sMM + (D*D*K);
  2327. p->uMM = p->isMM + (D*D*K);
  2328. p->logDetV = p->uMM + (D*D*K);
  2329. p->t = p->logDetV + K;
  2330. */
  2331. //cmArrayResizeVZ(c, &p->memA, cmReal_t, &p->gV,K, &p->uM,D*K, &p->sMM,D*D*K,
  2332. //&p->isMM,D*D*K, &p->uMM,D*D*K, &p->logDetV,K, &p->t,D*D, NULL );
  2333. p->gV = cmMemResizeZ( cmReal_t, p->gV, K );
  2334. p->uM = cmMemResizeZ( cmReal_t, p->uM, D*K);
  2335. p->sMM = cmMemResizeZ( cmReal_t, p->sMM, D*D*K);
  2336. p->isMM = cmMemResizeZ( cmReal_t, p->isMM, D*D*K);
  2337. p->uMM = cmMemResizeZ( cmReal_t, p->uMM, D*D*K);
  2338. p->logDetV = cmMemResizeZ( cmReal_t, p->logDetV, K);
  2339. p->t = cmMemResizeZ( cmReal_t, p->t, D*D );
  2340. p->K = K;
  2341. p->D = D;
  2342. p->uflags = uflags;
  2343. if( gV != NULL )
  2344. cmVOR_Copy(p->gV,K,gV);
  2345. if( uM != NULL )
  2346. cmVOR_Copy(p->uM,D*K,uM);
  2347. return _cmGmmUpdateCovar(p,sMM );
  2348. }
  2349. cmRC_t cmGmmFinal( cmGmm_t* p )
  2350. { return cmOkRC; }
  2351. typedef struct
  2352. {
  2353. cmGmm_t* p;
  2354. const cmReal_t* xM;
  2355. unsigned colCnt;
  2356. } _cmGmmRdFuncData_t;
  2357. const cmReal_t* _cmGmmReadFunc( void* userPtr, unsigned colIdx )
  2358. {
  2359. assert(colIdx < ((const _cmGmmRdFuncData_t*)userPtr)->colCnt);
  2360. return ((const _cmGmmRdFuncData_t*)userPtr)->xM + (colIdx * ((const _cmGmmRdFuncData_t*)userPtr)->p->D);
  2361. }
  2362. // xM[D,xN]
  2363. // yV[xN]
  2364. // yM[xN,K]
  2365. cmRC_t cmGmmEval( cmGmm_t* p, const cmReal_t* xM, unsigned xN, cmReal_t* yV, cmReal_t* yM )
  2366. {
  2367. _cmGmmRdFuncData_t r;
  2368. r.colCnt = xN;
  2369. r.p = p;
  2370. r.xM = xM;
  2371. return cmGmmEval2(p,_cmGmmReadFunc,&r,xN,yV,yM);
  2372. }
  2373. cmRC_t cmGmmEval2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* userFuncPtr, unsigned xN, cmReal_t* yV, cmReal_t* yM)
  2374. {
  2375. cmReal_t tV[xN];
  2376. unsigned k;
  2377. //cmVOR_PrintL("cV: ",p->obj.err.rpt, 1, p->K, p->gV);
  2378. //cmVOR_PrintL("uM:\n",p->obj.err.rpt, p->D, p->K, p->uM );
  2379. //
  2380. cmVOR_Fill(yV,xN,0);
  2381. // for each component PDF
  2382. for(k=0; k<p->K; k++)
  2383. {
  2384. const cmReal_t* meanV = p->uM + (k*p->D);
  2385. const cmReal_t* isM = p->isMM + (k*p->D*p->D);
  2386. cmReal_t* pV;
  2387. if( yM == NULL )
  2388. pV = tV;
  2389. else
  2390. pV = yM + (k*xN);
  2391. // evaluate the kth component PDF with xM[1:T]
  2392. //cmVOR_MultVarGaussPDF2( pV, xM, meanV, isM, p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
  2393. cmVOR_MultVarGaussPDF3( pV, readFunc, userFuncPtr, meanV, isM, p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
  2394. // apply the kth component weight
  2395. cmVOR_MultVS( pV, xN, p->gV[k] );
  2396. // sum the result into the output vector
  2397. cmVOR_AddVV( yV, xN, pV );
  2398. }
  2399. return cmOkRC;
  2400. }
  2401. // Evaluate each component for a single data point
  2402. // xV[D] - observed data point
  2403. // yV[K] - output contains the evaluation for each component
  2404. cmRC_t cmGmmEval3( cmGmm_t* p, const cmReal_t* xV, cmReal_t* yV )
  2405. {
  2406. unsigned k;
  2407. for(k=0; k<p->K; ++k)
  2408. {
  2409. const cmReal_t* meanV = p->uM + (k*p->D);
  2410. const cmReal_t* isM = p->isMM + (k*p->D*p->D);
  2411. // evaluate the kth component PDF with xM[1:T]
  2412. cmVOR_MultVarGaussPDF2( yV + k, xV, meanV, isM, p->logDetV[k], p->D, 1, cmIsFlag(p->uflags,cmGmmDiagFl) );
  2413. // apply the kth component weight
  2414. yV[k] *= p->gV[k];
  2415. }
  2416. return cmOkRC;
  2417. }
  2418. cmReal_t _cmGmmKmeansDistFunc( void* userPtr, const cmReal_t* v0, const cmReal_t* v1, unsigned vn )
  2419. { return cmVOR_EuclidDistance(v0,v1,vn); }
  2420. cmRC_t cmGmmRandomize2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* funcUserPtr, unsigned xN, const cmReal_t* uM, const bool* roFlV )
  2421. {
  2422. unsigned k;
  2423. unsigned iV[ p->K ];
  2424. if( uM == NULL )
  2425. roFlV = NULL;
  2426. // randomize the mixture coefficients
  2427. cmVOR_Random( p->gV, p->K, 0.0, 1.0 );
  2428. cmVOR_NormalizeProbability(p->gV,p->K);
  2429. // fill iV with random integers between 0 and xN-1
  2430. cmVOU_Random( iV, p->K, xN-1 );
  2431. // for each component
  2432. for(k=0; k<p->K; ++k)
  2433. {
  2434. cmReal_t r[ p->D ];
  2435. // if this component's mean is not read-only
  2436. if( roFlV==NULL || roFlV[k]==false )
  2437. {
  2438. const cmReal_t* xV = NULL;
  2439. if( uM == NULL )
  2440. xV = readFunc( funcUserPtr, iV[k] ); // read a random frame index
  2441. else
  2442. xV = uM + (k*p->D); // select a user supplied mean vector
  2443. assert( xV != NULL );
  2444. // set the random feature vector as this components mean value
  2445. cmVOR_Copy(p->uM+(k*p->D),p->D,xV);
  2446. }
  2447. cmReal_t* sM = p->sMM+(k*p->D*p->D);
  2448. // create a random covariance mtx
  2449. if( cmIsFlag(p->uflags,cmGmmDiagFl) )
  2450. {
  2451. // create a random diag. covar mtx
  2452. cmVOR_Random(r,p->D,0.0,1.0);
  2453. cmVOR_Diag(sM,p->D,r);
  2454. }
  2455. else
  2456. {
  2457. // create a random symetric positive definite matrix
  2458. cmVOR_RandSymPosDef(sM,p->D,p->t);
  2459. }
  2460. }
  2461. unsigned* classIdxV = cmMemAllocZ(unsigned, xN );
  2462. // if some components have read-only mean's
  2463. if( uM != NULL && roFlV != NULL )
  2464. {
  2465. assert( xN >= p->K );
  2466. for(k=0; k<p->K; ++k)
  2467. classIdxV[k] = roFlV[k];
  2468. }
  2469. // use kmeans clustering to move the means closer to their center values
  2470. if( cmIsFlag( p->uflags, cmGmmSkipKmeansFl) == false )
  2471. cmVOR_Kmeans2( classIdxV, p->uM, p->K, readFunc, p->D, xN, funcUserPtr, _cmGmmKmeansDistFunc, NULL, -1, 0 );
  2472. cmMemPtrFree(&classIdxV);
  2473. return _cmGmmUpdateCovar(p,p->sMM);
  2474. }
  2475. cmRC_t cmGmmRandomize( cmGmm_t* p, const cmReal_t* xM, unsigned xN )
  2476. {
  2477. _cmGmmRdFuncData_t r;
  2478. r.colCnt = xN;
  2479. r.p = p;
  2480. r.xM = xM;
  2481. return cmGmmRandomize2(p,_cmGmmReadFunc,&r,xN,NULL,NULL);
  2482. }
  2483. // xM[D,xN]
  2484. cmRC_t cmGmmTrain( cmGmm_t* p, const cmReal_t* xM, unsigned xN, unsigned* iterCntPtr )
  2485. {
  2486. unsigned i,k;
  2487. cmRC_t rc;
  2488. if((rc = cmGmmRandomize(p,xM,xN)) != cmOkRC )
  2489. return rc;
  2490. //cmGmmPrint(c,p);
  2491. // wM[xN,K]
  2492. cmReal_t wM[ xN * p->K ]; // wM[N,K] soft assignment mtx
  2493. unsigned wV[ xN ]; // wV[N] hard assignment vector
  2494. unsigned stopCnt = 0;
  2495. unsigned curStopCnt = 0;
  2496. if( iterCntPtr != NULL )
  2497. {
  2498. stopCnt = *iterCntPtr;
  2499. *iterCntPtr = 0;
  2500. }
  2501. else
  2502. {
  2503. // BUG BUG BUG
  2504. // stopCnt is used uninitialized when iterCntPtr == NULL
  2505. assert( 0 );
  2506. }
  2507. while(1)
  2508. {
  2509. //cmVOR_NormalizeProbability(p->gV,p->K);
  2510. cmCtxPrint(p->obj.ctx,"iter:%i --------------------------------------------\n",*iterCntPtr );
  2511. cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
  2512. cmVOR_PrintL("gV:\n", p->obj.err.rpt, 1, p->K, p->gV );
  2513. //cmGmmPrint(c,p);
  2514. for(k=0; k<p->K; ++k)
  2515. {
  2516. cmReal_t* wp = wM + (k*xN);
  2517. // calc the prob that each data point in xM[] was generated by the kth gaussian
  2518. // and store as a column vector in wM[:,k]
  2519. cmVOR_MultVarGaussPDF2( wp, xM, p->uM + (k*p->D), p->isMM + (k*p->D*p->D), p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
  2520. // scale the prob by the gain coeff for gaussian k
  2521. cmVOR_MultVS( wp, xN, p->gV[k]);
  2522. }
  2523. //cmVOR_PrintL("wM:\n",p->obj.err.rpt,xN,p->K,wM);
  2524. bool doneFl = true;
  2525. for(i=0; i<xN; ++i)
  2526. {
  2527. // form a probability for the ith data point weights
  2528. cmVOR_NormalizeProbabilityN( wM + i, p->K, xN);
  2529. // select the cluster to which the ith data point is most likely to belong
  2530. unsigned mi = cmVOR_MaxIndex(wM + i, p->K, xN);
  2531. // if the ith data point changed clusters
  2532. if( mi != wV[i] )
  2533. {
  2534. doneFl = false;
  2535. wV[i] = mi;
  2536. }
  2537. }
  2538. curStopCnt = doneFl ? curStopCnt+1 : 0;
  2539. // if no data points changed owners then the clustering is complete
  2540. if( curStopCnt == stopCnt )
  2541. {
  2542. //cmVOU_PrintL("wV: ",p->obj.err.rpt,xN,1,wV);
  2543. break;
  2544. }
  2545. // for each cluster
  2546. for(k=0; k<p->K; ++k)
  2547. {
  2548. cmReal_t* uV = p->uM + (k*p->D); // meanV[k]
  2549. cmReal_t* sM = p->sMM + (k*p->D*p->D); // covarM[k]
  2550. cmReal_t sw = cmVOR_Sum(wM + (k*xN), xN );
  2551. // update the kth weight
  2552. p->gV[k] = sw / xN;
  2553. // form a sum of all data points weighted by their soft assignment to cluster k
  2554. cmReal_t sumV[p->D];
  2555. cmVOR_MultVMV( sumV, p->D, xM, xN, wM + k*xN );
  2556. // update the mean[k]
  2557. cmVOR_DivVVS(uV, p->D, sumV, sw );
  2558. // update the covar[k]
  2559. // sM += ( W(i,k) .* ((X(:,i) - uV) * (X(:,i) - uV)'));
  2560. cmVOR_Fill(sM,p->D*p->D,0);
  2561. for(i=0; i<xN; ++i)
  2562. {
  2563. cmReal_t duV[ p->D ];
  2564. cmVOR_SubVVV( duV, p->D, xM + (i*p->D), uV ); // duV[] = xM[:,i] - uV[];
  2565. cmVOR_MultMMM( p->t, p->D, p->D, duV, duV, 1 ); // t[,] = duV[] * duV[]'
  2566. cmVOR_MultVS( p->t, p->D*p->D, wM[ k * xN + i ]); // t[,] *= wM[i,k]
  2567. cmVOR_AddVV( sM, p->D*p->D, p->t ); // sM[,] += t[,];
  2568. }
  2569. cmVOR_DivVS( sM, p->D*p->D, sw ); // sM[,] ./ sw;
  2570. }
  2571. // update the inverted covar mtx and covar det.
  2572. if((rc = _cmGmmUpdateCovar(p,p->sMM )) != cmOkRC )
  2573. return rc;
  2574. if( iterCntPtr != NULL )
  2575. *iterCntPtr += 1;
  2576. }
  2577. return cmOkRC;
  2578. }
  2579. // xM[D,xN]
  2580. cmRC_t cmGmmTrain2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* userFuncPtr, unsigned xN, unsigned* iterCntPtr, const cmReal_t* uM, const bool* roFlV, int maxIterCnt )
  2581. {
  2582. unsigned i,j,k;
  2583. cmRC_t rc;
  2584. // if uM[] is not set then ignore roFlV[]
  2585. if( uM == NULL )
  2586. roFlV=NULL;
  2587. if((rc = cmGmmRandomize2(p,readFunc,userFuncPtr,xN,uM,roFlV)) != cmOkRC )
  2588. return rc;
  2589. //cmGmmPrint(c,p);
  2590. // wM[xN,K] soft assignment mtx
  2591. cmReal_t* wM = cmMemAlloc(cmReal_t, xN * p->K );
  2592. // wV[N] hard assignment vector
  2593. unsigned* wV = cmMemAlloc(unsigned, xN );
  2594. unsigned stopCnt = 0;
  2595. unsigned curStopCnt = 0;
  2596. if( iterCntPtr != NULL )
  2597. {
  2598. stopCnt = *iterCntPtr;
  2599. *iterCntPtr = 0;
  2600. }
  2601. else
  2602. {
  2603. // BUG BUG BUG
  2604. // stopCnt is used uninitialized when iterCntPtr == NULL
  2605. assert( 0 );
  2606. }
  2607. while(1)
  2608. {
  2609. //cmCtxPrint(p->obj.ctx,"iter:%i --------------------------------------------\n",*iterCntPtr );
  2610. //cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
  2611. cmVOR_PrintL("gV:\n", p->obj.err.rpt, 1, p->K, p->gV );
  2612. //cmGmmPrint(c,p);
  2613. for(k=0; k<p->K; ++k)
  2614. {
  2615. cmReal_t* wp = wM + (k*xN);
  2616. // calc the prob that each data point in xM[] was generated by the kth gaussian
  2617. // and store as a column vector in wM[:,k]
  2618. cmVOR_MultVarGaussPDF3( wp, readFunc, userFuncPtr, p->uM + (k*p->D), p->isMM + (k*p->D*p->D), p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
  2619. // scale the prob by the gain coeff for gaussian k
  2620. cmVOR_MultVS( wp, xN, p->gV[k]);
  2621. }
  2622. //cmVOR_PrintL("wM:\n",p->obj.err.rpt,xN,p->K,wM);
  2623. bool doneFl = true;
  2624. unsigned changeCnt = 0;
  2625. for(i=0; i<xN; ++i)
  2626. {
  2627. // form a probability for the ith data point weights
  2628. cmVOR_NormalizeProbabilityN( wM + i, p->K, xN);
  2629. // select the cluster to which the ith data point is most likely to belong
  2630. unsigned mi = cmVOR_MaxIndex(wM + i, p->K, xN);
  2631. // if the ith data point changed clusters
  2632. if( mi != wV[i] )
  2633. {
  2634. ++changeCnt;
  2635. doneFl = false;
  2636. wV[i] = mi;
  2637. }
  2638. }
  2639. curStopCnt = doneFl ? curStopCnt+1 : 0;
  2640. printf("%i stopCnt:%i changeCnt:%i\n",*iterCntPtr,curStopCnt,changeCnt);
  2641. // if no data points changed owners then the clustering is complete
  2642. if( curStopCnt == stopCnt )
  2643. {
  2644. //cmVOU_PrintL("wV: ",p->obj.err.rpt,xN,1,wV);
  2645. break;
  2646. }
  2647. // if a maxIterCnt was given and the cur iter cnt exceeds the max iter cnt then stop
  2648. if( maxIterCnt>=1 && *iterCntPtr >= maxIterCnt )
  2649. break;
  2650. // form a sum of all data points weighted by their soft assignment to cluster k
  2651. // NOTE: cmGmmTrain() performs this step more efficiently because it use
  2652. // an LAPACK matrix multiply.
  2653. cmReal_t sumM[ p->D * p->K ];
  2654. cmVOR_Zero(sumM,p->D*p->K);
  2655. for(i=0; i<xN; ++i)
  2656. {
  2657. const cmReal_t* xV = readFunc( userFuncPtr, i );
  2658. assert( xV != NULL );
  2659. for(k=0; k<p->K; ++k)
  2660. {
  2661. cmReal_t weight = wM[ i + (k*xN)];
  2662. for(j=0; j<p->D; ++j)
  2663. sumM[ j + (k*p->D) ] += xV[j] * weight;
  2664. }
  2665. }
  2666. // for each cluster that is not marked as read-only
  2667. for(k=0; k<p->K; ++k)
  2668. {
  2669. cmReal_t* uV = p->uM + (k*p->D); // meanV[k]
  2670. cmReal_t* sM = p->sMM + (k*p->D*p->D); // covarM[k]
  2671. cmReal_t sw = cmVOR_Sum(wM + (k*xN), xN );
  2672. // update the kth weight
  2673. p->gV[k] = sw / xN;
  2674. // if this component's mean is not read-only
  2675. if( (roFlV==NULL || roFlV[k]==false) && sw != 0)
  2676. {
  2677. // get vector of all data points weighted by their soft assignment to cluster k
  2678. cmReal_t* sumV = sumM + (k*p->D); // sumV[p->D];
  2679. // update the mean[k]
  2680. cmVOR_DivVVS(uV, p->D, sumV, sw );
  2681. }
  2682. // update the covar[k]
  2683. // sM += ( W(i,k) .* ((X(:,i) - uV) * (X(:,i) - uV)'));
  2684. cmVOR_Fill(sM,p->D*p->D,0);
  2685. for(i=0; i<xN; ++i)
  2686. {
  2687. cmReal_t duV[ p->D ];
  2688. const cmReal_t* xV = readFunc( userFuncPtr, i );
  2689. assert( xV != NULL );
  2690. cmVOR_SubVVV( duV, p->D, xV, uV ); // duV[] = xM[:,i] - uV[];
  2691. cmVOR_MultMMM( p->t, p->D, p->D, duV, duV, 1 ); // t[,] = duV[] * duV[]'
  2692. cmVOR_MultVS( p->t, p->D*p->D, wM[ k * xN + i ]); // t[,] *= wM[i,k]
  2693. cmVOR_AddVV( sM, p->D*p->D, p->t ); // sM[,] += t[,];
  2694. }
  2695. if( sw != 0 )
  2696. cmVOR_DivVS( sM, p->D*p->D, sw ); // sM[,] ./ sw;
  2697. }
  2698. // update the inverted covar mtx and covar det.
  2699. if((rc = _cmGmmUpdateCovar(p,p->sMM )) != cmOkRC )
  2700. goto errLabel;
  2701. if( iterCntPtr != NULL )
  2702. *iterCntPtr += 1;
  2703. }
  2704. cmMemPtrFree(&wM);
  2705. cmMemPtrFree(&wV);
  2706. errLabel:
  2707. return cmOkRC;
  2708. }
  2709. cmRC_t cmGmmTrain3( cmGmm_t* p, const cmReal_t* xM, unsigned xN, unsigned* iterCntPtr )
  2710. {
  2711. _cmGmmRdFuncData_t r;
  2712. r.colCnt = xN;
  2713. r.p = p;
  2714. r.xM = xM;
  2715. return cmGmmTrain2(p,_cmGmmReadFunc,&r,xN,iterCntPtr,NULL,NULL,-1);
  2716. }
  2717. cmRC_t cmGmmGenerate( cmGmm_t* p, cmReal_t* yM, unsigned yN )
  2718. {
  2719. unsigned i=0;
  2720. unsigned kV[yN];
  2721. // use weighted random selection to choose the component for each output value
  2722. cmVOR_WeightedRandInt(kV,yN, p->gV, p->K );
  2723. // cmVOU_Print(p->obj.err.rpt,1,yN,kV);
  2724. for(i=0; i<yN; ++i)
  2725. {
  2726. const cmReal_t* uV = p->uM + (kV[i] * p->D);
  2727. unsigned idx = kV[i] * p->D * p->D;
  2728. //cmVOR_PrintL("sM\n",p->obj.err.rpt,p->D,p->D,sM);
  2729. if( cmIsFlag(p->uflags,cmGmmDiagFl) )
  2730. {
  2731. const cmReal_t* sM = p->sMM + idx;
  2732. cmVOR_RandomGaussDiagM( yM + (i*p->D), p->D, 1, uV, sM );
  2733. }
  2734. else
  2735. {
  2736. const cmReal_t* uM = p->uMM + idx;
  2737. cmVOR_RandomGaussNonDiagM2(yM + (i*p->D), p->D, 1, uV, uM );
  2738. }
  2739. }
  2740. return cmOkRC;
  2741. }
  2742. void cmGmmPrint( cmGmm_t* p, bool fl )
  2743. {
  2744. unsigned k;
  2745. //cmCtx* c = p->obj.ctx;
  2746. cmVOR_PrintL("gV: ", p->obj.err.rpt, 1, p->K, p->gV );
  2747. cmVOR_PrintL("mM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
  2748. for(k=0; k<p->K; ++k)
  2749. cmVOR_PrintL("sM:\n", p->obj.err.rpt, p->D, p->D, p->sMM + (k*p->D*p->D));
  2750. if( fl )
  2751. {
  2752. for(k=0; k<p->K; ++k)
  2753. cmVOR_PrintL("isM:\n", p->obj.err.rpt, p->D, p->D, p->isMM + (k*p->D*p->D));
  2754. for(k=0; k<p->K; ++k)
  2755. cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->D, p->uMM + (k*p->D*p->D));
  2756. cmVOR_PrintL("logDetV:\n", p->obj.err.rpt, 1, p->K, p->logDetV);
  2757. }
  2758. }
  2759. void cmGmmTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  2760. {
  2761. cmCtx* c = cmCtxAlloc(NULL,rpt,lhH,stH);
  2762. unsigned K = 2;
  2763. unsigned D = 2;
  2764. cmReal_t gV[ 2 ] = { .5, .5 };
  2765. cmReal_t uM[ 4 ] = { .3, .3, .8, .8 };
  2766. cmReal_t sMM[ 8 ] = { .1, 0, 0, .1, .1, 0, 0, .1 };
  2767. unsigned flags = cmGmmDiagFl ;
  2768. unsigned M = 100;
  2769. cmReal_t xM[ D*M ];
  2770. cmReal_t yV[ M ];
  2771. unsigned i,j;
  2772. cmPlotSetup("MultDimGauss Test",1,1);
  2773. cmGmm_t* p = cmGmmAlloc(c, NULL, K, D, gV, uM, sMM, flags );
  2774. if(0)
  2775. {
  2776. cmGmmPrint(p,true);
  2777. for(i=0; i<10; i++)
  2778. for(j=0; j<20; j+=2)
  2779. {
  2780. xM[(i*20)+j] = .1 * i;;
  2781. xM[(i*20)+j + 1] = .1 * (j/2);;
  2782. }
  2783. // octave equivalent
  2784. // x0= .1 * ones(1,10);
  2785. // x = [ 0*x0 1*x0 2*x0 3*x0 4*x0 5*x0 6*x0 7*x0 8*x0 9*x0];
  2786. // x = [x; repmat([0:.1:1],1,10)];
  2787. // y = mvnpdf(x',[.3 .3],[.1 0; 0 .1]); plot(y);
  2788. cmGmmEval(p,xM,M,yV,NULL);
  2789. //cmVOR_PrintL( "xM\n", rpt, D, M, xM );
  2790. cmVOR_PrintL( "yV\n", rpt, 10, 10, yV );
  2791. //printf("y:%f\n",yV[0]);
  2792. cmPlotLineD( NULL, NULL, yV, NULL, M, NULL, kSolidPlotLineId );
  2793. }
  2794. if(0)
  2795. {
  2796. cmReal_t yM[ D*M ];
  2797. cmReal_t yMt[ M*D ];
  2798. cmReal_t uMt[ p->K*D];
  2799. unsigned iterCnt = 10;
  2800. //srand( time(NULL) );
  2801. cmGmmGenerate( p, yM, M );
  2802. p->uflags = 0; // turn off diagonal condition
  2803. if( cmGmmTrain3( p, yM, M, &iterCnt ) != cmOkRC )
  2804. return;
  2805. cmCtxPrint(c,"iterCnt:%i\n",iterCnt);
  2806. cmGmmPrint( p, true );
  2807. cmVOR_Transpose(yMt, yM, D, M );
  2808. //cmVOR_PrintL("yMt\n",vReportFunc,M,D,yMt);
  2809. cmPlotLineD(NULL, yMt, yMt+M, NULL, M, "blue", kAsteriskPlotPtId );
  2810. cmVOR_Transpose( uMt, p->uM, D, p->K);
  2811. cmVOR_PrintL("uMt:\n", p->obj.err.rpt, p->K, p->D, uMt );
  2812. cmPlotLineD(NULL, uMt, uMt+p->K, NULL, p->D, "red", kXPlotPtId );
  2813. }
  2814. if(1)
  2815. {
  2816. cmGmmFree(&p);
  2817. cmReal_t cV0[] = { .7, .3 };
  2818. cmReal_t uM0[] = { .2, .1, .1, .2 };
  2819. cmReal_t sMM0[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
  2820. unsigned flags = 0;
  2821. K = 2;
  2822. D = 2;
  2823. cmGmm_t* p = cmGmmAlloc(c,NULL, K, D, cV0, uM0, sMM0, flags );
  2824. xM[0] = 0.117228;
  2825. xM[1] = 0.110079;
  2826. cmGmmEval(p,xM,1,yV,NULL);
  2827. cmCtxPrint(c,"y: %f\n",yV[0]);
  2828. }
  2829. cmPlotDraw();
  2830. cmGmmFree(&p);
  2831. cmCtxFree(&c);
  2832. }
  2833. //------------------------------------------------------------------------------------------------------------
  2834. cmChmm_t* cmChmmAlloc( cmCtx* c, cmChmm_t* ap, unsigned stateN, unsigned mixN, unsigned dimN, const cmReal_t* iV, const cmReal_t* aM )
  2835. {
  2836. cmChmm_t* p = cmObjAlloc(cmChmm_t,c,ap);
  2837. if( stateN >0 && dimN > 0 )
  2838. if( cmChmmInit(p,stateN,mixN,dimN,iV,aM) != cmOkRC )
  2839. cmChmmFree(&p);
  2840. return p;
  2841. }
  2842. cmRC_t cmChmmFree( cmChmm_t** pp )
  2843. {
  2844. cmRC_t rc = cmOkRC;
  2845. cmChmm_t* p = *pp;
  2846. if( pp == NULL || *pp == NULL )
  2847. return cmOkRC;
  2848. if((rc = cmChmmFinal(p)) != cmOkRC )
  2849. return rc;
  2850. cmMemPtrFree(&p->iV);
  2851. cmMemPtrFree(&p->aM);
  2852. cmMemPtrFree(&p->bV);
  2853. cmMemPtrFree(&p->bM);
  2854. cmObjFree(pp);
  2855. return cmOkRC;
  2856. }
  2857. cmRC_t cmChmmInit( cmChmm_t* p, unsigned stateN, unsigned mixN, unsigned dimN, const cmReal_t* iV, const cmReal_t* aM )
  2858. {
  2859. cmRC_t rc;
  2860. unsigned i;
  2861. if((rc = cmChmmFinal(p)) != cmOkRC )
  2862. return rc;
  2863. // iV[] aM
  2864. /*
  2865. unsigned n = stateN + (stateN*stateN);
  2866. p->iV = cmArrayResizeZ(c, &p->memA, n, cmReal_t );
  2867. p->aM = p->iV + stateN;
  2868. */
  2869. //cmArrayResizeVZ(c,&p->memA, cmReal_t, &p->iV,stateN, &p->aM, stateN*stateN, NULL );
  2870. p->iV = cmMemResizeZ( cmReal_t, p->iV, stateN );
  2871. p->aM = cmMemResizeZ( cmReal_t, p->aM, stateN * stateN );
  2872. p->bV = cmMemResizeZ( cmGmm_t*, p->bV, stateN );
  2873. p->N = stateN;
  2874. p->K = mixN;
  2875. p->D = dimN;
  2876. if( iV != NULL )
  2877. cmVOR_Copy(p->iV,p->N,iV);
  2878. if( aM != NULL )
  2879. cmVOR_Copy(p->aM,p->N*p->N,aM);
  2880. for(i=0; i<p->N; ++i)
  2881. p->bV[i] = cmGmmAlloc( p->obj.ctx, NULL, p->K, p->D, NULL, NULL, NULL, 0 );
  2882. //p->mfp = cmCtxAllocDebugFile( p->obj.ctx,"chmm");
  2883. return cmOkRC;
  2884. }
  2885. cmRC_t cmChmmFinal( cmChmm_t* p )
  2886. {
  2887. if( p != NULL )
  2888. {
  2889. unsigned i;
  2890. for(i=0; i<p->N; ++i)
  2891. cmGmmFree( &p->bV[i] );
  2892. cmMemPtrFree(&p->bM);
  2893. //if( p->mfp != NULL )
  2894. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  2895. }
  2896. return cmOkRC;
  2897. }
  2898. cmRC_t cmChmmRandomize( cmChmm_t* p, const cmReal_t* oM, unsigned T )
  2899. {
  2900. cmRC_t rc;
  2901. unsigned i;
  2902. unsigned N = p->N;
  2903. // randomize the initial state probabilities
  2904. cmVOR_Random( p->iV, N, 0.0, 1.0 );
  2905. cmVOR_NormalizeProbability( p->iV, N );
  2906. // randomize the state transition matrix
  2907. cmVOR_Random( p->aM, N*N, 0.0, 1.0 );
  2908. for(i=0; i<N; ++i)
  2909. {
  2910. cmVOR_NormalizeProbabilityN( p->aM + i, N, N ); // rows of aM must sum to 1.0
  2911. if((rc = cmGmmRandomize( p->bV[i], oM, T )) != cmOkRC) // randomize the GMM assoc'd with state i
  2912. return rc;
  2913. }
  2914. cmMemPtrFree(&p->bM); // force bM[] to be recalculated
  2915. return cmOkRC;
  2916. }
  2917. cmReal_t _cmChmmKmeansDist( void* userPtr, const cmReal_t* v0, const cmReal_t* v1, unsigned vn )
  2918. { return cmVOR_EuclidDistance(v0,v1,vn); }
  2919. cmRC_t cmChmmSegKMeans( cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t threshProb, unsigned maxIterCnt, unsigned iterCnt )
  2920. {
  2921. cmCtx* c = p->obj.ctx;
  2922. cmRC_t rc = cmOkRC;
  2923. unsigned i,j,k,t;
  2924. unsigned N = p->N;
  2925. unsigned K = p->K;
  2926. unsigned D = p->D;
  2927. //unsigned qV[T];
  2928. //cmReal_t alphaM[N*T];
  2929. //unsigned clusterIdxV[T];
  2930. //cmReal_t centroidM[D*N];
  2931. /*
  2932. unsigned sz = 2*ALIGN_B(T,unsigned) +
  2933. ALIGN_B(N*T,cmReal_t) +
  2934. ALIGN_B(D*N,cmReal_t);
  2935. cmArray mem;
  2936. cmArrayAlloc(c, &mem);
  2937. unsigned* qV = (unsigned*) cmArrayResize(c, &mem, sz, char);
  2938. cmReal_t* alphaM = (cmReal_t*) (qV + ALIGN_T(T, unsigned));
  2939. unsigned* clusterIdxV = (unsigned*) (alphaM + ALIGN_T(N*T,cmReal_t));
  2940. cmReal_t* centroidM = (cmReal_t*) (clusterIdxV + ALIGN_T(T, unsigned));
  2941. */
  2942. unsigned* qV = cmMemAlloc( unsigned, T );
  2943. cmReal_t* alphaM = cmMemAlloc( cmReal_t, N*T);
  2944. unsigned* clusterIdxV = cmMemAlloc( unsigned, T );
  2945. cmReal_t* centroidM = cmMemAlloc( cmReal_t, D*N);
  2946. cmReal_t logPr = 0;
  2947. bool reportFl = true;
  2948. cmChmmRandomize(p,oM,T);
  2949. // cluster the observations into N groups
  2950. cmVOR_Kmeans( qV, centroidM, N, oM, D, T, NULL, 0, false, _cmChmmKmeansDist, NULL );
  2951. for(i=0; i<maxIterCnt; ++i)
  2952. {
  2953. unsigned jnV[N];
  2954. if( reportFl )
  2955. cmCtxPrint(c,"SegKM: ----------------------------------------------------%i\n",i);
  2956. // get the count of data points in each state
  2957. cmVOU_Fill(jnV,N,0);
  2958. for(t=0; t<T; ++t)
  2959. ++jnV[ qV[t] ];
  2960. // for each state
  2961. for(j=0; j<N; ++j)
  2962. {
  2963. cmGmm_t* g = p->bV[j];
  2964. // cluster all datapoints which were assigned to state j
  2965. cmVOR_Kmeans( clusterIdxV, g->uM, K, oM, D, T, qV, j, false, _cmChmmKmeansDist, NULL );
  2966. // for each cluster
  2967. for(k=0; k<K; ++k)
  2968. {
  2969. unsigned kN = 0;
  2970. // kN is count of data points assigned to cluster k
  2971. for(t=0; t<T; ++t)
  2972. if( clusterIdxV[t] == k )
  2973. ++kN;
  2974. g->gV[k] = (cmReal_t)kN/jnV[j];
  2975. // the covar of the kth component is the sample covar of cluster k
  2976. cmVOR_GaussCovariance(g->sMM + (k*D*D), D, oM, T, g->uM + (k*D), clusterIdxV, k );
  2977. }
  2978. if((rc = _cmGmmUpdateCovar(g, g->sMM )) != cmOkRC )
  2979. goto errLabel;
  2980. }
  2981. if( i== 0 )
  2982. {
  2983. // count transitions from i to j
  2984. for(t=0; t<T-1; ++t)
  2985. p->aM[ (qV[t+1]*N) + qV[t] ] += 1.0;
  2986. for(j=0; j<N; ++j)
  2987. {
  2988. // normalize state transitions by dividing by times in each state
  2989. for(k=0; k<N; k++)
  2990. p->aM[ (k*N) + j ] /= jnV[j];
  2991. cmVOR_NormalizeProbabilityN(p->aM + j, N, N);
  2992. cmGmmEval( p->bV[j], oM, 1, p->iV + j, NULL );
  2993. }
  2994. }
  2995. if((rc = cmChmmTrain(p, oM, T, iterCnt,0,0 )) != cmOkRC )
  2996. goto errLabel;
  2997. // calculate the prob. that the new model generated the data
  2998. cmReal_t logPr0 = cmChmmForward(p,oM,T,alphaM,NULL);
  2999. cmReal_t dLogPr = logPr0 - logPr;
  3000. if( reportFl )
  3001. cmCtxPrint(c,"pr:%f d:%f\n",logPr0,dLogPr);
  3002. if( (dLogPr > 0) && (dLogPr < threshProb) )
  3003. break;
  3004. logPr = logPr0;
  3005. // fill qV[] with the state at each time t
  3006. cmChmmDecode(p,oM,T,qV);
  3007. }
  3008. errLabel:
  3009. cmMemPtrFree(&qV);
  3010. cmMemPtrFree(&alphaM);
  3011. cmMemPtrFree(&clusterIdxV);
  3012. cmMemPtrFree(&centroidM);
  3013. return rc;
  3014. }
  3015. cmRC_t cmChmmSetGmm( cmChmm_t* p, unsigned i, const cmReal_t* wV, const cmReal_t* uM, const cmReal_t* sMM, unsigned flags )
  3016. {
  3017. assert( i < p->N);
  3018. cmMemPtrFree(&p->bM); // force bM[] to be recalculated
  3019. return cmGmmInit(p->bV[i],p->K,p->D,wV,uM,sMM,flags);
  3020. }
  3021. // Return the probability of the observation for each state.
  3022. // oV[D] - multi-dim. observation data point
  3023. // pV[N] - probability of this observation for each state
  3024. void cmChmmObsProb( const cmChmm_t* p, const cmReal_t* oV, cmReal_t* prV )
  3025. {
  3026. unsigned i;
  3027. for(i=0; i<p->N; ++i)
  3028. cmGmmEval( p->bV[i], oV, 1, prV + i, NULL );
  3029. }
  3030. // oM[D,T] - observation matrix
  3031. // alphaM[N,T] - prob of being in each state and observtin oM(:,t)
  3032. // bM[N,T] - (optional) state-observation probability matrix
  3033. // logPrV[T] - (optional) record the log prob of the data given the model at each time step
  3034. // Returns sum(logPrV[T])
  3035. cmReal_t cmChmmForward( const cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t* alphaM, cmReal_t* logPrV )
  3036. {
  3037. unsigned t;
  3038. cmReal_t logPr = 0;
  3039. // calc the prob of starting in each state
  3040. if( p->bM == NULL )
  3041. cmChmmObsProb( p, oM, alphaM );
  3042. else
  3043. cmVOR_Copy( alphaM, p->N*T, p->bM );
  3044. cmVOR_MultVV( alphaM, p->N, p->iV );
  3045. cmReal_t s = cmVOR_Sum(alphaM,p->N);
  3046. cmVOR_DivVS(alphaM,p->N,s);
  3047. //cmVOR_PrintL("alpha:\n",p->obj.err.rpt,p->N,1,alphaM);
  3048. for(t=1; t<T; ++t)
  3049. {
  3050. cmReal_t tmp[p->N];
  3051. cmReal_t* alphaV = alphaM + t*p->N;
  3052. // calc the prob of the observation for each state
  3053. if( p->bM == NULL )
  3054. cmChmmObsProb(p,oM + (t*p->D), alphaV );
  3055. // calc. the prob. of transitioning to each state at time t, from each state at t-1
  3056. cmVOR_MultVVM(tmp,p->N, alphaM + ((t-1)*p->N), p->N, p->aM );
  3057. // calc the joint prob of transitioning from each state to each state and observing O(t).
  3058. cmVOR_MultVV(alphaV, p->N, tmp );
  3059. // scale the probabilities to prevent underflow
  3060. s = cmVOR_Sum(alphaV,p->N);
  3061. cmVOR_DivVS(alphaV,p->N,s);
  3062. // track the log prob. of the model having generated the data up to time t.
  3063. cmReal_t pr = log(s);
  3064. if( logPrV != NULL )
  3065. logPrV[t] = pr;
  3066. logPr += pr;
  3067. }
  3068. return logPr;
  3069. }
  3070. // oM[D,T]
  3071. // betaM[N,T]
  3072. void cmChmmBackward( const cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t* betaM )
  3073. {
  3074. cmVOR_Fill(betaM,p->N*T,1.0);
  3075. assert(T >= 2 );
  3076. int t = (int)T - 2;
  3077. for(; t>=0; --t)
  3078. {
  3079. cmReal_t tmp[p->N];
  3080. if( p->bM == NULL )
  3081. cmChmmObsProb(p,oM+((t+1)*p->D), tmp );
  3082. else
  3083. cmVOR_Copy(tmp,p->N,p->bM + ((t+1)*p->N));
  3084. cmVOR_MultVV(tmp,p->N,betaM + ((t+1)*p->N));
  3085. cmVOR_MultVMV(betaM+(t*p->N),p->N, p->aM, p->N, tmp );
  3086. cmVOR_NormalizeProbability(betaM+(t*p->N),p->N );
  3087. }
  3088. }
  3089. cmReal_t cmChmmCompare( const cmChmm_t* p0, const cmChmm_t* p1, unsigned T )
  3090. {
  3091. assert(p0->D == p1->D);
  3092. assert(p0->N == p1->N);
  3093. cmReal_t oM[p0->D*T];
  3094. cmReal_t alphaM[p0->N*T];
  3095. cmChmmGenerate(p0,oM,T,NULL);
  3096. cmReal_t logPr00 = cmChmmForward(p0,oM,T,alphaM,NULL);
  3097. cmReal_t logPr01 = cmChmmForward(p1,oM,T,alphaM,NULL);
  3098. cmChmmGenerate(p1,oM,T,NULL);
  3099. cmReal_t logPr10 = cmChmmForward(p0,oM,T,alphaM,NULL);
  3100. cmReal_t logPr11 = cmChmmForward(p1,oM,T,alphaM,NULL);
  3101. cmReal_t d0 = (logPr01-logPr00)/T;
  3102. cmReal_t d1 = (logPr10-logPr11)/T;
  3103. return (d0+d1)/2;
  3104. }
  3105. cmRC_t cmChmmGenerate( const cmChmm_t* p, cmReal_t* oM, unsigned T, unsigned* sV )
  3106. {
  3107. unsigned i,si;
  3108. // use weighted random selection to choose an intitial state
  3109. cmVOR_WeightedRandInt(&si, 1, p->iV, p->N );
  3110. for(i=0; i<T; ++i)
  3111. {
  3112. if( sV != NULL )
  3113. sV[i] = si;
  3114. // generate a random value using the GMM assoc'd with the current state
  3115. cmGmmGenerate( p->bV[si], oM + (i*p->D), 1 );
  3116. // choose the next state using the transition weights from the current state
  3117. cmVOR_WeightedRandInt(&si, 1, p->aM + (si*p->N), p->N );
  3118. }
  3119. return cmOkRC;
  3120. }
  3121. cmRC_t cmChmmDecode( cmChmm_t* p, const cmReal_t* oM, unsigned T, unsigned* yV )
  3122. {
  3123. int i,j,t;
  3124. unsigned N = p->N;
  3125. //unsigned psiM[N*T];
  3126. //cmReal_t delta[N];
  3127. /*
  3128. unsigned sz = ALIGN_B(N*T,unsigned) + ALIGN_B(N,cmReal_t);
  3129. cmArray mem;
  3130. cmArrayAlloc(c, &mem);
  3131. unsigned* psiM = (unsigned*) cmArrayResize(c, &mem, sz, char);
  3132. cmReal_t* delta = (cmReal_t*) (psiM + ALIGN_T(N*T,unsigned));
  3133. */
  3134. unsigned* psiM = cmMemAlloc( unsigned, N*T );
  3135. cmReal_t* delta= cmMemAlloc( cmReal_t, N );
  3136. // get the prob of starting in each state
  3137. if( p->bM == NULL )
  3138. cmChmmObsProb( p, oM, delta );
  3139. else
  3140. cmVOR_Copy( delta, N, p->bM );
  3141. cmVOR_MultVV( delta, p->N, p->iV );
  3142. cmVOR_NormalizeProbability(delta, p->N);
  3143. for(t=1; t<T; ++t)
  3144. {
  3145. cmReal_t mV[N];
  3146. const cmReal_t* ap = p->aM;
  3147. // calc. the most likely new state given the most likely prev state
  3148. // and the transition matrix
  3149. for(i=0; i<N; ++i)
  3150. {
  3151. const cmReal_t* dp = delta;
  3152. unsigned psiIdx = t*N + i;
  3153. mV[i] = *ap++ * *dp++;
  3154. psiM[ psiIdx ] = 0;
  3155. // find max value of: delta .* A(:,i)
  3156. for(j=1; j<N; ++j )
  3157. {
  3158. cmReal_t v = *ap++ * *dp++;
  3159. if( v > mV[i] )
  3160. {
  3161. mV[i] = v;
  3162. psiM[ psiIdx ] = j;
  3163. }
  3164. }
  3165. }
  3166. // mV[] now holds the prob. of the max likelihood state at time t
  3167. // for each possible state at t-1
  3168. // psiM[:,t] holds the index of the max likelihood state
  3169. // condition the most likely new state on the observations
  3170. if( p->bM == NULL )
  3171. cmChmmObsProb(p,oM + (t*p->D), delta);
  3172. else
  3173. cmVOR_Copy(delta, N, p->bM + (t*p->N) );
  3174. cmVOR_MultVV(delta, N, mV ); // condition it on the max. like current states
  3175. cmVOR_NormalizeProbability( delta, N ); // normalize the prob.
  3176. }
  3177. // unwind psiM[] to form the max. likelihood state sequence
  3178. yV[T-1] = cmVOR_MaxIndex(delta,N,1);
  3179. for(t=T-2; t>=0; --t)
  3180. yV[t] = psiM[ ((t+1)*N) + yV[t+1] ];
  3181. cmMemPtrFree(&psiM);
  3182. cmMemPtrFree(&delta);
  3183. return cmOkRC;
  3184. }
  3185. cmRC_t cmChmmTrain( cmChmm_t* p, const cmReal_t* oM, unsigned T, unsigned iterCnt, cmReal_t thresh, unsigned flags )
  3186. {
  3187. cmRC_t rc = cmOkRC;
  3188. unsigned i,j,k,t,d;
  3189. unsigned iter;
  3190. unsigned N = p->N;
  3191. unsigned K = p->K;
  3192. unsigned D = p->D;
  3193. unsigned De2 = D * D;
  3194. bool mixFl = !cmIsFlag(flags,kNoTrainMixCoeffChmmFl);
  3195. bool meanFl = !cmIsFlag(flags,kNoTrainMeanChmmFl);
  3196. bool covarFl = !cmIsFlag(flags,kNoTrainCovarChmmFl);
  3197. bool bFl = mixFl | meanFl | covarFl;
  3198. bool calcBFl = true;
  3199. bool progFl = false;
  3200. bool timeProgFl = false;
  3201. cmReal_t progInc = 0.1;
  3202. cmReal_t progFrac = 0;
  3203. cmReal_t logProb = 0;
  3204. //cmReal_t alphaM[N*T]; // alpha[N,T]
  3205. //cmReal_t betaM[N*T]; // betaM[N,T]
  3206. //cmReal_t logPrV[T];
  3207. //cmReal_t EpsM[N*N];
  3208. //cmReal_t BK[N*K*T];
  3209. //cmReal_t gamma_jk[N*K];
  3210. //cmReal_t uM[K*D*N];
  3211. //cmReal_t sMM[K*De2*N];
  3212. /*
  3213. unsigned sz = ALIGN_T(N*T, cmReal_t) +
  3214. ALIGN_T(N*T, cmReal_t) +
  3215. ALIGN_T(T, cmReal_t) +
  3216. ALIGN_T(N*N, cmReal_t) +
  3217. ALIGN_T(N*K*T, cmReal_t) +
  3218. ALIGN_T(N*K, cmReal_t) +
  3219. ALIGN_T(K*D*N, cmReal_t) +
  3220. ALIGN_T(K*De2*N,cmReal_t);
  3221. cmArray mem;
  3222. cmArrayAlloc(c, &mem);
  3223. cmReal_t* alphaM = cmArrayResize(c, &mem, sz, cmReal_t); // alpha[N,T]
  3224. cmReal_t* betaM = alphaM + ALIGN_T(N*T, cmReal_t); // betaM[N,T]
  3225. cmReal_t* logPrV = betaM + ALIGN_T(N*T, cmReal_t);
  3226. cmReal_t* EpsM = logPrV + ALIGN_T(T, cmReal_t);
  3227. cmReal_t* BK = EpsM + ALIGN_T(N*N, cmReal_t);
  3228. cmReal_t* gamma_jk = BK + ALIGN_T(N*K*T,cmReal_t);
  3229. cmReal_t* uM = gamma_jk + ALIGN_T(N*K, cmReal_t);
  3230. cmReal_t* sMM = uM + ALIGN_T(K*D*N,cmReal_t);
  3231. */
  3232. cmReal_t* alphaM = cmMemAlloc( cmReal_t, N*T );
  3233. cmReal_t* betaM = cmMemAlloc( cmReal_t, N*T );
  3234. cmReal_t* logPrV = cmMemAlloc( cmReal_t, T );
  3235. cmReal_t* EpsM = cmMemAlloc( cmReal_t, N*N );
  3236. cmReal_t* BK = cmMemAlloc( cmReal_t, N*K*T );
  3237. cmReal_t* gamma_jk = cmMemAlloc( cmReal_t, N*K );
  3238. cmReal_t* uM = cmMemAlloc( cmReal_t, K*D*N );
  3239. cmReal_t* sMM = cmMemAlloc( cmReal_t, K*De2*N );
  3240. if( thresh <=0 )
  3241. thresh = 0.0001;
  3242. //cmArrayResizeZ(c,&p->memC,N*T,cmReal_t);
  3243. p->bM = cmMemResizeZ( cmReal_t, p->bM, N*T);
  3244. for(iter=0; iter<iterCnt; ++iter)
  3245. {
  3246. // zero the mean and covar summation arrays
  3247. cmVOR_Fill(uM, K*D *N,0);
  3248. cmVOR_Fill(sMM, K*De2*N,0);
  3249. cmVOR_Fill(EpsM,N*N, 0);
  3250. cmVOR_Fill(gamma_jk,N*K,0);
  3251. //
  3252. // B[i,t] The prob that state i generated oM(:,t)
  3253. // BK[i,k,t] The prob that state i component k generated oM(:,t)
  3254. // Note: B[i,t] = sum(BK(i,k,:))
  3255. //
  3256. if( calcBFl || bFl )
  3257. {
  3258. calcBFl = false;
  3259. for(t=0; t<T; ++t)
  3260. {
  3261. // prob. that state i generated objservation O[t]
  3262. for(i=0; i<N; ++i )
  3263. cmGmmEval( p->bV[i], oM + (t*D), 1, p->bM + (t*N) + i, BK + (t*N*K) + (i*K) );
  3264. }
  3265. }
  3266. // alpha[N,T] is prob. of transitioning forward to each state given the observed data
  3267. cmReal_t logProb0 = cmChmmForward( p, oM, T, alphaM, logPrV );
  3268. // check for convergence
  3269. cmReal_t dLogProb = labs(logProb0-logProb) / ((labs(logProb0)+labs(logProb)+cmReal_EPSILON)/2);
  3270. if( dLogProb < thresh )
  3271. break;
  3272. logProb = logProb0;
  3273. // betaM[N,T] is prob of transitioning backward from each state given the observed data
  3274. cmChmmBackward(p, oM, T, betaM );
  3275. if(progFl)
  3276. cmCtxPrint(p->obj.ctx,"%i (%f) ",iter+1, dLogProb );
  3277. if(timeProgFl)
  3278. progFrac = progInc;
  3279. // for each time step
  3280. for(t=0; t<T-1; ++t)
  3281. {
  3282. // oV[D] is the observation at step t
  3283. const cmReal_t* oV = oM + (t*D);
  3284. //
  3285. // Update EpsM[N,N] (6.37)
  3286. // (prob. of being in state i at time t and transitioning
  3287. // to state j at time t+1)
  3288. //
  3289. cmReal_t E[N*N];
  3290. // for each possible state transition
  3291. for(i=0; i<N; ++i)
  3292. for(j=0; j<N; ++j)
  3293. {
  3294. E[ i + (j*N) ]
  3295. = exp(log(alphaM[ (t*N) + i ])
  3296. + log(p->aM[ i + (j*N) ])
  3297. + log(p->bM[ ((t+1)*N) + j ])
  3298. + log(betaM[ ((t+1)*N) + j ]));
  3299. }
  3300. cmVOR_NormalizeProbability( E, N*N );
  3301. cmVOR_AddVV( EpsM, N*N, E );
  3302. // If t==0 then update the initial state prob's
  3303. if( t == 0 )
  3304. {
  3305. for(i=0; i<N; ++i)
  3306. p->iV[i] = cmVOR_SumN(EpsM+i, N, N);
  3307. assert( cmVOR_IsNormal(p->iV,N) );
  3308. }
  3309. if( bFl )
  3310. {
  3311. //
  3312. // Calculate gamma_jk[]
  3313. //
  3314. cmReal_t gtjk[N*K]; // gamma_jk[N,K] at time t
  3315. cmReal_t abV[N]; //
  3316. // (alphaM[j,t] * betaM[j:t]) / (sum(alphaM[:,t] * betaM[:,t]))
  3317. cmVOR_MultVVV(abV,N,alphaM + t*N, betaM+t*N);
  3318. cmReal_t abSum = cmVOR_Sum(abV,N);
  3319. if( abSum<=0 )
  3320. assert(abSum>0);
  3321. cmVOR_DivVS(abV,N,abSum);
  3322. for(j=0; j<N; ++j)
  3323. {
  3324. cmReal_t bkSum = cmVOR_Sum(BK + (t*N*K) + (j*K), K );
  3325. for(k=0; k<K; ++k)
  3326. gtjk[ (k*N)+j ] = abV[j] * (BK[ (t*N*K) + (j*K) + k ] / bkSum);
  3327. }
  3328. // sum gtjk[N,K] into gamma_jk (integrate gamma over time)
  3329. cmVOR_AddVV( gamma_jk, N*K, gtjk );
  3330. // update the mean and covar numerators
  3331. for(j=0; j<N; ++j)
  3332. {
  3333. cmReal_t* uV = uM + (j*D*K);
  3334. cmReal_t* sV = sMM + (j*De2*K);
  3335. for(k=0; k<K; ++k,uV+=D,sV+=De2)
  3336. {
  3337. cmReal_t c = gtjk[ (k*N)+j ];
  3338. if( covarFl )
  3339. {
  3340. cmReal_t dV[D];
  3341. cmReal_t dM[D*D];
  3342. // covar numerator b[j].sM[k]
  3343. cmVOR_SubVVV(dV, D, oV, p->bV[j]->uM + (k*D));
  3344. cmVOR_MultMMM( dM, D, D, dV, dV, 1 );
  3345. cmVOR_MultVS( dM, De2, c );
  3346. cmVOR_AddVV( sV, De2, dM );
  3347. }
  3348. if( meanFl )
  3349. {
  3350. // mean numerator b[j].uM[k]
  3351. for(d=0; d<D; ++d)
  3352. uV[d] += c * oV[ d ];
  3353. }
  3354. }
  3355. }
  3356. }
  3357. if( timeProgFl && (t >= floor(T*progFrac)) )
  3358. {
  3359. cmCtxPrint(p->obj.ctx,"%i ", (unsigned)round(progFrac*100) );
  3360. progFrac+=progInc;
  3361. }
  3362. } // end time loop
  3363. for(i=0; i<N; ++i)
  3364. {
  3365. // update the state transition matrix
  3366. cmReal_t den = cmVOR_SumN(EpsM + i, N, N );
  3367. assert(den != 0 );
  3368. for(j=0; j<N; ++j)
  3369. p->aM[ i + (j*N) ] = EpsM[ i + (j*N) ] / den;
  3370. if( bFl )
  3371. {
  3372. // update the mean, covariance and mix coefficient
  3373. cmGmm_t* g = p->bV[i];
  3374. const cmReal_t* uV = uM + (i*D*K);
  3375. const cmReal_t* sMV = sMM + (i*De2*K);
  3376. for(k=0; k<K; ++k,uV+=D,sMV+=De2)
  3377. {
  3378. cmReal_t gjk = gamma_jk[ (k*N) + i ];
  3379. if( meanFl )
  3380. cmVOR_DivVVS(g->uM + (k*D), D, uV, gjk );
  3381. if( covarFl )
  3382. cmVOR_DivVVS(g->sMM + (k*De2), De2, sMV, gjk );
  3383. if( mixFl )
  3384. g->gV[k] = gjk / cmVOR_SumN( gamma_jk + i, K, N );
  3385. }
  3386. if((rc = _cmGmmUpdateCovar(g,g->sMM)) != cmOkRC )
  3387. goto errLabel;
  3388. }
  3389. }
  3390. assert( cmVOR_IsNormalZ(p->aM,N*N) );
  3391. if( timeProgFl )
  3392. cmCtxPrint(p->obj.ctx,"\n");
  3393. } // end iter loop
  3394. if( progFl)
  3395. cmCtxPrint(p->obj.ctx,"\n");
  3396. if( p->mfp != NULL )
  3397. {
  3398. // first line is iV[N]
  3399. cmMtxFileRealExec(p->mfp,p->iV,p->N);
  3400. // next N lines are aM[N,N]
  3401. for(i=0; i<p->N; ++i)
  3402. cmMtxFileRealExecN(p->mfp,p->aM + i,p->N,p->N);
  3403. // next T lines are bM[T,N]
  3404. if( p->bM != NULL )
  3405. for(i=0; i<T; ++i)
  3406. cmMtxFileRealExec(p->mfp, p->bM + (i*p->N),p->N);
  3407. }
  3408. errLabel:
  3409. cmMemPtrFree(&alphaM);
  3410. cmMemPtrFree(&betaM);
  3411. cmMemPtrFree(&logPrV);
  3412. cmMemPtrFree(&EpsM);
  3413. cmMemPtrFree(&BK);
  3414. cmMemPtrFree(&gamma_jk);
  3415. cmMemPtrFree(&uM);
  3416. cmMemPtrFree(&sMM);
  3417. return rc;
  3418. }
  3419. void cmChmmPrint( cmChmm_t* p )
  3420. {
  3421. unsigned i;
  3422. cmCtxPrint(p->obj.ctx,"======================================== \n");
  3423. cmVOR_PrintL("iV: ", p->obj.err.rpt, 1, p->N, p->iV);
  3424. cmVOR_PrintL("aM:\n", p->obj.err.rpt, p->N, p->N, p->aM);
  3425. for(i=0; i<p->N; ++i)
  3426. {
  3427. cmCtxPrint(p->obj.ctx,"bV[%i] ----------------- %i \n",i,i);
  3428. cmGmmPrint(p->bV[i],false);
  3429. }
  3430. }
  3431. void cmChmmTestForward( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  3432. {
  3433. cmReal_t oM[] = {
  3434. 0.117228, 0.110079,
  3435. 0.154646, 0.210436,
  3436. 0.947468, 0.558136,
  3437. 0.202023, 0.138123,
  3438. 0.929933, 0.456102,
  3439. 0.897566, 0.685078,
  3440. 0.945177, 0.663145,
  3441. 0.272399, 0.055107,
  3442. 0.863386, 0.621546,
  3443. 0.217545, 0.274709,
  3444. 0.838777, 0.650038,
  3445. 0.134966, 0.159472,
  3446. 0.053990, 0.264051,
  3447. 0.884269, 0.550019,
  3448. 0.764787, 0.554484,
  3449. 0.114771, 0.077518,
  3450. 0.835121, 0.606137,
  3451. 0.070733, 0.120015,
  3452. 0.819814, 0.588482,
  3453. 0.105511, 0.197699,
  3454. 0.824778, 0.533047,
  3455. 0.945223, 0.511411,
  3456. 0.126971, 0.050083,
  3457. 0.869497, 0.567737,
  3458. 0.144866, 0.197363,
  3459. 0.985726, 0.590402,
  3460. 0.181094, 0.192827,
  3461. 0.162179, 0.155297,
  3462. 1.034691, 0.513413,
  3463. 0.220708, 0.036158,
  3464. 0.750061, 0.671224,
  3465. 0.246971, 0.093246,
  3466. 0.997567, 0.680491,
  3467. 0.916887, 0.530981,
  3468. 0.022328, 0.121969,
  3469. 0.794031, 0.618081,
  3470. 0.845066, 0.625512,
  3471. 0.174731, 0.094773,
  3472. 0.968665, 0.652435,
  3473. 0.932484, 0.388081,
  3474. 0.202732, 0.148710,
  3475. 0.911307, 0.637139,
  3476. 0.211127, 0.201362,
  3477. 0.138152, 0.057290,
  3478. 0.819132, 0.579888,
  3479. 0.135625, 0.176140,
  3480. 0.146017, 0.157853,
  3481. 0.950319, 0.624150,
  3482. 0.285064, 0.038825,
  3483. 0.716844, 0.575189,
  3484. 0.907433, 0.504946,
  3485. 0.219772, 0.129993,
  3486. 0.076507, 0.193079,
  3487. 0.808906, 0.548409,
  3488. 0.880892, 0.523950,
  3489. 0.758099, 0.636729,
  3490. 1.014017, 0.557120,
  3491. 0.277888, 0.181492,
  3492. 0.877588, 0.508634,
  3493. 0.251266, 0.225890,
  3494. 0.990904, 0.482949,
  3495. 0.999899, 0.534579,
  3496. 0.904179, 0.707349,
  3497. 0.952879, 0.617955,
  3498. 0.172068, 0.151984,
  3499. 1.026262, 0.662600,
  3500. 0.812003, 0.430856,
  3501. 0.173393, 0.017885,
  3502. 0.099370, 0.146661,
  3503. 0.785785, 0.564333,
  3504. 0.698222, 0.449299,
  3505. 0.276539, 0.225314,
  3506. 0.799271, 0.618159,
  3507. 0.098813, 0.090839,
  3508. 0.883666, 0.554150,
  3509. 0.274934, 0.185403,
  3510. 0.200419, 0.109972,
  3511. 0.925076, 0.608610,
  3512. 0.864486, 0.348689,
  3513. 0.176733, 0.136235,
  3514. 0.967278, 0.656875,
  3515. 0.986994, 0.659877,
  3516. 1.015618, 0.596549,
  3517. 0.689903, 0.528107,
  3518. 0.978238, 0.630989,
  3519. 0.269847, 0.144358,
  3520. 0.092303, 0.139894,
  3521. 0.168185, 0.095327,
  3522. 0.897767, 0.584203,
  3523. 0.068316, 0.018452,
  3524. 0.953395, 0.530545,
  3525. 0.266405, 0.173987,
  3526. 0.233845, 0.205276,
  3527. 0.900060, 0.477108,
  3528. 0.052909, 0.053077,
  3529. 0.885850, 0.496546,
  3530. 0.268494, 0.104785,
  3531. 1.041405, 0.655079,
  3532. 1.055915, 0.697988,
  3533. 0.181569, 0.146840
  3534. };
  3535. unsigned i;
  3536. cmReal_t iV[] = { .5 , .5};
  3537. cmReal_t A[] = { .3, .6, .7, .4 };
  3538. cmReal_t cV0[] = { .7, .3 };
  3539. cmReal_t uM0[] = { .2, .1, .1, .2 };
  3540. cmReal_t sMM0[]= { .01, 0, 0, .01, .01, 0, 0, .01 };
  3541. cmReal_t cV1[] = { .2, .8 };
  3542. cmReal_t uM1[] = { .8, .9, .9, .5 };
  3543. cmReal_t sMM1[]= { .01, 0, 0, .01, .01, 0, 0, .01 };
  3544. unsigned T = 100;
  3545. unsigned N = 2;
  3546. unsigned K = 2;
  3547. unsigned D = 2;
  3548. cmReal_t alphaM[N*T];
  3549. cmReal_t betaM[N*T];
  3550. cmReal_t logPrV[T];
  3551. unsigned qV[T];
  3552. unsigned sV[T];
  3553. cmReal_t oMt[T*D];
  3554. // scale covariance
  3555. cmVOR_MultVS(sMM0,D*D*K,1);
  3556. cmVOR_MultVS(sMM1,D*D*K,1);
  3557. cmCtx c;
  3558. cmCtxAlloc(&c,rpt,lhH,stH);
  3559. cmChmm_t* p = cmChmmAlloc(&c,NULL,N,K,D,iV,A);
  3560. cmChmmSetGmm(p,0,cV0,uM0,sMM0,0);
  3561. cmChmmSetGmm(p,1,cV1,uM1,sMM1,0);
  3562. cmChmmPrint(p);
  3563. cmChmmGenerate(p, oM, T, sV );
  3564. cmChmmForward( p, oM, T, alphaM,logPrV );
  3565. //cmVOR_PrintL("logPrV:\n",rpt,1,T,logPrV);
  3566. cmCtxPrint(&c,"log prob:%f\n", cmVOR_Sum(logPrV,T));
  3567. cmChmmBackward( p, oM, T, betaM );
  3568. //cmVOR_PrintL("beta:\n",rpt,N,T,betaM);
  3569. cmChmmDecode(p,oM,T,qV);
  3570. cmVOU_PrintL("sV:\n",rpt,1,T,sV);
  3571. cmVOU_PrintL("qV:\n",rpt,1,T,qV);
  3572. unsigned d=0;
  3573. for(i=0; i<T; ++i)
  3574. d += sV[i] != qV[i];
  3575. cmCtxPrint(&c,"Diff:%i\n",d);
  3576. cmPlotSetup("Chmm Forward Test",1,1);
  3577. cmVOR_Transpose(oMt,oM,D,T);
  3578. cmPlotLineD(NULL, oMt, oMt+T, NULL, T, "blue", kXPlotPtId );
  3579. cmPlotDraw();
  3580. cmChmmFree(&p);
  3581. }
  3582. void cmChmmTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  3583. {
  3584. time_t t = time(NULL); //0x4b9e82aa; //time(NULL);
  3585. srand( t );
  3586. printf("TIME: 0x%x\n",(unsigned)t);
  3587. //cmChmmTestForward(vReportFunc);
  3588. //return;
  3589. unsigned i;
  3590. cmReal_t iV[] = { 1.0/3.0, 1.0/3.0, 1.0/3.0 };
  3591. cmReal_t A[] = { .1, .4, .7, .4, .2, .2 };
  3592. cmReal_t cV0[] = { .7, .3 };
  3593. cmReal_t uM0[] = { .2, .1, .1, .2 };
  3594. cmReal_t sMM0[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
  3595. cmReal_t cV1[] = { .2, .8 };
  3596. cmReal_t uM1[] = { .8, .9, .9, .8 };
  3597. cmReal_t sMM1[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
  3598. cmReal_t cV2[] = { .5, .5 };
  3599. cmReal_t uM2[] = { .5, .5, .5, .5 };
  3600. cmReal_t sMM2[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
  3601. cmReal_t kmThreshProb = 0.001;
  3602. unsigned kmMaxIterCnt = 10;
  3603. unsigned iterCnt = 20;
  3604. unsigned N = sizeof(iV) / sizeof(iV[0]);
  3605. unsigned K = sizeof(cV0) / sizeof(cV0[0]);
  3606. unsigned D = sizeof(uM0) / sizeof(uM0[0]) / K;
  3607. unsigned T = 100;
  3608. cmReal_t alphaM[N*T];
  3609. cmReal_t oM[D*T];
  3610. unsigned sV[T];
  3611. unsigned qV[T];
  3612. cmCtx c;
  3613. cmCtxAlloc(&c,rpt,lhH,stH);
  3614. cmCtxPrint(&c,"N:%i K:%i D:%i\n",N,K,D);
  3615. cmChmm_t* p = cmChmmAlloc(&c,NULL,N,K,D,iV,A);
  3616. cmChmmSetGmm(p,0,cV0,uM0,sMM0,0);
  3617. cmChmmSetGmm(p,1,cV1,uM1,sMM1,0);
  3618. cmChmmSetGmm(p,2,cV2,uM2,sMM2,0);
  3619. // generate data using the parameters above
  3620. cmChmmGenerate(p,oM,T,sV);
  3621. cmVOU_PrintL("sV: ",rpt,1,T,sV);
  3622. cmChmmRandomize(p,oM,T);
  3623. if(cmChmmSegKMeans(p,oM,T,kmThreshProb,kmMaxIterCnt,iterCnt) != cmOkRC )
  3624. goto errLabel;
  3625. if( cmChmmTrain(p,oM,T,iterCnt,0,0) != cmOkRC )
  3626. goto errLabel;
  3627. //cmChmmPrint(p);
  3628. cmChmmDecode(p,oM,T,qV);
  3629. cmReal_t pr = cmChmmForward(p,oM,T,alphaM,NULL);
  3630. cmCtxPrint(&c,"pr:%f\n",pr);
  3631. cmVOU_PrintL("sV:\n",rpt,1,T,sV);
  3632. cmVOU_PrintL("qV:\n",rpt,1,T,qV);
  3633. unsigned d=0;
  3634. for(i=0; i<T; ++i)
  3635. d += sV[i] != qV[i];
  3636. cmCtxPrint(&c,"Diff:%i\n",d);
  3637. errLabel:
  3638. cmChmmFree(&p);
  3639. }
  3640. //------------------------------------------------------------------------------------------------------------
  3641. cmChord* cmChordAlloc( cmCtx* c, cmChord* ap, const cmReal_t* chromaM, unsigned T )
  3642. {
  3643. unsigned i,j;
  3644. unsigned S = 6;
  3645. unsigned N = 24;
  3646. unsigned D = 12;
  3647. cmChord* p = cmObjAlloc(cmChord,c,ap);
  3648. p->h = cmChmmAlloc( p->obj.ctx, NULL, 0, 0, 0, NULL, NULL );
  3649. if( chromaM != NULL && T > 0 )
  3650. if( cmChordInit(p,chromaM,T) != cmOkRC )
  3651. cmChordFree(&p);
  3652. p->N = N;
  3653. p->D = D;
  3654. p->S = kTonalSpaceDimCnt;
  3655. /*
  3656. // iv[N] aM[N*N] uM[D*N] sMM[D*D*N] phiM[D*S] tsxxxV[S]
  3657. unsigned n = ALIGN_T(N, cmReal_t) +
  3658. ALIGN_T(N*N, cmReal_t) +
  3659. ALIGN_T(D*N, cmReal_t) +
  3660. ALIGN_T(D*D*N,cmReal_t) +
  3661. ALIGN_T(D*S, cmReal_t) +
  3662. 2*ALIGN_T(S, cmReal_t);
  3663. p->iV = cmArrayResizeZ(c, &p->memA, n, cmReal_t);
  3664. p->aM = p->iV + ALIGN_T(N, cmReal_t);
  3665. p->uM = p->aM + ALIGN_T(N*N, cmReal_t);
  3666. p->sMM = p->uM + ALIGN_T(D*N, cmReal_t);
  3667. p->phiM = p->sMM + ALIGN_T(D*D*N,cmReal_t);
  3668. p->tsMeanV = p->phiM + ALIGN_T(D*S, cmReal_t);
  3669. p->tsVarV = p->tsMeanV + ALIGN_T(S, cmReal_t);
  3670. */
  3671. p->iV = cmMemAllocZ( cmReal_t, N );
  3672. p->aM = cmMemAllocZ( cmReal_t, N*N);
  3673. p->uM = cmMemAllocZ( cmReal_t, D*N);
  3674. p->sMM = cmMemAllocZ( cmReal_t, D*D*N );
  3675. p->phiM = cmMemAllocZ( cmReal_t, D*S);
  3676. p->tsMeanV = cmMemAllocZ( cmReal_t, S );
  3677. p->tsVarV = cmMemAllocZ( cmReal_t, S );
  3678. // initialize iV[N] (HMM initial state probabilities)
  3679. cmVOR_Fill(p->iV,N,1.0/N);
  3680. // initialize aM[N,N] (HMM transition matrix)
  3681. cmReal_t epsilon = 0.01;
  3682. cmReal_t CMaj2any[] = { 12, 2, 8, 6, 4, 10, 0, 10, 4, 6, 8, 2, 5, 5, 9, 1, 11, 3, 7, 7, 3, 11, 1, 9 };
  3683. for(i=0; i<N; ++i)
  3684. {
  3685. cmVOR_Copy( p->aM+(i*N), N, CMaj2any );
  3686. cmVOR_Rotate( CMaj2any, N, 1 );
  3687. }
  3688. cmVOR_AddVS(p->aM, N*N, epsilon);
  3689. cmVOR_DivVS(p->aM, N*N, ( (N/2)*(N/2) ) + (N*epsilon) );
  3690. //cmVOR_PrintL("A:\n",p->obj.err.rpt,N,N,A);
  3691. // initialize sMM[D*D,N] (HMM covariance matrices)
  3692. cmReal_t diagMV[] = { 1, 0.2, 0.2, 0.2, 1.0, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 0.2 };
  3693. cmReal_t diagmV[] = { 1, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 0.2 };
  3694. cmReal_t Maj[D*D];
  3695. cmReal_t Min[D*D];
  3696. cmVOR_DiagZ(Maj,D,diagMV);
  3697. Maj[ (4*D) + 0 ] = 0.6; Maj[ (0*D) + 4 ] = 0.6;
  3698. Maj[ (7*D) + 0 ] = 0.8; Maj[ (0*D) + 7 ] = 0.8;
  3699. Maj[ (7*D) + 4 ] = 0.8; Maj[ (4*D) + 7 ] = 0.8;
  3700. cmVOR_DiagZ(Min,D,diagmV);
  3701. Min[ (3*D) + 0 ] = 0.6; Min[ (0*D) + 3 ] = 0.6;
  3702. Min[ (7*D) + 0 ] = 0.8; Min[ (0*D) + 7 ] = 0.8;
  3703. Min[ (7*D) + 3 ] = 0.8; Min[ (3*D) + 7 ] = 0.8;
  3704. cmReal_t* sM = p->sMM;
  3705. for(i=0; i<N/2; ++i,sM+=D*D)
  3706. cmVOR_RotateM( sM, D, D, Maj, i, i );
  3707. for(i=0; i<N/2; ++i,sM+=D*D)
  3708. cmVOR_RotateM( sM, D, D, Min, i, i );
  3709. /*
  3710. cmVOR_PrintL("Maj:\n",p->obj.err.rpt,D,D,Maj);
  3711. cmVOR_PrintL("Min:\n",p->obj.err.rpt,D,D,Min);
  3712. for(i=0; i<N; ++i)
  3713. {
  3714. cmCtxPrint(c,"%i----\n",i);
  3715. cmVOR_PrintL("sM:\n",p->obj.err.rpt,D,D,sMM + (i*D*D));
  3716. }
  3717. */
  3718. // initialize uM[D,N] (HMM GMM mean vectors)
  3719. cmVOR_Fill(p->uM,D*N,0);
  3720. for(i=0; i<D; ++i)
  3721. {
  3722. unsigned dom = (i+7) % D;
  3723. unsigned medM = (i+4) % D;
  3724. unsigned medm = (i+3) % D;
  3725. p->uM[ (i * D) + i ] = 1;
  3726. p->uM[ (i * D) + medM ] = 1;
  3727. p->uM[ (i * D) + dom ] = 1;
  3728. p->uM[ ((i+D) * D) + i ] = 1;
  3729. p->uM[ ((i+D) * D) + medm ] = 1;
  3730. p->uM[ ((i+D) * D) + dom ] = 1;
  3731. }
  3732. cmVOR_AddVS(p->uM,D*N,0.01);
  3733. for(i=0; i<N; ++i)
  3734. cmVOR_NormalizeProbability( p->uM + (i*D), D);
  3735. // initialize phiM[D,S]
  3736. cmReal_t phi[D*S];
  3737. for(i=0,j=0; i<D; ++i,++j)
  3738. phi[j] = sin( M_PI*7.0*i/6.0 );
  3739. for(i=0; i<D; ++i,++j)
  3740. phi[j] = cos( M_PI*7.0*i/6.0 );
  3741. for(i=0; i<D; ++i,++j)
  3742. phi[j] = sin( M_PI*3.0*i/2.0 );
  3743. for(i=0; i<D; ++i,++j)
  3744. phi[j] = cos( M_PI*3.0*i/2.0 );
  3745. for(i=0; i<D; ++i,++j)
  3746. phi[j] = 0.5 * sin( M_PI*2.0*i/3.0 );
  3747. for(i=0; i<D; ++i,++j)
  3748. phi[j] = 0.5 * cos( M_PI*2.0*i/3.0 );
  3749. cmVOR_Transpose(p->phiM,phi,D,S);
  3750. return p;
  3751. }
  3752. cmRC_t cmChordFree( cmChord** pp )
  3753. {
  3754. cmRC_t rc = cmOkRC;
  3755. cmChord* p = *pp;
  3756. if( pp == NULL || *pp == NULL )
  3757. return cmOkRC;
  3758. if((rc = cmChordFinal(p)) != cmOkRC )
  3759. return rc;
  3760. cmChmmFree( &p->h );
  3761. cmMemPtrFree(&p->iV);
  3762. cmMemPtrFree(&p->aM);
  3763. cmMemPtrFree(&p->uM);
  3764. cmMemPtrFree(&p->sMM);
  3765. cmMemPtrFree(&p->phiM);
  3766. cmMemPtrFree(&p->tsMeanV);
  3767. cmMemPtrFree(&p->tsVarV);
  3768. cmMemPtrFree(&p->chromaM);
  3769. cmMemPtrFree(&p->tsM);
  3770. cmMemPtrFree(&p->cdtsV);
  3771. cmObjFree(pp);
  3772. return rc;
  3773. }
  3774. cmRC_t cmChordInit( cmChord* p, const cmReal_t* chromaM, unsigned T )
  3775. {
  3776. cmRC_t rc = cmOkRC;
  3777. unsigned i;
  3778. unsigned N = p->N; // count of states
  3779. unsigned K = 1; // count of components per mixture
  3780. unsigned D = p->D; // dimensionality of the observation vector
  3781. unsigned S = p->S; //
  3782. cmReal_t alpha = 6.63261; // alpha norm coeff
  3783. if((rc = cmChordFinal(p)) != cmOkRC )
  3784. return rc;
  3785. // Create the hidden markov model
  3786. cmChmmInit( p->h, N, K, D, p->iV, p->aM);
  3787. // load the GMM's for each markov state
  3788. cmReal_t mixCoeff = 1.0;
  3789. bool diagFl = false;
  3790. for(i=0; i<N; ++i)
  3791. if((rc = cmChmmSetGmm(p->h, i, &mixCoeff, p->uM + (i*D), p->sMM+(i*D*D), diagFl )) != cmOkRC )
  3792. return rc;
  3793. // Allocate memory
  3794. // chromaM[D,T] tsM[S,T] cdtsV[T]
  3795. /*
  3796. unsigned n = ALIGN_T(D*T,cmReal_t) + ALIGN_T(S*T,cmReal_t) + ALIGN_T(T,cmReal_t);
  3797. p->chromaM = cmArrayResizeZ(c, &p->memB, n, cmReal_t);
  3798. p->tsM = p->chromaM + ALIGN_T(D*T,cmReal_t);
  3799. p->cdtsV = p->tsM + ALIGN_T(S*T,cmReal_t);
  3800. p->T = T;
  3801. */
  3802. p->chromaM = cmMemResizeZ( cmReal_t, p->chromaM, p->D*T );
  3803. p->tsM = cmMemResizeZ( cmReal_t, p->tsM, p->S*T );
  3804. p->cdtsV = cmMemResizeZ( cmReal_t, p->cdtsV, p->D*T );
  3805. p->T = T;
  3806. // Allocate local memory
  3807. // qV[], triadIntV[] triadSeqV[] tsNormsV[]
  3808. /*
  3809. n = 2*ALIGN_B(T,unsigned) + ALIGN_B(T,int) + ALIGN_B(T,cmReal_t);
  3810. cmArray mem;
  3811. cmArrayAlloc(c, &mem);
  3812. unsigned* qV = (unsigned*) cmArrayResize(c, &mem, n, char);
  3813. unsigned* triadSeqV = (unsigned*) (qV + ALIGN_T(T,unsigned));
  3814. int* triadIntV = (int*) (triadSeqV + ALIGN_T(T,unsigned));
  3815. cmReal_t* tsNormsV = (cmReal_t*) (triadIntV + ALIGN_T(T,int));
  3816. */
  3817. //unsigned qV[T];
  3818. //unsigned triadSeqV[T];
  3819. //int triadIntV[T];
  3820. //cmReal_t tsNormsV[T];
  3821. unsigned* qV = cmMemAlloc( unsigned, T );
  3822. unsigned* triadSeqV = cmMemAlloc( unsigned, T );
  3823. int* triadIntV = cmMemAlloc( int, T );
  3824. cmReal_t* tsNormsV = cmMemAlloc( cmReal_t, T );
  3825. // Take the alpha norm of chroma and store the result in p->chromaM[]
  3826. for(i=0; i<T; ++i)
  3827. p->chromaM[i] = cmVOR_AlphaNorm( chromaM + (i*D), D, alpha);
  3828. cmVOR_DivVVS(p->chromaM,D*T,chromaM, cmVOR_AlphaNorm(p->chromaM,T,alpha));
  3829. // Train the HMM iniital state prob. p->h->iV[] and transition matrix p->h->aM[]
  3830. unsigned flags = kNoTrainMixCoeffChmmFl | kNoTrainMeanChmmFl | kNoTrainCovarChmmFl;
  3831. unsigned iterCnt = 40;
  3832. if( chromaM != NULL && T > 0 )
  3833. if((rc = cmChmmTrain(p->h, p->chromaM, p->T, iterCnt, 0, flags )) != cmOkRC )
  3834. goto errLabel;
  3835. // Find the most likely chords using a Viterbi decoding of the chroma.
  3836. cmChmmDecode(p->h,p->chromaM,T,qV);
  3837. // Reorder the chord sequence cmcording to circle of fifths
  3838. unsigned map[] = {0, 14, 4, 18, 8, 22, 12, 2, 16, 6, 20, 10, 17, 7, 21, 11, 1, 15, 5, 19, 9, 23, 13, 3 };
  3839. for(i=0; i<T; ++i)
  3840. qV[i] = map[ qV[i] ];
  3841. //cmVOU_PrintL("qV:\n",p->obj.err.rpt,1,T,qV);
  3842. // Smooth the chord sequence with a median filter.
  3843. cmVOU_MedianFilt(qV,T,3,triadSeqV,1);
  3844. //cmVOU_PrintL("triadSeqV:\n",p->obj.err.rpt,1,T,triadSeqV);
  3845. // Calculate the chord change distance on the circle of fifths.
  3846. int d = 0;
  3847. for(i=0; i<T; ++i)
  3848. {
  3849. int v = abs(d);
  3850. assert(v<N);
  3851. v = (v<=11) ? v : -(12-(v-12));
  3852. if( i > 0 )
  3853. triadIntV[i-1] = (d < 0 ? -1 : 1) * v;
  3854. if( i + 1 < T)
  3855. d = triadSeqV[i+1] - triadSeqV[i];
  3856. }
  3857. // Project chroma into a 6D tonal space.
  3858. cmVOR_MultMMM( p->tsM,S,T,p->phiM,p->chromaM,D);
  3859. // Find the norm of p->tsM[6,T]
  3860. cmVOR_Fill(tsNormsV,T,0);
  3861. for(i=0; i<T; ++i)
  3862. tsNormsV[i] = cmVOR_MultSumVV( p->tsM + (i*S), p->tsM + (i*S), S );
  3863. cmVOR_PowVS(tsNormsV,T,0.5);
  3864. // Take the cosine distance.
  3865. p->cdtsV[0] = 1;
  3866. for(i=1; i<T; ++i)
  3867. p->cdtsV[i] = cmVOR_MultSumVV( p->tsM + ((i-1)*S), p->tsM + (i*S), S );
  3868. for(i=0; i<T-1; ++i)
  3869. p->cdtsV[i+1] /= tsNormsV[i] * tsNormsV[i+1];
  3870. //cmVOR_PrintL("tsNormsV:\n",p->obj.err.rpt,1,T,tsNormsV);
  3871. //cmVOR_PrintL("CDTS:\n", p->obj.err.rpt,1,T,p->cdtsV);
  3872. p->triadSeqMode = cmVOU_Mode(triadSeqV,T);
  3873. p->triadSeqVar = cmVOU_Variance(triadSeqV,T,NULL);
  3874. p->triadIntMean = cmVOI_Mean(triadIntV,T);
  3875. p->triadIntVar = cmVOI_Variance(triadIntV,T,&p->triadIntMean);
  3876. cmVOR_MeanM( p->tsMeanV, p->tsM, S, T, 1 );
  3877. cmVOR_VarianceM( p->tsVarV, p->tsM, S, T, p->tsMeanV, 1);
  3878. p->cdtsMean = cmVOR_Mean( p->cdtsV, T );
  3879. p->cdtsVar = cmVOR_Variance( p->cdtsV, T, &p->cdtsMean );
  3880. /*
  3881. cmReal_t tsMt[T*S];
  3882. cmKbRecd kb;
  3883. cmPlotInitialize(NULL);
  3884. cmPlotSetup("Chords",1,1);
  3885. cmCtxPrint(c,"%s\n","press any key");
  3886. //cmPlotLineD( NULL, NULL, p->cdtsV, NULL, T, NULL, kSolidPlotLineId );
  3887. cmVOR_Transpose(tsMt,p->tsM,S,T);
  3888. cmPlotLineMD(NULL, tsMt, NULL, T, S, kSolidPlotLineId);
  3889. cmPlotDraw();
  3890. cmKeyPress(&kb);
  3891. cmPlotFinalize();
  3892. */
  3893. errLabel:
  3894. cmMemPtrFree(&qV);
  3895. cmMemPtrFree(&triadSeqV);
  3896. cmMemPtrFree(&triadIntV);
  3897. cmMemPtrFree(&tsNormsV);
  3898. return rc;
  3899. }
  3900. cmRC_t cmChordFinal( cmChord* p )
  3901. { return cmOkRC; }
  3902. void cmChordTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  3903. {
  3904. cmCtx c;
  3905. cmCtxAlloc(&c,rpt,lhH,stH);
  3906. cmChord* p = cmChordAlloc(&c,NULL,NULL,0);
  3907. cmChordFree(&p);
  3908. }