From dc41f64df934d362b1369387bd5dde8d607c4286 Mon Sep 17 00:00:00 2001 From: Ruben van de Ven Date: Fri, 11 Apr 2025 22:47:48 +0200 Subject: [PATCH] Laser output tweaks --- trap/base.py | 1 + trap/shapes.py | 11 ++++++++ trap/stage.py | 75 +++++++++++++++++++++++++++++++++++++++++++++---- trap/tracker.py | 3 +- 4 files changed, 83 insertions(+), 7 deletions(-) create mode 100644 trap/shapes.py diff --git a/trap/base.py b/trap/base.py index 1ec6e90..5676367 100644 --- a/trap/base.py +++ b/trap/base.py @@ -206,6 +206,7 @@ class Track: source: Optional[int] = None # to keep track of processed tracks lost: bool = False created_at: Optional[float] = None + frame_index: int = 0 def __post_init__(self): if not self.created_at: diff --git a/trap/shapes.py b/trap/shapes.py new file mode 100644 index 0000000..d808ba3 --- /dev/null +++ b/trap/shapes.py @@ -0,0 +1,11 @@ +from typing import Tuple +from typing import List + + +PositionAndIntensity = Tuple[float,float,float] + +YOUR_FUTURE: List[PositionAndIntensity] = [[0.038513974429595775, 0.07736873483541283, 1.0], [0.0382757222679613, 0.1025670238237002, 1.0], [0.0541576489756651, 0.11774344049982961, 1.0], [0.07378058203832892, 0.10288416259806232, 1.0], [0.08722525924196169, 0.0814108126125096, 1.0], [0.09160263301646454, 0.08329815714053822, 1.0], [0.09534264273910519, 0.10840890101548864, 1.0], [0.09916028362894076, 0.13352390177514933, 1.0], [0.10232188216400727, 0.15872672729061565, 1.0], [0.10311951375159167, 0.18408246402623243, 1.0], [0.09840396204269339, 0.20891794923219542, 1.0], [0.08291530920434609, 0.22859913801644804, 1.0], [0.05919014784073643, 0.23216553617443603, 1.0], [0.04902797432052862, 0.21056988740659402, 1.0], [0.0542513157559533, 0.18586563467105832, 1.0], [0.0542513157559533, 0.18586563467105832, 0.0], [0.14461260563490763, 0.07955838236847201, 0.0], [0.14461260563490763, 0.07955838236847201, 1.0], [0.13558063166458387, 0.10103799255000086, 1.0], [0.15322441358055564, 0.11390007560940833, 1.0], [0.1735166365166195, 0.10116155041151406, 1.0], [0.17258100810588609, 0.07966943709972815, 1.0], [0.14961116043044345, 0.08026824305685826, 1.0], [0.14417649530494814, 0.10052196882994967, 1.0], [0.14417649530494814, 0.10052196882994967, 0.0], [0.21234130983179852, 0.07853085592549228, 0.0], [0.21234130983179852, 0.07853085592549228, 1.0], [0.21195135170310608, 0.09939847235719804, 1.0], [0.22573458952023626, 0.11739829833366248, 1.0], [0.24458035764122837, 0.1052878540835099, 1.0], [0.2538725565579727, 0.08313976080541961, 1.0], [0.255807813667129, 0.08850444116299301, 1.0], [0.26244752230288027, 0.11156142530321671, 1.0], [0.278628608562111, 0.11468355008686479, 1.0], [0.28547019597542156, 0.09159341243420857, 1.0], [0.2918362984056859, 0.06838022001936472, 1.0], [0.3005008421006701, 0.07935931502673148, 1.0], [0.30156825900234596, 0.10346905081241903, 1.0], [0.31102190009060793, 0.12433665737167442, 1.0], [0.33339185770206586, 0.11848359229504231, 1.0], [0.3513812674003706, 0.102558533670551, 1.0], [0.3513812674003706, 0.102558533670551, 0.0], [0.42686993143914653, 0.09185645985150065, 0.0], [0.42686993143914653, 0.09185645985150065, 1.0], [0.4412682414620315, 0.07286865615390165, 1.0], [0.45084183686010293, 0.05100171349006759, 1.0], [0.45347869677603575, 0.027341959266008523, 1.0], [0.4435151972236728, 0.00792103659488272, 1.0], [0.4345739206406735, 0.028355940891981273, 1.0], [0.4337797821877698, 0.052244466545959004, 1.0], [0.43513696480231173, 0.07614300509684381, 1.0], [0.43637325350823714, 0.10004361849723883, 1.0], [0.43591794294213454, 0.12396712012975415, 1.0], [0.4343358771636571, 0.14784577117143877, 1.0], [0.43004483999604426, 0.17138258277291252, 1.0], [0.42349546870936716, 0.19435274324567994, 1.0], [0.41636670815063503, 0.18274106404797463, 1.0], [0.41650904831900143, 0.1588259983867111, 1.0], [0.4188002169013092, 0.13505679100517512, 1.0], [0.4268723820603772, 0.11303119410963162, 1.0], [0.4500128242886166, 0.11386831079745637, 1.0], [0.47283807893581375, 0.10841107232468714, 1.0], [0.4872864841393783, 0.08990220621766647, 1.0], [0.48035728645082926, 0.08551550932203601, 1.0], [0.4834331213821757, 0.10892934066665313, 1.0], [0.5026251362218042, 0.11943702192634674, 1.0], [0.5195900915871043, 0.10344307624078676, 1.0], [0.5272883245930242, 0.08113828193061089, 1.0], [0.5267576251141751, 0.10047409952707571, 1.0], [0.5379009893510314, 0.12115852863792714, 1.0], [0.5599032847972661, 0.12145455136649168, 1.0], [0.5744503750287453, 0.10270148231874575, 1.0], [0.5840774712539069, 0.0808410245684861, 1.0], [0.5905433375024336, 0.05786040975866798, 1.0], [0.5934077572159745, 0.03411118608853734, 1.0], [0.5906347675237177, 0.04280542709179432, 1.0], [0.5882110565514761, 0.06662131767684432, 1.0], [0.5876546124770476, 0.09050869359531578, 1.0], [0.5927221122806438, 0.11371001041100556, 1.0], [0.6103545876688272, 0.12783118163410698, 1.0], [0.631373581233142, 0.11875861351991082, 1.0], [0.6428130770693629, 0.0980459105567277, 1.0], [0.6428130770693629, 0.0980459105567277, 0.0], [0.5915479833072788, 0.06721538442214654, 0.0], [0.5915479833072788, 0.06721538442214654, 1.0], [0.6125483814164788, 0.07773324751217321, 1.0], [0.6367947348281895, 0.07962813582103823, 1.0], [0.6440797272649885, 0.07979848741266705, 1.0], [0.6475016404268139, 0.10397501517479255, 1.0], [0.6626330560622519, 0.12190053501513858, 1.0], [0.6805257555025275, 0.107866704576016, 1.0], [0.6893844833457623, 0.08509819459432841, 1.0], [0.6898412200281951, 0.08179501862114127, 1.0], [0.6945160532454058, 0.10575378282261425, 1.0], [0.7111356017515474, 0.12205861820747824, 1.0], [0.7285494206041053, 0.10765446933485708, 1.0], [0.7358597194485729, 0.0843550868151773, 1.0], [0.7361559672985614, 0.08829286880876025, 1.0], [0.7390423305209906, 0.1126434479700143, 1.0], [0.7418058940557418, 0.12129911707099901, 1.0], [0.7436522097068493, 0.09687779686227331, 1.0], [0.7541835238456593, 0.07548783311279784, 1.0], [0.7768954753006071, 0.08156569310321993, 1.0], [0.7984424236797361, 0.09318965596188536, 1.0], [0.8216202473510118, 0.0935224312636923, 1.0], [0.8013323408542665, 0.08915080376332754, 1.0], [0.7884844494912847, 0.10852374739445168, 1.0], [0.800525183632502, 0.1282264852718212, 1.0], [0.8242488641121982, 0.13377113743721275, 1.0], [0.8487698023243145, 0.13342856150487792, 1.0], [0.8731271139483364, 0.13057866700963636, 1.0], [0.8731271139483364, 0.13057866700963636, 0.0], [0.9999714604629248, 0.09321081689567305, 0.0], [0.9999714604629248, 0.09321081689567305, 1.0], [1.0, 0.09297872770874008, 1.0], [0.9999064410407551, 0.09275721141104158, 1.0]] + +YOUR = [[0.009506738137690061, 0.05104076477286449, 1.0], [0.005921856231873919, 0.07733973841297721, 1.0], [0.006216186910409892, 0.10414604625334409, 1.0], [0.008752461243054144, 0.13081495531527204, 1.0], [0.015720642421307246, 0.1566321806386414, 1.0], [0.03266664781019444, 0.17668287955834536, 1.0], [0.05903237418256845, 0.1788613324500634, 1.0], [0.082537826157113, 0.16891860093199274, 1.0], [0.10164603107617634, 0.15012937698770487, 1.0], [0.12115578530350037, 0.13181898383490445, 1.0], [0.13727007499248245, 0.11039412808540193, 1.0], [0.15124007479331925, 0.08753557345081152, 1.0], [0.16371971877522337, 0.06383741926577463, 1.0], [0.17439060472325657, 0.039276626139675246, 1.0], [0.1759758718024845, 0.04305422417356857, 1.0], [0.17757786482334342, 0.06981247619384195, 1.0], [0.18070329315533282, 0.09640571706065973, 1.0], [0.1851215424769603, 0.1228475870103372, 1.0], [0.18941820361306322, 0.14930951209429372, 1.0], [0.19360958646106588, 0.17578792213916944, 1.0], [0.1976073158160567, 0.2022966553429962, 1.0], [0.20150430993278046, 0.22882002478088762, 1.0], [0.2052157484454822, 0.2553703543501013, 1.0], [0.20871892106923584, 0.28194614730031936, 1.0], [0.2115134773956657, 0.3086085783786331, 1.0], [0.2138572386411976, 0.33529823152920163, 1.0], [0.21429140739278582, 0.36210319885920544, 1.0], [0.21403866351603784, 0.388881226891698, 1.0], [0.21150658584631085, 0.4155698641506869, 1.0], [0.20686117463112197, 0.441843386218174, 1.0], [0.19910990350562116, 0.4675068343888, 1.0], [0.18612255973529046, 0.4908256397759356, 1.0], [0.17010915757362147, 0.5120009499017865, 1.0], [0.15007503369351857, 0.5298146709174564, 1.0], [0.12588381929270992, 0.5410662164516361, 1.0], [0.0999535729193916, 0.5466942665930086, 1.0], [0.0749645525945239, 0.5411053807076283, 1.0], [0.0559080163569219, 0.5233258272308214, 1.0], [0.04561546580258979, 0.4992192003089925, 1.0], [0.04279255894625653, 0.47273663641455715, 1.0], [0.04504923840993005, 0.446023303154448, 1.0], [0.05125543406068164, 0.420090406985782, 1.0], [0.05932890708955816, 0.3945264908280523, 1.0], [0.05932890708955816, 0.3945264908280523, 0.0], [0.34539988311533204, 0.0579728802920288, 0.0], [0.34539988311533204, 0.0579728802920288, 1.0], [0.32902586235747094, 0.07800131503964765, 1.0], [0.31900977671915987, 0.10333479582124688, 1.0], [0.3168089001532646, 0.13055337266640848, 1.0], [0.33085763583568856, 0.15342640491763876, 1.0], [0.35463916953464314, 0.16622621676101712, 1.0], [0.38180067198325, 0.16607107196852425, 1.0], [0.4063361550322455, 0.15385196107531487, 1.0], [0.4276049287757457, 0.1364751302317814, 1.0], [0.44457025324051497, 0.11528895197425702, 1.0], [0.4501377674010854, 0.08880170609376863, 1.0], [0.4412945797302475, 0.06350857785661705, 1.0], [0.41742394289222656, 0.05090666417273356, 1.0], [0.39053253977462554, 0.04869575158075358, 1.0], [0.3651255183583462, 0.05782206330611241, 1.0], [0.3437613202391389, 0.0748908441579518, 1.0], [0.33376406006673165, 0.09978952090775195, 1.0], [0.34401921565110044, 0.12434061051912094, 1.0], [0.34401921565110044, 0.12434061051912094, 0.0], [0.5598193050894343, 0.05471987937239209, 0.0], [0.5598193050894343, 0.05471987937239209, 1.0], [0.5534686529134756, 0.07148812928745896, 1.0], [0.5549566280713513, 0.09817979159294389, 1.0], [0.5593297223380959, 0.12453121597385056, 1.0], [0.5681987600528517, 0.14967853481921375, 1.0], [0.5848664952119835, 0.16988022616673765, 1.0], [0.6094051283153207, 0.18037146275739913, 1.0], [0.6324437080609985, 0.1691208239667061, 1.0], [0.652137222865768, 0.15106957648399505, 1.0], [0.6677176262111469, 0.12956221583768426, 1.0], [0.6793024323465844, 0.10551736250711242, 1.0], [0.6883434970029061, 0.08036898601130281, 1.0], [0.6941299844470303, 0.054305673204518644, 1.0], [0.692584041584101, 0.0522641991958823, 1.0], [0.6961811101772365, 0.07875590474064255, 1.0], [0.7006909348744301, 0.10511796261764655, 1.0], [0.7067026377709629, 0.13116307461630544, 1.0], [0.7165566644483221, 0.1559703958625604, 1.0], [0.7312300169873958, 0.1782497751954444, 1.0], [0.7538841320493372, 0.18973788040826925, 1.0], [0.7696754478889533, 0.1691741474460014, 1.0], [0.7780546146990763, 0.14380285978614823, 1.0], [0.7859290134191185, 0.1183412600365016, 1.0], [0.7923750235064942, 0.0923975809519497, 1.0], [0.7979627577738667, 0.0662838190166005, 1.0], [0.805298964350212, 0.040657490468887475, 1.0], [0.8145650522604783, 0.015589982935501005, 1.0], [0.8311156782687112, 0.01618395714242152, 1.0], [0.8371039729764935, 0.042184973486309986, 1.0], [0.8393180145162912, 0.06879804987800829, 1.0], [0.8401600640050448, 0.09553006154044377, 1.0], [0.8414985046684333, 0.12223634218488923, 1.0], [0.8440689653517326, 0.14884609784116348, 1.0], [0.8506326036394201, 0.17461519615690108, 1.0], [0.8655646659351066, 0.19599378230409736, 1.0], [0.8907042374786248, 0.20253296001361418, 1.0], [0.9166383601749319, 0.19658113364381366, 1.0], [0.9398386332217225, 0.1832746488065626, 1.0], [0.9621096143963949, 0.16846891816028328, 1.0], [0.9811564795261766, 0.14976779887909816, 1.0], [1.0, 0.13078807421538868, 1.0]] + +FUTURE = [[-0.18407281237284287, 0.5243734033935544, 1.0], [-0.15784586564711311, 0.49232653415745337, 1.0], [-0.1367446628174097, 0.45655604929794674, 1.0], [-0.12092936648303604, 0.41811274815303967, 1.0], [-0.11367196125979623, 0.37718758533566765, 1.0], [-0.1154767581219267, 0.33581625216805694, 1.0], [-0.1355354267518391, 0.30194044503015904, 1.0], [-0.1591491553578137, 0.33000986224579176, 1.0], [-0.16516257592970865, 0.3711403883196111, 1.0], [-0.16582816319319846, 0.4126990211330252, 1.0], [-0.16403026940389587, 0.45427139880163253, 1.0], [-0.16114501742937787, 0.49578461134061474, 1.0], [-0.159107681010325, 0.5373391840854801, 1.0], [-0.15887167064227717, 0.5789330336750337, 1.0], [-0.1605904718158808, 0.6205123163036583, 1.0], [-0.16314225635905347, 0.6620497925376314, 1.0], [-0.16894712708303491, 0.7032366928637622, 1.0], [-0.17760305707331364, 0.7439239603089169, 1.0], [-0.1880148019494103, 0.7841824803417963, 1.0], [-0.20802251704491997, 0.8003012081279335, 1.0], [-0.212331962076359, 0.7589620228028748, 1.0], [-0.21217744512252243, 0.7173863207773045, 1.0], [-0.2103089246389881, 0.6758140019110197, 1.0], [-0.2045704985223671, 0.6347144488696181, 1.0], [-0.1947784543795423, 0.5943804852683122, 1.0], [-0.16157960874371835, 0.5767076963914598, 1.0], [-0.12049471191598594, 0.5830603084681673, 1.0], [-0.07983069410673044, 0.5768589837702333, 1.0], [-0.045241964907762, 0.5544722215504083, 1.0], [-0.02376706135279352, 0.5191881021679552, 1.0], [-0.0329884435312738, 0.4886481103910843, 1.0], [-0.04316172643081175, 0.5272087263339329, 1.0], [-0.035011174642525844, 0.5677351889644544, 1.0], [-0.006313498946090049, 0.5963446454494684, 1.0], [0.03266001969481053, 0.5899721594456785, 1.0], [0.05956543447090927, 0.5587917227978596, 1.0], [0.0775824668275846, 0.5214042521621798, 1.0], [0.07793748859880921, 0.49922234327034876, 1.0], [0.08013714510989922, 0.5407200975464114, 1.0], [0.0931341865061384, 0.579962195707602, 1.0], [0.12141492394488787, 0.6094862393459278, 1.0], [0.1612959524473165, 0.6072414387658712, 1.0], [0.19096216894345433, 0.5789379589157119, 1.0], [0.21280666780261034, 0.5435644082739731, 1.0], [0.22916021519972682, 0.505382258080926, 1.0], [0.24323674428643757, 0.46626446581140524, 1.0], [0.25147313964841106, 0.42550087424496574, 1.0], [0.2557623101268601, 0.38413276013280884, 1.0], [0.25400272640585453, 0.35939251979214715, 1.0], [0.24980011353739556, 0.4007627455752819, 1.0], [0.24548431862399053, 0.4421539522166946, 1.0], [0.2420232739200252, 0.48360900515421834, 1.0], [0.24297651688309993, 0.5251564556084671, 1.0], [0.249569546411798, 0.5661852553457349, 1.0], [0.26895423119633916, 0.6023848247542106, 1.0], [0.30495902160495886, 0.619987652245809, 1.0], [0.34383813354590465, 0.6086542710913705, 1.0], [0.37198383320922207, 0.5785740063772731, 1.0], [0.3888981476555129, 0.5407961656716407, 1.0], [0.3888981476555129, 0.5407961656716407, 0.0], [0.25287429250991594, 0.4589922148926428, 0.0], [0.25287429250991594, 0.4589922148926428, 1.0], [0.28664712241196083, 0.4777043133907104, 1.0], [0.3255403756525194, 0.4911118608708812, 1.0], [0.36663465397911976, 0.4927908973817563, 1.0], [0.40312495349037347, 0.4771609198035301, 1.0], [0.3922914945897105, 0.503468479486839, 1.0], [0.3979425896562885, 0.5442852649595254, 1.0], [0.4134194427130914, 0.5823816534152172, 1.0], [0.44622817875938386, 0.6045734026390746, 1.0], [0.4797086744332584, 0.5834634144292555, 1.0], [0.49845117156114005, 0.5467450290148081, 1.0], [0.5122578279954287, 0.5080149307664742, 1.0], [0.513464004529373, 0.4733241576631237, 1.0], [0.5146639916221074, 0.5134984811866009, 1.0], [0.5232709930539835, 0.5538205229904855, 1.0], [0.5431487887698696, 0.589564261861241, 1.0], [0.5795540622289034, 0.6042487353480698, 1.0], [0.6088665589600876, 0.5784252020998061, 1.0], [0.6266328380355419, 0.5413794114713223, 1.0], [0.6361933389959588, 0.5013194327637942, 1.0], [0.6354580129653472, 0.49430034053122834, 1.0], [0.6382382160474952, 0.5354990864977485, 1.0], [0.6436658360121199, 0.5764016503133141, 1.0], [0.6512072742236578, 0.6167441254085214, 1.0], [0.6518235699670981, 0.5882161907635312, 1.0], [0.6545931978785503, 0.5470427822577073, 1.0], [0.6652168112248554, 0.507482583837264, 1.0], [0.6918557021941886, 0.4781992308197012, 1.0], [0.7310913041793997, 0.48882778158729345, 1.0], [0.7663606991828579, 0.5103034487425565, 1.0], [0.8033571709285953, 0.5283685512912586, 1.0], [0.8437454403327476, 0.5347978587775813, 1.0], [0.8540901310600703, 0.5145816126986249, 1.0], [0.8137975314869395, 0.5151272993825058, 1.0], [0.7831868541996628, 0.541977797973314, 1.0], [0.7756536912488347, 0.5811185322090929, 1.0], [0.7982740921536373, 0.6144695614789005, 1.0], [0.8357727621272076, 0.6307398223532755, 1.0], [0.8766556040438014, 0.63589433705425, 1.0], [0.9179208066196057, 0.6355435979674556, 1.0], [0.9590763658827985, 0.6325907493270039, 1.0], [1.0, 0.6271166627075447, 1.0]] \ No newline at end of file diff --git a/trap/stage.py b/trap/stage.py index 5a89d3c..b642431 100644 --- a/trap/stage.py +++ b/trap/stage.py @@ -7,13 +7,16 @@ import json import logging import time from typing import Dict, List, Optional, Tuple +import numpy as np from statemachine import Event, State, StateMachine from statemachine.exceptions import TransitionNotAllowed import zmq from sgan.sgan import data +from trap import shapes from trap.base import DataclassJSONEncoder, Frame, Track from trap.counter import CounterSender +from trap.laser_renderer import rotateMatrix from trap.node import Node from trap.timer import Timer from trap.utils import exponentialDecay, relativePointToPolar, relativePolarToPoint @@ -44,7 +47,7 @@ class TrackScenario(StateMachine): mark_lost = lost.from_(detected, substantial, first_prediction, corrected_prediction, loitering, play) - receive_prediction = detected.to(first_prediction) | first_prediction.to(corrected_prediction, cond="prediction_is_stale") | corrected_prediction.to(play, cond="prediction_is_playing") + receive_prediction = detected.to(first_prediction) | substantial.to(first_prediction) | first_prediction.to(corrected_prediction, cond="prediction_is_stale") | corrected_prediction.to(play, cond="prediction_is_playing") def __init__(self): self._track = None @@ -178,6 +181,9 @@ class DrawnScenario(TrackScenario): self.drawn_positions: List[Tuple[float,float]] = [] self.drawn_pred_history: List[Tuple[float,float]] = [] self.drawn_predictions: List[List[Tuple[float,float]]] = [] + + self.drawn_text = "" + self.drawn_text_lines: List[RenderableLine] = [] super().__init__() def update_drawn_positions(self) -> List: @@ -198,6 +204,8 @@ class DrawnScenario(TrackScenario): self.last_update_t = t # 1. track history, direct update + MAX_HISTORY = 80 + # positions = self._track.get_projected_history(None, self.camera)[-MAX_HISTORY:] positions = self._track.get_projected_history(None, self.camera) # TODO)) Limit history to N points, or N lenght for i, pos in enumerate(self.drawn_positions): @@ -219,6 +227,7 @@ class DrawnScenario(TrackScenario): if self.prediction_track and self.prediction_track.predictions: # 3. predictions + prediction_offset = self._track.frame_index - self.prediction_track.frame_index if len(self.prediction_track.predictions): for a, drawn_prediction in enumerate(self.drawn_predictions): for i, pos in enumerate(drawn_prediction): @@ -227,7 +236,7 @@ class DrawnScenario(TrackScenario): decay = 16 origin = self.drawn_positions[-1] drawn_r, drawn_angle = relativePointToPolar( origin, drawn_prediction[i]) - pred_r, pred_angle = relativePointToPolar(origin, self.prediction_track.predictions[a][i]) + pred_r, pred_angle = relativePointToPolar(origin, self.prediction_track.predictions[a][i+prediction_offset]) r = exponentialDecay(drawn_r, pred_r, decay, dt) angle = exponentialDecay(drawn_angle, pred_angle, decay, dt) x, y = relativePolarToPoint(origin, r, angle) @@ -236,24 +245,75 @@ class DrawnScenario(TrackScenario): # self.drawn_predictions[i][1] = int(exponentialDecay(self.drawn_predictions[i][1], self.prediction_track.predictions[i][1], decay, dt)) if len(self.prediction_track.predictions) > len(self.drawn_predictions): - self.drawn_predictions.extend(self.prediction_track.predictions[len(self.drawn_predictions):]) + for pred in self.prediction_track.predictions[len(self.drawn_predictions):]: + self.drawn_predictions.append(pred[prediction_offset:]) # for a, drawn_prediction in self.drawn_predictions: # if len(self.pred_coords) > len(self.drawn_predictions): # self.drawn_predictions.extend(self.pred_coords[len(self.drawn_predictions):]) def to_renderable_lines(self) -> RenderableLines: - lines = [] + lines: List[RenderableLine] = [] color = SrgbaColor(1.,0.,0.,1.-self.lost_factor()) # positions = [RenderablePosition.from_list(pos) for pos in self.drawn_positions] - points = [RenderablePoint(pos, color) for pos in self.drawn_positions] + points = [RenderablePoint(pos, color) for pos in self.drawn_positions[::4]] lines.append(RenderableLine(points)) if len(self.drawn_predictions): - color = SrgbaColor(0.,0.6,0.,1.-self.lost_factor()) + color = SrgbaColor(0.,0.5,0.,1.-self.lost_factor()) # positions = [RenderablePosition.from_list(pos) for pos in self.drawn_positions] points = [RenderablePoint(pos, color) for pos in self.drawn_predictions[0]] lines.append(RenderableLine(points)) + # # print(self.current_state) + # if self.current_state is self.first_prediction or self.current_state is self.corrected_prediction: + # shape = np.array(shapes.YOUR if time.time() % 2 > 1 else shapes.FUTURE) + # text = "your" if time.time() % 2 > 1 else "future" + # color = SrgbaColor(0.5,0.5,0.,1.-self.lost_factor()) + + # line = self.get_text_lines(text, shape, color) + # if not line: + # POSITION_INDEX = 50 + + # draw_pos = self.drawn_predictions[0][POSITION_INDEX-1] + # current_pos = self.drawn_positions[-1] + # angle = np.arctan2(draw_pos[0]-current_pos[0], draw_pos[1]-current_pos[1]) + np.pi + # # for i, line in enumerate(shape): + # # if i != 0: + # # continue + # points = np.array(shape[:,:2]) + + # avg_x = np.average(points[:,0]) + # avg_y = np.average(points[:,1]) + + # minx, maxx = np.min(points[:,0]), np.max(points[:,0]) + # miny, maxy = np.min(points[:,1]), np.max(points[:,1]) + + # sx = maxx-minx + # sy = maxy-miny + + # points[:,0] -= avg_x + # points[:,1] -= avg_y + # points /= (sx * 1.5) # scale to 1 + + # points @= rotateMatrix(angle) + + # points += draw_pos + + # points = [RenderablePoint.from_list(pos, color.with_alpha(intensity)) for pos, intensity in zip(points, shape[:,2])] + # self.drawn_text = text + # self.drawn_text_lines = [RenderableLine(points)] + # lines.extend(self.drawn_text_lines) + + return lines + + def get_text_lines(self, text, shape, color): + if self.drawn_text == text: + return self.drawn_text_lines + return None + + + + # drawn_pred_history # drawn_predictions @@ -277,6 +337,9 @@ class SrgbaColor(): blue: float alpha: float + def with_alpha(self, alpha: float) -> SrgbaColor: + return SrgbaColor(self.red, self.green, self.blue, alpha) + @dataclass class RenderablePoint(): position: RenderablePosition diff --git a/trap/tracker.py b/trap/tracker.py index c744cc2..98d62af 100644 --- a/trap/tracker.py +++ b/trap/tracker.py @@ -465,6 +465,7 @@ class Tracker: track = self.tracks[detection.track_id] track.track_id = detection.track_id # for new tracks track.fps = frame.camera.fps + track.frame_index = frame.index # track.fps = self.config.camera.fps # for new tracks track.history.append(detection) # add to history @@ -514,7 +515,7 @@ class Tracker: end_time = None tracker_dt = None w_time = None - displacement_filter = FinalDisplacementFilter(.2) + displacement_filter = FinalDisplacementFilter(.8) while self.is_running.is_set(): with timer_counter.get_lock():