libcm is a C development framework with an emphasis on audio signal processing applications.
Du kannst nicht mehr als 25 Themen auswählen Themen müssen mit entweder einem Buchstaben oder einer Ziffer beginnen. Sie können Bindestriche („-“) enthalten und bis zu 35 Zeichen lang sein.

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;
  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. }