diff --git a/modules/cluster_estimation/cluster_estimation.py b/modules/cluster_estimation/cluster_estimation.py index 31675f47..2ef6c080 100644 --- a/modules/cluster_estimation/cluster_estimation.py +++ b/modules/cluster_estimation/cluster_estimation.py @@ -62,7 +62,7 @@ class ClusterEstimation: __MAX_MODEL_ITERATIONS = 1000 # Real-world scenario Hyperparameters - __MAX_NUM_COMPONENTS = 10 # assumed maximum number of real landing pads + __MAX_NUM_COMPONENTS = 1 # assumed maximum number of real landing pads # Hyperparameters to clean up model outputs __WEIGHT_DROP_THRESHOLD = 0.1 diff --git a/scripts/test-cluster-estimation.py b/scripts/test-cluster-estimation.py new file mode 100644 index 00000000..3788317a --- /dev/null +++ b/scripts/test-cluster-estimation.py @@ -0,0 +1,160 @@ +""" +Run cluster estimation on geolocation logs. +""" + +import numpy as np + +from modules.common.mavlink.modules import drone_odometry +from modules.common.kml.modules import ground_locations_to_kml +from modules.cluster_estimation import cluster_estimation +from modules import detection_in_world +from modules.flight_interface import local_global_conversion +from modules.common.kml.modules import location_ground + + +HOME_LOCATION_LATITUDE = 43.47445297241211 # degrees +HOME_LOCATION_LONGITUDE = -80.55301666259766 # degrees +HOME_LOCATION_ALTITUDE = 336.25 # metres above sea level + + +def main() -> int: + """ + Main function. + """ + geolocation_coordinates = [ + [[4.185526211574986, 5.582006662305729], [4.026089118774974, 5.701992978882628], [4.0668330663968675, 5.423062168994517], [3.9070668709420415, 5.541355916048218], [4.0466, 5.5617]], + [[4.70627056155958, 5.4393638950878795], [4.477391495885195, 5.6845012924800145], [4.447769384717751, 5.200303169607887], [4.219012060525916, 5.442620789539209], [4.4625, 5.441]], + [[4.887035998250676, 5.538667531474986], [4.614574590381643, 5.834208834051292], [4.587081469528309, 5.260158217171519], [4.313369443891649, 5.552858257175539], [4.6008, 5.5457]], + [[5.041377651840208, 5.7249748573415085], [4.760810195276662, 6.028460458203864], [4.7075192015069085, 5.410689463487548], [4.42457110796263, 5.710844641042596], [4.734, 5.7179]], + [[5.038957578314052, 5.727592614501275], [4.793517360510267, 5.9930816091562855], [4.815352080052839, 5.517090154792434], [4.568515735041624, 5.7806272453357925], [4.8045, 5.754]], + [[5.279280334996544, 5.772104010052809], [5.085297718705633, 5.98531485699203], [4.9480914415770965, 5.472062116355725], [4.753881926299123, 5.683295135701659], [5.0164, 5.7277]], + [[5.405810956529973, 5.722305577750814], [5.232739088695327, 5.907549998853051], [5.125174712350758, 5.460487189737617], [4.951889982428222, 5.644656357572647], [5.1788, 5.6835]], + [[5.519206114379777, 5.762199925239946], [5.318731386355303, 5.978002663226067], [5.321543526945593, 5.578000447281297], [5.120788242417564, 5.792989439368518], [5.3201, 5.7776]], + [[5.8415796585063, 5.882785820058027], [5.6149987855815535, 6.1290093380461235], [5.644607191136346, 5.704272174481084], [5.418195828516096, 5.949015823218965], [5.6299, 5.9159]], + [[5.910061868313723, 5.791477494806315], [5.679774341352676, 6.045337221120402], [5.787156071271362, 5.67964433901754], [5.5566795561346325, 5.932867494245209], [5.7336, 5.8621]], + [[5.970970803687841, 5.833336125068241], [5.694900646234795, 6.135764471040107], [5.8346541962406375, 5.708142232005172], [5.5582829837184144, 6.009705164719912], [ 5.765, 5.9214]], + [[6.063365696313951, 5.9210516027897535], [5.799575990051606, 6.2081263182605335], [5.9168783302972665, 5.787695937986697], [5.653121627238746, 6.073788366034822], [5.8584, 5.9974]], + [[6.207317427482302, 6.12496857226319], [5.970855056692563, 6.383466770126852], [6.065707664640372, 5.998083530559679], [5.8294796060130745, 6.255394597279966], [6.0184, 6.1902]], + [[6.18203403197477, 6.152608116570993], [6.045203025759711, 6.30219034566027], [6.038087536233319, 6.023477892251982], [5.901394108953884, 6.172362040236555], [6.0416, 6.1625]], + [[6.088949149777445, 6.059993335765839], [5.896297332664977, 6.2700371007683495], [5.878539723739604, 5.870590072656063], [5.686224010782192, 6.0795023393132475], [5.8874, 6.0698]], + [[6.048525986634803, 6.027566334549212], [5.863515480195269, 6.229937151697326], [5.840128324624054, 5.839561232430017], [5.65531547660462, 6.040740764414652], [5.8518, 6.0342]], + [[6.012761019987244, 5.897426125558723], [5.8180791391857705, 6.109703390103866], [5.856791489983876, 5.755088729725379], [5.662123259697983, 5.966688945735891], [5.8375, 5.9321]], + [[5.886309252725437, 5.824342345542348], [5.694846076000429, 6.0321765750418495], [5.767575087255898, 5.713758750278909], [5.5759158403002855, 5.921165524793506], [5.7313, 5.8727]], + [[5.875643408441771, 5.8359325753204105], [5.7472472450344885, 5.975307270804355], [5.751751699416672, 5.720531275787209], [5.623218308175714, 5.859606952978197], [5.7495, 5.8478]], + [[6.015736596688168, 5.906555012144662], [5.8385709449743475, 6.0965154064881055], [5.860631499074526, 5.760672623112171], [5.6832741094235075, 5.950295768514289], [5.8496, 5.9284]], + [[6.011138876983829, 6.002847565689213], [5.83951081927105, 6.187895721004236], [5.8766137927086195, 5.873226644146271], [5.704415003725467, 6.05836149411503], [5.8581, 6.0306]], + [[6.817501364457802, 6.811211548128748], [6.643338919182888, 7.000987549555792], [6.631826640983548, 6.637982070964528], [6.457244246753677, 6.827321337222659], [6.6376, 6.8193]], + [[7.869064632968124, 7.584371541199387], [7.672055321545979, 7.795181236172278], [7.649688243689977, 7.380763129396626], [7.452718169655411, 7.590806438190684], [7.6609, 7.5876]], + [[8.453350242356125, 8.063226226689496], [8.2468509173136, 8.284322176808878], [8.344108258538787, 7.958709444831022], [8.137261521929865, 8.17970141990039], [8.2955, 8.1214]], + [[8.439571239915756, 8.07797921261632], [8.316759134637365, 8.209472422567954], [8.322476444472567, 7.965965588612953], [8.199442952243263, 8.097392479645602], [8.3196, 8.0877]], + [[4.8849453214042375, 6.49392713512155], [4.7172862921447445, 6.674989255534097], [4.757533421215061, 6.3717206748745525], [4.589492691764939, 6.552675130351005], [4.7374, 6.5233]], + [[4.9904242241935854, 6.95113877809072], [4.780269412488946, 7.176198815903519], [4.773926138176138, 6.747090882810249], [4.56351835396514, 6.9721491305082175], [4.7771, 6.9616]], + [[4.034220845137325, 7.827152739465393], [3.8482325938934916, 8.032202760838294], [3.8351590823301165, 7.636268942590317], [3.6476143666890977, 7.840096800612003], [3.8417, 7.8336]], + [[4.500968078300151, 7.03677678046758], [4.3076993303181474, 7.245156149947719], [4.295941017611811, 6.842124523839791], [4.101971056271476, 7.050311126475244], [4.3018, 7.0435]], + [[4.69291950311328, 7.469163444850651], [4.492799855126752, 7.684865916005095], [4.478675740913258, 7.264204297884659], [4.277701611210222, 7.479527353121976], [4.4857, 7.4743]], + [[4.78302587157066, 7.853980921740208], [4.579410994438896, 8.074472978300992], [4.564016017185538, 7.645818945546772], [4.359600659708217, 7.865950218992021], [4.5717, 7.86]], + [[4.818330936772966, 8.128892286246222], [4.607866502405176, 8.357989997675315], [4.603593254047474, 7.926253522102387], [4.392379937447623, 8.154917433327588], [4.6057, 8.1419]], + [[4.934614067329777, 8.2290109833567], [4.742039043039698, 8.43784013269339], [4.726821867939824, 8.032926742295777], [4.533701655896275, 8.241502135542], [4.7344, 8.2352]], + [[5.132062182738132, 8.315949154881041], [4.942728275982728, 8.52109923585868], [4.971775240492075, 8.166166488529516], [4.782255947521684, 8.370989298579456], [4.9573, 8.3435]], + [[4.628947341939864, 8.358378869892915], [4.479340661306051, 8.52332905998103], [4.4945162248173105, 8.231563972666057], [4.344424265414319, 8.39625397192151], [4.4869, 8.3773]], + [[5.331172288301148, 7.548744950485295], [5.1672908229199646, 7.721518434191613], [5.218955568090377, 7.442237956316294], [5.055064571953567, 7.615167901975702], [5.1931, 7.582]], + [[4.918111872341849, 8.00852796311179], [4.750698191127995, 8.191328312368066], [4.807915335792546, 7.904912156500839], [4.640223556324408, 8.087542131266117], [4.7793, 8.048]], + [[4.192107353358891, 8.674339583138115], [4.04028291946107, 8.848385159473397], [4.105336372756005, 8.590737131250789], [3.952713314683557, 8.764098417300918], [4.0729, 8.719]], + [[7.712348315821535, 5.094151733656956], [7.474157415501351, 5.334520490821095], [7.500647911235469, 4.89811464855387], [7.2642184448078, 5.14036428820488], [7.4873, 5.1174]], + [[4.624770637593847, 8.393372317897866], [4.426259248769246, 8.609595337104192], [4.459842803693747, 8.23942780406033], [4.261038136831687, 8.455366630546443], [4.4431, 8.4243]], + [[3.2413487447617033, 9.98742523759334], [3.0528320341940383, 10.206993684443635], [3.0882728483353397, 9.843940995428627], [2.898383417939194, 10.062048034572193], [3.0706, 10.025]], + [[4.390299679572775, 9.947674435068068], [4.202049823318649, 10.167168478063312], [4.230789690188757, 9.797100452669843], [4.040807262890744, 10.015078380357034], [4.2165, 9.9812]], + [[6.330679337409656, 9.539857144301791], [6.229544556434623, 9.649605885645558], [6.170372971107671, 9.391704099443574], [6.069323087854971, 9.501085800131728], [6.1999, 9.5205]], + [[5.7759357811223175, 7.17806114926309], [5.541200698634819, 7.414575577537262], [5.543265017590523, 6.96248650282339], [5.310229712413746, 7.200145403271479], [5.5422, 7.1891]], + [[3.195538657483529, 10.585525694931313], [3.001641976113199, 10.801997228523854], [2.98894832428004, 10.386430838553425], [2.792358174284721, 10.600163846580998], [2.9953, 10.593]], + [[6.986114509913753, 10.686070396103052], [6.760460910105086, 10.92752135063757], [6.730671500771291, 10.459830673802157], [6.50722245253162, 10.697909803636643], [6.7455, 10.692]], + [[6.8262896761296625, 8.951125872368571], [6.645336176300088, 9.144631629772476], [6.645425274644526, 8.784981162305835], [6.464923966409948, 8.977471312908511], [6.6454, 8.9643]], + [[4.715099103794107, 10.36624685378314], [4.4987284863234285, 10.612725709135743], [4.478426063197382, 10.136392560472919], [4.258716061811919, 10.379662719013407], [4.4885, 10.373]], + [[7.144861749548443, 9.941779104641023], [6.983248189818981, 10.113936009758046], [6.956975713185194, 9.770650138876334], [6.79620153219175, 9.941300263165544], [6.9701, 9.9415]], + [[8.168297805588239, 9.622049978424608], [8.088676227933979, 9.709049827940415], [7.906292325138477, 9.384959791578487], [7.8272348673781895, 9.470845613929056], [ 7.997, 9.5461]], + [[4.734759806586163, 7.453976743950106], [4.556563515773709, 7.6517822101161705], [4.5431029843543405, 7.276279030284072], [4.364160045277605, 7.473485271028996], [4.5498, 7.4637]], + [[6.373391062546584, 7.161765639209849], [6.17690503651411, 7.380652660556434], [6.142875914502367, 6.964715984233423], [5.947652358189025, 7.1816270623937175], [6.1598, 7.1717]], + [[5.739126562116987, 5.631004161380995], [5.601545296439637, 5.784905897864984], [5.515572370660386, 5.430111605774029], [5.3779343985032995, 5.583389485987536], [5.5585, 5.6072]], + [[3.809727936658294, 5.0534402559881775], [3.7021827641327723, 5.170541469988258], [3.60221020872191, 4.840462781914113], [3.493435625128472, 4.958444043512817], [3.6524, 5.0062]], + [[4.663120780042685, 9.676666956705608], [4.514319545760688, 9.842564899864966], [4.421821835174132, 9.44683800838495], [4.271517216867519, 9.61132792277697], [4.4679, 9.6441]], + [[5.909555367303418, 5.9250680129274205], [5.671168797771518, 6.18086028039063], [5.738699647144055, 5.766928512693156], [5.500453493147412, 6.022602930355153], [5.7049, 5.9739]], + [[5.03780610133638, 9.958585591402333], [4.795972506481414, 10.23283624939863], [4.765524957094382, 9.724930679715133], [4.5246526527097055, 9.99713086279457], [4.7807, 9.9779]], + [[2.5414668839196826, 12.28858190688671], [2.4865223905034886, 12.353643763403113], [2.3266726842715437, 12.093832106037429], [2.2711375618780862, 12.158369045394311], [2.4065, 12.224]], + [[4.257196720443639, 8.487852007552044], [4.070079112498899, 8.704995766117435], [4.094780739026903, 8.344735147376433], [3.90720077566303, 8.561549776849004], [4.0824, 8.5246]], + [[8.428575234923859, 6.630173710561145], [8.121779659379712, 6.947092782890215], [8.200141198603479, 6.434313322480296], [7.897540954016878, 6.75446034330909], [8.1606, 6.6929]], + [[5.868264308916791, 7.65806868502071], [5.689291513542875, 7.862460554158309], [5.6681440863824655, 7.482552154011366], [5.489137217167998, 7.686881824776223], [5.6787, 7.6725]], + [[3.8964936913977684, 7.628296531026679], [3.7451579530847834, 7.806126097684437], [3.753393465350552, 7.498270334730576], [3.601306761292089, 7.6752783139212095], [3.7493, 7.6517]], + [[5.013579444972925, 5.258424164729132], [4.855987310986054, 5.43461994934455], [4.77914879176049, 5.0423292824686134], [4.620745209365661, 5.21928843195486], [4.8177, 5.2389]], + [[6.223286510483825, 8.196106452919409], [6.019796777995957, 8.445893183517185], [5.985661836443988, 7.991671538206259], [5.781337372421162, 8.24256543971169], [6.0028, 8.2193]], + [[7.127319403305152, 6.950606737471369], [6.829500834905211, 7.346766872388851], [6.7453833141344015, 6.6531794547802345], [6.4461781552425705, 7.049562028270647], [6.7874, 7]], + [[6.879104138534094, 8.458675276408396], [6.612236589820532, 8.814263001493664], [6.5371351221782446, 8.193227852345622], [6.269132656566754, 8.548781374177187], [6.5747, 8.5036]], + [[5.750794937455901, 12.834380728351041], [5.551779977910674, 13.127234093808253], [5.458547830653061, 12.606319909511752], [5.256466573227954, 12.89645794633425], [5.5049, 12.865]], + [[12.236136677048247, 6.397780530317671], [11.934915975220974, 6.73219912822947], [11.921276331392079, 6.154883300987378], [11.623774759235417, 6.491804041597814], [11.928, 6.4451]], + [[11.35167019478413, 13.918823108639327], [11.113752845723774, 14.217734805829613], [11.00396074551618, 13.665600824836297], [10.768483114589309, 13.961174001785437], [11.059, 13.94]], + [[10.76888592754167, 19.783521677263693], [10.724480754884983, 19.830390815399497], [10.510155031975321, 19.54789122104505], [10.466075538745912, 19.59431504577501], [10.617, 19.689]], + [[14.18096199364197, 8.448704715530768], [13.920204922330358, 8.727787167996576], [13.924512848477843, 8.222989172171664], [13.665584139297211, 8.504176307851402], [13.922, 8.4765]], + [[0.4083845582816898, 11.904622977630932], [0.1207074208310688, 12.024708296773124], [0.31605682890974773, 11.633212977453566], [0.027432289522695212, 11.751043514294553], [.21877, 11.828]], + [[11.180242335145197, 9.541916495017208], [10.837351090022546, 9.643236607336567], [11.06817379587339, 9.225132938409587], [10.726125359591746, 9.328051272735896], [10.952, 9.4347]], + [[1.2354341459512899, 12.591700829180386], [0.9632273591369878, 12.705660068685964], [1.1613803572483885, 12.371466315320095], [0.8884873686573949, 12.483570808833838], [1.0627, 12.538]], + [[8.275353533025497, 7.820916970615602], [7.988943573792922, 7.918167059551431], [8.22561167166084, 7.638737949957483], [7.938214231120491, 7.736142494360259], [8.1071, 7.7786]], + [[1.8759885135548846, 12.65827272496592], [1.6076908617112269, 12.755331553213399], [1.7933483675477249, 12.373428878984681], [1.524691045363504, 12.468125659025729], [1.7009, 12.564]], + [[7.8758320217092015, 15.450666240401363], [7.583199557170406, 15.555042763310482], [7.763223174318077, 15.133340024188174], [7.471791685376341, 15.236157699171228], [7.6736, 15.343]], + [[15.461592377731538, 13.480020234698825], [15.080755850494851, 13.583281732584927], [15.340753032078418, 13.132368870117007], [14.96118705905374, 13.237831585705454], [ 15.21, 13.358]], + [[16.63522985467418, 12.789113318360588], [16.223576199687066, 12.90778814202509], [16.507793879251434, 12.436085761646803], [16.097789647248817, 12.557886211186025], [16.365, 12.673]], + [[7.387189516815292, 15.476818618168467], [7.052732640304554, 15.583935119487633], [7.282445893455895, 15.142517586197867], [6.947895229013318, 15.249358442539574], [7.1676, 15.363]], + [[8.184590673590511, 14.401596646404547], [7.8696480187916285, 14.504245650727809], [8.080292710679474, 14.086544078696978], [7.765668614593856, 14.188882606101314], [7.9751, 14.295]], + [[7.062756275196024, 7.557901842319923], [6.759149670259637, 7.656366740518709], [6.984872441718486, 7.322263787320892], [6.681473884962446, 7.420569542633346], [6.8721, 7.4892]], + [[6.826356420065267, 6.566031733182534], [6.527482947424045, 6.663990442793813], [6.7247738760398335, 6.258619416164071], [6.426052602054472, 6.356321262013435], [6.6262, 6.4612]], + [[6.892204835733332, 6.190599878639939], [6.594701847447061, 6.286326805933994], [6.791668604075844, 5.888103154550565], [6.494478735247395, 5.983605347837641], [6.6933, 6.0871]], + [[6.973789922305426, 6.626380415836077], [6.679903224360282, 6.722130599567213], [6.87326250793497, 6.3259929133683945], [6.579662530486343, 6.4214934551015945], [6.7767, 6.5239]], + [[6.752552853724982, 6.393952454770984], [6.454394307576185, 6.491872154126269], [6.651093640837316, 6.090477900190639], [6.353155982609031, 6.188100502136971], [6.5528, 6.291]], + [[6.703760899953288, 6.744511090100315], [6.417128888488336, 6.839186382404792], [6.6048454734328175, 6.4543717198129436], [6.318573945312485, 6.54866009390069], [6.5111, 6.6466]], + [[5.900820700294127, 6.429582846786106], [5.698305385285279, 6.495525120033584], [5.83071328028057, 6.221860629744277], [5.6284631830465495, 6.287585845775141], [5.7646, 6.3586]], + [[6.019122155271516, 6.273443654795807], [5.816002246448603, 6.339610204444365], [5.948855798764804, 6.066855280216651], [5.746017325791041, 6.132894966155826], [5.8825, 6.2031]], + [[5.967814141782397, 6.168893755625507], [5.750786089571373, 6.239486139672103], [5.893111630566762, 5.949225340452029], [5.676418121116962, 6.019681031942042], [ 5.822, 6.0942]], + [[5.737605211403367, 6.056461748111724], [5.512048954403366, 6.12889424904356], [5.6599989307558225, 5.824104250936089], [5.434801831966475, 5.896332562587451], [5.5861, 5.9763]], + [[5.532976120829551, 6.000424143191918], [5.305800747945616, 6.073800007443183], [5.457076490638636, 5.774494852377961], [5.230269887779285, 5.847609363487493], [5.3815, 5.924]], + [[5.356490063659976, 5.864814324893526], [5.13632441696498, 5.936186664236529], [5.280967853216133, 5.639341662476303], [5.061118012436216, 5.710430163062299], [5.2087, 5.7876]], + [[5.307082780891039, 5.8595828640065974], [5.087681587369265, 5.93034820640668], [5.231727957537256, 5.636695761304873], [5.012777684428902, 5.707159103461462], [5.1598, 5.7833]], + [[4.914165858826814, 5.774140473314807], [4.70719498615093, 5.841199166886812], [4.843636383168186, 5.561753644257615], [4.636961563664698, 5.62840097388753], [4.7755, 5.7013]], + [[4.939243900904852, 5.856317922364928], [4.736975669354161, 5.922192733669274], [4.868698896676785, 5.651054564718563], [4.666987578957457, 5.716605011033422], [ 4.803, 5.7864]], + [[4.845877489170694, 5.704414877632749], [4.639577335576276, 5.77118086740584], [4.777594618505529, 5.503572129879802], [4.571823382307328, 5.570000303371163], [4.7087, 5.6371]], + [[4.731818325584564, 5.582069474311983], [4.5329777277894925, 5.646242462687953], [4.664673345229449, 5.383161132123321], [4.466324222150111, 5.44703959433456], [4.5989, 5.5145]], + [[4.615464787590445, 5.509793520734127], [4.420314645516308, 5.573146081144531], [4.549391481338618, 5.316290504685117], [4.354812557688099, 5.3794160507077375], [ 4.485, 5.4445]], + [[4.47980223834068, 5.443132453713634], [4.306544954005624, 5.500183759217754], [4.420012288473538, 5.268656247783469], [4.247174834129765, 5.32545619539032], [4.3634, 5.3842]], + [[4.251742388940668, 5.32843340410147], [4.087673638894631, 5.381888975397149], [4.201905018806161, 5.178984187343557], [4.038063130821996, 5.232164703487941], [4.1449, 5.2803]], + [[4.138823115760495, 5.283149529477127], [3.9916505756369065, 5.330260745915433], [4.107083801304404, 5.186711929440434], [3.9600862638707004, 5.23368491676869], [4.0494, 5.2584]], + [[4.022798760809684, 5.349122689678391], [3.901643105647893, 5.389067391155831], [3.999182380315409, 5.280970057093686], [3.8782521765468543, 5.320752109480655], [3.9505, 5.3349]], + ] + geolocation_coordinates = np.array(geolocation_coordinates) + + detections_in_world = [] + for geolocation_coordinate in geolocation_coordinates: + result, detection_world = detection_in_world.DetectionInWorld.create( + geolocation_coordinate[:4], + geolocation_coordinate[4], + 0, + 1.0, + ) + if not result: + return -1 + detections_in_world.append(detection_world) + + result, estimator = cluster_estimation.ClusterEstimation.create(5, 5, 0) + if not result: + return -2 + + result, objects_in_world = estimator.run(detections_in_world, False) + + for object_in_world in objects_in_world: + print(object_in_world) + + return 0 + + +if __name__ == "__main__": + result_main = main() + if result_main < 0: + print(f"ERROR: Status code: {result_main}") + + print("Done!")