From 31e1d6d368f7c7f0f9aa5c3db751c4a2dad1878e Mon Sep 17 00:00:00 2001 From: kodonnell Date: Fri, 25 Nov 2016 11:17:54 +1300 Subject: [PATCH 1/5] WIP: playing round with directed candidates for #70 --- map-data/issue-70.osm.gz | Bin 0 -> 10399 bytes .../graphhopper/matching/GPXExtension.java | 30 +- .../com/graphhopper/matching/MapMatching.java | 1155 +++++++++-------- .../matching/MapMatching2Test.java | 22 + .../graphhopper/matching/MapMatchingTest.java | 4 +- matching-core/src/test/resources/issue-70.gpx | 25 + .../matching/http/MatchServlet.java | 1 + .../matching/http/MatchResultToJsonTest.java | 4 +- 8 files changed, 688 insertions(+), 553 deletions(-) create mode 100644 map-data/issue-70.osm.gz create mode 100644 matching-core/src/test/resources/issue-70.gpx diff --git a/map-data/issue-70.osm.gz b/map-data/issue-70.osm.gz new file mode 100644 index 0000000000000000000000000000000000000000..88b1a3d533247e2fe4d6b627dc4eccc8ffd9fca7 GIT binary patch literal 10399 zcmV;QC}7tgiwFpoN;g;l18H+}b!9C#FfMO%Z2-hw&2A*galY#*8a%B4b0+_P8aonf z32Urmtri~oU|j}HvX|^7**)lP&S-R!y$AW?gKoBE16sfju(G_yLWb7|OLxv2IkLK| zSW#J7%xq?|qy?m9HQ8TAMMiuP84>y6_kVtKJ$W-w8-giD+ZEo&AMV_gn$;Es*U(dFy^^2n){N=|tv)2>yOg=M{511m1Pqvq< z_41hTXRFuq^{e7W7eo<4v6_U&8$&pvPd=+WeQaRyK4%`dnQpU<|lv(?SbYPqRC zzW>bT`Re@i`Z+x&&yT+Moymu%tJ~%IW^%JwUe5rYkk6Q)f)J34`&lI<`%DvVcR!o` z{Qh%VHb1LB$B1%2d+z@9a&0nQykUo7eCoLJWxan6MK{rUd>GKRcS-Zszblezv;a&X%t}c?myo z-$#Yy0;JLa<=P4fKWI=2MHzGJpM1ETT}-ZC9G$J_v+ex+)#*Dx_2TG9KYjU=-722D zW%MpbXI+krUk;Pjvet5_kp_hO$mN)<%TWo-Vb-?a4u$_(`pD&QnRPiPemO)FXbzZ|9% zn7T(UC#xx-G=4e8Xx26bgeoreBbOs-*5$B-hW&vzkmbnvPzn|Tt?3dd@Y~sLLEH80v(FZ1 zuQrQ|rqp1(iHDN_$YV-RsG14}|1>y1{^X;NKl%9NZ^$8$l&7w`M)&f{8?-8k})L(-eoC=!)RS0f$+qZ*88pXO$Im9wj z8yhWE&}?C;3TZaAQ)b&7Bu2A=!%KxYyk(9?^kTInI&vz36)2$60#iT@c0Yi1RJn3V ztwOTW6ZLuMavz)7tTy3l+X7HCS5#HVs6yWdkTS1oR-~$n`6N+;b%t%FK@1_B)W?*|i0ah(tVAA&T2JC)Ft^*tw>i}ocP~tNZc}(;%1vk&9 zfS25}+>_Pm?RImun7mwFt?tbcgt-!_0cCE3wdfPkx|?>a;X3*<;?AX{%HG^#C170X zV>vA4(tsNVYd7+{0KTcQZ-6h7#(Nh#xI8-A1L|)S?|T>8aajgvWt$+zF&T?OKF7M4 zm#%KGMyEkaU=@Uck416?Cqx5SMLd%!f>mj4I&9YXtZRogKH@Kel{uS$q6E%$H}q<- zMyHXZu*yV+NF|V$Hde4^Hng|mAq_ z3$_ecpej^!7_66Es=*E=37!Hhr<|b!1T9_WI{-G?6chpr|G}I9*jG@pBdY=URU8~B zC1p40YQRPlm{MRnI?EL!UCUL2jgJn>fE7egWM++WHF5#4BbzQzTFebW)I4OmbaXsh z2&~d5A<8Ixpi2&GuqL<8CML$E3&4>Y0O#sPi4E9j)?Em!CfFK)rS)ic12(rBq=L1C zA+SKYxK}#72WxX%gG8_jHdq@hC&E9=X~0GY35CEK#*yjbmUvm125fYcPzbDLl4B1> zM99wC+}t1$tPxd~wFQo~yl>o~a?G*12+d~oeU=uu88L%83?LwrWs2_vv!6rC`B4CwN zoiYc~6EN<;+JvyC3|PfUN3t>qHo=1x0jpIB4%8L&omqFciKux1Z-Y-?Z% z3Tqe>eR+dD*pW9Fr!M4b^I78#4En%Q5?^;%3alU+>#Xp~JQ}bGtg8rEX1PLYztlO1K zi>ahjabFPg@y+%8iaG*V0R@7^ssq<{9IkP9knyAkr}{e-%_CKTnwo>gkx4Ul@%GyKP^VwMIIo zo9G^ag)mH+-i`HY5AqJ1Xo}f5@aZP zOv^81RSDLzr)=Qu$F~C)VSNe>2iO&{1(1&Eqa;(PR)`4Oa|lo)9n(iij;qIs6g=on z^{CMaxKBKX#L^%?)noc7C0(;XYwv?I^(t{2q+|Lh1;I1!$$+gN(o-|0k8%~$`0&U0 zRpSlPQGHZJo!b3T+V96`kdEo2xWP?w)V00$+f&PjTH!m5S%TTBt;283vd2uHFghi4 z%I`BWZ)R7w*Y9REP;i~JnmBYHoN%Sl*aTKJ?ezm4YAo&%DJO#>l~Zn$3_n3zpBRXu z`(M80LoK-Fk-9`1BGS|Zf)hZRMo{u11;c}llygM=L>iE$5tPhGISx)Jx77A0&;V%~ zLCKAj;jseTxZ;8U(lmmS7pXQ3Q+^6NJhY<{M)DQdQATLgAFT}yq)!29n&Hoj6s$8k zhXN}#VED(}QQ!z9lBpGjc+CthDFg~VL{Ei}RJx$IA1SRS;Iz(lVs?~a$ey}PZvbg^ zs4^Z>V^N_R5w1fdAk(3f^WsRgPy)Yw$-`D^U`MgiNL{88$<%6BeBb}}hY}OTWeN-h zP1vJ^OTUd}k924AqV@X69h1umAnuBRRyoVkVt zfA-lQCHR`|vjpw7Z*2XH4km#jSgY)a- zQt*#)^}fPQ#%Yj#wAx%XA+&KYc?+f#Qs9;)*LLrP&^63>%9j}?yyZ@$FNimtuYnmK zL1l-@u)Bvqk9i?!4KqF=kr^hH1hZ!%l{dFI2@t z?P*Gc1$TOURe5%UWT?XBTAb*RZpM$X&A!uJ{tl-^jo%AZ!yL`0J8O=LH3V_VJRhcY z?Z{~_UG^R?)i5QWvIO%^aDrJ7cST7*%!KO`;$Z4%-4ul5HZ9xJ!d=5m+vyo$Qc5VU z(6LhV@lCtuQscm6M5D}Nb@5K$7IH8X3W+kq zV2-An3=C4|r3>bD;M6d48>Cp6;L~-dd(^sV7Xg@QUMQzQk`hnru$l#TdSb|yQJR+H zGg&vokM@OZ70j{By8vg6tC|(lyCt0=BVn5Obb?0Xf87vTzq7f9IUvolFu@C< zs}n$5x#_i=0xVaWd9Unj0H&?34vA(+Oj&ec6%$za-tZl&?J-#NLLi= zn3X3|drZ^`D5#L_F0SgL0xbeQe5L`E71LfwWQM4N??cges5QI3m)Yb0<;R@2e3Lp}$`xA)egI)B} z4h`IRb5IH{1SKeTa>Jedt+t1orn@EJf)T^gtxEI*aMN_R3|u_IQC5>0190Q{Y9Y8_ z2k?YcpekNxMgup#I=dKLh05LmuJoF78o2QRbt$-7l6JG#K!$RKnZnA8kkJzE`p zcX>^9znU7FnXNIcAzT~g5BQdj(K32MnY$~d<Z0fI` ziQr0$;aZ3`{5kCVtCGgP^2QB0!PJOT+V}d2TE>&M`*;}(3Jaw z;HJiSTyxX8m;wG9nqry#E~?~^l#)*W66Z3|WobE!5A_|~>SpO6T!t>ki;hApZeP`g zcr9$z&7(4*M7R`Mi0{!@K;q^+dA!qwec({y$1KF=W#rg;+sS3yfK%=9j z{6MMU=+a5BsoFcitAWP1*=GkTbd`H(7rgXbN!I|4pZb&^sMG=jh1kdoHfo^J?7aX` z*rcc*ToUcoG&VrT(@_E8CB6n;aO0glH9(_d`2s+RA)O;f;@vph0F9r~l^v)OOk*9T z?))YIG&+7S0MuG^BM$-c;wQ=WK;zHz3IH`0T{BCBVqQI719VLDq%}HkCW09oa-s2M zm?i9}F&#U~yxc+qG&=iS!j5u-B&yu~F(IJQdFK+F2_KFVC4`^Ct$~imp1|NM++ruK z_D)J0pd)%FH-7@ZdLq3*bq|!qA2~#x>yZSBT8hq|-Ru_`*i0-w9bN#a5ojPS*iqq) z`fY$_cA-f?1#Sc+Qt>d*%q}zyC}S9?;34+J;*;M6fO3s1@1>&N%%=uuW*3?QRO!yC zvowr^I@X3plFCDXrVRct2AzAruBUzjOWgn*eU*EGvIApKJrr^CNQ09f;xlOZfif*| zcZiT)!9fFb#F;OOXgs4p3K1Cc2|(kMeED^h7|cgCbN43&8dV4QXz|UYI)_J+2Rmxj z&|L(jfhxmrqm`>B>C!v_>)~Q|e`K5;72I7ILcQisHzxX+uoGgYOtqUoe=)!Q{A%^4 zY5tUy3(`J56wW-N+mr-^cing2AdPnRC6G#ivQJbne-dqjG}_yjK)OGpy+^7XMQ$Gt z&d$%*)3e3)UFiI<`oQH1zH)hcb2{H0NYb9qY_{w9e7ifZ{O#S>cmMnTi@V?5{rBBB zcYnD1dUE&W`(MJ3{?7eN=mfHs+0aOjCy<%Or+LD-k?49Zk%gQV|*e{}I4Et5d zt>zhICd$ezI^|SE&#F@Yw#)>{;WuqrnMG&6is+ff?SEj+1j`^ZRaR!XIj>IAGhEz9 zr7$+f-lT;IT5kHCCN$fr=7pEo;{-gJ6eeJ~iGGqyIOATqdk)fSiposnCi+P-lMG!Q z<*LF}4$?}?vR09s>8A-zGi>Fp>pspPw6YXTkMNF2$a}mJpWDy!GL+9&4>7jewSaG2-hHv-~ic|k0*FDnc zc7!5s+~B(`g=OhT`Tv8Zl@zQ*NQ);)3y*IE)8at&Ur0^uS zw2bV`=K|-DLs}2^wLlBAzMZu-GfgR;f zu@072QY4anqZxdorX)c0pCGk)u(Xmwko-m*^+$5n@lEB+Sv65^unnp$+aCs>YZg$GZv4i?v_ zY~3n6^+9si-jzXIrGlHGyvY-Y%P3q61CB9^xJpGgYD~T^C^H>#*}mh1(D*R|MfMt=6$q5r*epDuRD?4$MCb{$ z%c8s3shjGS!;-U7oL!B|*%_YY4dNm!i{mRo+JObeccoV165Ja%+Y%R3Y2tDzVze2l zyQ!}4EWja6+XuOkT0FCpTJ25sXpp9s8Ol>+wWed=!PZjTT|c8-t9Z80Hhi0{^>MSMGST_?d$Z0W)D@iVR=4T&p&f9 zm^55!u4)_GeZWoNfeb#7p$9tjpf3=7(C)onZMJ9d?)OK_qMO>eTwGj&#H+{a`DSrG zUv3w(Yy6@6>4*OUHt{R4iQl~chr2)AedT|wo`Lw|_3ZP78#nHMqMmd3@ZV}rJFNcT zpT4}B{n_Mxtsg!I>_}-KkTjMJ7Bi)f0|TvL=~~ko)Y*Nb&3M3`2bM4?`X21&ANK!- z32COMvaNw>od?ylPReQE7rgIoPQSaVyz4Gi#{cg915YCLpR#??`%~-qubp1r{g2bh z$=z?^|NbYa=&$a+zWWdO-`@-={a>tS%d6SsuV?3rv-z9F*^u@h@Gb4_(E6%B{v&+X zAAj}0#$b$-T_6}&sZg0@DZEP%n485)^$FEePN(9&QsHKH0sLyY*>4F&+`zuB9aYr; ziXL;hdmwzp-#_rp<^F*RJ@9wL{(-+8_YZjB0rz*^{z2%=dFVTMXdS#89Nx^AwccN> zXK&`O-p$tMA(j2NyKg~nzMb6t9<=J)yWhS4XJD-#+)tAI?tmg5_Ph6*_{r+cYI3n& zonC)FpL|fMVCPrX^XtV0s95#!v&;2j1KV#I&{In_ki+;~E_G8l-LZ$Kr@&4aIiYIG zNV~h`hpXjc^67eZwsfIDrK_B3Bh^vIBr(pwfLq^yx(9ClVDAI?jjmSI4~%a?JKwPv z*!66A4qz*JeKuR)xPKoOS9C77w787tR_X`c-PJq@ipz!`h{uX6IyqTZT+|Kc4X8xF zxZpSX1q=`R<=Fa;gT)nHGMIHRdcRykN@2UarFG#$;6X_7tUXj<|KHw~^hR=9;d_2X z|AC3Q6qgc$9GpQ6!l-}IgAg>69$92}HD9snu_b#HK!7f4?&RCwSEPH_qwem3y-}Y<@33LP)jJ#R zYnbE9x6Vf6GJ{sxgQi`8YI^m&J!>9~D8hoC`}(in{qv`zMpz^d@>}-AyNdblz#vs~$NBu^`*xHh8Xo1zYA{?qs`T{xzD%CA^jX= z(Z}9d1XuAdm)~D~>}~T2;=O-<)|@o6H&S$#1QsIOA{OITJTebGI)R(p;I z`-|;je}CMZeYPoWh5JVpHkctmAA6hp4?d3#|to7-)q-QQMx@Vrma4VnIXSHDbx9l+KN6R!223J_T#UX_<9mwsIL5HTNp+Wh1)(zPHc^7v!=`*}POXy_R)`Gk#8w%FwJ*f?cQZesH>6&0C*V^Oxqfv^S>ZnhTuD9oZoTf)F2uI?!sg(eFfUzNZ)c1tm=S~XtFIO$L z5;zKjOsRrFX!4IOcrHLt{M|M-hK19BL_tO~3PohYYq{VBcFAHvt}AmJDDlh7Z@fwA&B;&UpReJc zQNi=TxiQV`?ZrVeDt-!j-tyJ@-IF45^h>Ep;z-b*MJpp4dmGL=B$GYR=bIVGimzP2 z>FUG$S-Xk3^*?FOns(%@3wlo9PJMCV`+so}2EfN`gf%6%Qwnz#o4-vG`5y3R_gyLn zgwRF;{c`C{yI_1<=atLM@ecF&kd1AOskzib=TB_aHjfTR*F5&2-jCpKtzRTXGA}CG z>RPM24c_F?p@y@=vG8=+S?8>LKQve@H+{RlDRMAj)wb3Z0s^cr|4*qPUbb%$nkKZs zhJ0v&(+^?r8d><^ECz53AvJ)`BDNShRzC*n%C)GNW=MF)7DHqBg6ujF=|q}BmPjfn zP*=GY2q?8K$O$8yQjW9AwZOJZoYhcQt^ntvVnJ>b^Mj|v(0i~mQ;<6koN|iPo}&`1 z$z#C~iTZ-v$}t!x7GxL)Q%U3~iQsgbXKscgGgYR5qXqV!@yo<=(oO6=QRrGwJE4h!49|g`Es8gK(2&D7Wm;fHj|EvkHAZqJdC8Y)vJ-BZ8#LVC znxCG(bF?^|&mOj?XNU6<+r4;*z6GC-GwQHO26kU%$KJ9{>^{pBa0asEf%N!2&h~}e zQ>-6LI6Ph)EhF~Xy|}~^#NQfqhGal2KvE+XTqnQgK8UE5WMj$2p-}E5*Xf}oE5^Ep#4V$gW+Q63Z+UzfyE|O{2Buz$tOM_8o;-QZ;>#Jy(<`A zIjcAtJkDwW_#RFW!`QT(xe%EFBr_wU7JB7Du z7mvqNGxUD@qOITgxL6ZhT5#XU9~~0Kn%Y6EA!{!dAcHFusGG=)jySHVR3jw>c0tAj z7aU8q6gctnKivDmMMOXMUmpFV!+5zMX`amAXkC1McE4@jJ6|nMroZMR50HG?heyen zkJ^lwk2@XtE=ZARsAZ*gPWxm9PO0k_0AsU?`(k|8`Z(3`!-t(QP*k8MI4Z_W=F zaE>en(81!XRDx&bLez?LgPz4cS14C%;)D> z{EuWB6g;!|`+(3>#z(2~H+NkF^kH!PJu_AGVOjhgrS`kuRc2fSVT-?K=7Rwy;rHQN z1GKV37f;`d*^Eg)r^_m2we$Ae=vubG!rFBvvp`Z7lcNU zd<2XvOBKddlfMN8IR*Zy9x%2z^WP$uj|=4QrijRK-T8u+n#zAxtJeqq-p=dPFH5HTEp>cN@O_h%p!;L$P#&_IF#djFZJt4BwXqDZyVLdfXFR!|d z^yvYY8w^dhIq%(ed7Lv6)?BG%r$uKES53PZ)}`s?RqgV8)wO=-{A{~uwe-LLYWW15 ztR}hg#Ld5RFMJ1`2#CCo+Q-ewuNOyA zYq_*{yq_?)<+>J3u?;bepAeiWh$ds#eDx0KnD;k#!Ay)IcWP$VqXFGbw~BLAMXll# zE80f5_r7Z3K0hZ_uZ^}KX$^6gwdSCilrHLA(Q3mg!FM+maW3vPIAi>E;^gZ74w<6O zHM16*Vad@D-H zylW;42kRDAmkDa2g}c;E6$)y9M5<6JID@#V#m15(js;r*Gc0RWRW;}lq05dLY<6a} zt_z6+>|EhCa;F*Ah38dEcMCHwYb80Tgo>5XVH~TtSw~X5teIuD(!A6<>4-xYau|%I z1uGoOJwR-YWrbsP&5SA3#tK|er3$$j91_??#>?o}a`2rtdW%Sh?y+H6$|)c>D(>Lh zMO>+C;L5sitgaQS%GL$i1dN&9A=7ryRtoI=LZ(!_HvTG)()lSL1u8PfHIuC7Wi$v{ z-h%%w3pGmDswz8UV>y_(g@*-V%^|^d;;d5lNeTt4V!E`I=El7YSSp_UorYyYIeIsY z6-w*KCbqO$$0wyLYeKMN$@?i={e`L-pNj=slaPx zWH+&^PITltc}sSPtpGlc_gw45#oO-0Lx^O^9+FTS!X+@=% zuC}JGQz@-#YK*I^ZHa^q3=O-&smqi#uR**~0~`|k6R-?7rXgYKRuf6F(zceGyim}H zJ!^ByWwai_Kn!j~pNc3LyQz>xi)O6uSze^-03PWfg=ERW;%wENJn9lAaFjny60hqF z&V%G;vy=Ji{bhUD;UymKo%oDOisOo@owV^LwbL#M>6>-g`-{WH>HJ{P_zGw3p2rdY z(?htvbKmy=*8z%Wug~DuXYNXe+2xnvX`_4wNLJwORwJK#I|47y09Dt@02!sNZ~zUW z3;~NBJa(cubDhgxtbIzEDdwCpD&L0c#JRZdh^(S6G}IN^(mU z6H*C~wnSS%IO~q=AXzv~LG>GiXwJ<#M2M^ zLY%sBF(Z(lwA!4h8r%jljKWuEmPjo=%dP0vN&rq!xq`tiGDCHzy6T$jlzHw{e+j-x0Ws5)*~02fR8K)Vv``#XktG<4Nwv2Pf|&Bs zw!jZl+me7DCj0J#9j5p$0XdKkR8mPj#VFA>v4C8*P)Sn~DS;!AcAW|cMF6C!fM$X` zn%egSazvX9)$mP0MiiPcRErQnTc)Zfn3l=tNFU5Hwa^5`GEHDx@RcdynBZ0@)fqj& zo*-2w_9cO+Oe*&+Yzkp2eNf7jU6PUU6&WXJEI9+_>tgL)))ZG1flnz;OA1Di;%oZ7lm zW9R0Z8#WHw7vDEeP8PG%^VRJ2S7H7K4Q^EeF-aTW9K?jY+&;$SXRj>Jj(`4>{{dl9 J1am}E0RV$LP#gdN literal 0 HcmV?d00001 diff --git a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java index 1c357c8e..da0dcb2a 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java +++ b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java @@ -17,6 +17,9 @@ */ package com.graphhopper.matching; +import java.util.List; + +import com.graphhopper.routing.VirtualEdgeIteratorState; import com.graphhopper.storage.index.QueryResult; import com.graphhopper.util.GPXEntry; @@ -27,19 +30,30 @@ public class GPXExtension { final GPXEntry entry; final QueryResult queryResult; - final int gpxListIndex; + private boolean directed; + public VirtualEdgeIteratorState incomingVirtualEdge; + public VirtualEdgeIteratorState outgoingVirtualEdge; - public GPXExtension(GPXEntry entry, QueryResult queryResult, int gpxListIndex) { - this.entry = entry; + public GPXExtension(GPXEntry entry, QueryResult queryResult) { + this.entry = entry; this.queryResult = queryResult; - this.gpxListIndex = gpxListIndex; + this.directed = false; + } + + public GPXExtension(GPXEntry entry, QueryResult queryResult, VirtualEdgeIteratorState incomingVirtualEdge, VirtualEdgeIteratorState outgoingVirtualEdge) { + this(entry, queryResult); + this.incomingVirtualEdge = incomingVirtualEdge; + this.outgoingVirtualEdge = outgoingVirtualEdge; + this.directed = true; } + public boolean isDirected() { + return directed; + } + @Override public String toString() { - return "entry:" + entry - + ", query distance:" + queryResult.getQueryDistance() - + ", gpxListIndex:" + gpxListIndex; + return "entry:" + entry + ", query distance:" + queryResult.getQueryDistance(); } public QueryResult getQueryResult() { @@ -49,4 +63,4 @@ public QueryResult getQueryResult() { public GPXEntry getEntry() { return entry; } -} +} \ No newline at end of file diff --git a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java index 08387cbc..18421316 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java +++ b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java @@ -1,541 +1,614 @@ -/* - * Licensed to GraphHopper GmbH under one or more contributor - * license agreements. See the NOTICE file distributed with this work for - * additional information regarding copyright ownership. - * - * GraphHopper GmbH licenses this file to you under the Apache License, - * Version 2.0 (the "License"); you may not use this file except in - * compliance with the License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package com.graphhopper.matching; - -import com.graphhopper.GraphHopper; -import com.graphhopper.matching.util.HmmProbabilities; -import com.graphhopper.matching.util.TimeStep; -import com.graphhopper.routing.weighting.Weighting; -import com.bmw.hmm.SequenceState; -import com.bmw.hmm.ViterbiAlgorithm; -import com.graphhopper.routing.AlgorithmOptions; -import com.graphhopper.routing.Path; -import com.graphhopper.routing.QueryGraph; -import com.graphhopper.routing.RoutingAlgorithm; -import com.graphhopper.routing.RoutingAlgorithmFactory; -import com.graphhopper.routing.ch.CHAlgoFactoryDecorator; -import com.graphhopper.routing.ch.PrepareContractionHierarchies; -import com.graphhopper.routing.util.*; -import com.graphhopper.routing.weighting.FastestWeighting; -import com.graphhopper.storage.CHGraph; -import com.graphhopper.storage.Graph; -import com.graphhopper.storage.index.LocationIndexTree; -import com.graphhopper.storage.index.QueryResult; -import com.graphhopper.util.*; -import com.graphhopper.util.shapes.GHPoint; -import java.util.ArrayList; -import java.util.Collection; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.Map.Entry; - -/** - * This class matches real world GPX entries to the digital road network stored - * in GraphHopper. The Viterbi algorithm is used to compute the most likely - * sequence of map matching candidates. The Viterbi algorithm takes into account - * the distance between GPX entries and map matching candidates as well as the - * routing distances between consecutive map matching candidates. - * - *

- * See http://en.wikipedia.org/wiki/Map_matching and Newson, Paul, and John - * Krumm. "Hidden Markov map matching through noise and sparseness." Proceedings - * of the 17th ACM SIGSPATIAL International Conference on Advances in Geographic - * Information Systems. ACM, 2009. - * - * @author Peter Karich - * @author Michael Zilske - * @author Stefan Holder - * @author kodonnell - */ -public class MapMatching { - - private final Graph routingGraph; - private final LocationIndexMatch locationIndex; - private double measurementErrorSigma = 50.0; - private double transitionProbabilityBeta = 0.00959442; - private final int nodeCount; - private DistanceCalc distanceCalc = new DistancePlaneProjection(); - private final RoutingAlgorithmFactory algoFactory; - private final AlgorithmOptions algoOptions; - - public MapMatching(GraphHopper hopper, AlgorithmOptions algoOptions) { - this.locationIndex = new LocationIndexMatch(hopper.getGraphHopperStorage(), - (LocationIndexTree) hopper.getLocationIndex()); - - // create hints from algoOptions, so we can create the algorithm factory - HintsMap hints = new HintsMap(); - for (Entry entry : algoOptions.getHints().toMap().entrySet()) { - hints.put(entry.getKey(), entry.getValue()); - } - - // default is non-CH - if (!hints.has(Parameters.CH.DISABLE)) { - hints.put(Parameters.CH.DISABLE, true); - } - - // TODO ugly workaround, duplicate data: hints can have 'vehicle' but algoOptions.weighting too!? - // Similar problem in GraphHopper class - String vehicle = hints.getVehicle(); - if (vehicle.isEmpty()) { - if (algoOptions.hasWeighting()) { - vehicle = algoOptions.getWeighting().getFlagEncoder().toString(); - } else { - vehicle = hopper.getEncodingManager().fetchEdgeEncoders().get(0).toString(); - } - hints.setVehicle(vehicle); - } - - if (!hopper.getEncodingManager().supports(vehicle)) { - throw new IllegalArgumentException("Vehicle " + vehicle + " unsupported. " - + "Supported are: " + hopper.getEncodingManager()); - } - - algoFactory = hopper.getAlgorithmFactory(hints); - - Weighting weighting = null; - CHAlgoFactoryDecorator chFactoryDecorator = hopper.getCHFactoryDecorator(); - boolean forceFlexibleMode = hints.getBool(Parameters.CH.DISABLE, false); - if (chFactoryDecorator.isEnabled() && !forceFlexibleMode) { - if (!(algoFactory instanceof PrepareContractionHierarchies)) { - throw new IllegalStateException("Although CH was enabled a non-CH algorithm factory was returned " + algoFactory); - } - - weighting = ((PrepareContractionHierarchies) algoFactory).getWeighting(); - this.routingGraph = hopper.getGraphHopperStorage().getGraph(CHGraph.class, weighting); - } else { - weighting = algoOptions.hasWeighting() - ? algoOptions.getWeighting() - : new FastestWeighting(hopper.getEncodingManager().getEncoder(vehicle), algoOptions.getHints()); - this.routingGraph = hopper.getGraphHopperStorage(); - } - - this.algoOptions = AlgorithmOptions.start(algoOptions).weighting(weighting).build(); - this.nodeCount = routingGraph.getNodes(); - } - - public void setDistanceCalc(DistanceCalc distanceCalc) { - this.distanceCalc = distanceCalc; - } - - /** - * Beta parameter of the exponential distribution for modeling transition - * probabilities. - */ - public void setTransitionProbabilityBeta(double transitionProbabilityBeta) { - this.transitionProbabilityBeta = transitionProbabilityBeta; - } - - /** - * Standard deviation of the normal distribution [m] used for modeling the - * GPS error. - */ - public void setMeasurementErrorSigma(double measurementErrorSigma) { - this.measurementErrorSigma = measurementErrorSigma; - } - - /** - * This method does the actual map matching. - *

- * @param gpxList the input list with GPX points which should match to edges - * of the graph specified in the constructor - */ - public MatchResult doWork(List gpxList) { - if (gpxList.size() < 2) { - throw new IllegalArgumentException("Too few coordinates in input file (" - + gpxList.size() + "). Correct format?"); - } - - final EdgeFilter edgeFilter = new DefaultEdgeFilter(algoOptions.getWeighting().getFlagEncoder()); - - // Compute all candidates first. - // TODO: Generate candidates on-the-fly within computeViterbiSequence() if this does not - // degrade performance. - final List allCandidates = new ArrayList<>(); - List> timeSteps = createTimeSteps(gpxList, - edgeFilter, allCandidates); - - if (allCandidates.size() < 2) { - throw new IllegalArgumentException("Too few matching coordinates (" - + allCandidates.size() + "). Wrong region imported?"); - } - if (timeSteps.size() < 2) { - throw new IllegalStateException("Coordinates produced too few time steps " - + timeSteps.size() + ", gpxList:" + gpxList.size()); - } - - final QueryGraph queryGraph = new QueryGraph(routingGraph).setUseEdgeExplorerCache(true); - queryGraph.lookup(allCandidates); - - List> seq = computeViterbiSequence(timeSteps, - gpxList, queryGraph); - - final EdgeExplorer explorer = queryGraph.createEdgeExplorer(edgeFilter); - MatchResult matchResult = computeMatchResult(seq, gpxList, allCandidates, explorer); - - return matchResult; - } - - /** - * Creates TimeSteps for the GPX entries but does not create emission or - * transition probabilities. - * - * @param outAllCandidates output parameter for all candidates, must be an - * empty list. - */ - private List> createTimeSteps(List gpxList, - EdgeFilter edgeFilter, List outAllCandidates) { - int indexGPX = 0; - TimeStep prevTimeStep = null; - final List> timeSteps = new ArrayList<>(); - - for (GPXEntry gpxEntry : gpxList) { - if (prevTimeStep == null - || distanceCalc.calcDist( - prevTimeStep.observation.getLat(), prevTimeStep.observation.getLon(), - gpxEntry.getLat(), gpxEntry.getLon()) > 2 * measurementErrorSigma - // always include last point - || indexGPX == gpxList.size() - 1) { - final List queryResults = locationIndex.findNClosest( - gpxEntry.lat, gpxEntry.lon, - edgeFilter, measurementErrorSigma); - outAllCandidates.addAll(queryResults); - final List candidates = new ArrayList<>(); - for (QueryResult candidate : queryResults) { - candidates.add(new GPXExtension(gpxEntry, candidate, indexGPX)); - } - final TimeStep timeStep - = new TimeStep<>(gpxEntry, candidates); - timeSteps.add(timeStep); - prevTimeStep = timeStep; - } - indexGPX++; - } - return timeSteps; - } - - private List> computeViterbiSequence( - List> timeSteps, List gpxList, - final QueryGraph queryGraph) { - final HmmProbabilities probabilities - = new HmmProbabilities(measurementErrorSigma, transitionProbabilityBeta); - final ViterbiAlgorithm viterbi = new ViterbiAlgorithm<>(); - - int timeStepCounter = 0; - TimeStep prevTimeStep = null; - for (TimeStep timeStep : timeSteps) { - computeEmissionProbabilities(timeStep, probabilities); - - if (prevTimeStep == null) { - viterbi.startWithInitialObservation(timeStep.observation, timeStep.candidates, - timeStep.emissionLogProbabilities); - } else { - computeTransitionProbabilities(prevTimeStep, timeStep, probabilities, queryGraph); - viterbi.nextStep(timeStep.observation, timeStep.candidates, - timeStep.emissionLogProbabilities, timeStep.transitionLogProbabilities, - timeStep.roadPaths); - } - if (viterbi.isBroken()) { - String likelyReasonStr = ""; - if (prevTimeStep != null) { - GPXEntry prevGPXE = prevTimeStep.observation; - GPXEntry gpxe = timeStep.observation; - double dist = distanceCalc.calcDist(prevGPXE.lat, prevGPXE.lon, - gpxe.lat, gpxe.lon); - if (dist > 2000) { - likelyReasonStr = "Too long distance to previous measurement? " - + Math.round(dist) + "m, "; - } - } - - throw new RuntimeException("Sequence is broken for submitted track at time step " - + timeStepCounter + " (" + gpxList.size() + " points). " + likelyReasonStr - + "observation:" + timeStep.observation + ", " - + timeStep.candidates.size() + " candidates: " + getSnappedCandidates(timeStep.candidates) - + ". If a match is expected consider increasing max_visited_nodes."); - } - - timeStepCounter++; - prevTimeStep = timeStep; - } - - return viterbi.computeMostLikelySequence(); - } - - private void computeEmissionProbabilities(TimeStep timeStep, - HmmProbabilities probabilities) { - for (GPXExtension candidate : timeStep.candidates) { - // road distance difference in meters - final double distance = candidate.getQueryResult().getQueryDistance(); - timeStep.addEmissionLogProbability(candidate, - probabilities.emissionLogProbability(distance)); - } - } - - private void computeTransitionProbabilities(TimeStep prevTimeStep, - TimeStep timeStep, - HmmProbabilities probabilities, - QueryGraph queryGraph) { - final double linearDistance = distanceCalc.calcDist(prevTimeStep.observation.lat, - prevTimeStep.observation.lon, timeStep.observation.lat, timeStep.observation.lon); - - // time difference in seconds - final double timeDiff - = (timeStep.observation.getTime() - prevTimeStep.observation.getTime()) / 1000.0; - - for (GPXExtension from : prevTimeStep.candidates) { - for (GPXExtension to : timeStep.candidates) { - RoutingAlgorithm algo = algoFactory.createAlgo(queryGraph, algoOptions); - // System.out.println("algo " + algo.getName()); - final Path path = algo.calcPath(from.getQueryResult().getClosestNode(), - to.getQueryResult().getClosestNode()); - if (path.isFound()) { - timeStep.addRoadPath(from, to, path); - final double transitionLogProbability = probabilities - .transitionLogProbability(path.getDistance(), linearDistance, timeDiff); - timeStep.addTransitionLogProbability(from, to, transitionLogProbability); - } - } - } - } - - private MatchResult computeMatchResult(List> seq, - List gpxList, List allCandidates, - EdgeExplorer explorer) { - // every virtual edge maps to its real edge where the orientation is already correct! - // TODO use traversal key instead of string! - final Map virtualEdgesMap = new HashMap<>(); - for (QueryResult candidate : allCandidates) { - fillVirtualEdges(virtualEdgesMap, explorer, candidate); - } - - MatchResult matchResult = computeMatchedEdges(seq, virtualEdgesMap); - computeGpxStats(gpxList, matchResult); - - return matchResult; - } - - private MatchResult computeMatchedEdges(List> seq, - Map virtualEdgesMap) { - List edgeMatches = new ArrayList<>(); - double distance = 0.0; - long time = 0; - EdgeIteratorState currentEdge = null; - List gpxExtensions = new ArrayList<>(); - GPXExtension queryResult = seq.get(0).state; - gpxExtensions.add(queryResult); - for (int j = 1; j < seq.size(); j++) { - queryResult = seq.get(j).state; - Path path = seq.get(j).transitionDescriptor; - distance += path.getDistance(); - time += path.getTime(); - for (EdgeIteratorState edgeIteratorState : path.calcEdges()) { - EdgeIteratorState directedRealEdge = resolveToRealEdge(virtualEdgesMap, - edgeIteratorState); - if (directedRealEdge == null) { - throw new RuntimeException("Did not find real edge for " - + edgeIteratorState.getEdge()); - } - if (currentEdge == null || !equalEdges(directedRealEdge, currentEdge)) { - if (currentEdge != null) { - EdgeMatch edgeMatch = new EdgeMatch(currentEdge, gpxExtensions); - edgeMatches.add(edgeMatch); - gpxExtensions = new ArrayList<>(); - } - currentEdge = directedRealEdge; - } - } - gpxExtensions.add(queryResult); - } - if (edgeMatches.isEmpty()) { - throw new IllegalStateException( - "No edge matches found for path. Too short? Sequence size " + seq.size()); - } - EdgeMatch lastEdgeMatch = edgeMatches.get(edgeMatches.size() - 1); - if (!gpxExtensions.isEmpty() && !equalEdges(currentEdge, lastEdgeMatch.getEdgeState())) { - edgeMatches.add(new EdgeMatch(currentEdge, gpxExtensions)); - } else { - lastEdgeMatch.getGpxExtensions().addAll(gpxExtensions); - } - MatchResult matchResult = new MatchResult(edgeMatches); - matchResult.setMatchMillis(time); - matchResult.setMatchLength(distance); - return matchResult; - } - - /** - * Calculate GPX stats to determine quality of matching. - */ - private void computeGpxStats(List gpxList, MatchResult matchResult) { - double gpxLength = 0; - GPXEntry prevEntry = gpxList.get(0); - for (int i = 1; i < gpxList.size(); i++) { - GPXEntry entry = gpxList.get(i); - gpxLength += distanceCalc.calcDist(prevEntry.lat, prevEntry.lon, entry.lat, entry.lon); - prevEntry = entry; - } - - long gpxMillis = gpxList.get(gpxList.size() - 1).getTime() - gpxList.get(0).getTime(); - matchResult.setGPXEntriesMillis(gpxMillis); - matchResult.setGPXEntriesLength(gpxLength); - } - - private boolean equalEdges(EdgeIteratorState edge1, EdgeIteratorState edge2) { - return edge1.getEdge() == edge2.getEdge() - && edge1.getBaseNode() == edge2.getBaseNode() - && edge1.getAdjNode() == edge2.getAdjNode(); - } - - private EdgeIteratorState resolveToRealEdge(Map virtualEdgesMap, - EdgeIteratorState edgeIteratorState) { - if (isVirtualNode(edgeIteratorState.getBaseNode()) - || isVirtualNode(edgeIteratorState.getAdjNode())) { - return virtualEdgesMap.get(virtualEdgesMapKey(edgeIteratorState)); - } else { - return edgeIteratorState; - } - } - - private boolean isVirtualNode(int node) { - return node >= nodeCount; - } - - /** - * Fills the minFactorMap with weights for the virtual edges. - */ - private void fillVirtualEdges(Map virtualEdgesMap, - EdgeExplorer explorer, QueryResult qr) { - if (isVirtualNode(qr.getClosestNode())) { - EdgeIterator iter = explorer.setBaseNode(qr.getClosestNode()); - while (iter.next()) { - int node = traverseToClosestRealAdj(explorer, iter); - if (node == qr.getClosestEdge().getAdjNode()) { - virtualEdgesMap.put(virtualEdgesMapKey(iter), - qr.getClosestEdge().detach(false)); - virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), - qr.getClosestEdge().detach(true)); - } else if (node == qr.getClosestEdge().getBaseNode()) { - virtualEdgesMap.put(virtualEdgesMapKey(iter), qr.getClosestEdge().detach(true)); - virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), - qr.getClosestEdge().detach(false)); - } else { - throw new RuntimeException(); - } - } - } - } - - private String virtualEdgesMapKey(EdgeIteratorState iter) { - return iter.getBaseNode() + "-" + iter.getEdge() + "-" + iter.getAdjNode(); - } - - private String reverseVirtualEdgesMapKey(EdgeIteratorState iter) { - return iter.getAdjNode() + "-" + iter.getEdge() + "-" + iter.getBaseNode(); - } - - private int traverseToClosestRealAdj(EdgeExplorer explorer, EdgeIteratorState edge) { - if (!isVirtualNode(edge.getAdjNode())) { - return edge.getAdjNode(); - } - - EdgeIterator iter = explorer.setBaseNode(edge.getAdjNode()); - while (iter.next()) { - if (iter.getAdjNode() != edge.getBaseNode()) { - return traverseToClosestRealAdj(explorer, iter); - } - } - throw new IllegalStateException("Cannot find adjacent edge " + edge); - } - - private String getSnappedCandidates(Collection candidates) { - String str = ""; - for (GPXExtension gpxe : candidates) { - if (!str.isEmpty()) { - str += ", "; - } - str += "distance: " + gpxe.queryResult.getQueryDistance() + " to " - + gpxe.queryResult.getSnappedPoint(); - } - return "[" + str + "]"; - } - - private void printMinDistances(List> timeSteps) { - TimeStep prevStep = null; - int index = 0; - for (TimeStep ts : timeSteps) { - if (prevStep != null) { - double dist = distanceCalc.calcDist( - prevStep.observation.lat, prevStep.observation.lon, - ts.observation.lat, ts.observation.lon); - double minCand = Double.POSITIVE_INFINITY; - for (GPXExtension prevGPXE : prevStep.candidates) { - for (GPXExtension gpxe : ts.candidates) { - GHPoint psp = prevGPXE.queryResult.getSnappedPoint(); - GHPoint sp = gpxe.queryResult.getSnappedPoint(); - double tmpDist = distanceCalc.calcDist(psp.lat, psp.lon, sp.lat, sp.lon); - if (tmpDist < minCand) { - minCand = tmpDist; - } - } - } - System.out.println(index + ": " + Math.round(dist) + "m, minimum candidate: " - + Math.round(minCand) + "m"); - index++; - } - - prevStep = ts; - } - } - - // TODO: Make setFromNode and processEdge public in Path and then remove this. - private static class MyPath extends Path { - - public MyPath(Graph graph, Weighting weighting) { - super(graph, weighting); - } - - @Override - public Path setFromNode(int from) { - return super.setFromNode(from); - } - - @Override - public void processEdge(int edgeId, int adjNode, int prevEdgeId) { - super.processEdge(edgeId, adjNode, prevEdgeId); - } - } - - public Path calcPath(MatchResult mr) { - MyPath p = new MyPath(routingGraph, algoOptions.getWeighting()); - if (!mr.getEdgeMatches().isEmpty()) { - int prevEdge = EdgeIterator.NO_EDGE; - p.setFromNode(mr.getEdgeMatches().get(0).getEdgeState().getBaseNode()); - for (EdgeMatch em : mr.getEdgeMatches()) { - p.processEdge(em.getEdgeState().getEdge(), em.getEdgeState().getAdjNode(), prevEdge); - prevEdge = em.getEdgeState().getEdge(); - } - - // TODO p.setWeight(weight); - p.setFound(true); - - return p; - } else { - return p; - } - } -} +/* + * Licensed to GraphHopper GmbH under one or more contributor + * license agreements. See the NOTICE file distributed with this work for + * additional information regarding copyright ownership. + * + * GraphHopper GmbH licenses this file to you under the Apache License, + * Version 2.0 (the "License"); you may not use this file except in + * compliance with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package com.graphhopper.matching; + +import com.graphhopper.routing.VirtualEdgeIteratorState; +import com.graphhopper.GraphHopper; +import com.graphhopper.matching.util.HmmProbabilities; +import com.graphhopper.matching.util.TimeStep; +import com.graphhopper.routing.weighting.Weighting; +import com.bmw.hmm.SequenceState; +import com.bmw.hmm.ViterbiAlgorithm; +import com.graphhopper.routing.AlgorithmOptions; +import com.graphhopper.routing.Path; +import com.graphhopper.routing.QueryGraph; +import com.graphhopper.routing.RoutingAlgorithm; +import com.graphhopper.routing.RoutingAlgorithmFactory; +import com.graphhopper.routing.ch.CHAlgoFactoryDecorator; +import com.graphhopper.routing.ch.PrepareContractionHierarchies; +import com.graphhopper.routing.util.*; +import com.graphhopper.routing.weighting.FastestWeighting; +import com.graphhopper.storage.CHGraph; +import com.graphhopper.storage.Graph; +import com.graphhopper.storage.index.LocationIndexTree; +import com.graphhopper.storage.index.QueryResult; +import com.graphhopper.util.*; +import com.graphhopper.util.shapes.GHPoint; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Map.Entry; + +/** + * This class matches real world GPX entries to the digital road network stored + * in GraphHopper. The Viterbi algorithm is used to compute the most likely + * sequence of map matching candidates. The Viterbi algorithm takes into account + * the distance between GPX entries and map matching candidates as well as the + * routing distances between consecutive map matching candidates. + * + *

+ * See http://en.wikipedia.org/wiki/Map_matching and Newson, Paul, and John + * Krumm. "Hidden Markov map matching through noise and sparseness." Proceedings + * of the 17th ACM SIGSPATIAL International Conference on Advances in Geographic + * Information Systems. ACM, 2009. + * + * @author Peter Karich + * @author Michael Zilske + * @author Stefan Holder + * @author kodonnell + */ +public class MapMatching { + + private final Graph routingGraph; + private final LocationIndexMatch locationIndex; + private double measurementErrorSigma = 50.0; + private double transitionProbabilityBeta = 0.00959442; + private final int nodeCount; + private DistanceCalc distanceCalc = new DistancePlaneProjection(); + private final RoutingAlgorithmFactory algoFactory; + private final AlgorithmOptions algoOptions; + + public MapMatching(GraphHopper hopper, AlgorithmOptions algoOptions) { + this.locationIndex = new LocationIndexMatch(hopper.getGraphHopperStorage(), + (LocationIndexTree) hopper.getLocationIndex()); + + // create hints from algoOptions, so we can create the algorithm factory + HintsMap hints = new HintsMap(); + for (Entry entry : algoOptions.getHints().toMap().entrySet()) { + hints.put(entry.getKey(), entry.getValue()); + } + + // default is non-CH + if (!hints.has(Parameters.CH.DISABLE)) { + hints.put(Parameters.CH.DISABLE, true); + } + + // TODO ugly workaround, duplicate data: hints can have 'vehicle' but algoOptions.weighting too!? + // Similar problem in GraphHopper class + String vehicle = hints.getVehicle(); + if (vehicle.isEmpty()) { + if (algoOptions.hasWeighting()) { + vehicle = algoOptions.getWeighting().getFlagEncoder().toString(); + } else { + vehicle = hopper.getEncodingManager().fetchEdgeEncoders().get(0).toString(); + } + hints.setVehicle(vehicle); + } + + if (!hopper.getEncodingManager().supports(vehicle)) { + throw new IllegalArgumentException("Vehicle " + vehicle + " unsupported. " + + "Supported are: " + hopper.getEncodingManager()); + } + + algoFactory = hopper.getAlgorithmFactory(hints); + + Weighting weighting = null; + CHAlgoFactoryDecorator chFactoryDecorator = hopper.getCHFactoryDecorator(); + boolean forceFlexibleMode = hints.getBool(Parameters.CH.DISABLE, false); + if (chFactoryDecorator.isEnabled() && !forceFlexibleMode) { + if (!(algoFactory instanceof PrepareContractionHierarchies)) { + throw new IllegalStateException("Although CH was enabled a non-CH algorithm factory was returned " + algoFactory); + } + + weighting = ((PrepareContractionHierarchies) algoFactory).getWeighting(); + this.routingGraph = hopper.getGraphHopperStorage().getGraph(CHGraph.class, weighting); + } else { + weighting = algoOptions.hasWeighting() + ? algoOptions.getWeighting() + : new FastestWeighting(hopper.getEncodingManager().getEncoder(vehicle), algoOptions.getHints()); + this.routingGraph = hopper.getGraphHopperStorage(); + } + + this.algoOptions = AlgorithmOptions.start(algoOptions).weighting(weighting).build(); + this.nodeCount = routingGraph.getNodes(); + } + + public void setDistanceCalc(DistanceCalc distanceCalc) { + this.distanceCalc = distanceCalc; + } + + /** + * Beta parameter of the exponential distribution for modeling transition + * probabilities. + */ + public void setTransitionProbabilityBeta(double transitionProbabilityBeta) { + this.transitionProbabilityBeta = transitionProbabilityBeta; + } + + /** + * Standard deviation of the normal distribution [m] used for modeling the + * GPS error. + */ + public void setMeasurementErrorSigma(double measurementErrorSigma) { + this.measurementErrorSigma = measurementErrorSigma; + } + + /** + * This method does the actual map matching. + *

+ * @param gpxList the input list with GPX points which should match to edges + * of the graph specified in the constructor + */ + public MatchResult doWork(List gpxList) { + if (gpxList.size() < 2) { + throw new IllegalArgumentException("Too few coordinates in input file (" + + gpxList.size() + "). Correct format?"); + } + + // filter the entries: + List filteredGPXEntries = filterGPXEntries(gpxList); + if (filteredGPXEntries.size() < 2) { + throw new IllegalStateException("Only " + filteredGPXEntries.size() + " filtered GPX entries (from " + gpxList.size() + "), but two or more are needed"); + } + + // now find each of the entries in the graph: + final EdgeFilter edgeFilter = new DefaultEdgeFilter(algoOptions.getWeighting().getFlagEncoder()); + List> queriesPerEntry = findGPXEntriesInGraph(filteredGPXEntries, edgeFilter); + + // now look up the entries up in the graph: + final QueryGraph queryGraph = new QueryGraph(routingGraph).setUseEdgeExplorerCache(true); + List allQueryResults = new ArrayList(); + for (List qrs: queriesPerEntry) + allQueryResults.addAll(qrs); + queryGraph.lookup(allQueryResults); + + // create candidates from the entries in the graph (a candidate is basically an entry + direction): + List> timeSteps = createTimeSteps(filteredGPXEntries, queriesPerEntry, queryGraph); + + // viterbify: + List> seq = computeViterbiSequence(timeSteps, gpxList, queryGraph); + + // finally, extract the result: + final EdgeExplorer explorer = queryGraph.createEdgeExplorer(edgeFilter); + MatchResult matchResult = computeMatchResult(seq, filteredGPXEntries, queriesPerEntry, explorer); + + return matchResult; + } + + /** + * Filters GPX entries to only those which will be used for map matching (i.e. those which + * are separated by at least 2 * measurementErrorSigman + */ + private List filterGPXEntries(List gpxList) { + List filtered = new ArrayList(); + GPXEntry prevEntry = null; + int last = gpxList.size() - 1; + for (int i = 0; i <= last; i++) { + GPXEntry gpxEntry = gpxList.get(i); + if (i == 0 || i == last || distanceCalc.calcDist( + prevEntry.getLat(), prevEntry.getLon(), + gpxEntry.getLat(), gpxEntry.getLon()) > 2 * measurementErrorSigma) { + filtered.add(gpxEntry); + prevEntry = gpxEntry; + } + } + return filtered; + } + /** + * Find the possible locations of each qpxEntry in the graph. + */ + private List> findGPXEntriesInGraph(List gpxList, EdgeFilter edgeFilter) { + + List> gpxEntryLocations = new ArrayList>(); + for (GPXEntry gpxEntry : gpxList) { + gpxEntryLocations.add(locationIndex.findNClosest(gpxEntry.lat, gpxEntry.lon, edgeFilter, measurementErrorSigma)); + } + return gpxEntryLocations; + } + + + /** + * Creates TimeSteps for the GPX entries but does not create emission or + * transition probabilities. + * + * @param outAllCandidates output parameter for all candidates, must be an + * empty list. + */ + private List> createTimeSteps(List filteredGPXEntries, + List> queriesPerEntry, QueryGraph queryGraph) { + + final List> timeSteps = new ArrayList<>(); + + int n = filteredGPXEntries.size(); + assert queriesPerEntry.size() == n; + for (int i = 0; i < n; i++) { + + GPXEntry gpxEntry = filteredGPXEntries.get(i); + List queryResults = queriesPerEntry.get(i); + + // as discussed in #51, if the closest node is virtual (i.e. inner-link) then we need to create two candidates: + // one for each direction of each virtual edge. For example, in A---X---B, we'd add the edges A->X and B->X. Note + // that we add the edges with an incoming direction (i.e. A->X not X->A). We can choose to enforce the incoming/outgoing + // direction with the third argument of queryGraph.enforceHeading + List candidates = new ArrayList(); + for (QueryResult qr: queryResults) { + int closestNode = qr.getClosestNode(); + if (queryGraph.isVirtualNode(closestNode)) { + // get virtual edges: + List virtualEdges = new ArrayList(); + EdgeIterator iter = queryGraph.createEdgeExplorer().setBaseNode(closestNode); + while (iter.next()) { + if (queryGraph.isVirtualEdge(iter.getEdge())) { + virtualEdges.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode())); + } + } + assert virtualEdges.size() == 2; + + // create a candidate for each: the candidate being the querypoint plus the virtual edge to favour. Note + // that we favour the virtual edge by *unfavoring* the rest, so we need to record these. + VirtualEdgeIteratorState e1 = virtualEdges.get(0); + VirtualEdgeIteratorState e2 = virtualEdges.get(1); + for (int j = 0; j < 2; j++) { + // get favored/unfavored edges: + VirtualEdgeIteratorState incomingVirtualEdge = j == 0 ? e1 : e2; + VirtualEdgeIteratorState outgoingVirtualEdge = j == 0 ? e2 : e1; + // create candidate + QueryResult vqr = new QueryResult(qr.getQueryPoint().lat, qr.getQueryPoint().lon); + vqr.setQueryDistance(qr.getQueryDistance()); + vqr.setClosestNode(qr.getClosestNode()); + vqr.setWayIndex(qr.getWayIndex()); + vqr.setSnappedPosition(qr.getSnappedPosition()); + vqr.setClosestEdge(qr.getClosestEdge()); + vqr.calcSnappedPoint(distanceCalc); + GPXExtension candidate = new GPXExtension(gpxEntry, vqr, incomingVirtualEdge, outgoingVirtualEdge); + candidates.add(candidate); + } + } else { + // just add the real edge, undirected + GPXExtension candidate = new GPXExtension(gpxEntry, qr); + candidates.add(candidate); + } + } + + final TimeStep timeStep = new TimeStep<>(gpxEntry, candidates); + timeSteps.add(timeStep); + } + return timeSteps; + } + + private List> computeViterbiSequence( + List> timeSteps, List gpxList, + final QueryGraph queryGraph) { + final HmmProbabilities probabilities + = new HmmProbabilities(measurementErrorSigma, transitionProbabilityBeta); + final ViterbiAlgorithm viterbi = new ViterbiAlgorithm<>(); + + int timeStepCounter = 0; + TimeStep prevTimeStep = null; + for (TimeStep timeStep : timeSteps) { + computeEmissionProbabilities(timeStep, probabilities); + + if (prevTimeStep == null) { + viterbi.startWithInitialObservation(timeStep.observation, timeStep.candidates, + timeStep.emissionLogProbabilities); + } else { + computeTransitionProbabilities(prevTimeStep, timeStep, probabilities, queryGraph); + viterbi.nextStep(timeStep.observation, timeStep.candidates, + timeStep.emissionLogProbabilities, timeStep.transitionLogProbabilities, + timeStep.roadPaths); + } + if (viterbi.isBroken()) { + String likelyReasonStr = ""; + if (prevTimeStep != null) { + GPXEntry prevGPXE = prevTimeStep.observation; + GPXEntry gpxe = timeStep.observation; + double dist = distanceCalc.calcDist(prevGPXE.lat, prevGPXE.lon, + gpxe.lat, gpxe.lon); + if (dist > 2000) { + likelyReasonStr = "Too long distance to previous measurement? " + + Math.round(dist) + "m, "; + } + } + + throw new RuntimeException("Sequence is broken for submitted track at time step " + + timeStepCounter + " (" + gpxList.size() + " points). " + likelyReasonStr + + "observation:" + timeStep.observation + ", " + + timeStep.candidates.size() + " candidates: " + getSnappedCandidates(timeStep.candidates) + + ". If a match is expected consider increasing max_visited_nodes."); + } + + timeStepCounter++; + prevTimeStep = timeStep; + } + + return viterbi.computeMostLikelySequence(); + } + + private void computeEmissionProbabilities(TimeStep timeStep, + HmmProbabilities probabilities) { + for (GPXExtension candidate : timeStep.candidates) { + // road distance difference in meters + final double distance = candidate.getQueryResult().getQueryDistance(); + timeStep.addEmissionLogProbability(candidate, + probabilities.emissionLogProbability(distance)); + } + } + + private void computeTransitionProbabilities(TimeStep prevTimeStep, + TimeStep timeStep, + HmmProbabilities probabilities, + QueryGraph queryGraph) { + final double linearDistance = distanceCalc.calcDist(prevTimeStep.observation.lat, + prevTimeStep.observation.lon, timeStep.observation.lat, timeStep.observation.lon); + + // time difference in seconds + final double timeDiff + = (timeStep.observation.getTime() - prevTimeStep.observation.getTime()) / 1000.0; + + for (GPXExtension from : prevTimeStep.candidates) { + for (GPXExtension to : timeStep.candidates) { + RoutingAlgorithm algo = algoFactory.createAlgo(queryGraph, algoOptions); + // enforce heading if required: + if (from.isDirected()) { + from.incomingVirtualEdge.setUnfavored(true); + } + if (to.isDirected()) { + // unfavor the favour virtual edge + to.outgoingVirtualEdge.setUnfavored(true); + } + final Path path = algo.calcPath(from.getQueryResult().getClosestNode(), to.getQueryResult().getClosestNode()); + queryGraph.clearUnfavoredStatus(); + if (path.isFound()) { + timeStep.addRoadPath(from, to, path); + final double transitionLogProbability = probabilities + .transitionLogProbability(path.getDistance(), linearDistance, timeDiff); + timeStep.addTransitionLogProbability(from, to, transitionLogProbability); + } + } + } + } + + private MatchResult computeMatchResult(List> seq, + List gpxList, List> queriesPerEntry, + EdgeExplorer explorer) { + // every virtual edge maps to its real edge where the orientation is already correct! + // TODO use traversal key instead of string! + final Map virtualEdgesMap = new HashMap<>(); + for (List queryResults: queriesPerEntry) { + for (QueryResult qr: queryResults) { + fillVirtualEdges(virtualEdgesMap, explorer, qr); + } + } + + MatchResult matchResult = computeMatchedEdges(seq, virtualEdgesMap); + computeGpxStats(gpxList, matchResult); + + return matchResult; + } + + private MatchResult computeMatchedEdges(List> seq, + Map virtualEdgesMap) { + List edgeMatches = new ArrayList<>(); + double distance = 0.0; + long time = 0; + EdgeIteratorState currentEdge = null; + List gpxExtensions = new ArrayList<>(); + GPXExtension queryResult = seq.get(0).state; + gpxExtensions.add(queryResult); + for (int j = 1; j < seq.size(); j++) { + queryResult = seq.get(j).state; + Path path = seq.get(j).transitionDescriptor; + distance += path.getDistance(); + time += path.getTime(); + for (EdgeIteratorState edgeIteratorState : path.calcEdges()) { + EdgeIteratorState directedRealEdge = resolveToRealEdge(virtualEdgesMap, + edgeIteratorState); + if (directedRealEdge == null) { + throw new RuntimeException("Did not find real edge for " + + edgeIteratorState.getEdge()); + } + if (currentEdge == null || !equalEdges(directedRealEdge, currentEdge)) { + if (currentEdge != null) { + EdgeMatch edgeMatch = new EdgeMatch(currentEdge, gpxExtensions); + edgeMatches.add(edgeMatch); + gpxExtensions = new ArrayList<>(); + } + currentEdge = directedRealEdge; + } + } + gpxExtensions.add(queryResult); + } + if (edgeMatches.isEmpty()) { + throw new IllegalStateException( + "No edge matches found for path. Too short? Sequence size " + seq.size()); + } + EdgeMatch lastEdgeMatch = edgeMatches.get(edgeMatches.size() - 1); + if (!gpxExtensions.isEmpty() && !equalEdges(currentEdge, lastEdgeMatch.getEdgeState())) { + edgeMatches.add(new EdgeMatch(currentEdge, gpxExtensions)); + } else { + lastEdgeMatch.getGpxExtensions().addAll(gpxExtensions); + } + MatchResult matchResult = new MatchResult(edgeMatches); + matchResult.setMatchMillis(time); + matchResult.setMatchLength(distance); + return matchResult; + } + + /** + * Calculate GPX stats to determine quality of matching. + */ + private void computeGpxStats(List gpxList, MatchResult matchResult) { + double gpxLength = 0; + GPXEntry prevEntry = gpxList.get(0); + for (int i = 1; i < gpxList.size(); i++) { + GPXEntry entry = gpxList.get(i); + gpxLength += distanceCalc.calcDist(prevEntry.lat, prevEntry.lon, entry.lat, entry.lon); + prevEntry = entry; + } + + long gpxMillis = gpxList.get(gpxList.size() - 1).getTime() - gpxList.get(0).getTime(); + matchResult.setGPXEntriesMillis(gpxMillis); + matchResult.setGPXEntriesLength(gpxLength); + } + + private boolean equalEdges(EdgeIteratorState edge1, EdgeIteratorState edge2) { + return edge1.getEdge() == edge2.getEdge() + && edge1.getBaseNode() == edge2.getBaseNode() + && edge1.getAdjNode() == edge2.getAdjNode(); + } + + private EdgeIteratorState resolveToRealEdge(Map virtualEdgesMap, + EdgeIteratorState edgeIteratorState) { + if (isVirtualNode(edgeIteratorState.getBaseNode()) + || isVirtualNode(edgeIteratorState.getAdjNode())) { + return virtualEdgesMap.get(virtualEdgesMapKey(edgeIteratorState)); + } else { + return edgeIteratorState; + } + } + + private boolean isVirtualNode(int node) { + return node >= nodeCount; + } + + /** + * Fills the minFactorMap with weights for the virtual edges. + */ + private void fillVirtualEdges(Map virtualEdgesMap, + EdgeExplorer explorer, QueryResult qr) { + if (isVirtualNode(qr.getClosestNode())) { + EdgeIterator iter = explorer.setBaseNode(qr.getClosestNode()); + while (iter.next()) { + int node = traverseToClosestRealAdj(explorer, iter); + if (node == qr.getClosestEdge().getAdjNode()) { + virtualEdgesMap.put(virtualEdgesMapKey(iter), + qr.getClosestEdge().detach(false)); + virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), + qr.getClosestEdge().detach(true)); + } else if (node == qr.getClosestEdge().getBaseNode()) { + virtualEdgesMap.put(virtualEdgesMapKey(iter), qr.getClosestEdge().detach(true)); + virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), + qr.getClosestEdge().detach(false)); + } else { + throw new RuntimeException(); + } + } + } + } + + private String virtualEdgesMapKey(EdgeIteratorState iter) { + return iter.getBaseNode() + "-" + iter.getEdge() + "-" + iter.getAdjNode(); + } + + private String reverseVirtualEdgesMapKey(EdgeIteratorState iter) { + return iter.getAdjNode() + "-" + iter.getEdge() + "-" + iter.getBaseNode(); + } + + private int traverseToClosestRealAdj(EdgeExplorer explorer, EdgeIteratorState edge) { + if (!isVirtualNode(edge.getAdjNode())) { + return edge.getAdjNode(); + } + + EdgeIterator iter = explorer.setBaseNode(edge.getAdjNode()); + while (iter.next()) { + if (iter.getAdjNode() != edge.getBaseNode()) { + return traverseToClosestRealAdj(explorer, iter); + } + } + throw new IllegalStateException("Cannot find adjacent edge " + edge); + } + + private String getSnappedCandidates(Collection candidates) { + String str = ""; + for (GPXExtension gpxe : candidates) { + if (!str.isEmpty()) { + str += ", "; + } + str += "distance: " + gpxe.queryResult.getQueryDistance() + " to " + + gpxe.queryResult.getSnappedPoint(); + } + return "[" + str + "]"; + } + + private void printMinDistances(List> timeSteps) { + TimeStep prevStep = null; + int index = 0; + for (TimeStep ts : timeSteps) { + if (prevStep != null) { + double dist = distanceCalc.calcDist( + prevStep.observation.lat, prevStep.observation.lon, + ts.observation.lat, ts.observation.lon); + double minCand = Double.POSITIVE_INFINITY; + for (GPXExtension prevGPXE : prevStep.candidates) { + for (GPXExtension gpxe : ts.candidates) { + GHPoint psp = prevGPXE.queryResult.getSnappedPoint(); + GHPoint sp = gpxe.queryResult.getSnappedPoint(); + double tmpDist = distanceCalc.calcDist(psp.lat, psp.lon, sp.lat, sp.lon); + if (tmpDist < minCand) { + minCand = tmpDist; + } + } + } + System.out.println(index + ": " + Math.round(dist) + "m, minimum candidate: " + + Math.round(minCand) + "m"); + index++; + } + + prevStep = ts; + } + } + + // TODO: Make setFromNode and processEdge public in Path and then remove this. + private static class MyPath extends Path { + + public MyPath(Graph graph, Weighting weighting) { + super(graph, weighting); + } + + @Override + public Path setFromNode(int from) { + return super.setFromNode(from); + } + + @Override + public void processEdge(int edgeId, int adjNode, int prevEdgeId) { + super.processEdge(edgeId, adjNode, prevEdgeId); + } + } + + public Path calcPath(MatchResult mr) { + MyPath p = new MyPath(routingGraph, algoOptions.getWeighting()); + if (!mr.getEdgeMatches().isEmpty()) { + int prevEdge = EdgeIterator.NO_EDGE; + p.setFromNode(mr.getEdgeMatches().get(0).getEdgeState().getBaseNode()); + for (EdgeMatch em : mr.getEdgeMatches()) { + p.processEdge(em.getEdgeState().getEdge(), em.getEdgeState().getAdjNode(), prevEdge); + prevEdge = em.getEdgeState().getEdge(); + } + + // TODO p.setWeight(weight); + p.setFound(true); + + return p; + } else { + return p; + } + } +} \ No newline at end of file diff --git a/matching-core/src/test/java/com/graphhopper/matching/MapMatching2Test.java b/matching-core/src/test/java/com/graphhopper/matching/MapMatching2Test.java index bb093459..f2c83630 100644 --- a/matching-core/src/test/java/com/graphhopper/matching/MapMatching2Test.java +++ b/matching-core/src/test/java/com/graphhopper/matching/MapMatching2Test.java @@ -62,4 +62,26 @@ public void testIssue13() { assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 2.5); assertEquals(28790, mr.getMatchMillis(), 50); } + + @Test + public void testIssue70() { + CarFlagEncoder encoder = new CarFlagEncoder(); + TestGraphHopper hopper = new TestGraphHopper(); + hopper.setDataReaderFile("../map-data/issue-70.osm.gz"); + hopper.setGraphHopperLocation("../target/mapmatchingtest-70"); + hopper.setEncodingManager(new EncodingManager(encoder)); + hopper.importOrLoad(); + + AlgorithmOptions opts = AlgorithmOptions.start().build(); + MapMatching mapMatching = new MapMatching(hopper, opts); + + List inputGPXEntries = new GPXFile(). + doImport("./src/test/resources/issue-70.gpx").getEntries(); + MatchResult mr = mapMatching.doWork(inputGPXEntries); + + assertEquals(Arrays.asList("Милана Видака", "Милана Видака", "Милана Видака", + "Бранка Радичевића", "Бранка Радичевића", "Здравка Челара"), + fetchStreets(mr.getEdgeMatches())); + // TODO: length/time + } } diff --git a/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java b/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java index df5c6c5f..02521cbc 100644 --- a/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java +++ b/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java @@ -82,7 +82,7 @@ public static Collection algoOptions() { // force CH AlgorithmOptions chOpts = AlgorithmOptions.start() - .maxVisitedNodes(40) + .maxVisitedNodes(1000) .hints(new PMap().put(Parameters.CH.DISABLE, false)) .build(); @@ -208,7 +208,7 @@ public void testSmallSeparatedSearchDistance() { MatchResult mr = mapMatching.doWork(inputGPXEntries); assertEquals(Arrays.asList("Weinligstraße", "Weinligstraße", "Weinligstraße", "Fechnerstraße", "Fechnerstraße"), fetchStreets(mr.getEdgeMatches())); - assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 11); + assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 11); // TODO: this should be around 300m according to Google ... need to check assertEquals(mr.getGpxEntriesMillis(), mr.getMatchMillis(), 3000); } diff --git a/matching-core/src/test/resources/issue-70.gpx b/matching-core/src/test/resources/issue-70.gpx new file mode 100644 index 00000000..ce8c4faa --- /dev/null +++ b/matching-core/src/test/resources/issue-70.gpx @@ -0,0 +1,25 @@ + + + + converted track + + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + + + \ No newline at end of file diff --git a/matching-web/src/main/java/com/graphhopper/matching/http/MatchServlet.java b/matching-web/src/main/java/com/graphhopper/matching/http/MatchServlet.java index b43d688b..9b584449 100644 --- a/matching-web/src/main/java/com/graphhopper/matching/http/MatchServlet.java +++ b/matching-web/src/main/java/com/graphhopper/matching/http/MatchServlet.java @@ -64,6 +64,7 @@ public class MatchServlet extends GraphHopperServlet { public void doPost(HttpServletRequest httpReq, HttpServletResponse httpRes) throws ServletException, IOException { + logger.info("posted"); String infoStr = httpReq.getRemoteAddr() + " " + httpReq.getLocale(); String inType = "gpx"; String contentType = httpReq.getContentType(); diff --git a/matching-web/src/test/java/com/graphhopper/matching/http/MatchResultToJsonTest.java b/matching-web/src/test/java/com/graphhopper/matching/http/MatchResultToJsonTest.java index 2adda29a..2d993e9a 100644 --- a/matching-web/src/test/java/com/graphhopper/matching/http/MatchResultToJsonTest.java +++ b/matching-web/src/test/java/com/graphhopper/matching/http/MatchResultToJsonTest.java @@ -58,8 +58,8 @@ public GHPoint3D getSnappedPoint() { } }; - list.add(new GPXExtension(new GPXEntry(-3.4446, -38.9996, 100000), queryResult1, 1)); - list.add(new GPXExtension(new GPXEntry(-3.4448, -38.9999, 100001), queryResult2, 1)); + list.add(new GPXExtension(new GPXEntry(-3.4446, -38.9996, 100000), queryResult1)); + list.add(new GPXExtension(new GPXEntry(-3.4448, -38.9999, 100001), queryResult2)); return list; } From 070230aae4841c965458068055c03c7f83cceb1d Mon Sep 17 00:00:00 2001 From: kodonnell Date: Fri, 25 Nov 2016 11:49:13 +1300 Subject: [PATCH 2/5] crlf === dum --- .../com/graphhopper/matching/MapMatching.java | 1226 ++++++++--------- 1 file changed, 613 insertions(+), 613 deletions(-) diff --git a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java index 18421316..b290b042 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java +++ b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java @@ -1,614 +1,614 @@ -/* - * Licensed to GraphHopper GmbH under one or more contributor - * license agreements. See the NOTICE file distributed with this work for - * additional information regarding copyright ownership. - * - * GraphHopper GmbH licenses this file to you under the Apache License, - * Version 2.0 (the "License"); you may not use this file except in - * compliance with the License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -package com.graphhopper.matching; - -import com.graphhopper.routing.VirtualEdgeIteratorState; -import com.graphhopper.GraphHopper; -import com.graphhopper.matching.util.HmmProbabilities; -import com.graphhopper.matching.util.TimeStep; -import com.graphhopper.routing.weighting.Weighting; -import com.bmw.hmm.SequenceState; -import com.bmw.hmm.ViterbiAlgorithm; -import com.graphhopper.routing.AlgorithmOptions; -import com.graphhopper.routing.Path; -import com.graphhopper.routing.QueryGraph; -import com.graphhopper.routing.RoutingAlgorithm; -import com.graphhopper.routing.RoutingAlgorithmFactory; -import com.graphhopper.routing.ch.CHAlgoFactoryDecorator; -import com.graphhopper.routing.ch.PrepareContractionHierarchies; -import com.graphhopper.routing.util.*; -import com.graphhopper.routing.weighting.FastestWeighting; -import com.graphhopper.storage.CHGraph; -import com.graphhopper.storage.Graph; -import com.graphhopper.storage.index.LocationIndexTree; -import com.graphhopper.storage.index.QueryResult; -import com.graphhopper.util.*; -import com.graphhopper.util.shapes.GHPoint; -import java.util.ArrayList; -import java.util.Collection; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.Map.Entry; - -/** - * This class matches real world GPX entries to the digital road network stored - * in GraphHopper. The Viterbi algorithm is used to compute the most likely - * sequence of map matching candidates. The Viterbi algorithm takes into account - * the distance between GPX entries and map matching candidates as well as the - * routing distances between consecutive map matching candidates. - * - *

- * See http://en.wikipedia.org/wiki/Map_matching and Newson, Paul, and John - * Krumm. "Hidden Markov map matching through noise and sparseness." Proceedings - * of the 17th ACM SIGSPATIAL International Conference on Advances in Geographic - * Information Systems. ACM, 2009. - * - * @author Peter Karich - * @author Michael Zilske - * @author Stefan Holder - * @author kodonnell - */ -public class MapMatching { - - private final Graph routingGraph; - private final LocationIndexMatch locationIndex; - private double measurementErrorSigma = 50.0; - private double transitionProbabilityBeta = 0.00959442; - private final int nodeCount; - private DistanceCalc distanceCalc = new DistancePlaneProjection(); - private final RoutingAlgorithmFactory algoFactory; - private final AlgorithmOptions algoOptions; - - public MapMatching(GraphHopper hopper, AlgorithmOptions algoOptions) { - this.locationIndex = new LocationIndexMatch(hopper.getGraphHopperStorage(), - (LocationIndexTree) hopper.getLocationIndex()); - - // create hints from algoOptions, so we can create the algorithm factory - HintsMap hints = new HintsMap(); - for (Entry entry : algoOptions.getHints().toMap().entrySet()) { - hints.put(entry.getKey(), entry.getValue()); - } - - // default is non-CH - if (!hints.has(Parameters.CH.DISABLE)) { - hints.put(Parameters.CH.DISABLE, true); - } - - // TODO ugly workaround, duplicate data: hints can have 'vehicle' but algoOptions.weighting too!? - // Similar problem in GraphHopper class - String vehicle = hints.getVehicle(); - if (vehicle.isEmpty()) { - if (algoOptions.hasWeighting()) { - vehicle = algoOptions.getWeighting().getFlagEncoder().toString(); - } else { - vehicle = hopper.getEncodingManager().fetchEdgeEncoders().get(0).toString(); - } - hints.setVehicle(vehicle); - } - - if (!hopper.getEncodingManager().supports(vehicle)) { - throw new IllegalArgumentException("Vehicle " + vehicle + " unsupported. " - + "Supported are: " + hopper.getEncodingManager()); - } - - algoFactory = hopper.getAlgorithmFactory(hints); - - Weighting weighting = null; - CHAlgoFactoryDecorator chFactoryDecorator = hopper.getCHFactoryDecorator(); - boolean forceFlexibleMode = hints.getBool(Parameters.CH.DISABLE, false); - if (chFactoryDecorator.isEnabled() && !forceFlexibleMode) { - if (!(algoFactory instanceof PrepareContractionHierarchies)) { - throw new IllegalStateException("Although CH was enabled a non-CH algorithm factory was returned " + algoFactory); - } - - weighting = ((PrepareContractionHierarchies) algoFactory).getWeighting(); - this.routingGraph = hopper.getGraphHopperStorage().getGraph(CHGraph.class, weighting); - } else { - weighting = algoOptions.hasWeighting() - ? algoOptions.getWeighting() - : new FastestWeighting(hopper.getEncodingManager().getEncoder(vehicle), algoOptions.getHints()); - this.routingGraph = hopper.getGraphHopperStorage(); - } - - this.algoOptions = AlgorithmOptions.start(algoOptions).weighting(weighting).build(); - this.nodeCount = routingGraph.getNodes(); - } - - public void setDistanceCalc(DistanceCalc distanceCalc) { - this.distanceCalc = distanceCalc; - } - - /** - * Beta parameter of the exponential distribution for modeling transition - * probabilities. - */ - public void setTransitionProbabilityBeta(double transitionProbabilityBeta) { - this.transitionProbabilityBeta = transitionProbabilityBeta; - } - - /** - * Standard deviation of the normal distribution [m] used for modeling the - * GPS error. - */ - public void setMeasurementErrorSigma(double measurementErrorSigma) { - this.measurementErrorSigma = measurementErrorSigma; - } - - /** - * This method does the actual map matching. - *

- * @param gpxList the input list with GPX points which should match to edges - * of the graph specified in the constructor - */ - public MatchResult doWork(List gpxList) { - if (gpxList.size() < 2) { - throw new IllegalArgumentException("Too few coordinates in input file (" - + gpxList.size() + "). Correct format?"); - } - - // filter the entries: - List filteredGPXEntries = filterGPXEntries(gpxList); - if (filteredGPXEntries.size() < 2) { - throw new IllegalStateException("Only " + filteredGPXEntries.size() + " filtered GPX entries (from " + gpxList.size() + "), but two or more are needed"); - } - - // now find each of the entries in the graph: - final EdgeFilter edgeFilter = new DefaultEdgeFilter(algoOptions.getWeighting().getFlagEncoder()); - List> queriesPerEntry = findGPXEntriesInGraph(filteredGPXEntries, edgeFilter); - - // now look up the entries up in the graph: - final QueryGraph queryGraph = new QueryGraph(routingGraph).setUseEdgeExplorerCache(true); - List allQueryResults = new ArrayList(); - for (List qrs: queriesPerEntry) - allQueryResults.addAll(qrs); - queryGraph.lookup(allQueryResults); - - // create candidates from the entries in the graph (a candidate is basically an entry + direction): - List> timeSteps = createTimeSteps(filteredGPXEntries, queriesPerEntry, queryGraph); - - // viterbify: - List> seq = computeViterbiSequence(timeSteps, gpxList, queryGraph); - - // finally, extract the result: - final EdgeExplorer explorer = queryGraph.createEdgeExplorer(edgeFilter); - MatchResult matchResult = computeMatchResult(seq, filteredGPXEntries, queriesPerEntry, explorer); - - return matchResult; - } - - /** - * Filters GPX entries to only those which will be used for map matching (i.e. those which - * are separated by at least 2 * measurementErrorSigman - */ - private List filterGPXEntries(List gpxList) { - List filtered = new ArrayList(); - GPXEntry prevEntry = null; - int last = gpxList.size() - 1; - for (int i = 0; i <= last; i++) { - GPXEntry gpxEntry = gpxList.get(i); - if (i == 0 || i == last || distanceCalc.calcDist( - prevEntry.getLat(), prevEntry.getLon(), - gpxEntry.getLat(), gpxEntry.getLon()) > 2 * measurementErrorSigma) { - filtered.add(gpxEntry); - prevEntry = gpxEntry; - } - } - return filtered; - } - /** - * Find the possible locations of each qpxEntry in the graph. - */ - private List> findGPXEntriesInGraph(List gpxList, EdgeFilter edgeFilter) { - - List> gpxEntryLocations = new ArrayList>(); - for (GPXEntry gpxEntry : gpxList) { - gpxEntryLocations.add(locationIndex.findNClosest(gpxEntry.lat, gpxEntry.lon, edgeFilter, measurementErrorSigma)); - } - return gpxEntryLocations; - } - - - /** - * Creates TimeSteps for the GPX entries but does not create emission or - * transition probabilities. - * - * @param outAllCandidates output parameter for all candidates, must be an - * empty list. - */ - private List> createTimeSteps(List filteredGPXEntries, - List> queriesPerEntry, QueryGraph queryGraph) { - - final List> timeSteps = new ArrayList<>(); - - int n = filteredGPXEntries.size(); - assert queriesPerEntry.size() == n; - for (int i = 0; i < n; i++) { - - GPXEntry gpxEntry = filteredGPXEntries.get(i); - List queryResults = queriesPerEntry.get(i); - - // as discussed in #51, if the closest node is virtual (i.e. inner-link) then we need to create two candidates: - // one for each direction of each virtual edge. For example, in A---X---B, we'd add the edges A->X and B->X. Note - // that we add the edges with an incoming direction (i.e. A->X not X->A). We can choose to enforce the incoming/outgoing - // direction with the third argument of queryGraph.enforceHeading - List candidates = new ArrayList(); - for (QueryResult qr: queryResults) { - int closestNode = qr.getClosestNode(); - if (queryGraph.isVirtualNode(closestNode)) { - // get virtual edges: - List virtualEdges = new ArrayList(); - EdgeIterator iter = queryGraph.createEdgeExplorer().setBaseNode(closestNode); - while (iter.next()) { - if (queryGraph.isVirtualEdge(iter.getEdge())) { - virtualEdges.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode())); - } - } - assert virtualEdges.size() == 2; - - // create a candidate for each: the candidate being the querypoint plus the virtual edge to favour. Note - // that we favour the virtual edge by *unfavoring* the rest, so we need to record these. - VirtualEdgeIteratorState e1 = virtualEdges.get(0); - VirtualEdgeIteratorState e2 = virtualEdges.get(1); - for (int j = 0; j < 2; j++) { - // get favored/unfavored edges: - VirtualEdgeIteratorState incomingVirtualEdge = j == 0 ? e1 : e2; - VirtualEdgeIteratorState outgoingVirtualEdge = j == 0 ? e2 : e1; - // create candidate - QueryResult vqr = new QueryResult(qr.getQueryPoint().lat, qr.getQueryPoint().lon); - vqr.setQueryDistance(qr.getQueryDistance()); - vqr.setClosestNode(qr.getClosestNode()); - vqr.setWayIndex(qr.getWayIndex()); - vqr.setSnappedPosition(qr.getSnappedPosition()); - vqr.setClosestEdge(qr.getClosestEdge()); - vqr.calcSnappedPoint(distanceCalc); - GPXExtension candidate = new GPXExtension(gpxEntry, vqr, incomingVirtualEdge, outgoingVirtualEdge); - candidates.add(candidate); - } - } else { - // just add the real edge, undirected - GPXExtension candidate = new GPXExtension(gpxEntry, qr); - candidates.add(candidate); - } - } - - final TimeStep timeStep = new TimeStep<>(gpxEntry, candidates); - timeSteps.add(timeStep); - } - return timeSteps; - } - - private List> computeViterbiSequence( - List> timeSteps, List gpxList, - final QueryGraph queryGraph) { - final HmmProbabilities probabilities - = new HmmProbabilities(measurementErrorSigma, transitionProbabilityBeta); - final ViterbiAlgorithm viterbi = new ViterbiAlgorithm<>(); - - int timeStepCounter = 0; - TimeStep prevTimeStep = null; - for (TimeStep timeStep : timeSteps) { - computeEmissionProbabilities(timeStep, probabilities); - - if (prevTimeStep == null) { - viterbi.startWithInitialObservation(timeStep.observation, timeStep.candidates, - timeStep.emissionLogProbabilities); - } else { - computeTransitionProbabilities(prevTimeStep, timeStep, probabilities, queryGraph); - viterbi.nextStep(timeStep.observation, timeStep.candidates, - timeStep.emissionLogProbabilities, timeStep.transitionLogProbabilities, - timeStep.roadPaths); - } - if (viterbi.isBroken()) { - String likelyReasonStr = ""; - if (prevTimeStep != null) { - GPXEntry prevGPXE = prevTimeStep.observation; - GPXEntry gpxe = timeStep.observation; - double dist = distanceCalc.calcDist(prevGPXE.lat, prevGPXE.lon, - gpxe.lat, gpxe.lon); - if (dist > 2000) { - likelyReasonStr = "Too long distance to previous measurement? " - + Math.round(dist) + "m, "; - } - } - - throw new RuntimeException("Sequence is broken for submitted track at time step " - + timeStepCounter + " (" + gpxList.size() + " points). " + likelyReasonStr - + "observation:" + timeStep.observation + ", " - + timeStep.candidates.size() + " candidates: " + getSnappedCandidates(timeStep.candidates) - + ". If a match is expected consider increasing max_visited_nodes."); - } - - timeStepCounter++; - prevTimeStep = timeStep; - } - - return viterbi.computeMostLikelySequence(); - } - - private void computeEmissionProbabilities(TimeStep timeStep, - HmmProbabilities probabilities) { - for (GPXExtension candidate : timeStep.candidates) { - // road distance difference in meters - final double distance = candidate.getQueryResult().getQueryDistance(); - timeStep.addEmissionLogProbability(candidate, - probabilities.emissionLogProbability(distance)); - } - } - - private void computeTransitionProbabilities(TimeStep prevTimeStep, - TimeStep timeStep, - HmmProbabilities probabilities, - QueryGraph queryGraph) { - final double linearDistance = distanceCalc.calcDist(prevTimeStep.observation.lat, - prevTimeStep.observation.lon, timeStep.observation.lat, timeStep.observation.lon); - - // time difference in seconds - final double timeDiff - = (timeStep.observation.getTime() - prevTimeStep.observation.getTime()) / 1000.0; - - for (GPXExtension from : prevTimeStep.candidates) { - for (GPXExtension to : timeStep.candidates) { - RoutingAlgorithm algo = algoFactory.createAlgo(queryGraph, algoOptions); - // enforce heading if required: - if (from.isDirected()) { - from.incomingVirtualEdge.setUnfavored(true); - } - if (to.isDirected()) { - // unfavor the favour virtual edge - to.outgoingVirtualEdge.setUnfavored(true); - } - final Path path = algo.calcPath(from.getQueryResult().getClosestNode(), to.getQueryResult().getClosestNode()); - queryGraph.clearUnfavoredStatus(); - if (path.isFound()) { - timeStep.addRoadPath(from, to, path); - final double transitionLogProbability = probabilities - .transitionLogProbability(path.getDistance(), linearDistance, timeDiff); - timeStep.addTransitionLogProbability(from, to, transitionLogProbability); - } - } - } - } - - private MatchResult computeMatchResult(List> seq, - List gpxList, List> queriesPerEntry, - EdgeExplorer explorer) { - // every virtual edge maps to its real edge where the orientation is already correct! - // TODO use traversal key instead of string! - final Map virtualEdgesMap = new HashMap<>(); - for (List queryResults: queriesPerEntry) { - for (QueryResult qr: queryResults) { - fillVirtualEdges(virtualEdgesMap, explorer, qr); - } - } - - MatchResult matchResult = computeMatchedEdges(seq, virtualEdgesMap); - computeGpxStats(gpxList, matchResult); - - return matchResult; - } - - private MatchResult computeMatchedEdges(List> seq, - Map virtualEdgesMap) { - List edgeMatches = new ArrayList<>(); - double distance = 0.0; - long time = 0; - EdgeIteratorState currentEdge = null; - List gpxExtensions = new ArrayList<>(); - GPXExtension queryResult = seq.get(0).state; - gpxExtensions.add(queryResult); - for (int j = 1; j < seq.size(); j++) { - queryResult = seq.get(j).state; - Path path = seq.get(j).transitionDescriptor; - distance += path.getDistance(); - time += path.getTime(); - for (EdgeIteratorState edgeIteratorState : path.calcEdges()) { - EdgeIteratorState directedRealEdge = resolveToRealEdge(virtualEdgesMap, - edgeIteratorState); - if (directedRealEdge == null) { - throw new RuntimeException("Did not find real edge for " - + edgeIteratorState.getEdge()); - } - if (currentEdge == null || !equalEdges(directedRealEdge, currentEdge)) { - if (currentEdge != null) { - EdgeMatch edgeMatch = new EdgeMatch(currentEdge, gpxExtensions); - edgeMatches.add(edgeMatch); - gpxExtensions = new ArrayList<>(); - } - currentEdge = directedRealEdge; - } - } - gpxExtensions.add(queryResult); - } - if (edgeMatches.isEmpty()) { - throw new IllegalStateException( - "No edge matches found for path. Too short? Sequence size " + seq.size()); - } - EdgeMatch lastEdgeMatch = edgeMatches.get(edgeMatches.size() - 1); - if (!gpxExtensions.isEmpty() && !equalEdges(currentEdge, lastEdgeMatch.getEdgeState())) { - edgeMatches.add(new EdgeMatch(currentEdge, gpxExtensions)); - } else { - lastEdgeMatch.getGpxExtensions().addAll(gpxExtensions); - } - MatchResult matchResult = new MatchResult(edgeMatches); - matchResult.setMatchMillis(time); - matchResult.setMatchLength(distance); - return matchResult; - } - - /** - * Calculate GPX stats to determine quality of matching. - */ - private void computeGpxStats(List gpxList, MatchResult matchResult) { - double gpxLength = 0; - GPXEntry prevEntry = gpxList.get(0); - for (int i = 1; i < gpxList.size(); i++) { - GPXEntry entry = gpxList.get(i); - gpxLength += distanceCalc.calcDist(prevEntry.lat, prevEntry.lon, entry.lat, entry.lon); - prevEntry = entry; - } - - long gpxMillis = gpxList.get(gpxList.size() - 1).getTime() - gpxList.get(0).getTime(); - matchResult.setGPXEntriesMillis(gpxMillis); - matchResult.setGPXEntriesLength(gpxLength); - } - - private boolean equalEdges(EdgeIteratorState edge1, EdgeIteratorState edge2) { - return edge1.getEdge() == edge2.getEdge() - && edge1.getBaseNode() == edge2.getBaseNode() - && edge1.getAdjNode() == edge2.getAdjNode(); - } - - private EdgeIteratorState resolveToRealEdge(Map virtualEdgesMap, - EdgeIteratorState edgeIteratorState) { - if (isVirtualNode(edgeIteratorState.getBaseNode()) - || isVirtualNode(edgeIteratorState.getAdjNode())) { - return virtualEdgesMap.get(virtualEdgesMapKey(edgeIteratorState)); - } else { - return edgeIteratorState; - } - } - - private boolean isVirtualNode(int node) { - return node >= nodeCount; - } - - /** - * Fills the minFactorMap with weights for the virtual edges. - */ - private void fillVirtualEdges(Map virtualEdgesMap, - EdgeExplorer explorer, QueryResult qr) { - if (isVirtualNode(qr.getClosestNode())) { - EdgeIterator iter = explorer.setBaseNode(qr.getClosestNode()); - while (iter.next()) { - int node = traverseToClosestRealAdj(explorer, iter); - if (node == qr.getClosestEdge().getAdjNode()) { - virtualEdgesMap.put(virtualEdgesMapKey(iter), - qr.getClosestEdge().detach(false)); - virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), - qr.getClosestEdge().detach(true)); - } else if (node == qr.getClosestEdge().getBaseNode()) { - virtualEdgesMap.put(virtualEdgesMapKey(iter), qr.getClosestEdge().detach(true)); - virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), - qr.getClosestEdge().detach(false)); - } else { - throw new RuntimeException(); - } - } - } - } - - private String virtualEdgesMapKey(EdgeIteratorState iter) { - return iter.getBaseNode() + "-" + iter.getEdge() + "-" + iter.getAdjNode(); - } - - private String reverseVirtualEdgesMapKey(EdgeIteratorState iter) { - return iter.getAdjNode() + "-" + iter.getEdge() + "-" + iter.getBaseNode(); - } - - private int traverseToClosestRealAdj(EdgeExplorer explorer, EdgeIteratorState edge) { - if (!isVirtualNode(edge.getAdjNode())) { - return edge.getAdjNode(); - } - - EdgeIterator iter = explorer.setBaseNode(edge.getAdjNode()); - while (iter.next()) { - if (iter.getAdjNode() != edge.getBaseNode()) { - return traverseToClosestRealAdj(explorer, iter); - } - } - throw new IllegalStateException("Cannot find adjacent edge " + edge); - } - - private String getSnappedCandidates(Collection candidates) { - String str = ""; - for (GPXExtension gpxe : candidates) { - if (!str.isEmpty()) { - str += ", "; - } - str += "distance: " + gpxe.queryResult.getQueryDistance() + " to " - + gpxe.queryResult.getSnappedPoint(); - } - return "[" + str + "]"; - } - - private void printMinDistances(List> timeSteps) { - TimeStep prevStep = null; - int index = 0; - for (TimeStep ts : timeSteps) { - if (prevStep != null) { - double dist = distanceCalc.calcDist( - prevStep.observation.lat, prevStep.observation.lon, - ts.observation.lat, ts.observation.lon); - double minCand = Double.POSITIVE_INFINITY; - for (GPXExtension prevGPXE : prevStep.candidates) { - for (GPXExtension gpxe : ts.candidates) { - GHPoint psp = prevGPXE.queryResult.getSnappedPoint(); - GHPoint sp = gpxe.queryResult.getSnappedPoint(); - double tmpDist = distanceCalc.calcDist(psp.lat, psp.lon, sp.lat, sp.lon); - if (tmpDist < minCand) { - minCand = tmpDist; - } - } - } - System.out.println(index + ": " + Math.round(dist) + "m, minimum candidate: " - + Math.round(minCand) + "m"); - index++; - } - - prevStep = ts; - } - } - - // TODO: Make setFromNode and processEdge public in Path and then remove this. - private static class MyPath extends Path { - - public MyPath(Graph graph, Weighting weighting) { - super(graph, weighting); - } - - @Override - public Path setFromNode(int from) { - return super.setFromNode(from); - } - - @Override - public void processEdge(int edgeId, int adjNode, int prevEdgeId) { - super.processEdge(edgeId, adjNode, prevEdgeId); - } - } - - public Path calcPath(MatchResult mr) { - MyPath p = new MyPath(routingGraph, algoOptions.getWeighting()); - if (!mr.getEdgeMatches().isEmpty()) { - int prevEdge = EdgeIterator.NO_EDGE; - p.setFromNode(mr.getEdgeMatches().get(0).getEdgeState().getBaseNode()); - for (EdgeMatch em : mr.getEdgeMatches()) { - p.processEdge(em.getEdgeState().getEdge(), em.getEdgeState().getAdjNode(), prevEdge); - prevEdge = em.getEdgeState().getEdge(); - } - - // TODO p.setWeight(weight); - p.setFound(true); - - return p; - } else { - return p; - } - } +/* + * Licensed to GraphHopper GmbH under one or more contributor + * license agreements. See the NOTICE file distributed with this work for + * additional information regarding copyright ownership. + * + * GraphHopper GmbH licenses this file to you under the Apache License, + * Version 2.0 (the "License"); you may not use this file except in + * compliance with the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +package com.graphhopper.matching; + +import com.graphhopper.routing.VirtualEdgeIteratorState; +import com.graphhopper.GraphHopper; +import com.graphhopper.matching.util.HmmProbabilities; +import com.graphhopper.matching.util.TimeStep; +import com.graphhopper.routing.weighting.Weighting; +import com.bmw.hmm.SequenceState; +import com.bmw.hmm.ViterbiAlgorithm; +import com.graphhopper.routing.AlgorithmOptions; +import com.graphhopper.routing.Path; +import com.graphhopper.routing.QueryGraph; +import com.graphhopper.routing.RoutingAlgorithm; +import com.graphhopper.routing.RoutingAlgorithmFactory; +import com.graphhopper.routing.ch.CHAlgoFactoryDecorator; +import com.graphhopper.routing.ch.PrepareContractionHierarchies; +import com.graphhopper.routing.util.*; +import com.graphhopper.routing.weighting.FastestWeighting; +import com.graphhopper.storage.CHGraph; +import com.graphhopper.storage.Graph; +import com.graphhopper.storage.index.LocationIndexTree; +import com.graphhopper.storage.index.QueryResult; +import com.graphhopper.util.*; +import com.graphhopper.util.shapes.GHPoint; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Map.Entry; + +/** + * This class matches real world GPX entries to the digital road network stored + * in GraphHopper. The Viterbi algorithm is used to compute the most likely + * sequence of map matching candidates. The Viterbi algorithm takes into account + * the distance between GPX entries and map matching candidates as well as the + * routing distances between consecutive map matching candidates. + * + *

+ * See http://en.wikipedia.org/wiki/Map_matching and Newson, Paul, and John + * Krumm. "Hidden Markov map matching through noise and sparseness." Proceedings + * of the 17th ACM SIGSPATIAL International Conference on Advances in Geographic + * Information Systems. ACM, 2009. + * + * @author Peter Karich + * @author Michael Zilske + * @author Stefan Holder + * @author kodonnell + */ +public class MapMatching { + + private final Graph routingGraph; + private final LocationIndexMatch locationIndex; + private double measurementErrorSigma = 50.0; + private double transitionProbabilityBeta = 0.00959442; + private final int nodeCount; + private DistanceCalc distanceCalc = new DistancePlaneProjection(); + private final RoutingAlgorithmFactory algoFactory; + private final AlgorithmOptions algoOptions; + + public MapMatching(GraphHopper hopper, AlgorithmOptions algoOptions) { + this.locationIndex = new LocationIndexMatch(hopper.getGraphHopperStorage(), + (LocationIndexTree) hopper.getLocationIndex()); + + // create hints from algoOptions, so we can create the algorithm factory + HintsMap hints = new HintsMap(); + for (Entry entry : algoOptions.getHints().toMap().entrySet()) { + hints.put(entry.getKey(), entry.getValue()); + } + + // default is non-CH + if (!hints.has(Parameters.CH.DISABLE)) { + hints.put(Parameters.CH.DISABLE, true); + } + + // TODO ugly workaround, duplicate data: hints can have 'vehicle' but algoOptions.weighting too!? + // Similar problem in GraphHopper class + String vehicle = hints.getVehicle(); + if (vehicle.isEmpty()) { + if (algoOptions.hasWeighting()) { + vehicle = algoOptions.getWeighting().getFlagEncoder().toString(); + } else { + vehicle = hopper.getEncodingManager().fetchEdgeEncoders().get(0).toString(); + } + hints.setVehicle(vehicle); + } + + if (!hopper.getEncodingManager().supports(vehicle)) { + throw new IllegalArgumentException("Vehicle " + vehicle + " unsupported. " + + "Supported are: " + hopper.getEncodingManager()); + } + + algoFactory = hopper.getAlgorithmFactory(hints); + + Weighting weighting = null; + CHAlgoFactoryDecorator chFactoryDecorator = hopper.getCHFactoryDecorator(); + boolean forceFlexibleMode = hints.getBool(Parameters.CH.DISABLE, false); + if (chFactoryDecorator.isEnabled() && !forceFlexibleMode) { + if (!(algoFactory instanceof PrepareContractionHierarchies)) { + throw new IllegalStateException("Although CH was enabled a non-CH algorithm factory was returned " + algoFactory); + } + + weighting = ((PrepareContractionHierarchies) algoFactory).getWeighting(); + this.routingGraph = hopper.getGraphHopperStorage().getGraph(CHGraph.class, weighting); + } else { + weighting = algoOptions.hasWeighting() + ? algoOptions.getWeighting() + : new FastestWeighting(hopper.getEncodingManager().getEncoder(vehicle), algoOptions.getHints()); + this.routingGraph = hopper.getGraphHopperStorage(); + } + + this.algoOptions = AlgorithmOptions.start(algoOptions).weighting(weighting).build(); + this.nodeCount = routingGraph.getNodes(); + } + + public void setDistanceCalc(DistanceCalc distanceCalc) { + this.distanceCalc = distanceCalc; + } + + /** + * Beta parameter of the exponential distribution for modeling transition + * probabilities. + */ + public void setTransitionProbabilityBeta(double transitionProbabilityBeta) { + this.transitionProbabilityBeta = transitionProbabilityBeta; + } + + /** + * Standard deviation of the normal distribution [m] used for modeling the + * GPS error. + */ + public void setMeasurementErrorSigma(double measurementErrorSigma) { + this.measurementErrorSigma = measurementErrorSigma; + } + + /** + * This method does the actual map matching. + *

+ * @param gpxList the input list with GPX points which should match to edges + * of the graph specified in the constructor + */ + public MatchResult doWork(List gpxList) { + if (gpxList.size() < 2) { + throw new IllegalArgumentException("Too few coordinates in input file (" + + gpxList.size() + "). Correct format?"); + } + + // filter the entries: + List filteredGPXEntries = filterGPXEntries(gpxList); + if (filteredGPXEntries.size() < 2) { + throw new IllegalStateException("Only " + filteredGPXEntries.size() + " filtered GPX entries (from " + gpxList.size() + "), but two or more are needed"); + } + + // now find each of the entries in the graph: + final EdgeFilter edgeFilter = new DefaultEdgeFilter(algoOptions.getWeighting().getFlagEncoder()); + List> queriesPerEntry = findGPXEntriesInGraph(filteredGPXEntries, edgeFilter); + + // now look up the entries up in the graph: + final QueryGraph queryGraph = new QueryGraph(routingGraph).setUseEdgeExplorerCache(true); + List allQueryResults = new ArrayList(); + for (List qrs: queriesPerEntry) + allQueryResults.addAll(qrs); + queryGraph.lookup(allQueryResults); + + // create candidates from the entries in the graph (a candidate is basically an entry + direction): + List> timeSteps = createTimeSteps(filteredGPXEntries, queriesPerEntry, queryGraph); + + // viterbify: + List> seq = computeViterbiSequence(timeSteps, gpxList, queryGraph); + + // finally, extract the result: + final EdgeExplorer explorer = queryGraph.createEdgeExplorer(edgeFilter); + MatchResult matchResult = computeMatchResult(seq, filteredGPXEntries, queriesPerEntry, explorer); + + return matchResult; + } + + /** + * Filters GPX entries to only those which will be used for map matching (i.e. those which + * are separated by at least 2 * measurementErrorSigman + */ + private List filterGPXEntries(List gpxList) { + List filtered = new ArrayList(); + GPXEntry prevEntry = null; + int last = gpxList.size() - 1; + for (int i = 0; i <= last; i++) { + GPXEntry gpxEntry = gpxList.get(i); + if (i == 0 || i == last || distanceCalc.calcDist( + prevEntry.getLat(), prevEntry.getLon(), + gpxEntry.getLat(), gpxEntry.getLon()) > 2 * measurementErrorSigma) { + filtered.add(gpxEntry); + prevEntry = gpxEntry; + } + } + return filtered; + } + /** + * Find the possible locations of each qpxEntry in the graph. + */ + private List> findGPXEntriesInGraph(List gpxList, EdgeFilter edgeFilter) { + + List> gpxEntryLocations = new ArrayList>(); + for (GPXEntry gpxEntry : gpxList) { + gpxEntryLocations.add(locationIndex.findNClosest(gpxEntry.lat, gpxEntry.lon, edgeFilter, measurementErrorSigma)); + } + return gpxEntryLocations; + } + + + /** + * Creates TimeSteps for the GPX entries but does not create emission or + * transition probabilities. + * + * @param outAllCandidates output parameter for all candidates, must be an + * empty list. + */ + private List> createTimeSteps(List filteredGPXEntries, + List> queriesPerEntry, QueryGraph queryGraph) { + + final List> timeSteps = new ArrayList<>(); + + int n = filteredGPXEntries.size(); + assert queriesPerEntry.size() == n; + for (int i = 0; i < n; i++) { + + GPXEntry gpxEntry = filteredGPXEntries.get(i); + List queryResults = queriesPerEntry.get(i); + + // as discussed in #51, if the closest node is virtual (i.e. inner-link) then we need to create two candidates: + // one for each direction of each virtual edge. For example, in A---X---B, we'd add the edges A->X and B->X. Note + // that we add the edges with an incoming direction (i.e. A->X not X->A). We can choose to enforce the incoming/outgoing + // direction with the third argument of queryGraph.enforceHeading + List candidates = new ArrayList(); + for (QueryResult qr: queryResults) { + int closestNode = qr.getClosestNode(); + if (queryGraph.isVirtualNode(closestNode)) { + // get virtual edges: + List virtualEdges = new ArrayList(); + EdgeIterator iter = queryGraph.createEdgeExplorer().setBaseNode(closestNode); + while (iter.next()) { + if (queryGraph.isVirtualEdge(iter.getEdge())) { + virtualEdges.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode())); + } + } + assert virtualEdges.size() == 2; + + // create a candidate for each: the candidate being the querypoint plus the virtual edge to favour. Note + // that we favour the virtual edge by *unfavoring* the rest, so we need to record these. + VirtualEdgeIteratorState e1 = virtualEdges.get(0); + VirtualEdgeIteratorState e2 = virtualEdges.get(1); + for (int j = 0; j < 2; j++) { + // get favored/unfavored edges: + VirtualEdgeIteratorState incomingVirtualEdge = j == 0 ? e1 : e2; + VirtualEdgeIteratorState outgoingVirtualEdge = j == 0 ? e2 : e1; + // create candidate + QueryResult vqr = new QueryResult(qr.getQueryPoint().lat, qr.getQueryPoint().lon); + vqr.setQueryDistance(qr.getQueryDistance()); + vqr.setClosestNode(qr.getClosestNode()); + vqr.setWayIndex(qr.getWayIndex()); + vqr.setSnappedPosition(qr.getSnappedPosition()); + vqr.setClosestEdge(qr.getClosestEdge()); + vqr.calcSnappedPoint(distanceCalc); + GPXExtension candidate = new GPXExtension(gpxEntry, vqr, incomingVirtualEdge, outgoingVirtualEdge); + candidates.add(candidate); + } + } else { + // just add the real edge, undirected + GPXExtension candidate = new GPXExtension(gpxEntry, qr); + candidates.add(candidate); + } + } + + final TimeStep timeStep = new TimeStep<>(gpxEntry, candidates); + timeSteps.add(timeStep); + } + return timeSteps; + } + + private List> computeViterbiSequence( + List> timeSteps, List gpxList, + final QueryGraph queryGraph) { + final HmmProbabilities probabilities + = new HmmProbabilities(measurementErrorSigma, transitionProbabilityBeta); + final ViterbiAlgorithm viterbi = new ViterbiAlgorithm<>(); + + int timeStepCounter = 0; + TimeStep prevTimeStep = null; + for (TimeStep timeStep : timeSteps) { + computeEmissionProbabilities(timeStep, probabilities); + + if (prevTimeStep == null) { + viterbi.startWithInitialObservation(timeStep.observation, timeStep.candidates, + timeStep.emissionLogProbabilities); + } else { + computeTransitionProbabilities(prevTimeStep, timeStep, probabilities, queryGraph); + viterbi.nextStep(timeStep.observation, timeStep.candidates, + timeStep.emissionLogProbabilities, timeStep.transitionLogProbabilities, + timeStep.roadPaths); + } + if (viterbi.isBroken()) { + String likelyReasonStr = ""; + if (prevTimeStep != null) { + GPXEntry prevGPXE = prevTimeStep.observation; + GPXEntry gpxe = timeStep.observation; + double dist = distanceCalc.calcDist(prevGPXE.lat, prevGPXE.lon, + gpxe.lat, gpxe.lon); + if (dist > 2000) { + likelyReasonStr = "Too long distance to previous measurement? " + + Math.round(dist) + "m, "; + } + } + + throw new RuntimeException("Sequence is broken for submitted track at time step " + + timeStepCounter + " (" + gpxList.size() + " points). " + likelyReasonStr + + "observation:" + timeStep.observation + ", " + + timeStep.candidates.size() + " candidates: " + getSnappedCandidates(timeStep.candidates) + + ". If a match is expected consider increasing max_visited_nodes."); + } + + timeStepCounter++; + prevTimeStep = timeStep; + } + + return viterbi.computeMostLikelySequence(); + } + + private void computeEmissionProbabilities(TimeStep timeStep, + HmmProbabilities probabilities) { + for (GPXExtension candidate : timeStep.candidates) { + // road distance difference in meters + final double distance = candidate.getQueryResult().getQueryDistance(); + timeStep.addEmissionLogProbability(candidate, + probabilities.emissionLogProbability(distance)); + } + } + + private void computeTransitionProbabilities(TimeStep prevTimeStep, + TimeStep timeStep, + HmmProbabilities probabilities, + QueryGraph queryGraph) { + final double linearDistance = distanceCalc.calcDist(prevTimeStep.observation.lat, + prevTimeStep.observation.lon, timeStep.observation.lat, timeStep.observation.lon); + + // time difference in seconds + final double timeDiff + = (timeStep.observation.getTime() - prevTimeStep.observation.getTime()) / 1000.0; + + for (GPXExtension from : prevTimeStep.candidates) { + for (GPXExtension to : timeStep.candidates) { + RoutingAlgorithm algo = algoFactory.createAlgo(queryGraph, algoOptions); + // enforce heading if required: + if (from.isDirected()) { + from.incomingVirtualEdge.setUnfavored(true); + } + if (to.isDirected()) { + // unfavor the favour virtual edge + to.outgoingVirtualEdge.setUnfavored(true); + } + final Path path = algo.calcPath(from.getQueryResult().getClosestNode(), to.getQueryResult().getClosestNode()); + queryGraph.clearUnfavoredStatus(); + if (path.isFound()) { + timeStep.addRoadPath(from, to, path); + final double transitionLogProbability = probabilities + .transitionLogProbability(path.getDistance(), linearDistance, timeDiff); + timeStep.addTransitionLogProbability(from, to, transitionLogProbability); + } + } + } + } + + private MatchResult computeMatchResult(List> seq, + List gpxList, List> queriesPerEntry, + EdgeExplorer explorer) { + // every virtual edge maps to its real edge where the orientation is already correct! + // TODO use traversal key instead of string! + final Map virtualEdgesMap = new HashMap<>(); + for (List queryResults: queriesPerEntry) { + for (QueryResult qr: queryResults) { + fillVirtualEdges(virtualEdgesMap, explorer, qr); + } + } + + MatchResult matchResult = computeMatchedEdges(seq, virtualEdgesMap); + computeGpxStats(gpxList, matchResult); + + return matchResult; + } + + private MatchResult computeMatchedEdges(List> seq, + Map virtualEdgesMap) { + List edgeMatches = new ArrayList<>(); + double distance = 0.0; + long time = 0; + EdgeIteratorState currentEdge = null; + List gpxExtensions = new ArrayList<>(); + GPXExtension queryResult = seq.get(0).state; + gpxExtensions.add(queryResult); + for (int j = 1; j < seq.size(); j++) { + queryResult = seq.get(j).state; + Path path = seq.get(j).transitionDescriptor; + distance += path.getDistance(); + time += path.getTime(); + for (EdgeIteratorState edgeIteratorState : path.calcEdges()) { + EdgeIteratorState directedRealEdge = resolveToRealEdge(virtualEdgesMap, + edgeIteratorState); + if (directedRealEdge == null) { + throw new RuntimeException("Did not find real edge for " + + edgeIteratorState.getEdge()); + } + if (currentEdge == null || !equalEdges(directedRealEdge, currentEdge)) { + if (currentEdge != null) { + EdgeMatch edgeMatch = new EdgeMatch(currentEdge, gpxExtensions); + edgeMatches.add(edgeMatch); + gpxExtensions = new ArrayList<>(); + } + currentEdge = directedRealEdge; + } + } + gpxExtensions.add(queryResult); + } + if (edgeMatches.isEmpty()) { + throw new IllegalStateException( + "No edge matches found for path. Too short? Sequence size " + seq.size()); + } + EdgeMatch lastEdgeMatch = edgeMatches.get(edgeMatches.size() - 1); + if (!gpxExtensions.isEmpty() && !equalEdges(currentEdge, lastEdgeMatch.getEdgeState())) { + edgeMatches.add(new EdgeMatch(currentEdge, gpxExtensions)); + } else { + lastEdgeMatch.getGpxExtensions().addAll(gpxExtensions); + } + MatchResult matchResult = new MatchResult(edgeMatches); + matchResult.setMatchMillis(time); + matchResult.setMatchLength(distance); + return matchResult; + } + + /** + * Calculate GPX stats to determine quality of matching. + */ + private void computeGpxStats(List gpxList, MatchResult matchResult) { + double gpxLength = 0; + GPXEntry prevEntry = gpxList.get(0); + for (int i = 1; i < gpxList.size(); i++) { + GPXEntry entry = gpxList.get(i); + gpxLength += distanceCalc.calcDist(prevEntry.lat, prevEntry.lon, entry.lat, entry.lon); + prevEntry = entry; + } + + long gpxMillis = gpxList.get(gpxList.size() - 1).getTime() - gpxList.get(0).getTime(); + matchResult.setGPXEntriesMillis(gpxMillis); + matchResult.setGPXEntriesLength(gpxLength); + } + + private boolean equalEdges(EdgeIteratorState edge1, EdgeIteratorState edge2) { + return edge1.getEdge() == edge2.getEdge() + && edge1.getBaseNode() == edge2.getBaseNode() + && edge1.getAdjNode() == edge2.getAdjNode(); + } + + private EdgeIteratorState resolveToRealEdge(Map virtualEdgesMap, + EdgeIteratorState edgeIteratorState) { + if (isVirtualNode(edgeIteratorState.getBaseNode()) + || isVirtualNode(edgeIteratorState.getAdjNode())) { + return virtualEdgesMap.get(virtualEdgesMapKey(edgeIteratorState)); + } else { + return edgeIteratorState; + } + } + + private boolean isVirtualNode(int node) { + return node >= nodeCount; + } + + /** + * Fills the minFactorMap with weights for the virtual edges. + */ + private void fillVirtualEdges(Map virtualEdgesMap, + EdgeExplorer explorer, QueryResult qr) { + if (isVirtualNode(qr.getClosestNode())) { + EdgeIterator iter = explorer.setBaseNode(qr.getClosestNode()); + while (iter.next()) { + int node = traverseToClosestRealAdj(explorer, iter); + if (node == qr.getClosestEdge().getAdjNode()) { + virtualEdgesMap.put(virtualEdgesMapKey(iter), + qr.getClosestEdge().detach(false)); + virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), + qr.getClosestEdge().detach(true)); + } else if (node == qr.getClosestEdge().getBaseNode()) { + virtualEdgesMap.put(virtualEdgesMapKey(iter), qr.getClosestEdge().detach(true)); + virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), + qr.getClosestEdge().detach(false)); + } else { + throw new RuntimeException(); + } + } + } + } + + private String virtualEdgesMapKey(EdgeIteratorState iter) { + return iter.getBaseNode() + "-" + iter.getEdge() + "-" + iter.getAdjNode(); + } + + private String reverseVirtualEdgesMapKey(EdgeIteratorState iter) { + return iter.getAdjNode() + "-" + iter.getEdge() + "-" + iter.getBaseNode(); + } + + private int traverseToClosestRealAdj(EdgeExplorer explorer, EdgeIteratorState edge) { + if (!isVirtualNode(edge.getAdjNode())) { + return edge.getAdjNode(); + } + + EdgeIterator iter = explorer.setBaseNode(edge.getAdjNode()); + while (iter.next()) { + if (iter.getAdjNode() != edge.getBaseNode()) { + return traverseToClosestRealAdj(explorer, iter); + } + } + throw new IllegalStateException("Cannot find adjacent edge " + edge); + } + + private String getSnappedCandidates(Collection candidates) { + String str = ""; + for (GPXExtension gpxe : candidates) { + if (!str.isEmpty()) { + str += ", "; + } + str += "distance: " + gpxe.queryResult.getQueryDistance() + " to " + + gpxe.queryResult.getSnappedPoint(); + } + return "[" + str + "]"; + } + + private void printMinDistances(List> timeSteps) { + TimeStep prevStep = null; + int index = 0; + for (TimeStep ts : timeSteps) { + if (prevStep != null) { + double dist = distanceCalc.calcDist( + prevStep.observation.lat, prevStep.observation.lon, + ts.observation.lat, ts.observation.lon); + double minCand = Double.POSITIVE_INFINITY; + for (GPXExtension prevGPXE : prevStep.candidates) { + for (GPXExtension gpxe : ts.candidates) { + GHPoint psp = prevGPXE.queryResult.getSnappedPoint(); + GHPoint sp = gpxe.queryResult.getSnappedPoint(); + double tmpDist = distanceCalc.calcDist(psp.lat, psp.lon, sp.lat, sp.lon); + if (tmpDist < minCand) { + minCand = tmpDist; + } + } + } + System.out.println(index + ": " + Math.round(dist) + "m, minimum candidate: " + + Math.round(minCand) + "m"); + index++; + } + + prevStep = ts; + } + } + + // TODO: Make setFromNode and processEdge public in Path and then remove this. + private static class MyPath extends Path { + + public MyPath(Graph graph, Weighting weighting) { + super(graph, weighting); + } + + @Override + public Path setFromNode(int from) { + return super.setFromNode(from); + } + + @Override + public void processEdge(int edgeId, int adjNode, int prevEdgeId) { + super.processEdge(edgeId, adjNode, prevEdgeId); + } + } + + public Path calcPath(MatchResult mr) { + MyPath p = new MyPath(routingGraph, algoOptions.getWeighting()); + if (!mr.getEdgeMatches().isEmpty()) { + int prevEdge = EdgeIterator.NO_EDGE; + p.setFromNode(mr.getEdgeMatches().get(0).getEdgeState().getBaseNode()); + for (EdgeMatch em : mr.getEdgeMatches()) { + p.processEdge(em.getEdgeState().getEdge(), em.getEdgeState().getAdjNode(), prevEdge); + prevEdge = em.getEdgeState().getEdge(); + } + + // TODO p.setWeight(weight); + p.setFound(true); + + return p; + } else { + return p; + } + } } \ No newline at end of file From 68e68ec13b3921287350502a59a086fa545290d0 Mon Sep 17 00:00:00 2001 From: kane Date: Sat, 26 Nov 2016 09:36:57 +1300 Subject: [PATCH 3/5] d'oh virtual edges are directed ... --- .../graphhopper/matching/GPXExtension.java | 10 +++--- .../com/graphhopper/matching/MapMatching.java | 35 ++++++++++--------- .../graphhopper/matching/VirtualEdgePair.java | 21 +++++++++++ 3 files changed, 45 insertions(+), 21 deletions(-) create mode 100644 matching-core/src/main/java/com/graphhopper/matching/VirtualEdgePair.java diff --git a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java index da0dcb2a..9792ae14 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java +++ b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java @@ -31,8 +31,8 @@ public class GPXExtension { final GPXEntry entry; final QueryResult queryResult; private boolean directed; - public VirtualEdgeIteratorState incomingVirtualEdge; - public VirtualEdgeIteratorState outgoingVirtualEdge; + public VirtualEdgePair incomingVirtualPair; + public VirtualEdgePair outgoingVirtualPair; public GPXExtension(GPXEntry entry, QueryResult queryResult) { this.entry = entry; @@ -40,10 +40,10 @@ public GPXExtension(GPXEntry entry, QueryResult queryResult) { this.directed = false; } - public GPXExtension(GPXEntry entry, QueryResult queryResult, VirtualEdgeIteratorState incomingVirtualEdge, VirtualEdgeIteratorState outgoingVirtualEdge) { + public GPXExtension(GPXEntry entry, QueryResult queryResult, VirtualEdgePair incomingVirtualPair, VirtualEdgePair outgoingVirtualPair) { this(entry, queryResult); - this.incomingVirtualEdge = incomingVirtualEdge; - this.outgoingVirtualEdge = outgoingVirtualEdge; + this.incomingVirtualPair = incomingVirtualPair; + this.outgoingVirtualPair = outgoingVirtualPair; this.directed = true; } diff --git a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java index b290b042..4fde4ceb 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java +++ b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java @@ -40,6 +40,7 @@ import com.graphhopper.util.*; import com.graphhopper.util.shapes.GHPoint; import java.util.ArrayList; +import java.util.Arrays; import java.util.Collection; import java.util.HashMap; import java.util.List; @@ -252,23 +253,24 @@ private List> createTimeSteps(List virtualEdges = new ArrayList(); + List virtualEdgePairs = new ArrayList(); EdgeIterator iter = queryGraph.createEdgeExplorer().setBaseNode(closestNode); while (iter.next()) { if (queryGraph.isVirtualEdge(iter.getEdge())) { - virtualEdges.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode())); + VirtualEdgePair virtualEdgePair = new VirtualEdgePair(iter, queryGraph); + virtualEdgePairs.add(virtualEdgePair); } } - assert virtualEdges.size() == 2; + assert virtualEdgePairs.size() == 2; // create a candidate for each: the candidate being the querypoint plus the virtual edge to favour. Note // that we favour the virtual edge by *unfavoring* the rest, so we need to record these. - VirtualEdgeIteratorState e1 = virtualEdges.get(0); - VirtualEdgeIteratorState e2 = virtualEdges.get(1); + VirtualEdgePair e1 = virtualEdgePairs.get(0); + VirtualEdgePair e2 = virtualEdgePairs.get(1); for (int j = 0; j < 2; j++) { // get favored/unfavored edges: - VirtualEdgeIteratorState incomingVirtualEdge = j == 0 ? e1 : e2; - VirtualEdgeIteratorState outgoingVirtualEdge = j == 0 ? e2 : e1; + VirtualEdgePair incomingVirtualEdgePair = j == 0 ? e1 : e2; + VirtualEdgePair outgoingVirtualEdgePair = j == 0 ? e2 : e1; // create candidate QueryResult vqr = new QueryResult(qr.getQueryPoint().lat, qr.getQueryPoint().lon); vqr.setQueryDistance(qr.getQueryDistance()); @@ -277,7 +279,7 @@ private List> createTimeSteps(List pair = new ArrayList(); + + public VirtualEdgePair(EdgeIteratorState iter, QueryGraph queryGraph) { + pair.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode())); + pair.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getBaseNode())); + } + + public void setUnfavored(boolean favor) { + pair.get(0).setUnfavored(favor); + pair.get(1).setUnfavored(favor); + } +} From 9d07f33d1ad57737a40827c9b4b81a10eb9bd0d5 Mon Sep 17 00:00:00 2001 From: kodonnell Date: Tue, 29 Nov 2016 08:04:51 +1300 Subject: [PATCH 4/5] what about directed virtual edge quadruples? --- .../DirectedVirtualEdgeQuadruple.java | 51 +++++++++++++++++++ .../graphhopper/matching/GPXExtension.java | 8 ++- .../com/graphhopper/matching/MapMatching.java | 48 ++++++----------- .../graphhopper/matching/VirtualEdgePair.java | 21 -------- .../graphhopper/matching/MapMatchingTest.java | 1 + 5 files changed, 71 insertions(+), 58 deletions(-) create mode 100644 matching-core/src/main/java/com/graphhopper/matching/DirectedVirtualEdgeQuadruple.java delete mode 100644 matching-core/src/main/java/com/graphhopper/matching/VirtualEdgePair.java diff --git a/matching-core/src/main/java/com/graphhopper/matching/DirectedVirtualEdgeQuadruple.java b/matching-core/src/main/java/com/graphhopper/matching/DirectedVirtualEdgeQuadruple.java new file mode 100644 index 00000000..29d36820 --- /dev/null +++ b/matching-core/src/main/java/com/graphhopper/matching/DirectedVirtualEdgeQuadruple.java @@ -0,0 +1,51 @@ +package com.graphhopper.matching; + +import java.util.ArrayList; +import com.graphhopper.routing.QueryGraph; +import com.graphhopper.routing.VirtualEdgeIteratorState; +import com.graphhopper.util.EdgeIterator; + +/* + * At a virtual node we get four virtual edges: + * + * GPX + * | + * | + * v1 | v2 + * --->--- --->---- + * N + * ---<--- ---<---- + * v3 v4 + * + * This is to represent this quadruple. We call it 'directed' in the sense that we allow only + * only of the edges to be used (by unfavoring the other three). + */ +public class DirectedVirtualEdgeQuadruple { + + public ArrayList quad = new ArrayList(); + public int favoured; + + public DirectedVirtualEdgeQuadruple(int virtualNode, QueryGraph queryGraph, int favoured) { + this.favoured = favoured; + EdgeIterator iter = queryGraph.createEdgeExplorer().setBaseNode(virtualNode); + while (iter.next()) { + quad.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode())); + } + assert quad.size() == 2; + // add reverse + for (int i = 0; i < 2; i++) { + VirtualEdgeIteratorState e = quad.get(i); + // TODO: is this the correct way to reverse? Or the following? + // quad.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(e.getEdge(), e.getBaseNode())); + quad.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(e.getEdge(), e.getAdjNode())); + } + } + + public void setFavouringOfUnfavored(boolean favor) { + for (int i = 0; i < 4; i++) { + if (i != favoured) { + quad.get(i).setUnfavored(favor); + } + } + } +} diff --git a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java index 9792ae14..df467d47 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java +++ b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java @@ -31,8 +31,7 @@ public class GPXExtension { final GPXEntry entry; final QueryResult queryResult; private boolean directed; - public VirtualEdgePair incomingVirtualPair; - public VirtualEdgePair outgoingVirtualPair; + public DirectedVirtualEdgeQuadruple virtualEdgeQuadruple; public GPXExtension(GPXEntry entry, QueryResult queryResult) { this.entry = entry; @@ -40,10 +39,9 @@ public GPXExtension(GPXEntry entry, QueryResult queryResult) { this.directed = false; } - public GPXExtension(GPXEntry entry, QueryResult queryResult, VirtualEdgePair incomingVirtualPair, VirtualEdgePair outgoingVirtualPair) { + public GPXExtension(GPXEntry entry, QueryResult queryResult, DirectedVirtualEdgeQuadruple virtualEdgeQuadruple) { this(entry, queryResult); - this.incomingVirtualPair = incomingVirtualPair; - this.outgoingVirtualPair = outgoingVirtualPair; + this.virtualEdgeQuadruple = virtualEdgeQuadruple; this.directed = true; } diff --git a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java index 4fde4ceb..1416d08b 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java +++ b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java @@ -188,7 +188,8 @@ public MatchResult doWork(List gpxList) { // finally, extract the result: final EdgeExplorer explorer = queryGraph.createEdgeExplorer(edgeFilter); - MatchResult matchResult = computeMatchResult(seq, filteredGPXEntries, queriesPerEntry, explorer); + MatchResult matchResult = computeMatchResult(seq, gpxList, queriesPerEntry, explorer); +// MatchResult matchResult = computeMatchResult(seq, gpxList, queriesPerEntry, explorer); return matchResult; } @@ -244,34 +245,16 @@ private List> createTimeSteps(List queryResults = queriesPerEntry.get(i); - // as discussed in #51, if the closest node is virtual (i.e. inner-link) then we need to create two candidates: - // one for each direction of each virtual edge. For example, in A---X---B, we'd add the edges A->X and B->X. Note - // that we add the edges with an incoming direction (i.e. A->X not X->A). We can choose to enforce the incoming/outgoing - // direction with the third argument of queryGraph.enforceHeading List candidates = new ArrayList(); for (QueryResult qr: queryResults) { int closestNode = qr.getClosestNode(); if (queryGraph.isVirtualNode(closestNode)) { - // get virtual edges: - List virtualEdgePairs = new ArrayList(); - EdgeIterator iter = queryGraph.createEdgeExplorer().setBaseNode(closestNode); - while (iter.next()) { - if (queryGraph.isVirtualEdge(iter.getEdge())) { - VirtualEdgePair virtualEdgePair = new VirtualEdgePair(iter, queryGraph); - virtualEdgePairs.add(virtualEdgePair); - } - } - assert virtualEdgePairs.size() == 2; - - // create a candidate for each: the candidate being the querypoint plus the virtual edge to favour. Note - // that we favour the virtual edge by *unfavoring* the rest, so we need to record these. - VirtualEdgePair e1 = virtualEdgePairs.get(0); - VirtualEdgePair e2 = virtualEdgePairs.get(1); - for (int j = 0; j < 2; j++) { - // get favored/unfavored edges: - VirtualEdgePair incomingVirtualEdgePair = j == 0 ? e1 : e2; - VirtualEdgePair outgoingVirtualEdgePair = j == 0 ? e2 : e1; - // create candidate + // if virtual, create four candidates: one for each virtual edge around the virtual node ... + List virtualEdgeQuads = new ArrayList(); + for (int favoured = 0; favoured < 4; favoured++) { + DirectedVirtualEdgeQuadruple veq = new DirectedVirtualEdgeQuadruple(closestNode, queryGraph, favoured); + virtualEdgeQuads.add(veq); + // create candidate QueryResult vqr = new QueryResult(qr.getQueryPoint().lat, qr.getQueryPoint().lon); vqr.setQueryDistance(qr.getQueryDistance()); vqr.setClosestNode(qr.getClosestNode()); @@ -279,9 +262,10 @@ private List> createTimeSteps(List pair = new ArrayList(); - - public VirtualEdgePair(EdgeIteratorState iter, QueryGraph queryGraph) { - pair.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode())); - pair.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getBaseNode())); - } - - public void setUnfavored(boolean favor) { - pair.get(0).setUnfavored(favor); - pair.get(1).setUnfavored(favor); - } -} diff --git a/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java b/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java index 02521cbc..3f2ed3fe 100644 --- a/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java +++ b/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java @@ -208,6 +208,7 @@ public void testSmallSeparatedSearchDistance() { MatchResult mr = mapMatching.doWork(inputGPXEntries); assertEquals(Arrays.asList("Weinligstraße", "Weinligstraße", "Weinligstraße", "Fechnerstraße", "Fechnerstraße"), fetchStreets(mr.getEdgeMatches())); + // TODO: test these individually, not combined? I.e. this would pass if both were zero ... assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 11); // TODO: this should be around 300m according to Google ... need to check assertEquals(mr.getGpxEntriesMillis(), mr.getMatchMillis(), 3000); } From 0f3fafc01dac907d84752ec418ee9621981f7782 Mon Sep 17 00:00:00 2001 From: kodonnell Date: Tue, 29 Nov 2016 10:06:54 +1300 Subject: [PATCH 5/5] tab to space --- .../graphhopper/matching/GPXExtension.java | 6 +- .../com/graphhopper/matching/MapMatching.java | 120 +++++++++--------- 2 files changed, 63 insertions(+), 63 deletions(-) diff --git a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java index df467d47..d9d89354 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java +++ b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java @@ -34,7 +34,7 @@ public class GPXExtension { public DirectedVirtualEdgeQuadruple virtualEdgeQuadruple; public GPXExtension(GPXEntry entry, QueryResult queryResult) { - this.entry = entry; + this.entry = entry; this.queryResult = queryResult; this.directed = false; } @@ -46,7 +46,7 @@ public GPXExtension(GPXEntry entry, QueryResult queryResult, DirectedVirtualEdge } public boolean isDirected() { - return directed; + return directed; } @Override @@ -61,4 +61,4 @@ public QueryResult getQueryResult() { public GPXEntry getEntry() { return entry; } -} \ No newline at end of file +} diff --git a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java index 1416d08b..06dd78cf 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java +++ b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java @@ -177,7 +177,7 @@ public MatchResult doWork(List gpxList) { final QueryGraph queryGraph = new QueryGraph(routingGraph).setUseEdgeExplorerCache(true); List allQueryResults = new ArrayList(); for (List qrs: queriesPerEntry) - allQueryResults.addAll(qrs); + allQueryResults.addAll(qrs); queryGraph.lookup(allQueryResults); // create candidates from the entries in the graph (a candidate is basically an entry + direction): @@ -199,30 +199,30 @@ public MatchResult doWork(List gpxList) { * are separated by at least 2 * measurementErrorSigman */ private List filterGPXEntries(List gpxList) { - List filtered = new ArrayList(); - GPXEntry prevEntry = null; - int last = gpxList.size() - 1; - for (int i = 0; i <= last; i++) { - GPXEntry gpxEntry = gpxList.get(i); - if (i == 0 || i == last || distanceCalc.calcDist( - prevEntry.getLat(), prevEntry.getLon(), - gpxEntry.getLat(), gpxEntry.getLon()) > 2 * measurementErrorSigma) { - filtered.add(gpxEntry); - prevEntry = gpxEntry; - } - } - return filtered; + List filtered = new ArrayList(); + GPXEntry prevEntry = null; + int last = gpxList.size() - 1; + for (int i = 0; i <= last; i++) { + GPXEntry gpxEntry = gpxList.get(i); + if (i == 0 || i == last || distanceCalc.calcDist( + prevEntry.getLat(), prevEntry.getLon(), + gpxEntry.getLat(), gpxEntry.getLon()) > 2 * measurementErrorSigma) { + filtered.add(gpxEntry); + prevEntry = gpxEntry; + } + } + return filtered; } /** * Find the possible locations of each qpxEntry in the graph. */ private List> findGPXEntriesInGraph(List gpxList, EdgeFilter edgeFilter) { - - List> gpxEntryLocations = new ArrayList>(); - for (GPXEntry gpxEntry : gpxList) { - gpxEntryLocations.add(locationIndex.findNClosest(gpxEntry.lat, gpxEntry.lon, edgeFilter, measurementErrorSigma)); - } - return gpxEntryLocations; + + List> gpxEntryLocations = new ArrayList>(); + for (GPXEntry gpxEntry : gpxList) { + gpxEntryLocations.add(locationIndex.findNClosest(gpxEntry.lat, gpxEntry.lon, edgeFilter, measurementErrorSigma)); + } + return gpxEntryLocations; } @@ -234,44 +234,44 @@ private List> findGPXEntriesInGraph(List gpxList, Ed * empty list. */ private List> createTimeSteps(List filteredGPXEntries, - List> queriesPerEntry, QueryGraph queryGraph) { - + List> queriesPerEntry, QueryGraph queryGraph) { + final List> timeSteps = new ArrayList<>(); int n = filteredGPXEntries.size(); assert queriesPerEntry.size() == n; for (int i = 0; i < n; i++) { - - GPXEntry gpxEntry = filteredGPXEntries.get(i); - List queryResults = queriesPerEntry.get(i); + + GPXEntry gpxEntry = filteredGPXEntries.get(i); + List queryResults = queriesPerEntry.get(i); - List candidates = new ArrayList(); - for (QueryResult qr: queryResults) { - int closestNode = qr.getClosestNode(); - if (queryGraph.isVirtualNode(closestNode)) { - // if virtual, create four candidates: one for each virtual edge around the virtual node ... - List virtualEdgeQuads = new ArrayList(); - for (int favoured = 0; favoured < 4; favoured++) { - DirectedVirtualEdgeQuadruple veq = new DirectedVirtualEdgeQuadruple(closestNode, queryGraph, favoured); - virtualEdgeQuads.add(veq); - // create candidate - QueryResult vqr = new QueryResult(qr.getQueryPoint().lat, qr.getQueryPoint().lon); - vqr.setQueryDistance(qr.getQueryDistance()); - vqr.setClosestNode(qr.getClosestNode()); - vqr.setWayIndex(qr.getWayIndex()); - vqr.setSnappedPosition(qr.getSnappedPosition()); - vqr.setClosestEdge(qr.getClosestEdge()); - vqr.calcSnappedPoint(distanceCalc); - GPXExtension candidate = new GPXExtension(gpxEntry, vqr, veq); - candidates.add(candidate); - } - - } else { - // just add the real edge, undirected - GPXExtension candidate = new GPXExtension(gpxEntry, qr); - candidates.add(candidate); - } - } + List candidates = new ArrayList(); + for (QueryResult qr: queryResults) { + int closestNode = qr.getClosestNode(); + if (queryGraph.isVirtualNode(closestNode)) { + // if virtual, create four candidates: one for each virtual edge around the virtual node ... + List virtualEdgeQuads = new ArrayList(); + for (int favoured = 0; favoured < 4; favoured++) { + DirectedVirtualEdgeQuadruple veq = new DirectedVirtualEdgeQuadruple(closestNode, queryGraph, favoured); + virtualEdgeQuads.add(veq); + // create candidate + QueryResult vqr = new QueryResult(qr.getQueryPoint().lat, qr.getQueryPoint().lon); + vqr.setQueryDistance(qr.getQueryDistance()); + vqr.setClosestNode(qr.getClosestNode()); + vqr.setWayIndex(qr.getWayIndex()); + vqr.setSnappedPosition(qr.getSnappedPosition()); + vqr.setClosestEdge(qr.getClosestEdge()); + vqr.calcSnappedPoint(distanceCalc); + GPXExtension candidate = new GPXExtension(gpxEntry, vqr, veq); + candidates.add(candidate); + } + + } else { + // just add the real edge, undirected + GPXExtension candidate = new GPXExtension(gpxEntry, qr); + candidates.add(candidate); + } + } final TimeStep timeStep = new TimeStep<>(gpxEntry, candidates); timeSteps.add(timeStep); @@ -353,15 +353,15 @@ private void computeTransitionProbabilities(TimeStep virtualEdgesMap = new HashMap<>(); for (List queryResults: queriesPerEntry) { - for (QueryResult qr: queryResults) { - fillVirtualEdges(virtualEdgesMap, explorer, qr); - } + for (QueryResult qr: queryResults) { + fillVirtualEdges(virtualEdgesMap, explorer, qr); + } } MatchResult matchResult = computeMatchedEdges(seq, virtualEdgesMap); @@ -598,4 +598,4 @@ public Path calcPath(MatchResult mr) { return p; } } -} \ No newline at end of file +}