123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703270427052706270727082709271027112712271327142715271627172718271927202721272227232724272527262727272827292730273127322733273427352736273727382739274027412742274327442745274627472748274927502751275227532754275527562757275827592760276127622763276427652766276727682769277027712772277327742775277627772778277927802781278227832784278527862787278827892790279127922793279427952796279727982799280028012802280328042805280628072808280928102811281228132814281528162817281828192820282128222823282428252826282728282829283028312832283328342835283628372838283928402841284228432844284528462847284828492850285128522853285428552856285728582859286028612862286328642865286628672868286928702871287228732874287528762877287828792880288128822883288428852886288728882889289028912892289328942895289628972898289929002901290229032904290529062907290829092910291129122913291429152916291729182919292029212922292329242925292629272928292929302931293229332934293529362937293829392940294129422943294429452946294729482949295029512952295329542955295629572958295929602961296229632964296529662967296829692970297129722973297429752976297729782979298029812982298329842985298629872988298929902991299229932994299529962997299829993000300130023003300430053006300730083009301030113012301330143015301630173018301930203021302230233024302530263027302830293030303130323033303430353036303730383039304030413042304330443045304630473048304930503051305230533054305530563057305830593060306130623063306430653066306730683069307030713072307330743075307630773078307930803081308230833084308530863087308830893090309130923093309430953096309730983099310031013102310331043105310631073108310931103111311231133114311531163117311831193120312131223123312431253126312731283129313031313132313331343135313631373138313931403141314231433144314531463147314831493150315131523153315431553156315731583159316031613162316331643165316631673168316931703171317231733174317531763177317831793180318131823183318431853186318731883189319031913192319331943195319631973198319932003201320232033204320532063207320832093210321132123213321432153216321732183219322032213222322332243225322632273228322932303231323232333234323532363237323832393240324132423243324432453246324732483249325032513252325332543255325632573258325932603261326232633264326532663267326832693270327132723273327432753276327732783279328032813282328332843285328632873288328932903291329232933294329532963297329832993300330133023303330433053306330733083309331033113312331333143315331633173318331933203321332233233324332533263327332833293330333133323333333433353336333733383339334033413342334333443345334633473348334933503351335233533354335533563357335833593360336133623363336433653366336733683369337033713372337333743375337633773378337933803381338233833384338533863387338833893390339133923393339433953396339733983399340034013402340334043405340634073408340934103411341234133414341534163417341834193420342134223423342434253426342734283429343034313432343334343435343634373438343934403441344234433444344534463447344834493450345134523453345434553456345734583459346034613462346334643465346634673468346934703471347234733474347534763477347834793480348134823483348434853486348734883489349034913492349334943495349634973498349935003501350235033504350535063507350835093510351135123513351435153516351735183519352035213522352335243525352635273528352935303531353235333534353535363537353835393540354135423543354435453546354735483549355035513552355335543555355635573558355935603561356235633564356535663567356835693570357135723573357435753576357735783579358035813582358335843585358635873588358935903591359235933594359535963597359835993600360136023603360436053606360736083609361036113612361336143615361636173618361936203621362236233624362536263627362836293630363136323633363436353636363736383639364036413642364336443645364636473648364936503651365236533654365536563657365836593660366136623663366436653666366736683669367036713672367336743675367636773678367936803681368236833684368536863687368836893690369136923693369436953696369736983699370037013702370337043705370637073708370937103711371237133714371537163717371837193720372137223723372437253726372737283729373037313732373337343735373637373738373937403741374237433744374537463747374837493750375137523753375437553756375737583759376037613762376337643765376637673768376937703771377237733774377537763777377837793780378137823783378437853786378737883789379037913792379337943795379637973798379938003801380238033804380538063807380838093810381138123813381438153816381738183819382038213822382338243825382638273828382938303831383238333834383538363837383838393840384138423843384438453846384738483849385038513852385338543855385638573858385938603861386238633864386538663867386838693870387138723873387438753876387738783879388038813882388338843885388638873888388938903891389238933894389538963897389838993900390139023903390439053906390739083909391039113912391339143915391639173918391939203921392239233924392539263927392839293930393139323933393439353936393739383939394039413942394339443945394639473948394939503951395239533954395539563957395839593960396139623963396439653966396739683969397039713972397339743975397639773978397939803981398239833984398539863987398839893990399139923993399439953996399739983999400040014002400340044005400640074008400940104011401240134014401540164017401840194020402140224023402440254026402740284029403040314032403340344035403640374038403940404041404240434044404540464047404840494050405140524053405440554056405740584059406040614062406340644065406640674068406940704071407240734074407540764077407840794080408140824083408440854086408740884089409040914092409340944095409640974098409941004101410241034104410541064107410841094110411141124113411441154116411741184119412041214122412341244125412641274128412941304131413241334134413541364137413841394140414141424143414441454146414741484149415041514152415341544155415641574158415941604161416241634164416541664167416841694170417141724173417441754176417741784179418041814182418341844185418641874188418941904191419241934194419541964197419841994200420142024203420442054206420742084209421042114212421342144215421642174218421942204221422242234224422542264227422842294230423142324233423442354236423742384239424042414242424342444245424642474248424942504251425242534254425542564257425842594260426142624263426442654266426742684269427042714272427342744275427642774278427942804281428242834284428542864287428842894290429142924293429442954296429742984299430043014302430343044305430643074308430943104311431243134314431543164317431843194320432143224323432443254326432743284329433043314332433343344335433643374338433943404341434243434344434543464347434843494350435143524353435443554356435743584359436043614362436343644365436643674368436943704371437243734374437543764377437843794380438143824383438443854386438743884389439043914392439343944395439643974398439944004401440244034404440544064407440844094410441144124413441444154416441744184419442044214422442344244425442644274428442944304431443244334434443544364437443844394440444144424443444444454446444744484449445044514452445344544455445644574458445944604461446244634464446544664467446844694470447144724473447444754476447744784479448044814482448344844485448644874488448944904491449244934494449544964497449844994500450145024503450445054506450745084509451045114512451345144515451645174518451945204521452245234524452545264527452845294530453145324533453445354536453745384539454045414542454345444545454645474548454945504551455245534554455545564557455845594560456145624563456445654566456745684569457045714572457345744575457645774578457945804581458245834584458545864587458845894590459145924593459445954596459745984599460046014602460346044605460646074608460946104611461246134614461546164617461846194620462146224623462446254626462746284629463046314632463346344635463646374638463946404641464246434644464546464647464846494650465146524653465446554656465746584659466046614662466346644665466646674668466946704671467246734674467546764677467846794680468146824683468446854686468746884689469046914692469346944695469646974698469947004701470247034704470547064707470847094710471147124713471447154716471747184719472047214722472347244725472647274728472947304731473247334734473547364737473847394740474147424743474447454746474747484749475047514752475347544755475647574758475947604761476247634764476547664767476847694770477147724773477447754776477747784779478047814782478347844785478647874788478947904791479247934794479547964797479847994800480148024803480448054806480748084809481048114812481348144815481648174818481948204821482248234824482548264827482848294830483148324833483448354836483748384839484048414842484348444845484648474848484948504851485248534854485548564857485848594860486148624863486448654866486748684869487048714872487348744875487648774878487948804881488248834884488548864887488848894890489148924893489448954896489748984899490049014902490349044905490649074908490949104911491249134914491549164917491849194920492149224923492449254926492749284929493049314932493349344935493649374938493949404941494249434944494549464947494849494950495149524953495449554956495749584959496049614962496349644965496649674968496949704971497249734974497549764977497849794980498149824983498449854986498749884989499049914992499349944995499649974998499950005001500250035004500550065007500850095010501150125013501450155016501750185019502050215022502350245025502650275028502950305031503250335034503550365037503850395040504150425043504450455046504750485049505050515052505350545055505650575058505950605061506250635064506550665067506850695070507150725073507450755076507750785079508050815082508350845085508650875088508950905091509250935094509550965097509850995100510151025103510451055106510751085109511051115112511351145115511651175118511951205121512251235124512551265127512851295130513151325133513451355136513751385139514051415142514351445145514651475148514951505151515251535154515551565157515851595160516151625163516451655166516751685169517051715172517351745175517651775178517951805181518251835184518551865187518851895190519151925193519451955196519751985199520052015202520352045205520652075208520952105211521252135214521552165217521852195220522152225223522452255226522752285229523052315232523352345235523652375238523952405241524252435244524552465247524852495250525152525253525452555256525752585259526052615262526352645265526652675268526952705271527252735274527552765277527852795280528152825283528452855286528752885289529052915292529352945295529652975298529953005301530253035304530553065307530853095310531153125313531453155316531753185319532053215322532353245325532653275328532953305331533253335334533553365337533853395340534153425343534453455346534753485349535053515352535353545355535653575358535953605361536253635364536553665367536853695370537153725373537453755376537753785379538053815382538353845385538653875388538953905391539253935394539553965397539853995400540154025403540454055406540754085409541054115412541354145415541654175418541954205421542254235424542554265427542854295430543154325433543454355436543754385439544054415442544354445445544654475448544954505451545254535454545554565457545854595460546154625463546454655466546754685469547054715472547354745475547654775478547954805481548254835484548554865487548854895490549154925493549454955496549754985499550055015502550355045505550655075508550955105511551255135514551555165517551855195520552155225523552455255526552755285529553055315532553355345535553655375538553955405541554255435544554555465547554855495550555155525553555455555556555755585559556055615562556355645565556655675568556955705571557255735574557555765577557855795580558155825583558455855586558755885589559055915592559355945595559655975598559956005601560256035604560556065607560856095610561156125613561456155616561756185619562056215622562356245625562656275628562956305631563256335634563556365637563856395640564156425643564456455646564756485649565056515652565356545655565656575658565956605661566256635664566556665667566856695670567156725673567456755676567756785679568056815682568356845685568656875688568956905691569256935694569556965697569856995700570157025703570457055706570757085709571057115712571357145715571657175718571957205721572257235724572557265727572857295730573157325733573457355736573757385739574057415742574357445745574657475748574957505751575257535754575557565757575857595760576157625763576457655766576757685769577057715772577357745775577657775778577957805781578257835784578557865787578857895790579157925793579457955796579757985799580058015802580358045805580658075808580958105811581258135814581558165817581858195820582158225823582458255826582758285829583058315832583358345835583658375838583958405841584258435844584558465847584858495850585158525853585458555856585758585859586058615862586358645865586658675868586958705871587258735874587558765877587858795880588158825883588458855886588758885889589058915892589358945895589658975898589959005901590259035904590559065907590859095910591159125913591459155916591759185919592059215922592359245925592659275928592959305931593259335934593559365937593859395940594159425943594459455946594759485949595059515952595359545955595659575958595959605961596259635964596559665967596859695970597159725973597459755976597759785979598059815982598359845985598659875988598959905991599259935994599559965997599859996000600160026003600460056006600760086009601060116012601360146015601660176018601960206021602260236024602560266027602860296030603160326033603460356036603760386039604060416042604360446045604660476048604960506051605260536054605560566057605860596060606160626063606460656066606760686069607060716072607360746075607660776078607960806081608260836084608560866087608860896090609160926093609460956096609760986099610061016102610361046105610661076108610961106111611261136114611561166117611861196120612161226123612461256126612761286129613061316132613361346135613661376138613961406141614261436144614561466147614861496150615161526153615461556156615761586159616061616162616361646165616661676168616961706171617261736174617561766177617861796180618161826183618461856186618761886189619061916192619361946195619661976198619962006201620262036204620562066207620862096210621162126213621462156216621762186219622062216222622362246225622662276228622962306231623262336234623562366237623862396240624162426243624462456246624762486249625062516252625362546255625662576258625962606261626262636264626562666267626862696270627162726273627462756276627762786279628062816282628362846285628662876288628962906291629262936294629562966297629862996300630163026303630463056306630763086309631063116312631363146315631663176318631963206321632263236324632563266327632863296330633163326333633463356336633763386339634063416342634363446345634663476348634963506351635263536354635563566357635863596360636163626363636463656366636763686369637063716372637363746375637663776378637963806381638263836384638563866387638863896390639163926393639463956396639763986399640064016402640364046405640664076408640964106411641264136414641564166417641864196420642164226423642464256426642764286429643064316432643364346435643664376438643964406441644264436444644564466447644864496450645164526453645464556456645764586459646064616462646364646465646664676468646964706471647264736474647564766477647864796480648164826483648464856486648764886489649064916492649364946495649664976498649965006501650265036504650565066507650865096510651165126513651465156516651765186519652065216522652365246525652665276528652965306531653265336534653565366537653865396540654165426543654465456546654765486549655065516552655365546555655665576558655965606561656265636564656565666567656865696570657165726573657465756576657765786579658065816582658365846585658665876588658965906591659265936594659565966597659865996600660166026603660466056606660766086609661066116612661366146615661666176618661966206621662266236624662566266627662866296630663166326633 |
- /*
- * Copyright 2010 SRI International
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
- #ifndef OPEN_KARTO_KARTO_H
- #define OPEN_KARTO_KARTO_H
- #include <string>
- #include <fstream>
- #include <limits>
- #include <algorithm>
- #include <map>
- #include <vector>
- #include <iostream>
- #include <iomanip>
- #include <sstream>
- #include <stdexcept>
- #include <math.h>
- #include <float.h>
- #include <stdio.h>
- #include <assert.h>
- #include <string.h>
- #include <boost/thread.hpp>
- #ifdef USE_POCO
- #include <Poco/Mutex.h>
- #endif
- #include <open_karto/Math.h>
- #include <open_karto/Macros.h>
- #define KARTO_Object(name) \
- virtual const char* GetClassName() const { return #name; } \
- virtual kt_objecttype GetObjectType() const { return ObjectType_##name; }
- typedef kt_int32u kt_objecttype;
- const kt_objecttype ObjectType_None = 0x00000000;
- const kt_objecttype ObjectType_Sensor = 0x00001000;
- const kt_objecttype ObjectType_SensorData = 0x00002000;
- const kt_objecttype ObjectType_CustomData = 0x00004000;
- const kt_objecttype ObjectType_Misc = 0x10000000;
- const kt_objecttype ObjectType_Drive = ObjectType_Sensor | 0x01;
- const kt_objecttype ObjectType_LaserRangeFinder = ObjectType_Sensor | 0x02;
- const kt_objecttype ObjectType_Camera = ObjectType_Sensor | 0x04;
- const kt_objecttype ObjectType_DrivePose = ObjectType_SensorData | 0x01;
- const kt_objecttype ObjectType_LaserRangeScan = ObjectType_SensorData | 0x02;
- const kt_objecttype ObjectType_LocalizedRangeScan = ObjectType_SensorData | 0x04;
- const kt_objecttype ObjectType_CameraImage = ObjectType_SensorData | 0x08;
- const kt_objecttype ObjectType_LocalizedRangeScanWithPoints = ObjectType_SensorData | 0x16;
- const kt_objecttype ObjectType_Header = ObjectType_Misc | 0x01;
- const kt_objecttype ObjectType_Parameters = ObjectType_Misc | 0x02;
- const kt_objecttype ObjectType_DatasetInfo = ObjectType_Misc | 0x04;
- const kt_objecttype ObjectType_Module = ObjectType_Misc | 0x08;
- namespace karto
- {
- /**
- * \defgroup OpenKarto OpenKarto Module
- */
- /*@{*/
- /**
- * Exception class. All exceptions thrown from Karto will inherit from this class or be of this class
- */
- class KARTO_EXPORT Exception
- {
- public:
- /**
- * Constructor with exception message
- * @param rMessage exception message (default: "Karto Exception")
- * @param errorCode error code (default: 0)
- */
- Exception(const std::string& rMessage = "Karto Exception", kt_int32s errorCode = 0)
- : m_Message(rMessage)
- , m_ErrorCode(errorCode)
- {
- }
- /**
- * Copy constructor
- */
- Exception(const Exception& rException)
- : m_Message(rException.m_Message)
- , m_ErrorCode(rException.m_ErrorCode)
- {
- }
- /**
- * Destructor
- */
- virtual ~Exception()
- {
- }
- public:
- /**
- * Assignment operator
- */
- Exception& operator = (const Exception& rException)
- {
- m_Message = rException.m_Message;
- m_ErrorCode = rException.m_ErrorCode;
- return *this;
- }
- public:
- /**
- * Gets the exception message
- * @return error message as string
- */
- const std::string& GetErrorMessage() const
- {
- return m_Message;
- }
- /**
- * Gets error code
- * @return error code
- */
- kt_int32s GetErrorCode()
- {
- return m_ErrorCode;
- }
- public:
- /**
- * Write exception to output stream
- * @param rStream output stream
- * @param rException exception to write
- */
- friend KARTO_EXPORT std::ostream& operator << (std::ostream& rStream, Exception& rException);
- private:
- std::string m_Message;
- kt_int32s m_ErrorCode;
- }; // class Exception
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Subclass this class to make a non-copyable class (copy
- * constructor and assignment operator are private)
- */
- class KARTO_EXPORT NonCopyable
- {
- private:
- NonCopyable(const NonCopyable&);
- const NonCopyable& operator=(const NonCopyable&);
- protected:
- NonCopyable()
- {
- }
- virtual ~NonCopyable()
- {
- }
- }; // class NonCopyable
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Singleton class ensures only one instance of T is created
- */
- template <class T>
- class Singleton
- {
- public:
- /**
- * Constructor
- */
- Singleton()
- : m_pPointer(NULL)
- {
- }
- /**
- * Destructor
- */
- virtual ~Singleton()
- {
- delete m_pPointer;
- }
- /**
- * Gets the singleton
- * @return singleton
- */
- T* Get()
- {
- #ifdef USE_POCO
- Poco::FastMutex::ScopedLock lock(m_Mutex);
- #endif
- if (m_pPointer == NULL)
- {
- m_pPointer = new T;
- }
- return m_pPointer;
- }
- private:
- T* m_pPointer;
- #ifdef USE_POCO
- Poco::FastMutex m_Mutex;
- #endif
- private:
- Singleton(const Singleton&);
- const Singleton& operator=(const Singleton&);
- };
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Functor
- */
- class KARTO_EXPORT Functor
- {
- public:
- /**
- * Functor function
- */
- virtual void operator() (kt_int32u) {};
- }; // Functor
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- class AbstractParameter;
- /**
- * Type declaration of AbstractParameter vector
- */
- typedef std::vector<AbstractParameter*> ParameterVector;
- /**
- * Parameter manager.
- */
- class KARTO_EXPORT ParameterManager : public NonCopyable
- {
- public:
- /**
- * Default constructor
- */
- ParameterManager()
- {
- }
- /**
- * Destructor
- */
- virtual ~ParameterManager()
- {
- Clear();
- }
- public:
- /**
- * Adds the parameter to this manager
- * @param pParameter
- */
- void Add(AbstractParameter* pParameter);
- /**
- * Gets the parameter of the given name
- * @param rName
- * @return parameter of given name
- */
- AbstractParameter* Get(const std::string& rName)
- {
- if (m_ParameterLookup.find(rName) != m_ParameterLookup.end())
- {
- return m_ParameterLookup[rName];
- }
- std::cout << "Unknown parameter: " << rName << std::endl;
- return NULL;
- }
- /**
- * Clears the manager of all parameters
- */
- void Clear();
- /**
- * Gets all parameters
- * @return vector of all parameters
- */
- inline const ParameterVector& GetParameterVector() const
- {
- return m_Parameters;
- }
- public:
- /**
- * Gets the parameter with the given name
- * @param rName
- * @return parameter of given name
- */
- AbstractParameter* operator() (const std::string& rName)
- {
- return Get(rName);
- }
- private:
- ParameterManager(const ParameterManager&);
- const ParameterManager& operator=(const ParameterManager&);
- private:
- ParameterVector m_Parameters;
- std::map<std::string, AbstractParameter*> m_ParameterLookup;
- }; // ParameterManager
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- // valid names
- // 'Test' -- no scope
- // '/Test' -- no scope will be parsed to 'Test'
- // '/scope/Test' - 'scope' scope and 'Test' name
- // '/scope/name/Test' - 'scope/name' scope and 'Test' name
- //
- class Name
- {
- public:
- /**
- * Constructor
- */
- Name()
- {
- }
- /**
- * Constructor
- */
- Name(const std::string& rName)
- {
- Parse(rName);
- }
- /**
- * Constructor
- */
- Name(const Name& rOther)
- : m_Name(rOther.m_Name)
- , m_Scope(rOther.m_Scope)
- {
- }
- /**
- * Destructor
- */
- virtual ~Name()
- {
- }
- public:
- /**
- * Gets the name of this name
- * @return name
- */
- inline const std::string& GetName() const
- {
- return m_Name;
- }
- /**
- * Sets the name
- * @param rName name
- */
- inline void SetName(const std::string& rName)
- {
- std::string::size_type pos = rName.find_last_of('/');
- if (pos != 0 && pos != std::string::npos)
- {
- throw Exception("Name can't contain a scope!");
- }
- m_Name = rName;
- }
- /**
- * Gets the scope of this name
- * @return scope
- */
- inline const std::string& GetScope() const
- {
- return m_Scope;
- }
- /**
- * Sets the scope of this name
- * @param rScope scope
- */
- inline void SetScope(const std::string& rScope)
- {
- m_Scope = rScope;
- }
- /**
- * Returns a string representation of this name
- * @return string representation of this name
- */
- inline std::string ToString() const
- {
- if (m_Scope == "")
- {
- return m_Name;
- }
- else
- {
- std::string name;
- name.append("/");
- name.append(m_Scope);
- name.append("/");
- name.append(m_Name);
- return name;
- }
- }
- public:
- /**
- * Assignment operator.
- */
- Name& operator = (const Name& rOther)
- {
- if (&rOther != this)
- {
- m_Name = rOther.m_Name;
- m_Scope = rOther.m_Scope;
- }
- return *this;
- }
- /**
- * Equality operator.
- */
- kt_bool operator == (const Name& rOther) const
- {
- return (m_Name == rOther.m_Name) && (m_Scope == rOther.m_Scope);
- }
- /**
- * Inequality operator.
- */
- kt_bool operator != (const Name& rOther) const
- {
- return !(*this == rOther);
- }
- /**
- * Less than operator.
- */
- kt_bool operator < (const Name& rOther) const
- {
- return ToString() < rOther.ToString();
- }
- /**
- * Write Name onto output stream
- * @param rStream output stream
- * @param rName to write
- */
- friend inline std::ostream& operator << (std::ostream& rStream, const Name& rName)
- {
- rStream << rName.ToString();
- return rStream;
- }
- private:
- /**
- * Parse the given string into a Name object
- * @param rName name
- */
- void Parse(const std::string& rName)
- {
- std::string::size_type pos = rName.find_last_of('/');
- if (pos == std::string::npos)
- {
- m_Name = rName;
- }
- else
- {
- m_Scope = rName.substr(0, pos);
- m_Name = rName.substr(pos+1, rName.size());
- // remove '/' from m_Scope if first!!
- if (m_Scope.size() > 0 && m_Scope[0] == '/')
- {
- m_Scope = m_Scope.substr(1, m_Scope.size());
- }
- }
- }
- /**
- * Validates the given string as a Name
- * @param rName name
- */
- void Validate(const std::string& rName)
- {
- if (rName.empty())
- {
- return;
- }
- char c = rName[0];
- if (IsValidFirst(c))
- {
- for (size_t i = 1; i < rName.size(); ++i)
- {
- c = rName[i];
- if (!IsValid(c))
- {
- throw Exception("Invalid character in name. Valid characters must be within the ranges A-Z, a-z, 0-9, '/', '_' and '-'.");
- }
- }
- }
- else
- {
- throw Exception("Invalid first character in name. Valid characters must be within the ranges A-Z, a-z, and '/'.");
- }
- }
- /**
- * Whether the character is valid as a first character (alphanumeric or /)
- * @param c character
- * @return true if the character is a valid first character
- */
- inline kt_bool IsValidFirst(char c)
- {
- return (isalpha(c) || c == '/');
- }
- /**
- * Whether the character is a valid character (alphanumeric, /, _, or -)
- * @param c character
- * @return true if the character is valid
- */
- inline kt_bool IsValid(char c)
- {
- return (isalnum(c) || c == '/' || c == '_' || c == '-');
- }
- private:
- std::string m_Name;
- std::string m_Scope;
- };
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Abstract base class for Karto objects.
- */
- class KARTO_EXPORT Object : public NonCopyable
- {
- public:
- /**
- * Default constructor
- */
- Object();
- /**
- * Constructs an object with the given name
- * @param rName
- */
- Object(const Name& rName);
- /**
- * Default constructor
- */
- virtual ~Object();
- public:
- /**
- * Gets the name of this object
- * @return name
- */
- inline const Name& GetName() const
- {
- return m_Name;
- }
- /**
- * Gets the class name of this object
- * @return class name
- */
- virtual const char* GetClassName() const = 0;
- /**
- * Gets the type of this object
- * @return object type
- */
- virtual kt_objecttype GetObjectType() const = 0;
- /**
- * Gets the parameter manager of this dataset
- * @return parameter manager
- */
- virtual inline ParameterManager* GetParameterManager()
- {
- return m_pParameterManager;
- }
- /**
- * Gets the named parameter
- * @param rName name of parameter
- * @return parameter
- */
- inline AbstractParameter* GetParameter(const std::string& rName) const
- {
- return m_pParameterManager->Get(rName);
- }
- /**
- * Sets the parameter with the given name with the given value
- * @param rName name
- * @param value value
- */
- template<typename T>
- inline void SetParameter(const std::string& rName, T value);
- /**
- * Gets all parameters
- * @return parameters
- */
- inline const ParameterVector& GetParameters() const
- {
- return m_pParameterManager->GetParameterVector();
- }
- private:
- Object(const Object&);
- const Object& operator=(const Object&);
- private:
- Name m_Name;
- ParameterManager* m_pParameterManager;
- };
- /**
- * Type declaration of Object vector
- */
- typedef std::vector<Object*> ObjectVector;
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Whether the object is a sensor
- * @param pObject object
- * @return whether the object is a sensor
- */
- inline kt_bool IsSensor(Object* pObject)
- {
- return (pObject->GetObjectType() & ObjectType_Sensor) == ObjectType_Sensor;
- }
- /**
- * Whether the object is sensor data
- * @param pObject object
- * @return whether the object is sensor data
- */
- inline kt_bool IsSensorData(Object* pObject)
- {
- return (pObject->GetObjectType() & ObjectType_SensorData) == ObjectType_SensorData;
- }
- /**
- * Whether the object is a laser range finder
- * @param pObject object
- * @return whether the object is a laser range finder
- */
- inline kt_bool IsLaserRangeFinder(Object* pObject)
- {
- return (pObject->GetObjectType() & ObjectType_LaserRangeFinder) == ObjectType_LaserRangeFinder;
- }
- /**
- * Whether the object is a localized range scan
- * @param pObject object
- * @return whether the object is a localized range scan
- */
- inline kt_bool IsLocalizedRangeScan(Object* pObject)
- {
- return (pObject->GetObjectType() & ObjectType_LocalizedRangeScan) == ObjectType_LocalizedRangeScan;
- }
- /**
- * Whether the object is a localized range scan with points
- * @param pObject object
- * @return whether the object is a localized range scan with points
- */
- inline kt_bool IsLocalizedRangeScanWithPoints(Object* pObject)
- {
- return (pObject->GetObjectType() & ObjectType_LocalizedRangeScanWithPoints) == ObjectType_LocalizedRangeScanWithPoints;
- }
- /**
- * Whether the object is a Parameters object
- * @param pObject object
- * @return whether the object is a Parameters object
- */
- inline kt_bool IsParameters(Object* pObject)
- {
- return (pObject->GetObjectType() & ObjectType_Parameters) == ObjectType_Parameters;
- }
- /**
- * Whether the object is a DatasetInfo object
- * @param pObject object
- * @return whether the object is a DatasetInfo object
- */
- inline kt_bool IsDatasetInfo(Object* pObject)
- {
- return (pObject->GetObjectType() & ObjectType_DatasetInfo) == ObjectType_DatasetInfo;
- }
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Abstract base class for Karto modules.
- */
- class KARTO_EXPORT Module : public Object
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(Module)
- // @endcond
- public:
- /**
- * Construct a Module
- * @param rName module name
- */
- Module(const std::string& rName);
- /**
- * Destructor
- */
- virtual ~Module();
- public:
- /**
- * Reset the module
- */
- virtual void Reset() = 0;
- /**
- * Process an Object
- */
- virtual kt_bool Process(karto::Object*)
- {
- return false;
- }
- private:
- Module(const Module&);
- const Module& operator=(const Module&);
- };
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Represents a size (width, height) in 2-dimensional real space.
- */
- template<typename T>
- class Size2
- {
- public:
- /**
- * Default constructor
- */
- Size2()
- : m_Width(0)
- , m_Height(0)
- {
- }
- /**
- * Constructor initializing point location
- * @param width
- * @param height
- */
- Size2(T width, T height)
- : m_Width(width)
- , m_Height(height)
- {
- }
- /**
- * Copy constructor
- * @param rOther
- */
- Size2(const Size2& rOther)
- : m_Width(rOther.m_Width)
- , m_Height(rOther.m_Height)
- {
- }
- public:
- /**
- * Gets the width
- * @return the width
- */
- inline const T GetWidth() const
- {
- return m_Width;
- }
- /**
- * Sets the width
- * @param width
- */
- inline void SetWidth(T width)
- {
- m_Width = width;
- }
- /**
- * Gets the height
- * @return the height
- */
- inline const T GetHeight() const
- {
- return m_Height;
- }
- /**
- * Sets the height
- * @param height
- */
- inline void SetHeight(T height)
- {
- m_Height = height;
- }
- /**
- * Assignment operator
- */
- inline Size2& operator = (const Size2& rOther)
- {
- m_Width = rOther.m_Width;
- m_Height = rOther.m_Height;
- return(*this);
- }
- /**
- * Equality operator
- */
- inline kt_bool operator == (const Size2& rOther) const
- {
- return (m_Width == rOther.m_Width && m_Height == rOther.m_Height);
- }
- /**
- * Inequality operator
- */
- inline kt_bool operator != (const Size2& rOther) const
- {
- return (m_Width != rOther.m_Width || m_Height != rOther.m_Height);
- }
- /**
- * Write Size2 onto output stream
- * @param rStream output stream
- * @param rSize to write
- */
- friend inline std::ostream& operator << (std::ostream& rStream, const Size2& rSize)
- {
- rStream << "(" << rSize.m_Width << ", " << rSize.m_Height << ")";
- return rStream;
- }
- private:
- T m_Width;
- T m_Height;
- }; // Size2<T>
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Represents a vector (x, y) in 2-dimensional real space.
- */
- template<typename T>
- class Vector2
- {
- public:
- /**
- * Default constructor
- */
- Vector2()
- {
- m_Values[0] = 0;
- m_Values[1] = 0;
- }
- /**
- * Constructor initializing vector location
- * @param x
- * @param y
- */
- Vector2(T x, T y)
- {
- m_Values[0] = x;
- m_Values[1] = y;
- }
- public:
- /**
- * Gets the x-coordinate of this vector2
- * @return the x-coordinate of the vector2
- */
- inline const T& GetX() const
- {
- return m_Values[0];
- }
- /**
- * Sets the x-coordinate of this vector2
- * @param x the x-coordinate of the vector2
- */
- inline void SetX(const T& x)
- {
- m_Values[0] = x;
- }
- /**
- * Gets the y-coordinate of this vector2
- * @return the y-coordinate of the vector2
- */
- inline const T& GetY() const
- {
- return m_Values[1];
- }
- /**
- * Sets the y-coordinate of this vector2
- * @param y the y-coordinate of the vector2
- */
- inline void SetY(const T& y)
- {
- m_Values[1] = y;
- }
- /**
- * Floor point operator
- * @param rOther
- */
- inline void MakeFloor(const Vector2& rOther)
- {
- if ( rOther.m_Values[0] < m_Values[0] ) m_Values[0] = rOther.m_Values[0];
- if ( rOther.m_Values[1] < m_Values[1] ) m_Values[1] = rOther.m_Values[1];
- }
- /**
- * Ceiling point operator
- * @param rOther
- */
- inline void MakeCeil(const Vector2& rOther)
- {
- if ( rOther.m_Values[0] > m_Values[0] ) m_Values[0] = rOther.m_Values[0];
- if ( rOther.m_Values[1] > m_Values[1] ) m_Values[1] = rOther.m_Values[1];
- }
- /**
- * Returns the square of the length of the vector
- * @return square of the length of the vector
- */
- inline kt_double SquaredLength() const
- {
- return math::Square(m_Values[0]) + math::Square(m_Values[1]);
- }
- /**
- * Returns the length of the vector (x and y).
- * @return length of the vector
- */
- inline kt_double Length() const
- {
- return sqrt(SquaredLength());
- }
- /**
- * Returns the square distance to the given vector
- * @returns square distance to the given vector
- */
- inline kt_double SquaredDistance(const Vector2& rOther) const
- {
- return (*this - rOther).SquaredLength();
- }
- /**
- * Gets the distance to the other vector2
- * @param rOther
- * @return distance to other vector2
- */
- inline kt_double Distance(const Vector2& rOther) const
- {
- return sqrt(SquaredDistance(rOther));
- }
- public:
- /**
- * In place Vector2 addition.
- */
- inline void operator += (const Vector2& rOther)
- {
- m_Values[0] += rOther.m_Values[0];
- m_Values[1] += rOther.m_Values[1];
- }
- /**
- * In place Vector2 subtraction.
- */
- inline void operator -= (const Vector2& rOther)
- {
- m_Values[0] -= rOther.m_Values[0];
- m_Values[1] -= rOther.m_Values[1];
- }
- /**
- * Addition operator
- * @param rOther
- * @return vector resulting from adding this vector with the given vector
- */
- inline const Vector2 operator + (const Vector2& rOther) const
- {
- return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]);
- }
- /**
- * Subtraction operator
- * @param rOther
- * @return vector resulting from subtracting this vector from the given vector
- */
- inline const Vector2 operator - (const Vector2& rOther) const
- {
- return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]);
- }
- /**
- * In place scalar division operator
- * @param scalar
- */
- inline void operator /= (T scalar)
- {
- m_Values[0] /= scalar;
- m_Values[1] /= scalar;
- }
- /**
- * Divides a Vector2
- * @param scalar
- * @return scalar product
- */
- inline const Vector2 operator / (T scalar) const
- {
- return Vector2(m_Values[0] / scalar, m_Values[1] / scalar);
- }
- /**
- * Computes the dot product between the two vectors
- * @param rOther
- * @return dot product
- */
- inline kt_double operator * (const Vector2& rOther) const
- {
- return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1];
- }
- /**
- * Scales the vector by the given scalar
- * @param scalar
- */
- inline const Vector2 operator * (T scalar) const
- {
- return Vector2(m_Values[0] * scalar, m_Values[1] * scalar);
- }
- /**
- * Subtract the vector by the given scalar
- * @param scalar
- */
- inline const Vector2 operator - (T scalar) const
- {
- return Vector2(m_Values[0] - scalar, m_Values[1] - scalar);
- }
- /**
- * In place scalar multiplication operator
- * @param scalar
- */
- inline void operator *= (T scalar)
- {
- m_Values[0] *= scalar;
- m_Values[1] *= scalar;
- }
- /**
- * Equality operator returns true if the corresponding x, y values of each Vector2 are the same values.
- * @param rOther
- */
- inline kt_bool operator == (const Vector2& rOther) const
- {
- return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]);
- }
- /**
- * Inequality operator returns true if any of the corresponding x, y values of each Vector2 not the same.
- * @param rOther
- */
- inline kt_bool operator != (const Vector2& rOther) const
- {
- return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]);
- }
- /**
- * Less than operator
- * @param rOther
- * @return true if left vector is less than right vector
- */
- inline kt_bool operator < (const Vector2& rOther) const
- {
- if (m_Values[0] < rOther.m_Values[0])
- return true;
- else if (m_Values[0] > rOther.m_Values[0])
- return false;
- else
- return (m_Values[1] < rOther.m_Values[1]);
- }
- /**
- * Write Vector2 onto output stream
- * @param rStream output stream
- * @param rVector to write
- */
- friend inline std::ostream& operator << (std::ostream& rStream, const Vector2& rVector)
- {
- rStream << rVector.GetX() << " " << rVector.GetY();
- return rStream;
- }
- /**
- * Read Vector2 from input stream
- * @param rStream input stream
- */
- friend inline std::istream& operator >> (std::istream& rStream, const Vector2& /*rVector*/)
- {
- // Implement me!! TODO(lucbettaieb): What the what? Do I need to implement this?
- return rStream;
- }
- private:
- T m_Values[2];
- }; // Vector2<T>
- /**
- * Type declaration of Vector2<kt_double> vector
- */
- typedef std::vector< Vector2<kt_double> > PointVectorDouble;
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Represents a vector (x, y, z) in 3-dimensional real space.
- */
- template<typename T>
- class Vector3
- {
- public:
- /**
- * Default constructor
- */
- Vector3()
- {
- m_Values[0] = 0;
- m_Values[1] = 0;
- m_Values[2] = 0;
- }
- /**
- * Constructor initializing point location
- * @param x
- * @param y
- * @param z
- */
- Vector3(T x, T y, T z)
- {
- m_Values[0] = x;
- m_Values[1] = y;
- m_Values[2] = z;
- }
- /**
- * Copy constructor
- * @param rOther
- */
- Vector3(const Vector3& rOther)
- {
- m_Values[0] = rOther.m_Values[0];
- m_Values[1] = rOther.m_Values[1];
- m_Values[2] = rOther.m_Values[2];
- }
- public:
- /**
- * Gets the x-component of this vector
- * @return x-component
- */
- inline const T& GetX() const
- {
- return m_Values[0];
- }
- /**
- * Sets the x-component of this vector
- * @param x
- */
- inline void SetX(const T& x)
- {
- m_Values[0] = x;
- }
- /**
- * Gets the y-component of this vector
- * @return y-component
- */
- inline const T& GetY() const
- {
- return m_Values[1];
- }
- /**
- * Sets the y-component of this vector
- * @param y
- */
- inline void SetY(const T& y)
- {
- m_Values[1] = y;
- }
- /**
- * Gets the z-component of this vector
- * @return z-component
- */
- inline const T& GetZ() const
- {
- return m_Values[2];
- }
- /**
- * Sets the z-component of this vector
- * @param z
- */
- inline void SetZ(const T& z)
- {
- m_Values[2] = z;
- }
- /**
- * Floor vector operator
- * @param rOther Vector3d
- */
- inline void MakeFloor(const Vector3& rOther)
- {
- if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
- if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
- if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2];
- }
- /**
- * Ceiling vector operator
- * @param rOther Vector3d
- */
- inline void MakeCeil(const Vector3& rOther)
- {
- if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
- if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
- if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2];
- }
- /**
- * Returns the square of the length of the vector
- * @return square of the length of the vector
- */
- inline kt_double SquaredLength() const
- {
- return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]);
- }
- /**
- * Returns the length of the vector.
- * @return Length of the vector
- */
- inline kt_double Length() const
- {
- return sqrt(SquaredLength());
- }
- /**
- * Returns a string representation of this vector
- * @return string representation of this vector
- */
- inline std::string ToString() const
- {
- std::stringstream converter;
- converter.precision(std::numeric_limits<double>::digits10);
- converter << GetX() << " " << GetY() << " " << GetZ();
- return converter.str();
- }
- public:
- /**
- * Assignment operator
- */
- inline Vector3& operator = (const Vector3& rOther)
- {
- m_Values[0] = rOther.m_Values[0];
- m_Values[1] = rOther.m_Values[1];
- m_Values[2] = rOther.m_Values[2];
- return *this;
- }
- /**
- * Binary vector add.
- * @param rOther
- * @return vector sum
- */
- inline const Vector3 operator + (const Vector3& rOther) const
- {
- return Vector3(m_Values[0] + rOther.m_Values[0],
- m_Values[1] + rOther.m_Values[1],
- m_Values[2] + rOther.m_Values[2]);
- }
- /**
- * Binary vector add.
- * @param scalar
- * @return sum
- */
- inline const Vector3 operator + (kt_double scalar) const
- {
- return Vector3(m_Values[0] + scalar,
- m_Values[1] + scalar,
- m_Values[2] + scalar);
- }
- /**
- * Binary vector subtract.
- * @param rOther
- * @return vector difference
- */
- inline const Vector3 operator - (const Vector3& rOther) const
- {
- return Vector3(m_Values[0] - rOther.m_Values[0],
- m_Values[1] - rOther.m_Values[1],
- m_Values[2] - rOther.m_Values[2]);
- }
- /**
- * Binary vector subtract.
- * @param scalar
- * @return difference
- */
- inline const Vector3 operator - (kt_double scalar) const
- {
- return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar);
- }
- /**
- * Scales the vector by the given scalar
- * @param scalar
- */
- inline const Vector3 operator * (T scalar) const
- {
- return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar);
- }
- /**
- * Equality operator returns true if the corresponding x, y, z values of each Vector3 are the same values.
- * @param rOther
- */
- inline kt_bool operator == (const Vector3& rOther) const
- {
- return (m_Values[0] == rOther.m_Values[0] &&
- m_Values[1] == rOther.m_Values[1] &&
- m_Values[2] == rOther.m_Values[2]);
- }
- /**
- * Inequality operator returns true if any of the corresponding x, y, z values of each Vector3 not the same.
- * @param rOther
- */
- inline kt_bool operator != (const Vector3& rOther) const
- {
- return (m_Values[0] != rOther.m_Values[0] ||
- m_Values[1] != rOther.m_Values[1] ||
- m_Values[2] != rOther.m_Values[2]);
- }
- /**
- * Write Vector3 onto output stream
- * @param rStream output stream
- * @param rVector to write
- */
- friend inline std::ostream& operator << (std::ostream& rStream, const Vector3& rVector)
- {
- rStream << rVector.ToString();
- return rStream;
- }
- /**
- * Read Vector3 from input stream
- * @param rStream input stream
- */
- friend inline std::istream& operator >> (std::istream& rStream, const Vector3& /*rVector*/)
- {
- // Implement me!!
- return rStream;
- }
- private:
- T m_Values[3];
- }; // Vector3
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Defines an orientation as a quaternion rotation using the positive Z axis as the zero reference.
- * <BR>
- * Q = w + ix + jy + kz <BR>
- * w = c_1 * c_2 * c_3 - s_1 * s_2 * s_3 <BR>
- * x = s_1 * s_2 * c_3 + c_1 * c_2 * s_3 <BR>
- * y = s_1 * c_2 * c_3 + c_1 * s_2 * s_3 <BR>
- * z = c_1 * s_2 * c_3 - s_1 * c_2 * s_3 <BR>
- * where <BR>
- * c_1 = cos(theta/2) <BR>
- * c_2 = cos(phi/2) <BR>
- * c_3 = cos(psi/2) <BR>
- * s_1 = sin(theta/2) <BR>
- * s_2 = sin(phi/2) <BR>
- * s_3 = sin(psi/2) <BR>
- * and <BR>
- * theta is the angle of rotation about the Y axis measured from the Z axis. <BR>
- * phi is the angle of rotation about the Z axis measured from the X axis. <BR>
- * psi is the angle of rotation about the X axis measured from the Y axis. <BR>
- * (All angles are right-handed.)
- */
- class Quaternion
- {
- public:
- /**
- * Create a quaternion with default (x=0, y=0, z=0, w=1) values
- */
- inline Quaternion()
- {
- m_Values[0] = 0.0;
- m_Values[1] = 0.0;
- m_Values[2] = 0.0;
- m_Values[3] = 1.0;
- }
- /**
- * Create a quaternion using x, y, z, w values.
- * @param x
- * @param y
- * @param z
- * @param w
- */
- inline Quaternion(kt_double x, kt_double y, kt_double z, kt_double w)
- {
- m_Values[0] = x;
- m_Values[1] = y;
- m_Values[2] = z;
- m_Values[3] = w;
- }
- /**
- * Copy constructor
- */
- inline Quaternion(const Quaternion& rQuaternion)
- {
- m_Values[0] = rQuaternion.m_Values[0];
- m_Values[1] = rQuaternion.m_Values[1];
- m_Values[2] = rQuaternion.m_Values[2];
- m_Values[3] = rQuaternion.m_Values[3];
- }
- public:
- /**
- * Returns the X-value
- * @return Return the X-value of the quaternion
- */
- inline kt_double GetX() const
- {
- return m_Values[0];
- }
- /**
- * Sets the X-value
- * @param x X-value of the quaternion
- */
- inline void SetX(kt_double x)
- {
- m_Values[0] = x;
- }
- /**
- * Returns the Y-value
- * @return Return the Y-value of the quaternion
- */
- inline kt_double GetY() const
- {
- return m_Values[1];
- }
- /**
- * Sets the Y-value
- * @param y Y-value of the quaternion
- */
- inline void SetY(kt_double y)
- {
- m_Values[1] = y;
- }
- /**
- * Returns the Z-value
- * @return Return the Z-value of the quaternion
- */
- inline kt_double GetZ() const
- {
- return m_Values[2];
- }
- /**
- * Sets the Z-value
- * @param z Z-value of the quaternion
- */
- inline void SetZ(kt_double z)
- {
- m_Values[2] = z;
- }
- /**
- * Returns the W-value
- * @return Return the W-value of the quaternion
- */
- inline kt_double GetW() const
- {
- return m_Values[3];
- }
- /**
- * Sets the W-value
- * @param w W-value of the quaternion
- */
- inline void SetW(kt_double w)
- {
- m_Values[3] = w;
- }
- /**
- * Converts this quaternion into Euler angles
- * Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
- * @param rYaw
- * @param rPitch
- * @param rRoll
- */
- void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const
- {
- kt_double test = m_Values[0] * m_Values[1] + m_Values[2] * m_Values[3];
- if (test > 0.499)
- {
- // singularity at north pole
- rYaw = 2 * atan2(m_Values[0], m_Values[3]);;
- rPitch = KT_PI_2;
- rRoll = 0;
- }
- else if (test < -0.499)
- {
- // singularity at south pole
- rYaw = -2 * atan2(m_Values[0], m_Values[3]);
- rPitch = -KT_PI_2;
- rRoll = 0;
- }
- else
- {
- kt_double sqx = m_Values[0] * m_Values[0];
- kt_double sqy = m_Values[1] * m_Values[1];
- kt_double sqz = m_Values[2] * m_Values[2];
- rYaw = atan2(2 * m_Values[1] * m_Values[3] - 2 * m_Values[0] * m_Values[2], 1 - 2 * sqy - 2 * sqz);
- rPitch = asin(2 * test);
- rRoll = atan2(2 * m_Values[0] * m_Values[3] - 2 * m_Values[1] * m_Values[2], 1 - 2 * sqx - 2 * sqz);
- }
- }
- /**
- * Set x,y,z,w values of the quaternion based on Euler angles.
- * Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
- * @param yaw
- * @param pitch
- * @param roll
- */
- void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll)
- {
- kt_double angle;
- angle = yaw * 0.5;
- kt_double cYaw = cos(angle);
- kt_double sYaw = sin(angle);
- angle = pitch * 0.5;
- kt_double cPitch = cos(angle);
- kt_double sPitch = sin(angle);
- angle = roll * 0.5;
- kt_double cRoll = cos(angle);
- kt_double sRoll = sin(angle);
- m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll;
- m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll;
- m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll;
- m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll;
- }
- /**
- * Assignment operator
- * @param rQuaternion
- */
- inline Quaternion& operator = (const Quaternion& rQuaternion)
- {
- m_Values[0] = rQuaternion.m_Values[0];
- m_Values[1] = rQuaternion.m_Values[1];
- m_Values[2] = rQuaternion.m_Values[2];
- m_Values[3] = rQuaternion.m_Values[3];
- return(*this);
- }
- /**
- * Equality operator returns true if the corresponding x, y, z, w values of each quaternion are the same values.
- * @param rOther
- */
- inline kt_bool operator == (const Quaternion& rOther) const
- {
- return (m_Values[0] == rOther.m_Values[0] &&
- m_Values[1] == rOther.m_Values[1] &&
- m_Values[2] == rOther.m_Values[2] &&
- m_Values[3] == rOther.m_Values[3]);
- }
- /**
- * Inequality operator returns true if any of the corresponding x, y, z, w values of each quaternion not the same.
- * @param rOther
- */
- inline kt_bool operator != (const Quaternion& rOther) const
- {
- return (m_Values[0] != rOther.m_Values[0] ||
- m_Values[1] != rOther.m_Values[1] ||
- m_Values[2] != rOther.m_Values[2] ||
- m_Values[3] != rOther.m_Values[3]);
- }
- /**
- * Write this quaternion onto output stream
- * @param rStream output stream
- * @param rQuaternion
- */
- friend inline std::ostream& operator << (std::ostream& rStream, const Quaternion& rQuaternion)
- {
- rStream << rQuaternion.m_Values[0] << " "
- << rQuaternion.m_Values[1] << " "
- << rQuaternion.m_Values[2] << " "
- << rQuaternion.m_Values[3];
- return rStream;
- }
- private:
- kt_double m_Values[4];
- };
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Stores x, y, width and height that represents the location and size of a rectangle
- * (x, y) is at bottom left in mapper!
- */
- template<typename T>
- class Rectangle2
- {
- public:
- /**
- * Default constructor
- */
- Rectangle2()
- {
- }
- /**
- * Constructor initializing rectangle parameters
- * @param x x-coordinate of left edge of rectangle
- * @param y y-coordinate of bottom edge of rectangle
- * @param width width of rectangle
- * @param height height of rectangle
- */
- Rectangle2(T x, T y, T width, T height)
- : m_Position(x, y)
- , m_Size(width, height)
- {
- }
- /**
- * Constructor initializing rectangle parameters
- * @param rPosition (x,y)-coordinate of rectangle
- * @param rSize Size of the rectangle
- */
- Rectangle2(const Vector2<T>& rPosition, const Size2<T>& rSize)
- : m_Position(rPosition)
- , m_Size(rSize)
- {
- }
- /**
- * Copy constructor
- */
- Rectangle2(const Rectangle2& rOther)
- : m_Position(rOther.m_Position)
- , m_Size(rOther.m_Size)
- {
- }
- public:
- /**
- * Gets the x-coordinate of the left edge of this rectangle
- * @return the x-coordinate of the left edge of this rectangle
- */
- inline T GetX() const
- {
- return m_Position.GetX();
- }
- /**
- * Sets the x-coordinate of the left edge of this rectangle
- * @param x the x-coordinate of the left edge of this rectangle
- */
- inline void SetX(T x)
- {
- m_Position.SetX(x);
- }
- /**
- * Gets the y-coordinate of the bottom edge of this rectangle
- * @return the y-coordinate of the bottom edge of this rectangle
- */
- inline T GetY() const
- {
- return m_Position.GetY();
- }
- /**
- * Sets the y-coordinate of the bottom edge of this rectangle
- * @param y the y-coordinate of the bottom edge of this rectangle
- */
- inline void SetY(T y)
- {
- m_Position.SetY(y);
- }
- /**
- * Gets the width of this rectangle
- * @return the width of this rectangle
- */
- inline T GetWidth() const
- {
- return m_Size.GetWidth();
- }
- /**
- * Sets the width of this rectangle
- * @param width the width of this rectangle
- */
- inline void SetWidth(T width)
- {
- m_Size.SetWidth(width);
- }
- /**
- * Gets the height of this rectangle
- * @return the height of this rectangle
- */
- inline T GetHeight() const
- {
- return m_Size.GetHeight();
- }
- /**
- * Sets the height of this rectangle
- * @param height the height of this rectangle
- */
- inline void SetHeight(T height)
- {
- m_Size.SetHeight(height);
- }
- /**
- * Gets the position of this rectangle
- * @return the position of this rectangle
- */
- inline const Vector2<T>& GetPosition() const
- {
- return m_Position;
- }
- /**
- * Sets the position of this rectangle
- * @param rX x
- * @param rY y
- */
- inline void SetPosition(const T& rX, const T& rY)
- {
- m_Position = Vector2<T>(rX, rY);
- }
- /**
- * Sets the position of this rectangle
- * @param rPosition position
- */
- inline void SetPosition(const Vector2<T>& rPosition)
- {
- m_Position = rPosition;
- }
- /**
- * Gets the size of this rectangle
- * @return the size of this rectangle
- */
- inline const Size2<T>& GetSize() const
- {
- return m_Size;
- }
- /**
- * Sets the size of this rectangle
- * @param rSize size
- */
- inline void SetSize(const Size2<T>& rSize)
- {
- m_Size = rSize;
- }
- /**
- * Gets the center of this rectangle
- * @return the center of this rectangle
- */
- inline const Vector2<T> GetCenter() const
- {
- return Vector2<T>(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5);
- }
- public:
- /**
- * Assignment operator
- */
- Rectangle2& operator = (const Rectangle2& rOther)
- {
- m_Position = rOther.m_Position;
- m_Size = rOther.m_Size;
- return *this;
- }
- /**
- * Equality operator
- */
- inline kt_bool operator == (const Rectangle2& rOther) const
- {
- return (m_Position == rOther.m_Position && m_Size == rOther.m_Size);
- }
- /**
- * Inequality operator
- */
- inline kt_bool operator != (const Rectangle2& rOther) const
- {
- return (m_Position != rOther.m_Position || m_Size != rOther.m_Size);
- }
- private:
- Vector2<T> m_Position;
- Size2<T> m_Size;
- }; // Rectangle2
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- class Pose3;
- /**
- * Defines a position (x, y) in 2-dimensional space and heading.
- */
- class Pose2
- {
- public:
- /**
- * Default Constructor
- */
- Pose2()
- : m_Heading(0.0)
- {
- }
- /**
- * Constructor initializing pose parameters
- * @param rPosition position
- * @param heading heading
- **/
- Pose2(const Vector2<kt_double>& rPosition, kt_double heading)
- : m_Position(rPosition)
- , m_Heading(heading)
- {
- }
- /**
- * Constructor initializing pose parameters
- * @param x x-coordinate
- * @param y y-coordinate
- * @param heading heading
- **/
- Pose2(kt_double x, kt_double y, kt_double heading)
- : m_Position(x, y)
- , m_Heading(heading)
- {
- }
- /**
- * Constructs a Pose2 object from a Pose3.
- */
- Pose2(const Pose3& rPose);
- /**
- * Copy constructor
- */
- Pose2(const Pose2& rOther)
- : m_Position(rOther.m_Position)
- , m_Heading(rOther.m_Heading)
- {
- }
- public:
- /**
- * Returns the x-coordinate
- * @return the x-coordinate of the pose
- */
- inline kt_double GetX() const
- {
- return m_Position.GetX();
- }
- /**
- * Sets the x-coordinate
- * @param x the x-coordinate of the pose
- */
- inline void SetX(kt_double x)
- {
- m_Position.SetX(x);
- }
- /**
- * Returns the y-coordinate
- * @return the y-coordinate of the pose
- */
- inline kt_double GetY() const
- {
- return m_Position.GetY();
- }
- /**
- * Sets the y-coordinate
- * @param y the y-coordinate of the pose
- */
- inline void SetY(kt_double y)
- {
- m_Position.SetY(y);
- }
- /**
- * Returns the position
- * @return the position of the pose
- */
- inline const Vector2<kt_double>& GetPosition() const
- {
- return m_Position;
- }
- /**
- * Sets the position
- * @param rPosition of the pose
- */
- inline void SetPosition(const Vector2<kt_double>& rPosition)
- {
- m_Position = rPosition;
- }
- /**
- * Returns the heading of the pose (in radians)
- * @return the heading of the pose
- */
- inline kt_double GetHeading() const
- {
- return m_Heading;
- }
- /**
- * Sets the heading
- * @param heading of the pose
- */
- inline void SetHeading(kt_double heading)
- {
- m_Heading = heading;
- }
- /**
- * Return the squared distance between two Pose2
- * @return squared distance
- */
- inline kt_double SquaredDistance(const Pose2& rOther) const
- {
- return m_Position.SquaredDistance(rOther.m_Position);
- }
- public:
- /**
- * Assignment operator
- */
- inline Pose2& operator = (const Pose2& rOther)
- {
- m_Position = rOther.m_Position;
- m_Heading = rOther.m_Heading;
- return *this;
- }
- /**
- * Equality operator
- */
- inline kt_bool operator == (const Pose2& rOther) const
- {
- return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading);
- }
- /**
- * Inequality operator
- */
- inline kt_bool operator != (const Pose2& rOther) const
- {
- return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading);
- }
- /**
- * In place Pose2 add.
- */
- inline void operator += (const Pose2& rOther)
- {
- m_Position += rOther.m_Position;
- m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading);
- }
- /**
- * Binary Pose2 add
- * @param rOther
- * @return Pose2 sum
- */
- inline Pose2 operator + (const Pose2& rOther) const
- {
- return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading));
- }
- /**
- * Binary Pose2 subtract
- * @param rOther
- * @return Pose2 difference
- */
- inline Pose2 operator - (const Pose2& rOther) const
- {
- return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading));
- }
- /**
- * Read pose from input stream
- * @param rStream input stream
- */
- friend inline std::istream& operator >> (std::istream& rStream, const Pose2& /*rPose*/)
- {
- // Implement me!!
- return rStream;
- }
- /**
- * Write this pose onto output stream
- * @param rStream output stream
- * @param rPose to read
- */
- friend inline std::ostream& operator << (std::ostream& rStream, const Pose2& rPose)
- {
- rStream << rPose.m_Position.GetX() << " " << rPose.m_Position.GetY() << " " << rPose.m_Heading;
- return rStream;
- }
- private:
- Vector2<kt_double> m_Position;
- kt_double m_Heading;
- }; // Pose2
- /**
- * Type declaration of Pose2 vector
- */
- typedef std::vector< Pose2 > Pose2Vector;
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Defines a position and orientation in 3-dimensional space.
- * Karto uses a right-handed coordinate system with X, Y as the 2-D ground plane and X is forward and Y is left.
- * Values in Vector3 used to define position must have units of meters.
- * The value of angle when defining orientation in two dimensions must be in units of radians.
- * The definition of orientation in three dimensions uses quaternions.
- */
- class Pose3
- {
- public:
- /**
- * Default constructor
- */
- Pose3()
- {
- }
- /**
- * Create a new Pose3 object from the given position.
- * @param rPosition position vector in three space.
- */
- Pose3(const Vector3<kt_double>& rPosition)
- : m_Position(rPosition)
- {
- }
- /**
- * Create a new Pose3 object from the given position and orientation.
- * @param rPosition position vector in three space.
- * @param rOrientation quaternion orientation in three space.
- */
- Pose3(const Vector3<kt_double>& rPosition, const karto::Quaternion& rOrientation)
- : m_Position(rPosition)
- , m_Orientation(rOrientation)
- {
- }
- /**
- * Copy constructor
- */
- Pose3(const Pose3& rOther)
- : m_Position(rOther.m_Position)
- , m_Orientation(rOther.m_Orientation)
- {
- }
- /**
- * Constructs a Pose3 object from a Pose2.
- */
- Pose3(const Pose2& rPose)
- {
- m_Position = Vector3<kt_double>(rPose.GetX(), rPose.GetY(), 0.0);
- m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0);
- }
- public:
- /**
- * Get the position of the pose as a 3D vector as const. Values have units of meters.
- * @return 3-dimensional position vector as const
- */
- inline const Vector3<kt_double>& GetPosition() const
- {
- return m_Position;
- }
- /**
- * Set the position of the pose as a 3D vector. Values have units of meters.
- * @return 3-dimensional position vector
- */
- inline void SetPosition(const Vector3<kt_double>& rPosition)
- {
- m_Position = rPosition;
- }
- /**
- * Get the orientation quaternion of the pose as const.
- * @return orientation quaternion as const
- */
- inline const Quaternion& GetOrientation() const
- {
- return m_Orientation;
- }
- /**
- * Get the orientation quaternion of the pose.
- * @return orientation quaternion
- */
- inline void SetOrientation(const Quaternion& rOrientation)
- {
- m_Orientation = rOrientation;
- }
- /**
- * Returns a string representation of this pose
- * @return string representation of this pose
- */
- inline std::string ToString()
- {
- std::stringstream converter;
- converter.precision(std::numeric_limits<double>::digits10);
- converter << GetPosition() << " " << GetOrientation();
- return converter.str();
- }
- public:
- /**
- * Assignment operator
- */
- inline Pose3& operator = (const Pose3& rOther)
- {
- m_Position = rOther.m_Position;
- m_Orientation = rOther.m_Orientation;
- return *this;
- }
- /**
- * Equality operator
- */
- inline kt_bool operator == (const Pose3& rOther) const
- {
- return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation);
- }
- /**
- * Inequality operator
- */
- inline kt_bool operator != (const Pose3& rOther) const
- {
- return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation);
- }
- /**
- * Write Pose3 onto output stream
- * @param rStream output stream
- * @param rPose to write
- */
- friend inline std::ostream& operator << (std::ostream& rStream, const Pose3& rPose)
- {
- rStream << rPose.GetPosition() << ", " << rPose.GetOrientation();
- return rStream;
- }
- /**
- * Read Pose3 from input stream
- * @param rStream input stream
- */
- friend inline std::istream& operator >> (std::istream& rStream, const Pose3& /*rPose*/)
- {
- // Implement me!!
- return rStream;
- }
- private:
- Vector3<kt_double> m_Position;
- Quaternion m_Orientation;
- }; // Pose3
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Defines a Matrix 3 x 3 class.
- */
- class Matrix3
- {
- public:
- /**
- * Default constructor
- */
- Matrix3()
- {
- Clear();
- }
- /**
- * Copy constructor
- */
- inline Matrix3(const Matrix3& rOther)
- {
- memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
- }
- public:
- /**
- * Sets this matrix to identity matrix
- */
- void SetToIdentity()
- {
- memset(m_Matrix, 0, 9*sizeof(kt_double));
- for (kt_int32s i = 0; i < 3; i++)
- {
- m_Matrix[i][i] = 1.0;
- }
- }
- /**
- * Sets this matrix to zero matrix
- */
- void Clear()
- {
- memset(m_Matrix, 0, 9*sizeof(kt_double));
- }
- /**
- * Sets this matrix to be the rotation matrix of rotation around given axis
- * @param x x-coordinate of axis
- * @param y y-coordinate of axis
- * @param z z-coordinate of axis
- * @param radians amount of rotation
- */
- void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
- {
- kt_double cosRadians = cos(radians);
- kt_double sinRadians = sin(radians);
- kt_double oneMinusCos = 1.0 - cosRadians;
- kt_double xx = x * x;
- kt_double yy = y * y;
- kt_double zz = z * z;
- kt_double xyMCos = x * y * oneMinusCos;
- kt_double xzMCos = x * z * oneMinusCos;
- kt_double yzMCos = y * z * oneMinusCos;
- kt_double xSin = x * sinRadians;
- kt_double ySin = y * sinRadians;
- kt_double zSin = z * sinRadians;
- m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
- m_Matrix[0][1] = xyMCos - zSin;
- m_Matrix[0][2] = xzMCos + ySin;
- m_Matrix[1][0] = xyMCos + zSin;
- m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
- m_Matrix[1][2] = yzMCos - xSin;
- m_Matrix[2][0] = xzMCos - ySin;
- m_Matrix[2][1] = yzMCos + xSin;
- m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
- }
- /**
- * Returns transposed version of this matrix
- * @return transposed matrix
- */
- Matrix3 Transpose() const
- {
- Matrix3 transpose;
- for (kt_int32u row = 0; row < 3; row++)
- {
- for (kt_int32u col = 0; col < 3; col++)
- {
- transpose.m_Matrix[row][col] = m_Matrix[col][row];
- }
- }
- return transpose;
- }
- /**
- * Returns the inverse of the matrix
- */
- Matrix3 Inverse() const
- {
- Matrix3 kInverse = *this;
- kt_bool haveInverse = InverseFast(kInverse, 1e-14);
- if (haveInverse == false)
- {
- assert(false);
- }
- return kInverse;
- }
- /**
- * Internal helper method for inverse matrix calculation
- * This code is lifted from the OgreMatrix3 class!!
- */
- kt_bool InverseFast(Matrix3& rkInverse, kt_double fTolerance = KT_TOLERANCE) const
- {
- // Invert a 3x3 using cofactors. This is about 8 times faster than
- // the Numerical Recipes code which uses Gaussian elimination.
- rkInverse.m_Matrix[0][0] = m_Matrix[1][1]*m_Matrix[2][2] - m_Matrix[1][2]*m_Matrix[2][1];
- rkInverse.m_Matrix[0][1] = m_Matrix[0][2]*m_Matrix[2][1] - m_Matrix[0][1]*m_Matrix[2][2];
- rkInverse.m_Matrix[0][2] = m_Matrix[0][1]*m_Matrix[1][2] - m_Matrix[0][2]*m_Matrix[1][1];
- rkInverse.m_Matrix[1][0] = m_Matrix[1][2]*m_Matrix[2][0] - m_Matrix[1][0]*m_Matrix[2][2];
- rkInverse.m_Matrix[1][1] = m_Matrix[0][0]*m_Matrix[2][2] - m_Matrix[0][2]*m_Matrix[2][0];
- rkInverse.m_Matrix[1][2] = m_Matrix[0][2]*m_Matrix[1][0] - m_Matrix[0][0]*m_Matrix[1][2];
- rkInverse.m_Matrix[2][0] = m_Matrix[1][0]*m_Matrix[2][1] - m_Matrix[1][1]*m_Matrix[2][0];
- rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1];
- rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0];
- kt_double fDet = m_Matrix[0][0]*rkInverse.m_Matrix[0][0] +
- m_Matrix[0][1]*rkInverse.m_Matrix[1][0] +
- m_Matrix[0][2]*rkInverse.m_Matrix[2][0];
- if (fabs(fDet) <= fTolerance)
- {
- return false;
- }
- kt_double fInvDet = 1.0/fDet;
- for (size_t row = 0; row < 3; row++)
- {
- for (size_t col = 0; col < 3; col++)
- {
- rkInverse.m_Matrix[row][col] *= fInvDet;
- }
- }
- return true;
- }
- /**
- * Returns a string representation of this matrix
- * @return string representation of this matrix
- */
- inline std::string ToString() const
- {
- std::stringstream converter;
- converter.precision(std::numeric_limits<double>::digits10);
- for (int row = 0; row < 3; row++)
- {
- for (int col = 0; col < 3; col++)
- {
- converter << m_Matrix[row][col] << " ";
- }
- }
- return converter.str();
- }
- public:
- /**
- * Assignment operator
- */
- inline Matrix3& operator = (const Matrix3& rOther)
- {
- memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
- return *this;
- }
- /**
- * Matrix element access, allows use of construct mat(r, c)
- * @param row
- * @param column
- * @return reference to mat(r,c)
- */
- inline kt_double& operator()(kt_int32u row, kt_int32u column)
- {
- return m_Matrix[row][column];
- }
- /**
- * Read-only matrix element access, allows use of construct mat(r, c)
- * @param row
- * @param column
- * @return mat(r,c)
- */
- inline kt_double operator()(kt_int32u row, kt_int32u column) const
- {
- return m_Matrix[row][column];
- }
- /**
- * Binary Matrix3 multiplication.
- * @param rOther
- * @return Matrix3 product
- */
- Matrix3 operator * (const Matrix3& rOther) const
- {
- Matrix3 product;
- for (size_t row = 0; row < 3; row++)
- {
- for (size_t col = 0; col < 3; col++)
- {
- product.m_Matrix[row][col] = m_Matrix[row][0]*rOther.m_Matrix[0][col] +
- m_Matrix[row][1]*rOther.m_Matrix[1][col] +
- m_Matrix[row][2]*rOther.m_Matrix[2][col];
- }
- }
- return product;
- }
- /**
- * Matrix3 and Pose2 multiplication - matrix * pose [3x3 * 3x1 = 3x1]
- * @param rPose2
- * @return Pose2 product
- */
- inline Pose2 operator * (const Pose2& rPose2) const
- {
- Pose2 pose2;
- pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] *
- rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading());
- pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] *
- rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading());
- pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] *
- rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading());
- return pose2;
- }
- /**
- * In place Matrix3 add.
- * @param rkMatrix
- */
- inline void operator += (const Matrix3& rkMatrix)
- {
- for (kt_int32u row = 0; row < 3; row++)
- {
- for (kt_int32u col = 0; col < 3; col++)
- {
- m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
- }
- }
- }
- /**
- * Write Matrix3 onto output stream
- * @param rStream output stream
- * @param rMatrix to write
- */
- friend inline std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix)
- {
- rStream << rMatrix.ToString();
- return rStream;
- }
- private:
- kt_double m_Matrix[3][3];
- }; // Matrix3
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Defines a general Matrix class.
- */
- class Matrix
- {
- public:
- /**
- * Constructs a matrix of size rows x columns
- */
- Matrix(kt_int32u rows, kt_int32u columns)
- : m_Rows(rows)
- , m_Columns(columns)
- , m_pData(NULL)
- {
- Allocate();
- Clear();
- }
- /**
- * Destructor
- */
- virtual ~Matrix()
- {
- delete [] m_pData;
- }
- public:
- /**
- * Set all entries to 0
- */
- void Clear()
- {
- if (m_pData != NULL)
- {
- memset(m_pData, 0, sizeof(kt_double) * m_Rows * m_Columns);
- }
- }
- /**
- * Gets the number of rows of the matrix
- * @return nubmer of rows
- */
- inline kt_int32u GetRows() const
- {
- return m_Rows;
- }
- /**
- * Gets the number of columns of the matrix
- * @return nubmer of columns
- */
- inline kt_int32u GetColumns() const
- {
- return m_Columns;
- }
- /**
- * Returns a reference to the entry at (row,column)
- * @param row
- * @param column
- * @return reference to entry at (row,column)
- */
- inline kt_double& operator()(kt_int32u row, kt_int32u column)
- {
- RangeCheck(row, column);
- return m_pData[row + column * m_Rows];
- }
- /**
- * Returns a const reference to the entry at (row,column)
- * @param row
- * @param column
- * @return const reference to entry at (row,column)
- */
- inline const kt_double& operator()(kt_int32u row, kt_int32u column) const
- {
- RangeCheck(row, column);
- return m_pData[row + column * m_Rows];
- }
- private:
- /**
- * Allocate space for the matrix
- */
- void Allocate()
- {
- try
- {
- if (m_pData != NULL)
- {
- delete[] m_pData;
- }
- m_pData = new kt_double[m_Rows * m_Columns];
- }
- catch (const std::bad_alloc& ex)
- {
- throw Exception("Matrix allocation error");
- }
- if (m_pData == NULL)
- {
- throw Exception("Matrix allocation error");
- }
- }
- /**
- * Checks if (row,column) is a valid entry into the matrix
- * @param row
- * @param column
- */
- inline void RangeCheck(kt_int32u row, kt_int32u column) const
- {
- if (math::IsUpTo(row, m_Rows) == false)
- {
- throw Exception("Matrix - RangeCheck ERROR!!!!");
- }
- if (math::IsUpTo(column, m_Columns) == false)
- {
- throw Exception("Matrix - RangeCheck ERROR!!!!");
- }
- }
- private:
- kt_int32u m_Rows;
- kt_int32u m_Columns;
- kt_double* m_pData;
- }; // Matrix
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Defines a bounding box in 2-dimensional real space.
- */
- class BoundingBox2
- {
- public:
- /*
- * Default constructor
- */
- BoundingBox2()
- : m_Minimum(999999999999999999.99999, 999999999999999999.99999)
- , m_Maximum(-999999999999999999.99999, -999999999999999999.99999)
- {
- }
- public:
- /**
- * Get bounding box minimum
- */
- inline const Vector2<kt_double>& GetMinimum() const
- {
- return m_Minimum;
- }
- /**
- * Set bounding box minimum
- */
- inline void SetMinimum(const Vector2<kt_double>& mMinimum)
- {
- m_Minimum = mMinimum;
- }
- /**
- * Get bounding box maximum
- */
- inline const Vector2<kt_double>& GetMaximum() const
- {
- return m_Maximum;
- }
- /**
- * Set bounding box maximum
- */
- inline void SetMaximum(const Vector2<kt_double>& rMaximum)
- {
- m_Maximum = rMaximum;
- }
- /**
- * Get the size of the bounding box
- */
- inline Size2<kt_double> GetSize() const
- {
- Vector2<kt_double> size = m_Maximum - m_Minimum;
- return Size2<kt_double>(size.GetX(), size.GetY());
- }
- /**
- * Add vector to bounding box
- */
- inline void Add(const Vector2<kt_double>& rPoint)
- {
- m_Minimum.MakeFloor(rPoint);
- m_Maximum.MakeCeil(rPoint);
- }
- /**
- * Add other bounding box to bounding box
- */
- inline void Add(const BoundingBox2& rBoundingBox)
- {
- Add(rBoundingBox.GetMinimum());
- Add(rBoundingBox.GetMaximum());
- }
- /**
- * Whether the given point is in the bounds of this box
- * @param rPoint
- * @return in bounds?
- */
- inline kt_bool IsInBounds(const Vector2<kt_double>& rPoint) const
- {
- return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
- math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
- }
- private:
- Vector2<kt_double> m_Minimum;
- Vector2<kt_double> m_Maximum;
- }; // BoundingBox2
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Implementation of a Pose2 transform
- */
- class Transform
- {
- public:
- /**
- * Constructs a transformation from the origin to the given pose
- * @param rPose pose
- */
- Transform(const Pose2& rPose)
- {
- SetTransform(Pose2(), rPose);
- }
- /**
- * Constructs a transformation from the first pose to the second pose
- * @param rPose1 first pose
- * @param rPose2 second pose
- */
- Transform(const Pose2& rPose1, const Pose2& rPose2)
- {
- SetTransform(rPose1, rPose2);
- }
- public:
- /**
- * Transforms the pose according to this transform
- * @param rSourcePose pose to transform from
- * @return transformed pose
- */
- inline Pose2 TransformPose(const Pose2& rSourcePose)
- {
- Pose2 newPosition = m_Transform + m_Rotation * rSourcePose;
- kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading());
- return Pose2(newPosition.GetPosition(), angle);
- }
- /**
- * Inverse transformation of the pose according to this transform
- * @param rSourcePose pose to transform from
- * @return transformed pose
- */
- inline Pose2 InverseTransformPose(const Pose2& rSourcePose)
- {
- Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform);
- kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() - m_Transform.GetHeading());
- // components of transform
- return Pose2(newPosition.GetPosition(), angle);
- }
- private:
- /**
- * Sets this to be the transformation from the first pose to the second pose
- * @param rPose1 first pose
- * @param rPose2 second pose
- */
- void SetTransform(const Pose2& rPose1, const Pose2& rPose2)
- {
- if (rPose1 == rPose2)
- {
- m_Rotation.SetToIdentity();
- m_InverseRotation.SetToIdentity();
- m_Transform = Pose2();
- return;
- }
- // heading transformation
- m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
- m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
- // position transformation
- Pose2 newPosition;
- if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0)
- {
- newPosition = rPose2 - m_Rotation * rPose1;
- }
- else
- {
- newPosition = rPose2;
- }
- m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading());
- }
- private:
- // pose transformation
- Pose2 m_Transform;
- Matrix3 m_Rotation;
- Matrix3 m_InverseRotation;
- }; // Transform
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Enumerated type for valid LaserRangeFinder types
- */
- typedef enum
- {
- LaserRangeFinder_Custom = 0,
- LaserRangeFinder_Sick_LMS100 = 1,
- LaserRangeFinder_Sick_LMS200 = 2,
- LaserRangeFinder_Sick_LMS291 = 3,
- LaserRangeFinder_Hokuyo_UTM_30LX = 4,
- LaserRangeFinder_Hokuyo_URG_04LX = 5
- } LaserRangeFinderType;
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Abstract base class for Parameters
- */
- class AbstractParameter
- {
- public:
- /**
- * Constructs a parameter with the given name
- * @param rName
- * @param pParameterManger
- */
- AbstractParameter(const std::string& rName, ParameterManager* pParameterManger = NULL)
- : m_Name(rName)
- {
- // if parameter manager is provided add myself to it!
- if (pParameterManger != NULL)
- {
- pParameterManger->Add(this);
- }
- }
- /**
- * Constructs a parameter with the given name and description
- * @param rName
- * @param rDescription
- * @param pParameterManger
- */
- AbstractParameter(const std::string& rName,
- const std::string& rDescription,
- ParameterManager* pParameterManger = NULL)
- : m_Name(rName)
- , m_Description(rDescription)
- {
- // if parameter manager is provided add myself to it!
- if (pParameterManger != NULL)
- {
- pParameterManger->Add(this);
- }
- }
- /**
- * Destructor
- */
- virtual ~AbstractParameter()
- {
- }
- public:
- /**
- * Gets the name of this object
- * @return name
- */
- inline const std::string& GetName() const
- {
- return m_Name;
- }
- /**
- * Returns the parameter description
- * @return parameter description
- */
- inline const std::string& GetDescription() const
- {
- return m_Description;
- }
- /**
- * Get parameter value as string.
- * @return value as string
- */
- virtual const std::string GetValueAsString() const = 0;
- /**
- * Set parameter value from string.
- * @param rStringValue value as string
- */
- virtual void SetValueFromString(const std::string& rStringValue) = 0;
- /**
- * Clones the parameter
- * @return clone
- */
- virtual AbstractParameter* Clone() = 0;
- public:
- /**
- * Write this parameter onto output stream
- * @param rStream output stream
- * @param rParameter
- */
- friend std::ostream& operator << (std::ostream& rStream, const AbstractParameter& rParameter)
- {
- rStream.precision(6);
- rStream.flags(std::ios::fixed);
- rStream << rParameter.GetName() << " = " << rParameter.GetValueAsString();
- return rStream;
- }
- private:
- std::string m_Name;
- std::string m_Description;
- }; // AbstractParameter
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Parameter class
- */
- template<typename T>
- class Parameter : public AbstractParameter
- {
- public:
- /**
- * Parameter with given name and value
- * @param rName
- * @param value
- * @param pParameterManger
- */
- Parameter(const std::string& rName, T value, ParameterManager* pParameterManger = NULL)
- : AbstractParameter(rName, pParameterManger)
- , m_Value(value)
- {
- }
- /**
- * Parameter with given name, description and value
- * @param rName
- * @param rDescription
- * @param value
- * @param pParameterManger
- */
- Parameter(const std::string& rName,
- const std::string& rDescription,
- T value,
- ParameterManager* pParameterManger = NULL)
- : AbstractParameter(rName, rDescription, pParameterManger)
- , m_Value(value)
- {
- }
- /**
- * Destructor
- */
- virtual ~Parameter()
- {
- }
- public:
- /**
- * Gets value of parameter
- * @return parameter value
- */
- inline const T& GetValue() const
- {
- return m_Value;
- }
- /**
- * Sets value of parameter
- * @param rValue
- */
- inline void SetValue(const T& rValue)
- {
- m_Value = rValue;
- }
- /**
- * Gets value of parameter as string
- * @return string version of value
- */
- virtual const std::string GetValueAsString() const
- {
- std::stringstream converter;
- converter << m_Value;
- return converter.str();
- }
- /**
- * Sets value of parameter from string
- * @param rStringValue
- */
- virtual void SetValueFromString(const std::string& rStringValue)
- {
- std::stringstream converter;
- converter.str(rStringValue);
- converter >> m_Value;
- }
- /**
- * Clone this parameter
- * @return clone of this parameter
- */
- virtual Parameter* Clone()
- {
- return new Parameter(GetName(), GetDescription(), GetValue());
- }
- public:
- /**
- * Assignment operator
- */
- Parameter& operator = (const Parameter& rOther)
- {
- m_Value = rOther.m_Value;
- return *this;
- }
- /**
- * Sets the value of this parameter to given value
- */
- T operator = (T value)
- {
- m_Value = value;
- return m_Value;
- }
- protected:
- /**
- * Parameter value
- */
- T m_Value;
- }; // Parameter
- template<>
- inline void Parameter<kt_double>::SetValueFromString(const std::string& rStringValue)
- {
- int precision = std::numeric_limits<double>::digits10;
- std::stringstream converter;
- converter.precision(precision);
- converter.str(rStringValue);
- m_Value = 0.0;
- converter >> m_Value;
- }
- template<>
- inline const std::string Parameter<kt_double>::GetValueAsString() const
- {
- std::stringstream converter;
- converter.precision(std::numeric_limits<double>::digits10);
- converter << m_Value;
- return converter.str();
- }
- template<>
- inline void Parameter<kt_bool>::SetValueFromString(const std::string& rStringValue)
- {
- if (rStringValue == "true" || rStringValue == "TRUE")
- {
- m_Value = true;
- }
- else
- {
- m_Value = false;
- }
- }
- template<>
- inline const std::string Parameter<kt_bool>::GetValueAsString() const
- {
- if (m_Value == true)
- {
- return "true";
- }
- return "false";
- }
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Parameter enum class
- */
- class ParameterEnum : public Parameter<kt_int32s>
- {
- typedef std::map<std::string, kt_int32s> EnumMap;
- public:
- /**
- * Construct a Parameter object with name and value
- * @param rName parameter name
- * @param value of parameter
- * @param pParameterManger
- */
- ParameterEnum(const std::string& rName, kt_int32s value, ParameterManager* pParameterManger = NULL)
- : Parameter<kt_int32s>(rName, value, pParameterManger)
- {
- }
- /**
- * Destructor
- */
- virtual ~ParameterEnum()
- {
- }
- public:
- /**
- * Return a clone of this instance
- * @return clone
- */
- virtual Parameter<kt_int32s>* Clone()
- {
- ParameterEnum* pEnum = new ParameterEnum(GetName(), GetValue());
- pEnum->m_EnumDefines = m_EnumDefines;
- return pEnum;
- }
- /**
- * Set parameter value from string.
- * @param rStringValue value as string
- */
- virtual void SetValueFromString(const std::string& rStringValue)
- {
- if (m_EnumDefines.find(rStringValue) != m_EnumDefines.end())
- {
- m_Value = m_EnumDefines[rStringValue];
- }
- else
- {
- std::string validValues;
- const_forEach(EnumMap, &m_EnumDefines)
- {
- validValues += iter->first + ", ";
- }
- throw Exception("Unable to set enum: " + rStringValue + ". Valid values are: " + validValues);
- }
- }
- /**
- * Get parameter value as string.
- * @return value as string
- */
- virtual const std::string GetValueAsString() const
- {
- const_forEach(EnumMap, &m_EnumDefines)
- {
- if (iter->second == m_Value)
- {
- return iter->first;
- }
- }
- throw Exception("Unable to lookup enum");
- }
- /**
- * Defines the enum with the given name as having the given value
- * @param value
- * @param rName
- */
- void DefineEnumValue(kt_int32s value, const std::string& rName)
- {
- if (m_EnumDefines.find(rName) == m_EnumDefines.end())
- {
- m_EnumDefines[rName] = value;
- }
- else
- {
- std::cerr << "Overriding enum value: " << m_EnumDefines[rName] << " with " << value << std::endl;
- m_EnumDefines[rName] = value;
- assert(false);
- }
- }
- public:
- /**
- * Assignment operator
- */
- ParameterEnum& operator = (const ParameterEnum& rOther)
- {
- SetValue(rOther.GetValue());
- return *this;
- }
- /**
- * Assignment operator
- */
- kt_int32s operator = (kt_int32s value)
- {
- SetValue(value);
- return m_Value;
- }
- private:
- EnumMap m_EnumDefines;
- }; // ParameterEnum
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Set of parameters
- */
- class Parameters : public Object
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(Parameters)
- // @endcond
- public:
- /**
- * Parameters
- * @param rName
- */
- Parameters(const std::string& rName)
- : Object(rName)
- {
- }
- /**
- * Destructor
- */
- virtual ~Parameters()
- {
- }
- private:
- Parameters(const Parameters&);
- const Parameters& operator=(const Parameters&);
- }; // Parameters
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- class SensorData;
- /**
- * Abstract Sensor base class
- */
- class KARTO_EXPORT Sensor : public Object
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(Sensor)
- // @endcond
- protected:
- /**
- * Construct a Sensor
- * @param rName sensor name
- */
- Sensor(const Name& rName);
- public:
- /**
- * Destructor
- */
- virtual ~Sensor();
- public:
- /**
- * Gets this range finder sensor's offset
- * @return offset pose
- */
- inline const Pose2& GetOffsetPose() const
- {
- return m_pOffsetPose->GetValue();
- }
- /**
- * Sets this range finder sensor's offset
- * @param rPose
- */
- inline void SetOffsetPose(const Pose2& rPose)
- {
- m_pOffsetPose->SetValue(rPose);
- }
- /**
- * Validates sensor
- * @return true if valid
- */
- virtual kt_bool Validate() = 0;
- /**
- * Validates sensor data
- * @param pSensorData sensor data
- * @return true if valid
- */
- virtual kt_bool Validate(SensorData* pSensorData) = 0;
- private:
- /**
- * Restrict the copy constructor
- */
- Sensor(const Sensor&);
- /**
- * Restrict the assignment operator
- */
- const Sensor& operator=(const Sensor&);
- private:
- /**
- * Sensor offset pose
- */
- Parameter<Pose2>* m_pOffsetPose;
- }; // Sensor
- /**
- * Type declaration of Sensor vector
- */
- typedef std::vector<Sensor*> SensorVector;
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Type declaration of <Name, Sensor*> map
- */
- typedef std::map<Name, Sensor*> SensorManagerMap;
- /**
- * Manages sensors
- */
- class KARTO_EXPORT SensorManager
- {
- public:
- /**
- * Constructor
- */
- SensorManager()
- {
- }
- /**
- * Destructor
- */
- virtual ~SensorManager()
- {
- }
- public:
- /**
- * Get singleton instance of SensorManager
- */
- static SensorManager* GetInstance();
- public:
- /**
- * Registers a sensor by it's name. The Sensor name must be unique, if not sensor is not registered
- * unless override is set to true
- * @param pSensor sensor to register
- * @param override
- * @return true if sensor is registered with SensorManager, false if Sensor name is not unique
- */
- void RegisterSensor(Sensor* pSensor, kt_bool override = false)
- {
- Validate(pSensor);
- if ((m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) && !override)
- {
- throw Exception("Cannot register sensor: already registered: [" +
- pSensor->GetName().ToString() +
- "] (Consider setting 'override' to true)");
- }
- std::cout << "Registering sensor: [" << pSensor->GetName().ToString() << "]" << std::endl;
- m_Sensors[pSensor->GetName()] = pSensor;
- }
- /**
- * Unregisters the given sensor
- * @param pSensor sensor to unregister
- */
- void UnregisterSensor(Sensor* pSensor)
- {
- Validate(pSensor);
- if (m_Sensors.find(pSensor->GetName()) != m_Sensors.end())
- {
- std::cout << "Unregistering sensor: " << pSensor->GetName().ToString() << std::endl;
- m_Sensors.erase(pSensor->GetName());
- }
- else
- {
- throw Exception("Cannot unregister sensor: not registered: [" + pSensor->GetName().ToString() + "]");
- }
- }
- /**
- * Gets the sensor with the given name
- * @param rName name of sensor
- * @return sensor
- */
- Sensor* GetSensorByName(const Name& rName)
- {
- if (m_Sensors.find(rName) != m_Sensors.end())
- {
- return m_Sensors[rName];
- }
- throw Exception("Sensor not registered: [" + rName.ToString() + "] (Did you add the sensor to the Dataset?)");
- }
- /**
- * Gets the sensor with the given name
- * @param rName name of sensor
- * @return sensor
- */
- template<class T>
- T* GetSensorByName(const Name& rName)
- {
- Sensor* pSensor = GetSensorByName(rName);
- return dynamic_cast<T*>(pSensor);
- }
- /**
- * Gets all registered sensors
- * @return vector of all registered sensors
- */
- SensorVector GetAllSensors()
- {
- SensorVector sensors;
- forEach(SensorManagerMap, &m_Sensors)
- {
- sensors.push_back(iter->second);
- }
- return sensors;
- }
- protected:
- /**
- * Checks that given sensor is not NULL and has non-empty name
- * @param pSensor sensor to validate
- */
- static void Validate(Sensor* pSensor)
- {
- if (pSensor == NULL)
- {
- throw Exception("Invalid sensor: NULL");
- }
- else if (pSensor->GetName().ToString() == "")
- {
- throw Exception("Invalid sensor: nameless");
- }
- }
- protected:
- /**
- * Sensor map
- */
- SensorManagerMap m_Sensors;
- };
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Sensor that provides pose information relative to world coordinates.
- *
- * The user can set the offset pose of the drive sensor. If no value is provided by the user the default is no offset,
- * i.e, the sensor is initially at the world origin, oriented along the positive z axis.
- */
- class Drive : public Sensor
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(Drive)
- // @endcond
- public:
- /**
- * Constructs a Drive object
- */
- Drive(const std::string& rName)
- : Sensor(rName)
- {
- }
- /**
- * Destructor
- */
- virtual ~Drive()
- {
- }
- public:
- virtual kt_bool Validate()
- {
- return true;
- }
- virtual kt_bool Validate(SensorData* pSensorData)
- {
- if (pSensorData == NULL)
- {
- return false;
- }
- return true;
- }
- private:
- Drive(const Drive&);
- const Drive& operator=(const Drive&);
- }; // class Drive
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- class LocalizedRangeScan;
- class CoordinateConverter;
- /**
- * The LaserRangeFinder defines a laser sensor that provides the pose offset position of a localized range scan relative to the robot.
- * The user can set an offset pose for the sensor relative to the robot coordinate system. If no value is provided
- * by the user, the sensor is set to be at the origin of the robot coordinate system.
- * The LaserRangeFinder contains parameters for physical laser sensor used by the mapper for scan matching
- * Also contains information about the maximum range of the sensor and provides a threshold
- * for limiting the range of readings.
- * The optimal value for the range threshold depends on the angular resolution of the scan and
- * the desired map resolution. RangeThreshold should be set as large as possible while still
- * providing "solid" coverage between consecutive range readings. The diagram below illustrates
- * the relationship between map resolution and the range threshold.
- */
- class KARTO_EXPORT LaserRangeFinder : public Sensor
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(LaserRangeFinder)
- // @endcond
- public:
- /**
- * Destructor
- */
- virtual ~LaserRangeFinder()
- {
- }
- public:
- /**
- * Gets this range finder sensor's minimum range
- * @return minimum range
- */
- inline kt_double GetMinimumRange() const
- {
- return m_pMinimumRange->GetValue();
- }
- /**
- * Sets this range finder sensor's minimum range
- * @param minimumRange
- */
- inline void SetMinimumRange(kt_double minimumRange)
- {
- m_pMinimumRange->SetValue(minimumRange);
- SetRangeThreshold(GetRangeThreshold());
- }
- /**
- * Gets this range finder sensor's maximum range
- * @return maximum range
- */
- inline kt_double GetMaximumRange() const
- {
- return m_pMaximumRange->GetValue();
- }
- /**
- * Sets this range finder sensor's maximum range
- * @param maximumRange
- */
- inline void SetMaximumRange(kt_double maximumRange)
- {
- m_pMaximumRange->SetValue(maximumRange);
- SetRangeThreshold(GetRangeThreshold());
- }
- /**
- * Gets the range threshold
- * @return range threshold
- */
- inline kt_double GetRangeThreshold() const
- {
- return m_pRangeThreshold->GetValue();
- }
- /**
- * Sets the range threshold
- * @param rangeThreshold
- */
- inline void SetRangeThreshold(kt_double rangeThreshold)
- {
- // make sure rangeThreshold is within laser range finder range
- m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange()));
- if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false)
- {
- std::cout << "Info: clipped range threshold to be within minimum and maximum range!" << std::endl;
- }
- }
- /**
- * Gets this range finder sensor's minimum angle
- * @return minimum angle
- */
- inline kt_double GetMinimumAngle() const
- {
- return m_pMinimumAngle->GetValue();
- }
- /**
- * Sets this range finder sensor's minimum angle
- * @param minimumAngle
- */
- inline void SetMinimumAngle(kt_double minimumAngle)
- {
- m_pMinimumAngle->SetValue(minimumAngle);
- Update();
- }
- /**
- * Gets this range finder sensor's maximum angle
- * @return maximum angle
- */
- inline kt_double GetMaximumAngle() const
- {
- return m_pMaximumAngle->GetValue();
- }
- /**
- * Sets this range finder sensor's maximum angle
- * @param maximumAngle
- */
- inline void SetMaximumAngle(kt_double maximumAngle)
- {
- m_pMaximumAngle->SetValue(maximumAngle);
- Update();
- }
- /**
- * Gets this range finder sensor's angular resolution
- * @return angular resolution
- */
- inline kt_double GetAngularResolution() const
- {
- return m_pAngularResolution->GetValue();
- }
- /**
- * Sets this range finder sensor's angular resolution
- * @param angularResolution
- */
- inline void SetAngularResolution(kt_double angularResolution)
- {
- if (m_pType->GetValue() == LaserRangeFinder_Custom)
- {
- m_pAngularResolution->SetValue(angularResolution);
- }
- else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS100)
- {
- if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
- {
- m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
- }
- else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
- {
- m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
- }
- else
- {
- std::stringstream stream;
- stream << "Invalid resolution for Sick LMS100: ";
- stream << angularResolution;
- throw Exception(stream.str());
- }
- }
- else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 ||
- m_pType->GetValue() == LaserRangeFinder_Sick_LMS291)
- {
- if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
- {
- m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
- }
- else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
- {
- m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
- }
- else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00)))
- {
- m_pAngularResolution->SetValue(math::DegreesToRadians(1.00));
- }
- else
- {
- std::stringstream stream;
- stream << "Invalid resolution for Sick LMS291: ";
- stream << angularResolution;
- throw Exception(stream.str());
- }
- }
- else if (m_pType->GetValue() == LaserRangeFinder_Hokuyo_UTM_30LX)
- {
- if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
- {
- m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
- }
- else
- {
- std::stringstream stream;
- stream << "Invalid resolution for Hokuyo_UTM_30LX: ";
- stream << angularResolution;
- throw Exception(stream.str());
- }
- }
- else if (m_pType->GetValue() == LaserRangeFinder_Hokuyo_URG_04LX)
- {
- if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.352)))
- {
- m_pAngularResolution->SetValue(math::DegreesToRadians(0.352));
- }
- else
- {
- std::stringstream stream;
- stream << "Invalid resolution for Hokuyo_URG_04LX: ";
- stream << angularResolution;
- throw Exception(stream.str());
- }
- }
- else
- {
- throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom");
- }
- Update();
- }
- /**
- * Return Laser type
- */
- inline kt_int32s GetType()
- {
- return m_pType->GetValue();
- }
- /**
- * Gets the number of range readings each localized range scan must contain to be a valid scan.
- * @return number of range readings
- */
- inline kt_int32u GetNumberOfRangeReadings() const
- {
- return m_NumberOfRangeReadings;
- }
- virtual kt_bool Validate()
- {
- Update();
- if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false)
- {
- std::cout << "Please set range threshold to a value between ["
- << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl;
- return false;
- }
- return true;
- }
- virtual kt_bool Validate(SensorData* pSensorData);
- /**
- * Get point readings (potentially scale readings if given coordinate converter is not null)
- * @param pLocalizedRangeScan
- * @param pCoordinateConverter
- * @param ignoreThresholdPoints
- * @param flipY
- */
- const PointVectorDouble GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan,
- CoordinateConverter* pCoordinateConverter,
- kt_bool ignoreThresholdPoints = true,
- kt_bool flipY = false) const;
- public:
- /**
- * Create a laser range finder of the given type and ID
- * @param type
- * @param rName name of sensor - if no name is specified default name will be assigned
- * @return laser range finder
- */
- static LaserRangeFinder* CreateLaserRangeFinder(LaserRangeFinderType type, const Name& rName)
- {
- LaserRangeFinder* pLrf = NULL;
- switch (type)
- {
- // see http://www.hizook.com/files/publications/SICK_LMS100.pdf
- // set range threshold to 18m
- case LaserRangeFinder_Sick_LMS100:
- {
- pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 100"));
- // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm
- pLrf->m_pMinimumRange->SetValue(0.0);
- pLrf->m_pMaximumRange->SetValue(20.0);
- // 270 degree range, 50 Hz
- pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
- pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
- // 0.25 degree angular resolution
- pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
- pLrf->m_NumberOfRangeReadings = 1081;
- }
- break;
- // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
- // set range threshold to 10m
- case LaserRangeFinder_Sick_LMS200:
- {
- pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 200"));
- // Sensing range is 80 meters
- pLrf->m_pMinimumRange->SetValue(0.0);
- pLrf->m_pMaximumRange->SetValue(80.0);
- // 180 degree range, 75 Hz
- pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
- pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
- // 0.5 degree angular resolution
- pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
- pLrf->m_NumberOfRangeReadings = 361;
- }
- break;
- // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
- // set range threshold to 30m
- case LaserRangeFinder_Sick_LMS291:
- {
- pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 291"));
- // Sensing range is 80 meters
- pLrf->m_pMinimumRange->SetValue(0.0);
- pLrf->m_pMaximumRange->SetValue(80.0);
- // 180 degree range, 75 Hz
- pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
- pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
- // 0.5 degree angular resolution
- pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
- pLrf->m_NumberOfRangeReadings = 361;
- }
- break;
- // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf
- // set range threshold to 30m
- case LaserRangeFinder_Hokuyo_UTM_30LX:
- {
- pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo UTM-30LX"));
- // Sensing range is 30 meters
- pLrf->m_pMinimumRange->SetValue(0.1);
- pLrf->m_pMaximumRange->SetValue(30.0);
- // 270 degree range, 40 Hz
- pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
- pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
- // 0.25 degree angular resolution
- pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
- pLrf->m_NumberOfRangeReadings = 1081;
- }
- break;
- // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf
- // set range threshold to 3.5m
- case LaserRangeFinder_Hokuyo_URG_04LX:
- {
- pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo URG-04LX"));
- // Sensing range is 4 meters. It has detection problems when
- // scanning absorptive surfaces (such as black trimming).
- pLrf->m_pMinimumRange->SetValue(0.02);
- pLrf->m_pMaximumRange->SetValue(4.0);
- // 240 degree range, 10 Hz
- pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-120));
- pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(120));
- // 0.352 degree angular resolution
- pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.352));
- pLrf->m_NumberOfRangeReadings = 751;
- }
- break;
- case LaserRangeFinder_Custom:
- {
- pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("User-Defined LaserRangeFinder"));
- // Sensing range is 80 meters.
- pLrf->m_pMinimumRange->SetValue(0.0);
- pLrf->m_pMaximumRange->SetValue(80.0);
- // 180 degree range
- pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
- pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
- // 1.0 degree angular resolution
- pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(1.0));
- pLrf->m_NumberOfRangeReadings = 181;
- }
- break;
- }
- if (pLrf != NULL)
- {
- pLrf->m_pType->SetValue(type);
- Pose2 defaultOffset;
- pLrf->SetOffsetPose(defaultOffset);
- }
- return pLrf;
- }
- private:
- /**
- * Constructs a LaserRangeFinder object with given ID
- */
- LaserRangeFinder(const Name& rName)
- : Sensor(rName)
- , m_NumberOfRangeReadings(0)
- {
- m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
- m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
- m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
- m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
- m_pAngularResolution = new Parameter<kt_double>("AngularResolution",
- math::DegreesToRadians(1),
- GetParameterManager());
- m_pRangeThreshold = new Parameter<kt_double>("RangeThreshold", 12.0, GetParameterManager());
- m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager());
- m_pType->DefineEnumValue(LaserRangeFinder_Custom, "Custom");
- m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS100, "Sick_LMS100");
- m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS200, "Sick_LMS200");
- m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS291, "Sick_LMS291");
- m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_UTM_30LX, "Hokuyo_UTM_30LX");
- m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_URG_04LX, "Hokuyo_URG_04LX");
- }
- /**
- * Set the number of range readings based on the minimum and
- * maximum angles of the sensor and the angular resolution
- */
- void Update()
- {
- m_NumberOfRangeReadings = static_cast<kt_int32u>(math::Round((GetMaximumAngle() -
- GetMinimumAngle())
- / GetAngularResolution()) + 1);
- }
- private:
- LaserRangeFinder(const LaserRangeFinder&);
- const LaserRangeFinder& operator=(const LaserRangeFinder&);
- private:
- // sensor m_Parameters
- Parameter<kt_double>* m_pMinimumAngle;
- Parameter<kt_double>* m_pMaximumAngle;
- Parameter<kt_double>* m_pAngularResolution;
- Parameter<kt_double>* m_pMinimumRange;
- Parameter<kt_double>* m_pMaximumRange;
- Parameter<kt_double>* m_pRangeThreshold;
- ParameterEnum* m_pType;
- kt_int32u m_NumberOfRangeReadings;
- // static std::string LaserRangeFinderTypeNames[6];
- }; // LaserRangeFinder
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Enumerated type for valid grid cell states
- */
- typedef enum
- {
- GridStates_Unknown = 0,
- GridStates_Occupied = 100,
- GridStates_Free = 255
- } GridStates;
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * The CoordinateConverter class is used to convert coordinates between world and grid coordinates
- * In world coordinates 1.0 = 1 meter where 1 in grid coordinates = 1 pixel!
- * Default scale for coordinate converter is 20 that converters to 1 pixel = 0.05 meter
- */
- class CoordinateConverter
- {
- public:
- /**
- * Default constructor
- */
- CoordinateConverter()
- : m_Scale(20.0)
- {
- }
- public:
- /**
- * Scales the value
- * @param value
- * @return scaled value
- */
- inline kt_double Transform(kt_double value)
- {
- return value * m_Scale;
- }
- /**
- * Converts the point from world coordinates to grid coordinates
- * @param rWorld world coordinate
- * @param flipY
- * @return grid coordinate
- */
- inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
- {
- kt_double gridX = (rWorld.GetX() - m_Offset.GetX()) * m_Scale;
- kt_double gridY = 0.0;
- if (flipY == false)
- {
- gridY = (rWorld.GetY() - m_Offset.GetY()) * m_Scale;
- }
- else
- {
- gridY = (m_Size.GetHeight() / m_Scale - rWorld.GetY() + m_Offset.GetY()) * m_Scale;
- }
- return Vector2<kt_int32s>(static_cast<kt_int32s>(math::Round(gridX)), static_cast<kt_int32s>(math::Round(gridY)));
- }
- /**
- * Converts the point from grid coordinates to world coordinates
- * @param rGrid world coordinate
- * @param flipY
- * @return world coordinate
- */
- inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
- {
- kt_double worldX = m_Offset.GetX() + rGrid.GetX() / m_Scale;
- kt_double worldY = 0.0;
- if (flipY == false)
- {
- worldY = m_Offset.GetY() + rGrid.GetY() / m_Scale;
- }
- else
- {
- worldY = m_Offset.GetY() + (m_Size.GetHeight() - rGrid.GetY()) / m_Scale;
- }
- return Vector2<kt_double>(worldX, worldY);
- }
- /**
- * Gets the scale
- * @return scale
- */
- inline kt_double GetScale() const
- {
- return m_Scale;
- }
- /**
- * Sets the scale
- * @param scale
- */
- inline void SetScale(kt_double scale)
- {
- m_Scale = scale;
- }
- /**
- * Gets the offset
- * @return offset
- */
- inline const Vector2<kt_double>& GetOffset() const
- {
- return m_Offset;
- }
- /**
- * Sets the offset
- * @param rOffset
- */
- inline void SetOffset(const Vector2<kt_double>& rOffset)
- {
- m_Offset = rOffset;
- }
- /**
- * Sets the size
- * @param rSize
- */
- inline void SetSize(const Size2<kt_int32s>& rSize)
- {
- m_Size = rSize;
- }
- /**
- * Gets the size
- * @return size
- */
- inline const Size2<kt_int32s>& GetSize() const
- {
- return m_Size;
- }
- /**
- * Gets the resolution
- * @return resolution
- */
- inline kt_double GetResolution() const
- {
- return 1.0 / m_Scale;
- }
- /**
- * Sets the resolution
- * @param resolution
- */
- inline void SetResolution(kt_double resolution)
- {
- m_Scale = 1.0 / resolution;
- }
- /**
- * Gets the bounding box
- * @return bounding box
- */
- inline BoundingBox2 GetBoundingBox() const
- {
- BoundingBox2 box;
- kt_double minX = GetOffset().GetX();
- kt_double minY = GetOffset().GetY();
- kt_double maxX = minX + GetSize().GetWidth() * GetResolution();
- kt_double maxY = minY + GetSize().GetHeight() * GetResolution();
- box.SetMinimum(GetOffset());
- box.SetMaximum(Vector2<kt_double>(maxX, maxY));
- return box;
- }
- private:
- Size2<kt_int32s> m_Size;
- kt_double m_Scale;
- Vector2<kt_double> m_Offset;
- }; // CoordinateConverter
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Defines a grid class
- */
- template<typename T>
- class Grid
- {
- public:
- /**
- * Creates a grid of given size and resolution
- * @param width
- * @param height
- * @param resolution
- * @return grid pointer
- */
- static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
- {
- Grid* pGrid = new Grid(width, height);
- pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);
- return pGrid;
- }
- /**
- * Destructor
- */
- virtual ~Grid()
- {
- delete [] m_pData;
- delete m_pCoordinateConverter;
- }
- public:
- /**
- * Clear out the grid data
- */
- void Clear()
- {
- memset(m_pData, 0, GetDataSize() * sizeof(T));
- }
- /**
- * Returns a clone of this grid
- * @return grid clone
- */
- Grid* Clone()
- {
- Grid* pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution());
- pGrid->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
- memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
- return pGrid;
- }
- /**
- * Resizes the grid (deletes all old data)
- * @param width
- * @param height
- */
- virtual void Resize(kt_int32s width, kt_int32s height)
- {
- m_Width = width;
- m_Height = height;
- m_WidthStep = math::AlignValue<kt_int32s>(width, 8);
- if (m_pData != NULL)
- {
- delete[] m_pData;
- m_pData = NULL;
- }
- try
- {
- m_pData = new T[GetDataSize()];
- if (m_pCoordinateConverter == NULL)
- {
- m_pCoordinateConverter = new CoordinateConverter();
- }
- m_pCoordinateConverter->SetSize(Size2<kt_int32s>(width, height));
- }
- catch(...)
- {
- m_pData = NULL;
- m_Width = 0;
- m_Height = 0;
- m_WidthStep = 0;
- }
- Clear();
- }
- /**
- * Checks whether the given coordinates are valid grid indices
- * @param rGrid
- */
- inline kt_bool IsValidGridIndex(const Vector2<kt_int32s>& rGrid) const
- {
- return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height));
- }
- /**
- * Gets the index into the data pointer of the given grid coordinate
- * @param rGrid
- * @param boundaryCheck default value is true
- * @return grid index
- */
- virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
- {
- if (boundaryCheck == true)
- {
- if (IsValidGridIndex(rGrid) == false)
- {
- std::stringstream error;
- error << "Index " << rGrid << " out of range. Index must be between [0; "
- << m_Width << ") and [0; " << m_Height << ")";
- throw Exception(error.str());
- }
- }
- kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);
- if (boundaryCheck == true)
- {
- assert(math::IsUpTo(index, GetDataSize()));
- }
- return index;
- }
- /**
- * Gets the grid coordinate from an index
- * @param index
- * @return grid coordinate
- */
- Vector2<kt_int32s> IndexToGrid(kt_int32s index) const
- {
- Vector2<kt_int32s> grid;
- grid.SetY(index / m_WidthStep);
- grid.SetX(index - grid.GetY() * m_WidthStep);
- return grid;
- }
- /**
- * Converts the point from world coordinates to grid coordinates
- * @param rWorld world coordinate
- * @param flipY
- * @return grid coordinate
- */
- inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
- {
- return GetCoordinateConverter()->WorldToGrid(rWorld, flipY);
- }
- /**
- * Converts the point from grid coordinates to world coordinates
- * @param rGrid world coordinate
- * @param flipY
- * @return world coordinate
- */
- inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
- {
- return GetCoordinateConverter()->GridToWorld(rGrid, flipY);
- }
- /**
- * Gets pointer to data at given grid coordinate
- * @param rGrid grid coordinate
- * @return grid point
- */
- T* GetDataPointer(const Vector2<kt_int32s>& rGrid)
- {
- kt_int32s index = GridIndex(rGrid, true);
- return m_pData + index;
- }
- /**
- * Gets pointer to data at given grid coordinate
- * @param rGrid grid coordinate
- * @return grid point
- */
- T* GetDataPointer(const Vector2<kt_int32s>& rGrid) const
- {
- kt_int32s index = GridIndex(rGrid, true);
- return m_pData + index;
- }
- /**
- * Gets the width of the grid
- * @return width of the grid
- */
- inline kt_int32s GetWidth() const
- {
- return m_Width;
- };
- /**
- * Gets the height of the grid
- * @return height of the grid
- */
- inline kt_int32s GetHeight() const
- {
- return m_Height;
- };
- /**
- * Get the size as a Size2<kt_int32s>
- * @return size of the grid
- */
- inline const Size2<kt_int32s> GetSize() const
- {
- return Size2<kt_int32s>(m_Width, m_Height);
- }
- /**
- * Gets the width step in bytes
- * @return width step
- */
- inline kt_int32s GetWidthStep() const
- {
- return m_WidthStep;
- }
- /**
- * Gets the grid data pointer
- * @return data pointer
- */
- inline T* GetDataPointer()
- {
- return m_pData;
- }
- /**
- * Gets const grid data pointer
- * @return data pointer
- */
- inline T* GetDataPointer() const
- {
- return m_pData;
- }
- /**
- * Gets the allocated grid size in bytes
- * @return data size
- */
- inline kt_int32s GetDataSize() const
- {
- return m_WidthStep * m_Height;
- }
- /**
- * Get value at given grid coordinate
- * @param rGrid grid coordinate
- * @return value
- */
- inline T GetValue(const Vector2<kt_int32s>& rGrid) const
- {
- kt_int32s index = GridIndex(rGrid);
- return m_pData[index];
- }
- /**
- * Gets the coordinate converter for this grid
- * @return coordinate converter
- */
- inline CoordinateConverter* GetCoordinateConverter() const
- {
- return m_pCoordinateConverter;
- }
- /**
- * Gets the resolution
- * @return resolution
- */
- inline kt_double GetResolution() const
- {
- return GetCoordinateConverter()->GetResolution();
- }
- /**
- * Gets the grids bounding box
- * @return bounding box
- */
- inline BoundingBox2 GetBoundingBox() const
- {
- return GetCoordinateConverter()->GetBoundingBox();
- }
- /**
- * Increments all the grid cells from (x0, y0) to (x1, y1);
- * if applicable, apply f to each cell traced
- * @param x0
- * @param y0
- * @param x1
- * @param y1
- * @param f
- */
- void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL)
- {
- kt_bool steep = abs(y1 - y0) > abs(x1 - x0);
- if (steep)
- {
- std::swap(x0, y0);
- std::swap(x1, y1);
- }
- if (x0 > x1)
- {
- std::swap(x0, x1);
- std::swap(y0, y1);
- }
- kt_int32s deltaX = x1 - x0;
- kt_int32s deltaY = abs(y1 - y0);
- kt_int32s error = 0;
- kt_int32s ystep;
- kt_int32s y = y0;
- if (y0 < y1)
- {
- ystep = 1;
- }
- else
- {
- ystep = -1;
- }
- kt_int32s pointX;
- kt_int32s pointY;
- for (kt_int32s x = x0; x <= x1; x++)
- {
- if (steep)
- {
- pointX = y;
- pointY = x;
- }
- else
- {
- pointX = x;
- pointY = y;
- }
- error += deltaY;
- if (2 * error >= deltaX)
- {
- y += ystep;
- error -= deltaX;
- }
- Vector2<kt_int32s> gridIndex(pointX, pointY);
- if (IsValidGridIndex(gridIndex))
- {
- kt_int32s index = GridIndex(gridIndex, false);
- T* pGridPointer = GetDataPointer();
- pGridPointer[index]++;
- if (f != NULL)
- {
- (*f)(index);
- }
- }
- }
- }
- protected:
- /**
- * Constructs grid of given size
- * @param width
- * @param height
- */
- Grid(kt_int32s width, kt_int32s height)
- : m_pData(NULL)
- , m_pCoordinateConverter(NULL)
- {
- Resize(width, height);
- }
- private:
- kt_int32s m_Width; // width of grid
- kt_int32s m_Height; // height of grid
- kt_int32s m_WidthStep; // 8 bit aligned width of grid
- T* m_pData; // grid data
- // coordinate converter to convert between world coordinates and grid coordinates
- CoordinateConverter* m_pCoordinateConverter;
- }; // Grid
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * For making custom data
- */
- class CustomData : public Object
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(CustomData)
- // @endcond
- public:
- /**
- * Constructor
- */
- CustomData()
- : Object()
- {
- }
- /**
- * Destructor
- */
- virtual ~CustomData()
- {
- }
- public:
- /**
- * Write out this custom data as a string
- * @return string representation of this data object
- */
- virtual const std::string Write() const = 0;
- /**
- * Read in this custom data from a string
- * @param rValue string representation of this data object
- */
- virtual void Read(const std::string& rValue) = 0;
- private:
- CustomData(const CustomData&);
- const CustomData& operator=(const CustomData&);
- };
- /**
- * Type declaration of CustomData vector
- */
- typedef std::vector<CustomData*> CustomDataVector;
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * SensorData is a base class for all sensor data
- */
- class KARTO_EXPORT SensorData : public Object
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(SensorData)
- // @endcond
- public:
- /**
- * Destructor
- */
- virtual ~SensorData();
- public:
- /**
- * Gets sensor data id
- * @return sensor id
- */
- inline kt_int32s GetStateId() const
- {
- return m_StateId;
- }
- /**
- * Sets sensor data id
- * @param stateId id
- */
- inline void SetStateId(kt_int32s stateId)
- {
- m_StateId = stateId;
- }
- /**
- * Gets sensor unique id
- * @return unique id
- */
- inline kt_int32s GetUniqueId() const
- {
- return m_UniqueId;
- }
- /**
- * Sets sensor unique id
- * @param uniqueId
- */
- inline void SetUniqueId(kt_int32u uniqueId)
- {
- m_UniqueId = uniqueId;
- }
- /**
- * Gets sensor data time
- * @return time
- */
- inline kt_double GetTime() const
- {
- return m_Time;
- }
- /**
- * Sets sensor data time
- * @param time
- */
- inline void SetTime(kt_double time)
- {
- m_Time = time;
- }
- /**
- * Get the sensor that created this sensor data
- * @return sensor
- */
- inline const Name& GetSensorName() const
- {
- return m_SensorName;
- }
- /**
- * Add a CustomData object to sensor data
- * @param pCustomData
- */
- inline void AddCustomData(CustomData* pCustomData)
- {
- m_CustomData.push_back(pCustomData);
- }
- /**
- * Get all custom data objects assigned to sensor data
- * @return CustomDataVector&
- */
- inline const CustomDataVector& GetCustomData() const
- {
- return m_CustomData;
- }
- protected:
- /**
- * Construct a SensorData object with a sensor name
- */
- SensorData(const Name& rSensorName);
- private:
- /**
- * Restrict the copy constructor
- */
- SensorData(const SensorData&);
- /**
- * Restrict the assignment operator
- */
- const SensorData& operator=(const SensorData&);
- private:
- /**
- * ID unique to individual sensor
- */
- kt_int32s m_StateId;
- /**
- * ID unique across all sensor data
- */
- kt_int32s m_UniqueId;
- /**
- * Sensor that created this sensor data
- */
- Name m_SensorName;
- /**
- * Time the sensor data was created
- */
- kt_double m_Time;
- CustomDataVector m_CustomData;
- };
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Type declaration of range readings vector
- */
- typedef std::vector<kt_double> RangeReadingsVector;
- /**
- * LaserRangeScan representing the range readings from a laser range finder sensor.
- */
- class LaserRangeScan : public SensorData
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(LaserRangeScan)
- // @endcond
- public:
- /**
- * Constructs a scan from the given sensor with the given readings
- * @param rSensorName
- */
- LaserRangeScan(const Name& rSensorName)
- : SensorData(rSensorName)
- , m_pRangeReadings(NULL)
- , m_NumberOfRangeReadings(0)
- {
- }
- /**
- * Constructs a scan from the given sensor with the given readings
- * @param rSensorName
- * @param rRangeReadings
- */
- LaserRangeScan(const Name& rSensorName, const RangeReadingsVector& rRangeReadings)
- : SensorData(rSensorName)
- , m_pRangeReadings(NULL)
- , m_NumberOfRangeReadings(0)
- {
- assert(rSensorName.ToString() != "");
- SetRangeReadings(rRangeReadings);
- }
- /**
- * Destructor
- */
- virtual ~LaserRangeScan()
- {
- delete [] m_pRangeReadings;
- }
- public:
- /**
- * Gets the range readings of this scan
- * @return range readings of this scan
- */
- inline kt_double* GetRangeReadings() const
- {
- return m_pRangeReadings;
- }
- inline RangeReadingsVector GetRangeReadingsVector() const
- {
- return RangeReadingsVector(m_pRangeReadings, m_pRangeReadings + m_NumberOfRangeReadings);
- }
- /**
- * Sets the range readings for this scan
- * @param rRangeReadings
- */
- inline void SetRangeReadings(const RangeReadingsVector& rRangeReadings)
- {
- // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the heck is this??
- // if (rRangeReadings.size() != GetNumberOfRangeReadings())
- // {
- // std::stringstream error;
- // error << "Given number of readings (" << rRangeReadings.size()
- // << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")";
- // throw Exception(error.str());
- // }
- if (!rRangeReadings.empty())
- {
- if (rRangeReadings.size() != m_NumberOfRangeReadings)
- {
- // delete old readings
- delete [] m_pRangeReadings;
- // store size of array!
- m_NumberOfRangeReadings = static_cast<kt_int32u>(rRangeReadings.size());
- // allocate range readings
- m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
- }
- // copy readings
- kt_int32u index = 0;
- const_forEach(RangeReadingsVector, &rRangeReadings)
- {
- m_pRangeReadings[index++] = *iter;
- }
- }
- else
- {
- delete [] m_pRangeReadings;
- m_pRangeReadings = NULL;
- }
- }
- /**
- * Gets the laser range finder sensor that generated this scan
- * @return laser range finder sensor of this scan
- */
- inline LaserRangeFinder* GetLaserRangeFinder() const
- {
- return SensorManager::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorName());
- }
- /**
- * Gets the number of range readings
- * @return number of range readings
- */
- inline kt_int32u GetNumberOfRangeReadings() const
- {
- return m_NumberOfRangeReadings;
- }
- private:
- LaserRangeScan(const LaserRangeScan&);
- const LaserRangeScan& operator=(const LaserRangeScan&);
- private:
- kt_double* m_pRangeReadings;
- kt_int32u m_NumberOfRangeReadings;
- }; // LaserRangeScan
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * DrivePose representing the pose value of a drive sensor.
- */
- class DrivePose : public SensorData
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(DrivePose)
- // @endcond
- public:
- /**
- * Constructs a pose of the given drive sensor
- * @param rSensorName
- */
- DrivePose(const Name& rSensorName)
- : SensorData(rSensorName)
- {
- }
- /**
- * Destructor
- */
- virtual ~DrivePose()
- {
- }
- public:
- /**
- * Gets the odometric pose of this scan
- * @return odometric pose of this scan
- */
- inline const Pose3& GetOdometricPose() const
- {
- return m_OdometricPose;
- }
- /**
- * Sets the odometric pose of this scan
- * @param rPose
- */
- inline void SetOdometricPose(const Pose3& rPose)
- {
- m_OdometricPose = rPose;
- }
- private:
- DrivePose(const DrivePose&);
- const DrivePose& operator=(const DrivePose&);
- private:
- /**
- * Odometric pose of robot
- */
- Pose3 m_OdometricPose;
- }; // class DrivePose
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * The LocalizedRangeScan contains range data from a single sweep of a laser range finder sensor
- * in a two-dimensional space and position information. The odometer position is the position
- * reported by the robot when the range data was recorded. The corrected position is the position
- * calculated by the mapper (or localizer)
- * LocalizedRangeScan包含二维空间中激光测距仪传感器单次扫描的距离数据和位置信息。
- * 里程表位置是记录里程数据时机器人报告的位置。校正后的位置是由映射器(或定位器)计算的位置
- */
- class LocalizedRangeScan : public LaserRangeScan
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(LocalizedRangeScan)
- // @endcond
- public:
- /**
- * Constructs a range scan from the given range finder with the given readings
- */
- LocalizedRangeScan(const Name& rSensorName, const RangeReadingsVector& rReadings)
- : LaserRangeScan(rSensorName, rReadings)
- , m_IsDirty(true)
- {
- }
- /**
- * Destructor
- */
- virtual ~LocalizedRangeScan()
- {
- }
- private:
- mutable boost::shared_mutex m_Lock;
- public:
- /**
- * Gets the odometric pose of this scan
- * @return odometric pose of this scan
- */
- inline const Pose2& GetOdometricPose() const
- {
- return m_OdometricPose;
- }
- /**
- * Sets the odometric pose of this scan
- * @param rPose
- */
- inline void SetOdometricPose(const Pose2& rPose)
- {
- m_OdometricPose = rPose;
- }
- /**
- * Gets the (possibly corrected) robot pose at which this scan was taken. The corrected robot pose of the scan
- * is usually set by an external module such as a localization or mapping module when it is determined
- * that the original pose was incorrect. The external module will set the correct pose based on
- * additional sensor data and any context information it has. If the pose has not been corrected,
- * a call to this method returns the same pose as GetOdometricPose().
- * @return corrected pose
- */
- inline const Pose2& GetCorrectedPose() const
- {
- return m_CorrectedPose;
- }
- /**
- * Moves the scan by moving the robot pose to the given location.
- * @param rPose new pose of the robot of this scan
- */
- inline void SetCorrectedPose(const Pose2& rPose)
- {
- m_CorrectedPose = rPose;
- m_IsDirty = true;
- }
- /**
- * Gets barycenter of point readings
- */
- inline const Pose2& GetBarycenterPose() const
- {
- boost::shared_lock<boost::shared_mutex> lock(m_Lock);
- if (m_IsDirty)
- {
- // throw away constness and do an update!
- lock.unlock();
- boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
- const_cast<LocalizedRangeScan*>(this)->Update();
- }
- return m_BarycenterPose;
- }
- /**
- * Gets barycenter if the given parameter is true, otherwise returns the scanner pose
- * @param useBarycenter
- * @return barycenter if parameter is true, otherwise scanner pose
- */
- inline Pose2 GetReferencePose(kt_bool useBarycenter) const
- {
- boost::shared_lock<boost::shared_mutex> lock(m_Lock);
- if (m_IsDirty)
- {
- // throw away constness and do an update!
- lock.unlock();
- boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
- const_cast<LocalizedRangeScan*>(this)->Update();
- }
- return useBarycenter ? GetBarycenterPose() : GetSensorPose();
- }
- /**
- * Computes the position of the sensor
- * @return scan pose
- */
- inline Pose2 GetSensorPose() const
- {
- return GetSensorAt(m_CorrectedPose);
- }
- /**
- * Computes the robot pose given the corrected scan pose
- * @param rScanPose pose of the sensor
- */
- void SetSensorPose(const Pose2& rScanPose)
- {
- Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
- kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
- kt_double offsetHeading = deviceOffsetPose2.GetHeading();
- kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
- kt_double correctedHeading = math::NormalizeAngle(rScanPose.GetHeading());
- Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
- offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
- offsetHeading);
- m_CorrectedPose = rScanPose - worldSensorOffset;
- Update();
- }
- /**
- * Computes the position of the sensor if the robot were at the given pose
- * @param rPose
- * @return sensor pose
- */
- inline Pose2 GetSensorAt(const Pose2& rPose) const
- {
- return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose());
- }
- /**
- * Gets the bounding box of this scan
- * @return bounding box of this scan
- */
- inline const BoundingBox2& GetBoundingBox() const
- {
- boost::shared_lock<boost::shared_mutex> lock(m_Lock);
- if (m_IsDirty)
- {
- // throw away constness and do an update!
- lock.unlock();
- boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
- const_cast<LocalizedRangeScan*>(this)->Update();
- }
- return m_BoundingBox;
- }
- /**
- * Get point readings in local coordinates
- */
- inline const PointVectorDouble& GetPointReadings(kt_bool wantFiltered = false) const
- {
- boost::shared_lock<boost::shared_mutex> lock(m_Lock);
- if (m_IsDirty)
- {
- // throw away constness and do an update!
- lock.unlock();
- boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
- const_cast<LocalizedRangeScan*>(this)->Update();
- }
- if (wantFiltered == true)
- {
- return m_PointReadings;
- }
- else
- {
- return m_UnfilteredPointReadings;
- }
- }
- private:
- /**
- * Compute point readings based on range readings
- * Only range readings within [minimum range; range threshold] are returned
- */
- virtual void Update()
- {
- LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder();
- if (pLaserRangeFinder != NULL)
- {
- m_PointReadings.clear();
- m_UnfilteredPointReadings.clear();
- kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
- kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle();
- kt_double angularResolution = pLaserRangeFinder->GetAngularResolution();
- Pose2 scanPose = GetSensorPose();
- // compute point readings
- Vector2<kt_double> rangePointsSum;
- kt_int32u beamNum = 0;
- for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++)
- {
- kt_double rangeReading = GetRangeReadings()[i];
- if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold))
- {
- kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
- Vector2<kt_double> point;
- point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
- point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
- m_UnfilteredPointReadings.push_back(point);
- continue;
- }
- kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
- Vector2<kt_double> point;
- point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
- point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
- m_PointReadings.push_back(point);
- m_UnfilteredPointReadings.push_back(point);
- rangePointsSum += point;
- }
- // compute barycenter
- kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
- if (nPoints != 0.0)
- {
- Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
- m_BarycenterPose = Pose2(averagePosition, 0.0);
- }
- else
- {
- m_BarycenterPose = scanPose;
- }
- // calculate bounding box of scan
- m_BoundingBox = BoundingBox2();
- m_BoundingBox.Add(scanPose.GetPosition());
- forEach(PointVectorDouble, &m_PointReadings)
- {
- m_BoundingBox.Add(*iter);
- }
- }
- m_IsDirty = false;
- }
- private:
- LocalizedRangeScan(const LocalizedRangeScan&);
- const LocalizedRangeScan& operator=(const LocalizedRangeScan&);
- private:
- /**
- * Odometric pose of robot
- */
- Pose2 m_OdometricPose;
- /**
- * Corrected pose of robot calculated by mapper (or localizer)
- */
- Pose2 m_CorrectedPose;
- protected:
- /**
- * Average of all the point readings
- */
- Pose2 m_BarycenterPose;
- /**
- * Vector of point readings
- */
- PointVectorDouble m_PointReadings;
- /**
- * Vector of unfiltered point readings
- */
- PointVectorDouble m_UnfilteredPointReadings;
- /**
- * Bounding box of localized range scan
- */
- BoundingBox2 m_BoundingBox;
- /**
- * Internal flag used to update point readings, barycenter and bounding box
- */
- kt_bool m_IsDirty;
- }; // LocalizedRangeScan
- /**
- * Type declaration of LocalizedRangeScan vector
- */
- typedef std::vector<LocalizedRangeScan*> LocalizedRangeScanVector;
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * The LocalizedRangeScanWithPoints is an extension of the LocalizedRangeScan with precomputed points.
- */
- class LocalizedRangeScanWithPoints : public LocalizedRangeScan
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(LocalizedRangeScanWithPoints)
- // @endcond
- public:
- /**
- * Constructs a range scan from the given range finder with the given readings. Precomptued points should be
- * in the robot frame.
- */
- LocalizedRangeScanWithPoints(const Name& rSensorName, const RangeReadingsVector& rReadings,
- const PointVectorDouble& rPoints)
- : m_Points(rPoints),
- LocalizedRangeScan(rSensorName, rReadings)
- {
- }
- /**
- * Destructor
- */
- virtual ~LocalizedRangeScanWithPoints()
- {
- }
- private:
- /**
- * Update the points based on the latest sensor pose.
- */
- void Update()
- {
- m_PointReadings.clear();
- m_UnfilteredPointReadings.clear();
- Pose2 scanPose = GetSensorPose();
- Pose2 robotPose = GetCorrectedPose();
- // update point readings
- Vector2<kt_double> rangePointsSum;
- for (kt_int32u i = 0; i < m_Points.size(); i++)
- {
- // check if this has a NaN
- if (!std::isfinite(m_Points[i].GetX()) || !std::isfinite(m_Points[i].GetY()))
- {
- Vector2<kt_double> point(m_Points[i].GetX(), m_Points[i].GetY());
- m_UnfilteredPointReadings.push_back(point);
- continue;
- }
- // transform into world coords
- Pose2 pointPose(m_Points[i].GetX(), m_Points[i].GetY(), 0);
- Pose2 result = Transform(robotPose).TransformPose(pointPose);
- Vector2<kt_double> point(result.GetX(), result.GetY());
- m_PointReadings.push_back(point);
- m_UnfilteredPointReadings.push_back(point);
- rangePointsSum += point;
- }
- // compute barycenter
- kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
- if (nPoints != 0.0)
- {
- Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
- m_BarycenterPose = Pose2(averagePosition, 0.0);
- }
- else
- {
- m_BarycenterPose = scanPose;
- }
- // calculate bounding box of scan
- m_BoundingBox = BoundingBox2();
- m_BoundingBox.Add(scanPose.GetPosition());
- forEach(PointVectorDouble, &m_PointReadings)
- {
- m_BoundingBox.Add(*iter);
- }
- m_IsDirty = false;
- }
- private:
- LocalizedRangeScanWithPoints(const LocalizedRangeScanWithPoints&);
- const LocalizedRangeScanWithPoints& operator=(const LocalizedRangeScanWithPoints&);
- private:
- const PointVectorDouble m_Points;
- }; // LocalizedRangeScanWithPoints
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- class OccupancyGrid;
- class KARTO_EXPORT CellUpdater : public Functor
- {
- public:
- CellUpdater(OccupancyGrid* pGrid)
- : m_pOccupancyGrid(pGrid)
- {
- }
- /**
- * Updates the cell at the given index based on the grid's hits and pass counters
- * @param index
- */
- virtual void operator() (kt_int32u index);
- private:
- OccupancyGrid* m_pOccupancyGrid;
- }; // CellUpdater
- /**
- * Occupancy grid definition. See GridStates for possible grid values.
- */
- class OccupancyGrid : public Grid<kt_int8u>
- {
- friend class CellUpdater;
- friend class IncrementalOccupancyGrid;
- public:
- /**
- * Constructs an occupancy grid of given size
- * @param width
- * @param height
- * @param rOffset
- * @param resolution
- */
- OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2<kt_double>& rOffset, kt_double resolution)
- : Grid<kt_int8u>(width, height)
- , m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
- , m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
- , m_pCellUpdater(NULL)
- {
- m_pCellUpdater = new CellUpdater(this);
- if (karto::math::DoubleEqual(resolution, 0.0))
- {
- throw Exception("Resolution cannot be 0");
- }
- m_pMinPassThrough = new Parameter<kt_int32u>("MinPassThrough", 2);
- m_pOccupancyThreshold = new Parameter<kt_double>("OccupancyThreshold", 0.1);
- GetCoordinateConverter()->SetScale(1.0 / resolution);
- GetCoordinateConverter()->SetOffset(rOffset);
- }
- /**
- * Destructor
- */
- virtual ~OccupancyGrid()
- {
- delete m_pCellUpdater;
- delete m_pCellPassCnt;
- delete m_pCellHitsCnt;
- delete m_pMinPassThrough;
- delete m_pOccupancyThreshold;
- }
- public:
- /**
- * Create an occupancy grid from the given scans using the given resolution
- * @param rScans
- * @param resolution
- */
- static OccupancyGrid* CreateFromScans(const LocalizedRangeScanVector& rScans, kt_double resolution)
- {
- if (rScans.empty())
- {
- return NULL;
- }
- kt_int32s width, height;
- Vector2<kt_double> offset;
- ComputeDimensions(rScans, resolution, width, height, offset);
- OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
- pOccupancyGrid->CreateFromScans(rScans);
- return pOccupancyGrid;
- }
- /**
- * Make a clone
- * @return occupancy grid clone
- */
- OccupancyGrid* Clone() const
- {
- OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(),
- GetHeight(),
- GetCoordinateConverter()->GetOffset(),
- 1.0 / GetCoordinateConverter()->GetScale());
- memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
- pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize());
- pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone();
- pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone();
- return pOccupancyGrid;
- }
- /**
- * Check if grid point is free
- * @param rPose
- * @return whether the cell at the given point is free space
- */
- virtual kt_bool IsFree(const Vector2<kt_int32s>& rPose) const
- {
- kt_int8u* pOffsets = reinterpret_cast<kt_int8u*>(GetDataPointer(rPose));
- if (*pOffsets == GridStates_Free)
- {
- return true;
- }
- return false;
- }
- /**
- * Casts a ray from the given point (up to the given max range)
- * and returns the distance to the closest obstacle
- * @param rPose2
- * @param maxRange
- * @return distance to closest obstacle
- */
- virtual kt_double RayCast(const Pose2& rPose2, kt_double maxRange) const
- {
- double scale = GetCoordinateConverter()->GetScale();
- kt_double x = rPose2.GetX();
- kt_double y = rPose2.GetY();
- kt_double theta = rPose2.GetHeading();
- kt_double sinTheta = sin(theta);
- kt_double cosTheta = cos(theta);
- kt_double xStop = x + maxRange * cosTheta;
- kt_double xSteps = 1 + fabs(xStop - x) * scale;
- kt_double yStop = y + maxRange * sinTheta;
- kt_double ySteps = 1 + fabs(yStop - y) * scale;
- kt_double steps = math::Maximum(xSteps, ySteps);
- kt_double delta = maxRange / steps;
- kt_double distance = delta;
- for (kt_int32u i = 1; i < steps; i++)
- {
- kt_double x1 = x + distance * cosTheta;
- kt_double y1 = y + distance * sinTheta;
- Vector2<kt_int32s> gridIndex = WorldToGrid(Vector2<kt_double>(x1, y1));
- if (IsValidGridIndex(gridIndex) && IsFree(gridIndex))
- {
- distance = (i + 1) * delta;
- }
- else
- {
- break;
- }
- }
- return (distance < maxRange)? distance : maxRange;
- }
- /**
- * Sets the minimum number of beams that must pass through a cell before it
- * will be considered to be occupied or unoccupied.
- * This prevents stray beams from messing up the map.
- */
- void SetMinPassThrough(kt_int32u count)
- {
- m_pMinPassThrough->SetValue(count);
- }
- /**
- * Sets the minimum ratio of beams hitting cell to beams passing through
- * cell for cell to be marked as occupied.
- */
- void SetOccupancyThreshold(kt_double thresh)
- {
- m_pOccupancyThreshold->SetValue(thresh);
- }
- protected:
- /**
- * Get cell hit grid
- * @return Grid<kt_int32u>*
- */
- virtual Grid<kt_int32u>* GetCellHitsCounts()
- {
- return m_pCellHitsCnt;
- }
- /**
- * Get cell pass grid
- * @return Grid<kt_int32u>*
- */
- virtual Grid<kt_int32u>* GetCellPassCounts()
- {
- return m_pCellPassCnt;
- }
- protected:
- /**
- * Calculate grid dimensions from localized range scans
- * @param rScans
- * @param resolution
- * @param rWidth
- * @param rHeight
- * @param rOffset
- */
- static void ComputeDimensions(const LocalizedRangeScanVector& rScans,
- kt_double resolution,
- kt_int32s& rWidth,
- kt_int32s& rHeight,
- Vector2<kt_double>& rOffset)
- {
- BoundingBox2 boundingBox;
- const_forEach(LocalizedRangeScanVector, &rScans)
- {
- boundingBox.Add((*iter)->GetBoundingBox());
- }
- kt_double scale = 1.0 / resolution;
- Size2<kt_double> size = boundingBox.GetSize();
- rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));
- rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));
- rOffset = boundingBox.GetMinimum();
- }
- /**
- * Create grid using scans
- * @param rScans
- */
- virtual void CreateFromScans(const LocalizedRangeScanVector& rScans)
- {
- m_pCellPassCnt->Resize(GetWidth(), GetHeight());
- m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
- m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
- m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
- const_forEach(LocalizedRangeScanVector, &rScans)
- {
- LocalizedRangeScan* pScan = *iter;
- AddScan(pScan);
- }
- Update();
- }
- /**
- * Adds the scan's information to this grid's counters (optionally
- * update the grid's cells' occupancy status)
- * @param pScan
- * @param doUpdate whether to update the grid's cell's occupancy status
- * @return returns false if an endpoint fell off the grid, otherwise true
- */
- virtual kt_bool AddScan(LocalizedRangeScan* pScan, kt_bool doUpdate = false)
- {
- kt_double rangeThreshold = pScan->GetLaserRangeFinder()->GetRangeThreshold();
- kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
- kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange();
- Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();
- // get scan point readings
- const PointVectorDouble& rPointReadings = pScan->GetPointReadings(false);
- kt_bool isAllInMap = true;
- // draw lines from scan position to all point readings
- int pointIndex = 0;
- const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter)
- {
- Vector2<kt_double> point = *pointsIter;
- kt_double rangeReading = pScan->GetRangeReadings()[pointIndex];
- kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE);
- if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading))
- {
- // ignore these readings
- pointIndex++;
- continue;
- }
- else if (rangeReading >= rangeThreshold)
- {
- // trace up to range reading
- kt_double ratio = rangeThreshold / rangeReading;
- kt_double dx = point.GetX() - scanPosition.GetX();
- kt_double dy = point.GetY() - scanPosition.GetY();
- point.SetX(scanPosition.GetX() + ratio * dx);
- point.SetY(scanPosition.GetY() + ratio * dy);
- }
- kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
- if (!isInMap)
- {
- isAllInMap = false;
- }
- pointIndex++;
- }
- return isAllInMap;
- }
- /**
- * Traces a beam from the start position to the end position marking
- * the bookkeeping arrays accordingly.
- * @param rWorldFrom start position of beam
- * @param rWorldTo end position of beam
- * @param isEndPointValid is the reading within the range threshold?
- * @param doUpdate whether to update the cells' occupancy status immediately
- * @return returns false if an endpoint fell off the grid, otherwise true
- */
- virtual kt_bool RayTrace(const Vector2<kt_double>& rWorldFrom,
- const Vector2<kt_double>& rWorldTo,
- kt_bool isEndPointValid,
- kt_bool doUpdate = false)
- {
- assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
- Vector2<kt_int32s> gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);
- Vector2<kt_int32s> gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);
- CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
- m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);
- // for the end point
- if (isEndPointValid)
- {
- if (m_pCellPassCnt->IsValidGridIndex(gridTo))
- {
- kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);
- kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
- kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
- // increment cell pass through and hit count
- pCellPassCntPtr[index]++;
- pCellHitCntPtr[index]++;
- if (doUpdate)
- {
- (*m_pCellUpdater)(index);
- }
- }
- }
- return m_pCellPassCnt->IsValidGridIndex(gridTo);
- }
- /**
- * Updates a single cell's value based on the given counters
- * @param pCell
- * @param cellPassCnt
- * @param cellHitCnt
- */
- virtual void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
- {
- if (cellPassCnt > m_pMinPassThrough->GetValue())
- {
- kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);
- if (hitRatio > m_pOccupancyThreshold->GetValue())
- {
- *pCell = GridStates_Occupied;
- }
- else
- {
- *pCell = GridStates_Free;
- }
- }
- }
- /**
- * Update the grid based on the values in m_pCellHitsCnt and m_pCellPassCnt
- */
- virtual void Update()
- {
- assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
- // clear grid
- Clear();
- // set occupancy status of cells
- kt_int8u* pDataPtr = GetDataPointer();
- kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
- kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
- kt_int32u nBytes = GetDataSize();
- for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
- {
- UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
- }
- }
- /**
- * Resizes the grid (deletes all old data)
- * @param width
- * @param height
- */
- virtual void Resize(kt_int32s width, kt_int32s height)
- {
- Grid<kt_int8u>::Resize(width, height);
- m_pCellPassCnt->Resize(width, height);
- m_pCellHitsCnt->Resize(width, height);
- }
- protected:
- /**
- * Counters of number of times a beam passed through a cell
- */
- Grid<kt_int32u>* m_pCellPassCnt;
- /**
- * Counters of number of times a beam ended at a cell
- */
- Grid<kt_int32u>* m_pCellHitsCnt;
- private:
- /**
- * Restrict the copy constructor
- */
- OccupancyGrid(const OccupancyGrid&);
- /**
- * Restrict the assignment operator
- */
- const OccupancyGrid& operator=(const OccupancyGrid&);
- private:
- CellUpdater* m_pCellUpdater;
- ////////////////////////////////////////////////////////////
- // NOTE: These two values are dependent on the resolution. If the resolution is too small,
- // then not many beams will hit the cell!
- // Number of beams that must pass through a cell before it will be considered to be occupied
- // or unoccupied. This prevents stray beams from messing up the map.
- Parameter<kt_int32u>* m_pMinPassThrough;
- // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
- Parameter<kt_double>* m_pOccupancyThreshold;
- }; // OccupancyGrid
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Dataset info
- * Contains title, author and other information about the dataset
- */
- class DatasetInfo : public Object
- {
- public:
- // @cond EXCLUDE
- KARTO_Object(DatasetInfo)
- // @endcond
- public:
- DatasetInfo()
- : Object()
- {
- m_pTitle = new Parameter<std::string>("Title", "", GetParameterManager());
- m_pAuthor = new Parameter<std::string>("Author", "", GetParameterManager());
- m_pDescription = new Parameter<std::string>("Description", "", GetParameterManager());
- m_pCopyright = new Parameter<std::string>("Copyright", "", GetParameterManager());
- }
- virtual ~DatasetInfo()
- {
- }
- public:
- /**
- * Dataset title
- */
- const std::string& GetTitle() const
- {
- return m_pTitle->GetValue();
- }
- /**
- * Dataset author(s)
- */
- const std::string& GetAuthor() const
- {
- return m_pAuthor->GetValue();
- }
- /**
- * Dataset description
- */
- const std::string& GetDescription() const
- {
- return m_pDescription->GetValue();
- }
- /**
- * Dataset copyrights
- */
- const std::string& GetCopyright() const
- {
- return m_pCopyright->GetValue();
- }
- private:
- DatasetInfo(const DatasetInfo&);
- const DatasetInfo& operator=(const DatasetInfo&);
- private:
- Parameter<std::string>* m_pTitle;
- Parameter<std::string>* m_pAuthor;
- Parameter<std::string>* m_pDescription;
- Parameter<std::string>* m_pCopyright;
- }; // class DatasetInfo
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Karto dataset. Stores LaserRangeFinders and LocalizedRangeScans and manages memory of allocated LaserRangeFinders
- * and LocalizedRangeScans
- */
- class Dataset
- {
- public:
- /**
- * Default constructor
- */
- Dataset()
- : m_pDatasetInfo(NULL)
- {
- }
- /**
- * Destructor
- */
- virtual ~Dataset()
- {
- Clear();
- }
- public:
- /**
- * Adds object to this dataset
- * @param pObject
- */
- void Add(Object* pObject)
- {
- if (pObject != NULL)
- {
- if (dynamic_cast<Sensor*>(pObject))
- {
- Sensor* pSensor = dynamic_cast<Sensor*>(pObject);
- if (pSensor != NULL)
- {
- m_SensorNameLookup[pSensor->GetName()] = pSensor;
- karto::SensorManager::GetInstance()->RegisterSensor(pSensor);
- }
- m_Objects.push_back(pObject);
- }
- else if (dynamic_cast<SensorData*>(pObject))
- {
- SensorData* pSensorData = dynamic_cast<SensorData*>(pObject);
- m_Objects.push_back(pSensorData);
- }
- else if (dynamic_cast<DatasetInfo*>(pObject))
- {
- m_pDatasetInfo = dynamic_cast<DatasetInfo*>(pObject);
- }
- else
- {
- m_Objects.push_back(pObject);
- }
- }
- }
- /**
- * Get sensor states
- * @return sensor state
- */
- inline const ObjectVector& GetObjects() const
- {
- return m_Objects;
- }
- /**
- * Get dataset info
- * @return dataset info
- */
- inline DatasetInfo* GetDatasetInfo()
- {
- return m_pDatasetInfo;
- }
- /**
- * Delete all stored data
- */
- virtual void Clear()
- {
- for (std::map<Name, Sensor*>::iterator iter = m_SensorNameLookup.begin(); iter != m_SensorNameLookup.end(); ++iter)
- {
- karto::SensorManager::GetInstance()->UnregisterSensor(iter->second);
- }
- forEach(ObjectVector, &m_Objects)
- {
- delete *iter;
- }
- m_Objects.clear();
- if (m_pDatasetInfo != NULL)
- {
- delete m_pDatasetInfo;
- m_pDatasetInfo = NULL;
- }
- }
- private:
- std::map<Name, Sensor*> m_SensorNameLookup;
- ObjectVector m_Objects;
- DatasetInfo* m_pDatasetInfo;
- }; // Dataset
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * An array that can be resized as long as the size
- * does not exceed the initial capacity
- */
- class LookupArray
- {
- public:
- /**
- * Constructs lookup array
- */
- LookupArray()
- : m_pArray(NULL)
- , m_Capacity(0)
- , m_Size(0)
- {
- }
- /**
- * Destructor
- */
- virtual ~LookupArray()
- {
- assert(m_pArray != NULL);
- delete[] m_pArray;
- m_pArray = NULL;
- }
- public:
- /**
- * Clear array
- */
- void Clear()
- {
- memset(m_pArray, 0, sizeof(kt_int32s) * m_Capacity);
- }
- /**
- * Gets size of array
- * @return array size
- */
- kt_int32u GetSize() const
- {
- return m_Size;
- }
- /**
- * Sets size of array (resize if not big enough)
- * @param size
- */
- void SetSize(kt_int32u size)
- {
- assert(size != 0);
- if (size > m_Capacity)
- {
- if (m_pArray != NULL)
- {
- delete [] m_pArray;
- }
- m_Capacity = size;
- m_pArray = new kt_int32s[m_Capacity];
- }
- m_Size = size;
- }
- /**
- * Gets reference to value at given index
- * @param index
- * @return reference to value at index
- */
- inline kt_int32s& operator [] (kt_int32u index)
- {
- assert(index < m_Size);
- return m_pArray[index];
- }
- /**
- * Gets value at given index
- * @param index
- * @return value at index
- */
- inline kt_int32s operator [] (kt_int32u index) const
- {
- assert(index < m_Size);
- return m_pArray[index];
- }
- /**
- * Gets array pointer
- * @return array pointer
- */
- inline kt_int32s* GetArrayPointer()
- {
- return m_pArray;
- }
- /**
- * Gets array pointer
- * @return array pointer
- */
- inline kt_int32s* GetArrayPointer() const
- {
- return m_pArray;
- }
- private:
- kt_int32s* m_pArray;
- kt_int32u m_Capacity;
- kt_int32u m_Size;
- }; // LookupArray
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- /**
- * Create lookup tables for point readings at varying angles in grid.
- * For each angle, grid indexes are calculated for each range reading.
- * This is to speed up finding best angle/position for a localized range scan
- *
- * Used heavily in mapper and localizer.
- *
- * In the localizer, this is a huge speed up for calculating possible position. For each particle,
- * a probability is calculated. The range scan is the same, but all grid indexes at all possible angles are
- * calculated. So when calculating the particle probability at a specific angle, the index table is used
- * to look up probability in probability grid!
- *
- */
- template<typename T>
- class GridIndexLookup
- {
- public:
- /**
- * Construct a GridIndexLookup with a grid
- * @param pGrid
- */
- GridIndexLookup(Grid<T>* pGrid)
- : m_pGrid(pGrid)
- , m_Capacity(0)
- , m_Size(0)
- , m_ppLookupArray(NULL)
- {
- }
- /**
- * Destructor
- */
- virtual ~GridIndexLookup()
- {
- DestroyArrays();
- }
- public:
- /**
- * Gets the lookup array for a particular angle index
- * @param index
- * @return lookup array
- */
- const LookupArray* GetLookupArray(kt_int32u index) const
- {
- assert(math::IsUpTo(index, m_Size));
- return m_ppLookupArray[index];
- }
- /**
- * Get angles
- * @return std::vector<kt_double>& angles
- */
- const std::vector<kt_double>& GetAngles() const
- {
- return m_Angles;
- }
- /**
- * Compute lookup table of the points of the given scan for the given angular space
- * @param pScan the scan
- * @param angleCenter
- * @param angleOffset computes lookup arrays for the angles within this offset around angleStart
- * @param angleResolution how fine a granularity to compute lookup arrays in the angular space
- */
- void ComputeOffsets(LocalizedRangeScan* pScan,
- kt_double angleCenter,
- kt_double angleOffset,
- kt_double angleResolution)
- {
- assert(angleOffset != 0.0);
- assert(angleResolution != 0.0);
- kt_int32u nAngles = static_cast<kt_int32u>(math::Round(angleOffset * 2.0 / angleResolution) + 1);
- SetSize(nAngles);
- //////////////////////////////////////////////////////
- // convert points into local coordinates of scan pose
- const PointVectorDouble& rPointReadings = pScan->GetPointReadings();
- // compute transform to scan pose
- Transform transform(pScan->GetSensorPose());
- Pose2Vector localPoints;
- const_forEach(PointVectorDouble, &rPointReadings)
- {
- // do inverse transform to get points in local coordinates
- Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0));
- localPoints.push_back(vec);
- }
- //////////////////////////////////////////////////////
- // create lookup array for different angles
- kt_double angle = 0.0;
- kt_double startAngle = angleCenter - angleOffset;
- for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
- {
- angle = startAngle + angleIndex * angleResolution;
- ComputeOffsets(angleIndex, angle, localPoints, pScan);
- }
- // assert(math::DoubleEqual(angle, angleCenter + angleOffset));
- }
- private:
- /**
- * Compute lookup value of points for given angle
- * @param angleIndex
- * @param angle
- * @param rLocalPoints
- */
- void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector& rLocalPoints, LocalizedRangeScan* pScan)
- {
- m_ppLookupArray[angleIndex]->SetSize(static_cast<kt_int32u>(rLocalPoints.size()));
- m_Angles.at(angleIndex) = angle;
- // set up point array by computing relative offsets to points readings
- // when rotated by given angle
- const Vector2<kt_double>& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
- kt_double cosine = cos(angle);
- kt_double sine = sin(angle);
- kt_int32u readingIndex = 0;
- kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
- kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
- const_forEach(Pose2Vector, &rLocalPoints)
- {
- const Vector2<kt_double>& rPosition = iter->GetPosition();
- if (std::isnan(pScan->GetRangeReadings()[readingIndex]) || std::isinf(pScan->GetRangeReadings()[readingIndex]))
- {
- pAngleIndexPointer[readingIndex] = INVALID_SCAN;
- readingIndex++;
- continue;
- }
- // counterclockwise rotation and that rotation is about the origin (0, 0).
- Vector2<kt_double> offset;
- offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY());
- offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY());
- // have to compensate for the grid offset when getting the grid index
- Vector2<kt_int32s> gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
- // use base GridIndex to ignore ROI
- kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint, false);
- pAngleIndexPointer[readingIndex] = lookupIndex;
- readingIndex++;
- }
- assert(readingIndex == rLocalPoints.size());
- }
- /**
- * Sets size of lookup table (resize if not big enough)
- * @param size
- */
- void SetSize(kt_int32u size)
- {
- assert(size != 0);
- if (size > m_Capacity)
- {
- if (m_ppLookupArray != NULL)
- {
- DestroyArrays();
- }
- m_Capacity = size;
- m_ppLookupArray = new LookupArray*[m_Capacity];
- for (kt_int32u i = 0; i < m_Capacity; i++)
- {
- m_ppLookupArray[i] = new LookupArray();
- }
- }
- m_Size = size;
- m_Angles.resize(size);
- }
- /**
- * Delete the arrays
- */
- void DestroyArrays()
- {
- for (kt_int32u i = 0; i < m_Capacity; i++)
- {
- delete m_ppLookupArray[i];
- }
- delete[] m_ppLookupArray;
- m_ppLookupArray = NULL;
- }
- private:
- Grid<T>* m_pGrid;
- kt_int32u m_Capacity;
- kt_int32u m_Size;
- LookupArray **m_ppLookupArray;
- // for sanity check
- std::vector<kt_double> m_Angles;
- }; // class GridIndexLookup
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- inline Pose2::Pose2(const Pose3& rPose)
- : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY())
- {
- kt_double t1, t2;
- // calculates heading from orientation
- rPose.GetOrientation().ToEulerAngles(m_Heading, t1, t2);
- }
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- ////////////////////////////////////////////////////////////////////////////////////////
- // @cond EXCLUDE
- template<typename T>
- inline void Object::SetParameter(const std::string& rName, T value)
- {
- AbstractParameter* pParameter = GetParameter(rName);
- if (pParameter != NULL)
- {
- std::stringstream stream;
- stream << value;
- pParameter->SetValueFromString(stream.str());
- }
- else
- {
- throw Exception("Parameter does not exist: " + rName);
- }
- }
- template<>
- inline void Object::SetParameter(const std::string& rName, kt_bool value)
- {
- AbstractParameter* pParameter = GetParameter(rName);
- if (pParameter != NULL)
- {
- pParameter->SetValueFromString(value ? "true" : "false");
- }
- else
- {
- throw Exception("Parameter does not exist: " + rName);
- }
- }
- // @endcond
- /*@}*/
- } // namespace karto
- #endif // OPEN_KARTO_KARTO_H
|