From ab0dc46ed6f46d96b823e05508c469fb246ad9f1 Mon Sep 17 00:00:00 2001 From: Razvan Mihalyi Date: Wed, 24 Oct 2012 11:22:27 +0200 Subject: [PATCH] update svn to r733 --- .svn/wc.db | Bin 600064 -> 615424 bytes CMakeLists.txt | 6 +- CONTRIBUTORS | 7 +- README.scanserver | 1 + include/shapes/ransac.h | 1 - include/shapes/ransac_Boctree.h | 12 +- include/show/show.icc | 7 +- include/show/show1.icc | 3 + include/show/show_Boctree.h | 2 +- include/show/viewcull.h | 11 +- include/slam6d/ann_kd.h | 4 +- include/slam6d/basicScan.h | 6 +- include/slam6d/globals.icc | 15 + include/slam6d/kd.h | 66 +- include/slam6d/kdManaged.h | 75 +- include/slam6d/kdMeta.h | 87 +- include/slam6d/scan.h | 11 +- include/thermo/thermo.h | 9 +- include/veloslam/veloscan.h | 4 +- src/scanio/scan_io_velodyne.cc | 19 +- src/segmentation/CMakeLists.txt | 6 +- src/segmentation/scan2segments.cc | 159 ++- src/shapes/plane.cc | 2 +- src/show/CMakeLists.txt | 9 +- src/show/compacttree.cc | 1 + src/show/show_gl.cc | 1 + src/show/viewcull.cc | 2 + src/slam6d/CMakeLists.txt | 1 + src/slam6d/ann_kd.cc | 28 +- src/slam6d/basicScan.cc | 34 +- src/slam6d/fbr/CMakeLists.txt | 14 +- src/slam6d/icp6D.cc | 3 +- src/slam6d/kd.cc | 135 +-- src/slam6d/kdManaged.cc | 156 +-- src/slam6d/kdMeta.cc | 164 +--- src/slam6d/scan.cc | 6 +- src/thermo/CMakeLists.txt | 27 +- src/thermo/caliboard.cc | 241 ++++- src/thermo/thermo.cc | 1516 +++++++++++++++++++++-------- src/veloslam/veloscan.cc | 6 +- src/veloslam/veloslam.cc | 4 +- 41 files changed, 1679 insertions(+), 1182 deletions(-) diff --git a/.svn/wc.db b/.svn/wc.db index 2ba34b2f1e477483c024fc0e9d9c04284f09dc5d..d2fcbbb84e5ca582cd2f1fc8ed29235b8da02588 100644 GIT binary patch delta 43451 zcmbTf2Vhji_CJ1S=9b+M0)dc(-XWnR+Y2Bq0YWb#NM~E90s;b}5)%}oJnCXb5m7U&YU`X^Q&!ceyQ#1 zMtZ&56@`vj)1te$>ikj5C!)o5NNM?Jb(*Ew#*i}#o?abKZu796rcfKD&WJJmrR(J1S#%aTD*h_p#8dMR z#ZM0Vn$m1sI{1=PnxB6;ndJ}?zjkIC0r|*T^4z;+6Ap|3De4@%8QF z*{g2G?deu5t$gpQg{T_;@NE5f|F!p_)Okp&AOCIb<5l%5<9%xT6-HiF=sMPqyh`J# zfu`l1)_qD)7;&qYgwMH*jGEl-pkgW zuh~c%#ka53Vw<>NM-(E!Qmz4ha`A1s3$76Z%^N+;jUH$l? zNAuz<9~mgBV$|{PcBIDd-El>YTUP9E7GHTNJ6>mpUlgCk;K%OBOQ{Vs67z?-yPaK8 zo|=Dv#QW_Fp^BXAihcR<_}&R}=%eGWRZbW0iZKk`o1dA4ck3xvE6RT5N0LRxlhx#T z@{4kHJbhnjY0BY{GnD;)F{hN7;ZI}~#J zJpQmV?Du)t4Q@7#@)aqzobu50@!e^XSYkwW~mkJ5;B2lR5;wP7w5#3h5DCi4D!cK44Sy&i| z1pIzyp~K^F1p-coKP;edL<6xzql_vrCdn8)ow1ev(FuyT=D??`@$%Q7qzdx*hIt#NqIUVlJPj5CCJ= zqb_%}Fzok-BZV%f&rOd2oM|W9r!AT~b@p}hrcMjZP0h6+>@nmL5PAcbg3y;6e{FAl z_kwA$g^}xK&diybUFDc64Q<41#oi8r9kvzhuyK6*Aw6FC zdH}6Su4aEDKmPu^QTg83U2lFv1%0GzhG1+zQavAict%U`WrNtfWZ^I6h?V-c`-oH1XiEJGOpu4b~hqg zN$b*PnY?EPNn^!B6n=XKX&zsEIEfc!kPcN-7j#F0p2Bd%AC5-d{=$$i>JRw5g^{Qi zoXw9%v53p<5B+`Wnryy|sq^_R4YoCP{H_dA7xZwVmY@fT0?j{qIDsnfeRNPvm>`m; zKOWJ1i?UtWt?X3}DMys!%4f<+@l#FK2{6BSu~R^v~Cd8IZC`_p_TY10SA!UeuebrXi~ zLavd`*5_{!(vNp%WLly*a}Oie>Eb4dyns58c6hB?WMiTWv$7T$Lx_!M)Fyib-Tz#V z3U7r*tTH_VFkp!`$D-mP*zP)4A?7W?}jV@!y{|Z2YwbLfwC7K^ zB~8T&Pr~DIJhvSgCmwGE*c;oCeBP@)nSufddqsQEffF4pP{;zhnu)#WNq!)Uj8o~y zBu#eEG@C^50))uX{NoN}GIw<#W2<_+zXQpm#a($(ciNKgtVeV{qa*2#r>E0xY3AOJ zq&36MM>%3CC7$t$T+$~+;>bM;y@y>%PAI(Pb;eu#69@TCWqp*#(d)KaFQ7`|4`1Pu zQ!m9>eWli(XS+!ge!@wvssf|Gi{$V2kRA!bg;1PfwBw6?$ULl{&muXe+aHM)LRxhDBCd!h1b*&y7J7YdSI85H zIAT$UFA@p_JnR~t(}--~t1^j4b+KtA-t$ln|E&?J&8K7%H@7t=zwq4KS>oX!>{2cG zssJiWgh+it%b~QcetfuaN8MTGk&^ zwTtgBAQM#jEoq3mt9jG@L~LlXk+S|+@Y9Rg1nwUI?vlXXM+Ts@|3Hjeu4cPuhmrAiz&v~`*_NOXK7?fP;5Z`m5y2=b#*sdP z0WsQ1p{LkA`1&tqrIL~&d|k!cOdwBzHY8>b;_pw8TnNGkpD>Y>p=rV2xiJazbI&Bw zLlj8$Z4zk!Cb3pwL+Dx(4}H=WD6+vk{1QtgJf*2Ilh^HG6!7CA0-3~WV^mn;8$gAA z86mxQM~Nk-^q5X^c#|pQ3p9AU!V2i^7aBY?mDG{Uc^iOznBP8KQYtu(KmkF)X5|br zN8q?%w!GIY!71c}ve^V1l_W5;Vm6s8>Ijj9_nu2OCFbUrx#WqeI+gRtd{IY^x6cBy z1=kW|&MzR3R##d`76^Ql4fR_h8sc9qA^Y$pwh6Pch%`yU&1)~vHvEs<1(b3T4&Fgt zNfeuVmRkU$jWphKrC2!0Ld#Z>M{p@QUgav1ud^tWqQ%@;O|p}fd)X3pzp|L^V;^89 zy7St%(>lCaU!teXh>n~ai}k-|4y1au4sT9r1ARsm*U9{;)ucC7Q~2a3NCSSqNenY{ z4H-lk(1AOXX$J4!!O;0FangvYsr>#pX-d_)=Ds+YS3O&O%gBROQ2kLRXF~9Mvw~xA zflC)FKLLw?x)+jUA^h(Rj(4f-LDvf7BZ02mCPpVD$d*!4pKm8WT;IEdG zTD+{BwB~cmNgLj`oOI&p<;1~H>>^Dw)wBj0WZ3E^=1+~yy=J!BfY<4ZRmMyvQ)MY{ z*$+2&?ILx=vNqrqTZpZ>az_)*?!WM6LXn*p3C#k!)Z@>tLCrZYk|djQXH(7I)kb*e zMUrltI0`RTdt0z6`M(f1?PcHAg4-`^vkTLj8pJ$m2v=lx`PxUUl9D7 z*Ii`mVM!tXdy2RjKPt8#$xP=w!E&<5a3QmFP-ZLmcZ+g2f9`U^$X-|Fh!$9tO)yU27e-5(UP0tDJ z7BY}o`CNjVowTL#{1+rmN@0e+D7lv087p2CT*hX0J_rLMB;F#0b)`k*dGxrUeAS7% z5a;aLK7c04i5DG`j9Q9vKOd45B*?(5JWOVZS?}~ZIe;%IEG&E(B31Sqq^PQ(;tkTL zEjg_;7gM)Zc}jUp`2{pz00DLx*6FY^o7ShzsEgWoyEid);9V(k{Y-tE+8Bs6&_?2` z&NMg8H%S+w(kqmVHNEQd6Q9_0ZQk67TEOccAsuP~Qt@B>F$<26PlODWIuVA%T_1yg zOnH|qMS=4U%*gq7tNAsre2?_Q;D;;BM~9R9`I`UP+MC|@$qd2=ylR__J!?3>{}{Oe z?@E^4^SH$v#9os80m&;nL573L#qa8vV-}wvPr}w{%|ZXq#O@+RQtflH8O0V?{G5yx z`-7CrI-Ddz3y@6h2bO=52vN@3_bPtCCePYK40=N$Z_sV5AH71%V(TBt zc$Kv$Bhm1`aiQFU3(njBEW`tA$a{V!efYLBQgHn65auuaXOb#}$_27e>< zV2SKbMnYVgPHqLo?jc9WX_`#i(VlcHoljTNhv{DW7X5}X)`&S-0h`1Yum{;5_B#8L zeF?a`@mV)A{myMLt7G_rzIZXgCWfmts@6(;CnvsZC%#h>-*pn-bravIVyA3m?jkfm zsG7!|jHYwS=+k2C5(Q<+7mQ}gXQV5Cz(F&lVV2Ts6nYKwk$ix)%v`6^g@iw%(Kj&% zLgred(>(4o=v+*4uib(#88p8s{f|{g|F-4VahKHJjP!VIsHn z_|(JosG4pu?)xIxPcgXMB-$vSHWOUAn)3^*TL zTQYHTTP@m0a0e+4X(`}8$N2wJ===Dx#F#oX-$M6Y3TsSu(KVQ_x%}q3bh<$It+;!h ze^r;3;sq(cE&Lrg_IN670%H=DlA{zh`$0NsT%WGAkj|{EPe+SxC1qu$(<%&&B_cqzFJi4+>Dxd&@x3!=jWQqN|Hn?o6~;Dsmclk|DIKTBH27Ssy61G=g`jJ z4NzGJskKbd;7f$RyMlT6_-D1Pq06*svG4`iKHw+}dBUzj*mvDdx7X!?xz`bZ4GaoG z)aCO=gZL9GEM!r+9kraU=kN{(G+%lWjVdWLRiXJbl@^0U{!7204l}(CJxAPs#fH$F z*u2~-W0%^rz6SH|pO8gu#83324fxpxBuN{{45jIsUJdnjf(KJ+graIQ{%|`w3D!Mf z;rr7c-l#qOTxt^J8-+O`yzf=!UdWT#}p$^k)#8S##y+;l$$Fqsw^6albiQzzTYX7x zslHUZ@Q{->&s4izD9XEV+0QI-())z$DBNS*=c7_%@KG>DiP&bPpH7w7DE90w-Nkds zDSqirg$5!B$gJ!^!-B60LGwru6F0UuU5$rd_ojQR>*UdHq8&*swFl5W7D@2;2hjgn zbugy`A%}`O8=+~n7$RQ=sig5AhS1$87i42r4yFCodT>3CS60_l`gvv(md96p0SWZ` zMl_56+EVM89gBt|Zf6wwYry4l1`7RwLXX$)k9z!3*mMK_a2Ows2$braR@zicq}q7` zIGo*5OXeeAF|sH<#LHS~_joSQM9>AxUD#ddh=ilzpd;uFIHQr6Hx~B0LJ@zk&>i-K zy+L=BjpxqRTKCTGP}uEsI-*Wb6ec}i)a!AC{E>(&43k7SC>tVqBIn~~QGvWQ^&2)f*XkUJ1`Md3;G`$90k zh5SA@o5SxKO%wEYxS^Uu$)eqpA$up7P5ZFe1tou3Hv;^nVMOv5DfCxHXug*|qNLN- zl=>l=OrUe1Xx>5_WID3{IVPCVa%Tx2eC21GgY#MTZ zA)Tu-n^K==6~Y*LsE~GP2V$#dQC!vMMPAM1rz=QW`gC@YXX)}8pE-s0Y@l4^c?N$L zkJ_`ZmAYhwLc76pP(rt2PJWW0zD`-1FA0tH_HTzXHw#wL)YhttJ zhvvikn>)|C={#j_XwFoq$*nD2xJrRfJhtlgE$El1)vAMLzw#eij82<3efG3j^E=Or z%}0YR8*2``_u_TcHh$?`iwE@#jaa|N6* zUOIjLm^&H;<^_UL@akBhFAyw@dD&>5ldjI^_oS(A^MQ0VLQHi&#OVa;HC{lUOKd%S z^E?60!Yc60%7t`DwWQX3i4^Cg^l)@2L&c(MB!zj6b{C_RYG|Dj`roSSlO)x61H0WgH*>;M$!TNUui}%-`@?U(I4l~t^DpeG@Gx!miFlE55tY-^fQOMz=xnW8Vdznk$@`}ae4w?HcIX)kC(9=e&$;GC|~p+TRZ-*ITVuc$9jA7g}GEG zyGyAd1|orPyi$ssQbzr3r5wH3w9U#@)@bDp-n>kVR!Ra#yTXd!kjq&So{maR^K)S~ zNTW>^+MiB@b?ILEB7L8pW;$!k9IOu;%Vx7%*xhV9dyc(L-L$FcxC=(_DmBU8)WVi# z7sa8b{PcR7#wV<&9aXwZX}|;PXo6U!-pYrc^_ zje$z(?3<0W64z4yt-Oy85(^}i>6}eedclOBC3B5f^UZQ}(vY?pngP|IusRgtpi`L_ zvg6gf@IhLr($`54ca!-m57H%7(>d`Ws8p|Sp;4}Hf#=Rb>{VN6o?6_m_$EH|aq8kT zwo3HcGGJvv+3KOUuTkvcOJm9$q#d-6Sfhm|peO!bDC_$D9tI2AEYk^^e%p=zIf z9J3t0-#HZW!Jz)X_{TY_!%2m~?eD4r*T+?Rn{5DwKUG4JPlS z_u|@;s`t@8wgFgylM3BrChezUxm%zylP%ExGJZAE0^m;;fQ-8IU=e zmrchI<~>J6Fqy>W%IB)oz3ni#knMCHmhm=z=ks*DP!BBY#_8v21TTDZhEAfN6!3X3 zP((Wjgn1iZlCUdZr1^E|C@k3`1u<)1(HUeBulo|6%}~h6G-Ey1u9J!z5$)EAk2-ik zwb(xk%k|PhF|C%afr9!JC_U>Xx)f!YqcpC+OncWVE+}c3VIOYmXrI7)y+~W|N&lrn z0Flze0vEKmVNzInlSFpi;$c6)y)BOC~G$Mv7 zTq}?B@H?;zSop}leTU{HT~pFJ!=8)qgRcCwv#={{#nvvG6mu>iAO0Q{u1Tpp?|+Z> zN~ROAK-(3}<#xtT?=qqodI|sGeL7ZU>7*{YnZO4hlP|$J3`4^)+9ST>${bW{%_kg} z7%TOu%>RJ^e?Lwe@+lwCKIEAB_y=^Ptu`Ho_ID~|Cw+*GM{D1geMyHgX29=@mWO>w z`vaWyU(!+B_!=L>zoKCjNP2nkE83Hf|C$a)f$V$l*ECPq=qQaTG{*AEQ}eeFzUx~$ zl1Cq;lem784ny5JrZJFjI!Sx0#le!!d|zAEn9ux~+WCy{B>7s%`CS5)XqrOP5Qujr z0D6yqd5TUF^DE3)FY$uYbUGRY?zH7^pQb(dzrL5yU_R^fZ@;H??8Vt7?hI_MZ8_rk z(yeZ8_<`nFgkx6XlA#44qhw)AG%W4UG+hvaB)TttrOQ!Qn7^L-1QWRZEFFVq7Z$AZ zIeIk;FeWD-vP(7i-gER6%&t3rqc;fmNLgce>Bjy+A0{c=fY2iqY}>z(k!&0W5WKTL z8_j4$;isRZkMKc+b*XAwL`Pzr${glyu9M{&?SBpU!RJ+0U)B^^G#_QFF=0_=PILUCqzA=hUrHkpm=tjDe9-wd1 z&p?yiDoCaouFDR=MAy}H)no62r}H`u0ItoP)quSRom;NXghuS1#AR}0CU~61ks7mp zTs;B%L+b)H2%?zIS6^?dC2g7JiY6?X)sfmD>7$T7=5TdHoKTCl>ZP9bUgDO*yC1$p>7*ZlqQ_O4|e(qY6EUL#_1Dwl%CR=WHWRHRfqggCZ zGWTKddNnl0@xkrc_$rjHY!7y{s2y930x5AA?LmPRZ5c!-Ddu|Hu{;@|{HJJ>wFriR z_Q4wsQWt>utXLs_q&e%rH$J3$%n6;?8>EKq2mytTh|>*m-5ChD3;oW3KNgHeV}3sn zKIZVoJmDY$n1gI2cjT}`We&Cj3}dH*U8?~wxt)7`Oz?cUIqH748SqK>g`X`K>|fRy z--Yq2I=0r3&vUx68xjMl>{`WV?^5Vp2|oLx6?p>g+ZuGdhkyAK9i!6o%D-_pfV-|> zg=kK0aNDk65T)9ejL879w&jzjK$e=-mD%}CJ>VF!D#0Sih6%TNN-uOPob8jBBTnk+ z-mHKKL##@eLKxYH;L#rF;0F$!Ky1&2eb|^<#eGV~3Hhe2CEui3t>52{8Sy4(lli2+ z%rAx}JYA3W#iI4^$0k<|tsm}tT*<`dD(9i{N|v{~fT?PegkH7sPrg;DH_|fREI_(Q z|0+N8I!uRIcWWtp{au=i8w*($AAh%YjGOP<+EaFgnX_Jdo$zn(V4j3SWwnA>tAT7_ zd1`($N=ZwMC#)9xOC}*WK7-Hc!D{n8qgXdU^vftViXXp9>{wDL9X5*P)t2a~oKloi zuuJn$3;pB9#fwTEnRX{Q!4-eu1RkF85#&?3;6+}a&k5bn<@S2LZYa2gh)DGVeVjoT zZ1xcZsQO^(3(>bhUZi4_$)JyzWcTnJa(@)t2xNwGd=pl2Yo- z#KzdtKClM7!T{M@;j0X_Aulr24wwLGB8&L^kx*ec7Kj!)5l9;e#oU2t%;^uff?gOW z!!CEkk6`nQn8@lBUIyGg$ED!*SoA1ByYYwEsPw8ywwad!toKsP%;m=qs6ZVwMmlHk1toh8^PXU(K#m z=}*LtyYaYC9>K*{IQ%*3VT|U%v4U|~6l?ax9{$c))`SloCw6@ycbJvqSX?LySQFuS za&ClqKP|%6iEE+lS46-pEllI}r?4_y!UAJfPGPGBmyr_N)#K4{>2x+nHUMQgd4`l= z1fM{76U$cxu~WK1p&Qsha-2Q{82x5HShC%lZ+U|qk(2w_Y#vwbm22j3t!2mzH@c>~1D zCVTC+Q%cXewEkAcN8w+_9p**SiL-|$3@&yUVk!vtJ-xRYZB3no*q~l943yDS9 zQ^KG}v@f|{NUJUQ^-HS-RkLy_yGm_BIx0UPSo0oQ3x4t55HNSK7W7NlFjupe&{qpS zq?B!?%23`Rh}c8Sb3cBzA@SHS;Vs=FXXRfTjU0aW?QB3+%uyKdgxn61rQt^&M+9uc z8FhycfgA~V;F!R+!(Y!93Anb3bcnMcd*$q7LpGQ!#gp0IjhSv z|6^;fGZ%E4bA0@A_E1NFP*+(h7CeQ$1Sqctyk9^hUPOCRJ9!Puu@ug(4$Aj3a$_vQ zQC5zp0&)bJ{_i&G7Gv1NUzsP%lzaK4Rd6qpANaU5Rg6k0lVBbGk1Zz&L5m9MFYi}$ zp*cy2+)$_?gEh(WT+!sU{NO=DPRs2)>sigiH?Ck^_|z3Fz#S`CP}?qFo_&nL)Ewh` z|HZoU+yBM9Wvki$s2bphSF_es^_xGgW>YCuJ$z!EZI-N>+7-Hu+S%|my)rl=#6IS> zJK0tN`|lRz?YvVivfQ8Ql(97yLO}!NOekD}2bz@}v{9Q51~P3@$R@f)YKfonA2zU2 zDm_b7Ebvvl|Gh#Rkz2)xdw@%C-)rGg-8B2aHN%qm;EfWAwMCfMY~egXOcfusbd8!87m1 zFuHGrIeYX{mEWHYHSPJWFb7x?w)x#wHX^~3>hr6%OPU@o{EoGDu#KoBII>x}gDn-6 z5T5WPzTg>8<4GpFPc$Ok=O$YwIFzKp%qP&{l4qn;CzZhDJ%U4-kM3csBpVa)b%XcH zo}_d`_c0MYCzA2Zz5CdTYH%jKAoVn1Lri&5&X~pJUaZmE=!2F5C+vo{|B6N}R{LKz zNVEfWzYWiMUGiIb9X|x=BKr+?Gad@O;FWI(-qjszwOk>~A)E_uWz6V7+OrnQ=3 zQ+_+kZ^>Nlv z1ZX{|u;DP{{SHu}P@iC`RH#LW#AaaR(x&`W34#Q7?gbY=_>tgpQk+itSWcof?2p+% z9pcY@Qi?iG_~6gQ5M{l}&oROWK9hR6G_u)0XMOm$pRx&Jw8F43=`+^5EZH`wXt9nY zJNp<*D~dq?f?(uBz;XamHX^-U-dLeK8uJB0@Y#wyo{+~G@=nceb4}FSqKzjA|0lFi zyyWz|1MrguBW|B73M+aj81uQuFT1{G^RB4d3%SMNc0<|<2g9MlND#N{Ahns&Gs)hrn=pG3Gls+ zB4pPDoz5`8N0ht6jr=^PKNxX&orv2Gk6*U1X8TdM&mD1KsJ^f_;`fNCZwImsBQb0S zF1It}3?ZWu%6FdvtNlWE$Q=!b{DDBkEnE&^IAEgzpC=M=hohc=XZ*4SMK#+G;xW7{ z{y+@M4czVCLXQ*E;|sbX5uY#MMfPP>WLvttR{L1qB#A7_Fe25VNcoT{8Y+ycPh0K7(;bcm zV*mh&T)vRskD&Cz2*C6EBkYz@C3#ebpgOeO276i)I+JeW1AbvSldVW+VRWhKBvpI;LwV1C zDDVCcZR{WiJp##7570bq(QdoKFeRLH3TIdbZr(gLsEVGRK zppYM+CLgYu}1(VcXa)_7uCH9b~Vv zBkXlK!~cRw9I=$GNMHI@QDWDmv~I<@FL|r7Qm}8HUe8C0F9}aLFiqzFIN{pUL>I zBx=6J57klK9my-oG%^#Vea=5w^v?G*E2_ornOJkJS`1`5q zQ;n2N)Tsi=8iG};m$|*3dNpYx!?9M?496np5H1=)7{)=~tcBKvuS_$N`2DL`wiVHI ztzFMC&)k9_hZ@E)@IHSm6byTi178>@bOoY5ugejRAOXqiMt*+S;qxHx-^Gf!>o!_5 zBrAmN)$hSR7Yq6PA^7`UA-MNr5r@l-+#`3)69^%Y$mwI#cW0`dql>@3mYwith5QU;9x9QUKiX6k{s4)zM@^7ku$kJFWiNHrkZCI3Wl8+`y%1$L`qf&f zEwbhKUcUw~u0t&8=}%kxb+nIOu`U;L#gKgW$xO4H|ce5c{$M6wB-K{cwy3Ob&xe^E&*Or>isA4*zD%2aQBIHxT4FG zV*Wnc$&)Sz-b-4?#ANf;&DBhjy*r?`s@CRwUDSsGp4<|=IiT5wUDZ|CNQ5i2qN`ef z9qT5A^`tl9Y=hMNRp!3Bnw`LOpFzp3DA>#QT%jVX#Zt(+c2^(6wPXNiyQ}#Lo9_mZ z6SINb#8Y~zzpHGmGMA^$(XZto-t80_@u~SU`AxmR7%d&<%U-H5Pf2Uj#@-3SX9$mB zYmhh4)J(}&2V>`SDG|IGf(;K}PxiK}e_is=fJ|eyUK? zrBfH-9u3k{MdkvMheT>;B7I0WCz{-^^{94EbckH$fg>zrcIu$NnaFv*LboBW^#lHV zf!c%jpNklxl?7_@oudtCu>IA0)2yRgun#SR@egy9)g{TO<$+McIv85xFju8dCMh&&t8Ju8^+p<85fsS_44FCYtZV^3$6yV3&rxbu3eo7MQECT@Sm;MasokjR zXyiECek)n^6NQd9!>RmYJ}lvYTo!%;NJq#1FL-ee?_*B4jTL+=#DTpiYLrkghmrwqM-Hat} z#=G6CCUJF|dO}6MY(1WHubNplT}9AIIB~}drmKxq`e@bd67iI!SKYoaT^**fE{WS@ zE7OkbWOJ%2jGBSh+51(u|C*r=gAq_^BPE2IAAbp!wD#986!Hb#!ver?lP|MrB3mIy z#8FTpu4ysPzDBOKq~52mQS*`(FP$LbAhPYfMC1uVV6If_@{(Dq5GF4~GX!RQllKFU&e!7mMvo3Z&W%>Vu+v z8J2K^nr~+xDs^d=!an7;8`Wl;Q}Zd$TB0taY6re^iHhXYEPi;2x=={x1x4!JIE+S7 zEv%HtCBxaO{i15l?wi#+1&ETbJW`_GT?OK(rRq}Av52cdvO&GFTT~%pFH=|#x{SPt zS-Z!4vva*+)tm$yX3+JBzx= zN65QgO`d{@Hy;@LD8f}wu~g<_!`V!B8{1^Ih#KjnC`+>sO(-k>9HR=F>t)Fwa@J~A ztO`&?m7eW2`{0YT^{41m)a#zD!HX#6oq26*wpBQ_rdlT`{fa~;$%fz6<~Fsr1uP$S zUTyi$3R)g_er-$Vlgrcss^*%HmSGzgvcAYTDtpzYoBCa9ZuKJfU$4SKDH+3W>%}Tq z#;tqQe&ahS>lN_Jxe(~fz_v4S_Q;RQN5~i1t87!&L$u#Vj=(rZX(moJ7>nTMe4xS* z(hak{jqXRh$rsRH>#{a103I=)m9hHzm@p@ zR^t6ziRW)6?!T3|YLr|d`K#)zQDl`kE|etlf&2cgqz|8bulft@@qErkqzlj9sJ5qS zKEG?D`mT^p=ijG>A)T6U-={jogv*55W}DOlxJJ+$Qd!JLH>Iy_{BbdB?ZVzXjK` zU!~yljQ3K$*Gu{^5oQm;=e2EWQ)DF=rF}AySl80t?=N1=?!0ol+7r+Un#KuUm!((H z_W;~O%eU0-TqPCv6>{>yyDtsyB@gg&J0Vw|*seB#&$XyD{~|&qVuJ#F)($CD3Z7kb znMY9keR9c&zLx{;rNl~+#IRzA+6d5gDa{k=NBxT&;`PZNYDa+yVAF$+q05IGxMK0D zIlGioy{7)~R|(wh#*s_s)pX0<%V;}=jRbv*K>tkIx;VYGe z&2O*Eh7w`G>NZ0}CeD0M-7I+~n&+*L%WkDqa!Ou{UCONdKpiQ-m*P;{&(wb>HnNkS zse7u5D?eBJWr|ai$ON)R9NK6dd_`Zv5sA<8Mqj88S$e=_W}&f3=1*U!E)nx2<&Lf= zCG5fh$bYl00VTwn@6>4me5=y;LO_>$?Ah-HZ%>#eeo*5uWDWlj_Cp)rX?@lCN#4!+ zNu7kkL-Nk-d`3M=QZ!|rqRb;67jn zzN8_4_FI@fj{YjO`MPIerM%*nMphvwno#lOyskTgRY{^+6aF#P`Ux0?tk;8S4Xl$h z(K$T?uds+fk2NmoSlS~Kl2d2^hGEY{bZ-}u^USWTxYV7UAR(|mAe_YqL*5RO+jF7eVql`9x12qwk zv_*lNzoYdRn^X?rCmSM2FC%|D5s^J1GJ>ZbGlueecd4Bdg4Xd}klS8*LRwX;LeGcn zR<)+!FOo@34IxLg8QL6Q}Im!yd>?5fI!xu=#U)jK%;g;xkl4*w)Y6X5|h>eSJu z2)0mN!pGIqg#0bt_w_Xq@Bjk?%(V5jzATNr$&ECvwMvu8Df8K$q=B#pt>GtrVfv7Z zrOTyypS8NgvzB~yy4KL_m9FjAL==TemMCNi%Z90Z#hPB}9JY%wLbKb6>Dwtn zyaw{3CYqpfshWM;M9bs1G?mOmo_Mjoua?SJG}H1>0>x$z!PKR%$!z8tK#EZAS_4(`0zoGksPv^ zcWkLm-Q7m}TrBe!?X>6cCHLBW?X^6yS;+}Y&k;Cc8Q$K`LEz>w^RZk_o~R|alA|It z_G*__R#l0*wMQhHq?!BOT7lX`%pDF_t3Fgt9Gr%@7blA;&+!&Nvis!hD+$rHQ?$S$C}h0}z>NC*`Gec;CJrnmi3h?pF=G81>ZN z+O2p=8cD7b1zCC8EmZ~k^R!+~Xe0RPk?7|RiRwpQJw+HQ3^wFBCY@KowaC$)1MMqw@uht&51saQUsQJ2nS^@4~ zt7oL~1+DCL_{0kpIPg48xnT)qWI;+;6;YLa{zm#Y@ z@t%+rE0$_9(n;#qP43Y4p^gOnyE_D0%8{D;mTMCQ0J74$wHU?KWg0B>R>8S4P1vX; z@2}um0Rp!#awt}f!0oP9;P$jHjcMF_+}4h=AWnfWdh&*M;@HSvzBE#@f}ubt;tDyP zUJ(W4Mc@L08{A%hVKf$wBHTC#Zshl~Li6mGMjqh}Z-sSx!k0#E-tB86n~(bffrK)) zaQucG&c8OM?cSgvqFkQ7f|FOI#QPs{PM7kmIP8_aq^#gAZYBxFda+QIi$$ui>n<=> zoNj(unK5jpxhAB|ru>zyx`(g2AI#uEtx>hAk@Kin84F#GtieP#d`w#_Cb|%!^Zv)Q z&SH(_Oq1=JuxE>tyq;dIwc}rG*Vf}%_5Nw@*`dvn?Fy{9Vz(y!83MC5k4AiGLsPaZ zmfGBBY6#mr#(#QRdsj3sUO2iL_?Wau?$T0Gso0~*Gtndt*!ODxK}~t>-32i1`?M7( zkTG(5_G$eQM*O29)1g7oo8)?%(+_AG;s4#QdHG%YH8+3aW23!JUz6;$n><3}Ic>V- zYkss`M0pP6IR}jmyPwxYwy0!~IWK6!x%2J|+97=5bWWU}`Jy&2G0eRvleEJQYw%f$ zH_VE|+Gz1uveCd=q}>iVqLqq@A{*hWBbu=I<0Z`QQB9#c3Z&#w<0Oj4x4;{js2N*{50YvDQ^omqbd%ZJg+b+SAI;K+ur-R)V@>kDjv#jaiZwYX(|5FAt_vrvh#U?Ym)PuC(mo}#wVgUbEyu7As7kY+!b@1&2%wvp>~=T zOwUiu(;JC0{)MjZ$F;<|3PT?(28}VM@n-dOp*{ok()c&^biv`QBW&yIhhnc* z;N<#r{WeikEZje^51Q}$ulCZqf zNg)nFtNDW>ACPM zM042&KINgRjpo&3My|O$OTUKf?xd#(043#^O#puX998EK|P

Vw6^$N@I(Ee9@<^t;}2-~tcLiah;Fv0yTOH`A-P;{%51 zh!d8}%@=qvm_|eOm7+~>5VK;aew7%3WG7Q%(4hy7(8Z=93%(m68Wuw`E3VXK3^C>; zjo?e-rlD@vOEW~?suA!~ic=_5sxIE+t{kubJ=J5hfIMFLuL&m?_}7=|=^WEG0Z z6r2;wFN?4<>sO2SmnLiUZ0w7)or9-W&SZkPZy2IJw9u$^1*+4XpCk-i88f=bMaA{_x(iLt(0qQ8o3 zxlDVP>K_1pNt93Aq5qOVsLJK~hX7mJT2!Aci@&>4BEFbp928+a6ohQ+s^Q^U@lZ+# zm2v%~m=6izzV-T7vJn_^?$P_IP3kB(d>};b#F=~uA0nNr^c3b)e&x;X*WZysSoURF zJJ@2g^8@;uv?CjX%!C0*!yr45eeevNEccLzQUaIN>$DfWo<4!Rik9ppWdIY$ytU{; z=;877g^4K$;L+f%!B&l z{Er+a(-vNmqQmNEa_k7AB17ff$&ER7tR<;0x2v23YEBX{5x6&4-v8DClH|fI`X^NJ z10Elx&G_A$HIX1e#_$Ot&9nPq{e(o#?E-b>Dpc;!^Ck6?M-}oYE$7KQ_1o|UZBd?@ zZxE3fbo)+${Br9X^MsyPtJt_TDt?%uqkS-+Ty8YvOJKU?rMq;8z=Z7}ogU@-^u!yL z<$6ConXFaFTGRHVUMd+>tM5Y)xncRaP`l1oGFkHF86(gX?Ijr(V6SB)_HHslp=V%tORj5N%Xe)fZ}FSz8h7EH zYacb*@~)`{$iDe9dj_nud3L1bmK&Y;&1ZF$udHYE7ERYPumB0syS~xi%9~AO%_?Ir z1Qmfb-^p7PAf`tCF4INwAuejAW!IY%@3z$?{HJF2c1X0f)$Sa{iEfdQ)9VW%K*$+$ zx^OU_6HGVkLIy*~=W?LHjT7fbSj_PboJ+B8J$}}qqcafmMf{OMCo(l0h4?9=0HQ{t zL9aWEylMo{`@A^&kBybDojQcPwS)KAd_Bdj#oFBtWWYI~3Kk-Z0io_O{P1n)8t#jrDH(v-$p2HV6HP%}%wB=oz8AwKKW^Acyp`y9bN0Kr5Q6WFl zpV2_A#6k5h4O6cM@;Bs5TUIsDh67*v@LyUP_a$01D_a|Gv9+IWXM8Ej>KSc!XBi~f zYV=cud`iWyuAP-Xks*Lu!krHpv-ovhV^b9n-F!wLdr8u=^bGqn+qkmUNGz<|#W1+T zZ-6sd*izv)`dIRb*duV<&yQ#|pN|L_Q$?#{XMKu4yB`MvKNB$G79?q@&nW609ojsY zURhPKsL9`+me`gx%COI{U1?v4G+n(@eDcc%zoRRT$m-tJ7*h>O*D6rP zD0DcEtr){ox*4DHoQ6gr|Dl^9M?`<4SyaqYec(0nqD+U;7a3Y^&3x(H9kmG zLL`~-QFR~BPc($0BgodQoMhnmkVI4a!ZqkVV)%?iyF6ot@tn983p~@fM|6QTN8fX0 z-{KnLrtxld7ZnR?K)cX*O|TZpCdx~UPpVq2SZWm5=8++w0MeN!-(u`t+!6br+N5R<0Y}@%hj&9qk6o&V!6>r82%7ega51? z$@@fD4na>Htmy0MeBBB|C?xXTAuEmivQ-AJy02IT6vOWkSTV~uG-wjW+n#$@8`DHK za13`-_{PWdXz2Medgc3R_1 z2G&~+)m}@y@t?AWe;mN8u_(~yDaOTVV`8X%05FdcMmVV&!P*o zS=Q6gZ4DIK0KZfxIL}OT@%=_;A~w-keCq>-P^M)||Jnp#x^K4FSmH&!wnA&OZsn=@ z>4YQ8ZwVzE(Z&oMb78zt_2TVY@M5>EhVaISQHUP(D-?PK%g2jj2#-BtTqC%_T)aAk zzy64<37nf+t2nT%Uxs}G0H0<7zVtC;bfTHv+YDkIT19SF$j$U_nS0%dciCyo=Qobm zU+3F*8bXGX(0j@-(~mu2G!bnM_*0uLo-kB?-7Z5&1_C~QRk?BN1z0tMyswb==^KFV z4FBGXIA9{ruELS?eCEF|@QoI`!KL5WEhfl>jVw#>SJ~d)XR23_Puf#`y}P1%0Cy`a z1!?KGp^FK8#(rZ8f9-i=JkV|ie|^6Nj7Sr`@mb8|Z3m3WXj#ZG(ng;0oFOc_Vw7gZ zbA}CE;~w7nA563T zj~HdBB1N_zju^tQEg}=m%Ab~rZXqzI)-`5glKb#U$BZbNu`E8vFv*9G8~ssm0rc)47*KkI4FIrwU|b`{ zC{)?*pBay!TnfzRJ~M=sS|%q~es1(nk%u-~ViF9(JSB=#FK`&QKgV;KN7%B7a;{f2 zqL}MJ;>T$283dmNY+UWl4nx>cVv=tR%VsJJ6+=!MTdRPveNB44e2(F=;{SnPd}ie34`m+(XFoW<@RA4*bj$gt+eGzP0nPKafnF4=h!X9?6>eq_ER67cH<=a?Aza?{CZjvUO zZ~k%6pou{C!?%(gUD+UK`|FQz)bp7Zw0HZ<`^h>6wDab7JLKM_JWch>o@! zL`Tx5H_#>O@b6qUVczH^ zINKrtiiEddqQqA@;R~+ zo}5exVZ}UKL94o8<2BPcgi%a6kZ?ms*oOCP**r-&IruqUp8bHffRA|}&axHv zYqh!dIcabH=K<|avU`!OlUSC{OKg(22;TGilEkV3V~cG4((52grN*e>YgNubqioVb z+lrA`=V*1fXrA#!(z)2R=ENXbERLIYAK#rBcR7%WjkIkC zG)gvxztWX-G4)$)9VBVpAyHFKIa@AQ&?4XEwgrL*y31@r%92$4dzr*6x!M)nR$!`W z#_!Dh|4O_5C@HG!Tvc5?R?RRo4h%33Gk|~tJq}V`zgt*DAOs00uwsyHBvoDA3&B-E zJZ`efmNQwK)=fN|kodBDjG*hmn1$@l>M{`lS+bg(WbtIz9F6O$AURq5arCS?tJ!36 z^SxKqL-lk|QJ(k*7IgQmTd(eW@80|F{XYH?i4}VTM~5zT%m};gr}nj<*;g=?_e=jp zErNae3Y!#<)31*_^=*k^RoqHF& zByIdm9#nb!Eg-;o1gOyegl6Ygxe?yy{)YP|88{?upWl|2r;Xw>i*suKI-~Zl-Q>kX z(xHC-A&$D8!GE>ipdNTz^&{-5j~te!QPz!T4@*}I;^dM4JS@E|@Xh45VX23ld=2Zx z+uxJU2%$51_3*n7{~K*B_*ZEeH6)fl+x%1Md0h9t985OIJA&_w z+FAE2(sTHj>H5wq?5n{l^s00t{Swo8`p@Ww!q(vTe#Ulsb_83=4GCJ^sWkm)W>&2Z#IN@n1!Z1~QnWxD6E-+rY@-id2A z^V{3e<+SuN*$bw?$kEH>m15h&C=(MoUgSRng7yKJ&rRgyut@9dmZgN>10UXB2^+~% z6XYCzMZKzzC&(-KCrA|~=;2oR3*nT4yk%r(I(U2s(RPuFi^PP$E zQnF%_%pxutx17NBsBKP_WgO&9k!hBNjmCnVsfpz8r^xxx80Sru;l0uW4$$JrU86sI z=L-2j{D#f&J>4>$9JmH)$Tax@T(e`=6Vv1s6Ph^yML(WeD*T6VKe>E@w8&o{1+q{R zCLi0)ISeJnb4z;81WFyUlnQWGKzf-469xXn)(TLMe0g0|yjg!d^0|iRBTc6Q`9%Yg z3c8Kn$fBj=90jl-7D~aglfMn)uHa2X`j?b2tckoNWCh{z}Y4}HlEPA#&@M51NZ}-XcoRF1AX6EAU zF=({4ee%Lk&i+1md3U}dQOwlV05dhxn>~*#d|I4|uj1|d^^f21>Fbe6n@+Lv)k9CU z#TDzvmF`kt()kQ!b=gff-n{;Xr5kR(Y5nIjgWD@|7m%!5<%MJFc2(kUN%seaG|5kB z7xkLAn}O!9eh{O%p~ip|Qq9SqZ%KpB{o$XdR{7<8)TEM<&q!wji?R(5zP{#wh1-Vn zQ}jTgQd9vkV*nqaSa9;?FGR)H7iRj6D>Kytf&8rNfndSb6wNlWh#(4C$3g&MfFsIr zftr&qZ;HxSOQ`KkkhX7nC=hQJeAQ6Ejtvf4P>JVCikh>4?|=eP6E18vmoMKMl@C~) zL@wZE+XMM3LMpLtf0sI>;U-maf<-4fIOcFeGpA6)Kxdw%1 zKu9t%IGR^5fRIyE;D&2j39*;9x%~SOpQ@r(;W5XOR`?BBE4cgmgBt+p!YzWN*8*&k zT__b*pl4W)R&ek_bRY)h%eO@JcM{S@NJ=2yF8MleIKUa?6g-uJb9y#leo!xGQXD52 z&`RgnMVOFX^0JCsRC1mLj8N4!&7y+wzqv+!Lj2tHi=qC2 z8^Xywn3oh!F*E?9;-wZ$ikbv~5C^bZpO1=faN57F;c@?>$3p!9|3&e?bX9W|%T!B> zRxq`q1so?Ab)eZ+;Yu#WwR5R%ej@&d7Ptvr2q!7hZ7AU|ORM}0rxqQ?>_ ztjG0G&}Pm4U>92BY%i|fcK5Ekui5qGT^V}z(8}Ky&~jI6@^PTrlUfL$wKXjNnlc}y zHJft?u0Lv)=cTV_v3JL5Al3|An>NWOc3A7M@4^Wjie8)E0R)xDIpI8*OCQH({ulaC zrqWJnt1=;NmCgYqtck+huHZ(y7lKB_Q8s2W{1V^SHu_hPGourS`gwlR#jpO=cb^PK z@)r-q?27T^R@MUY%QyJRnW$o!p~;oDE|jojFiBUJum&2?#66@kjXun0QR_ zgiCk5Pa-J^?AVOcoB1Sc;+=VvZ|P1?rH} z2m>RYh%c{x0!n9@;)na<+`#zuW4g8teqW&N$984Xc!X%1K1Q2I5QD7#Olb76lV4#k zyAyrd>B>aPc9*GPyQ9u$!X zO>MJd9NQ#sp5|9UXBtZQ2_3rnjjQRQ1UkoB<>4%;Fb98Wwr^ zMT`92DSmlF%&3GeWBn6c26iSi*&T{q)oRRh(%7bNJVLM_oIJ%#aQM%s;qaHDE{Sd= zYhOt33Oo`M-c%L^EVhP9Meqz*CDm}iGKq>s(}0thFBhZYOWCpQD*ibADxX$89w7Aq zZqqKphf+1bYPx`>guiCNeJ&QQeA$c2Pf8#wz8N}fo?r01ERb0X7Mj5T0czRk07Yq` zkHP)4F2`VsB`~5DR+FaDae8h-k~R0X7K4+#Fz9OSWQrKp+H< zYc|J9pg4yU{*nJ{a%O#MLwd-r%gbH&xfFuXplqKV`Ro5ja*`k zMjtUVWK^!~0w<>-Zy7T>w=^D|nqKv|A-ZFBgCVN@HH?XQCJAZ4>jz4FGZ?6Ye-~#= zv7{X^rrh|(RI6|3nLfa!RIm*0;~qd5^hZ5IO_jM6C#SatCx@Qyrt;^Eh>iFx80 zub|E;08L4*In~=sfg}4UaAa={DHNq0HaUiOM-`t1o$0xPpRk^_0ksPE>g*&+$7T`0 zK1_Z>S;Y4@rP5Hnr*c&njvrg)xzubUGQ z;y)4Q07|9EH;LSn9PJDk@bl%nqT6pSAwP{Y66{`pGS^_&$cdpnL#OcU$OP-K>0H)Q z4IA0uJEP+38vOln*m~MafqZy|a2<4HIKk{=LOb+31KQEel}vhI0`KRJsC+YFE&ZmM z;pXdWpf@TRn3IN~0=yjwPD?T2{DH_Eo?abp1Csza_imPQ5_!eVe))FDA9Wv+_UVGJ zE_?u`0F-qDY2E^|=B^Hu15g4`Bup^0luAfmQ4eJ{9F;Vc@BfeNY!wDl}p^sMEmD#rg0qw8$FK>;?2gZM*ReDE0kdJsEtCn)H@WpC7kdH|1rOo zTY@>Qq=zO{uI}2m_GRQ-mw+&9F-Q`3FQz1kL4ZBh2F5Xg`lP_70k)?9|40&dkp~y2 zdYdwXvnu^v}*iVt2?4?yh9$ zs@_{e-9e(9NsL!%U&#;HN1YKS1!_Xw)pUxJ0 z@D`?5tS<0aXHbOzuOvLUrpB|1BM)9D_EN68W^&(k;=J(mpaIiJBlv~})+0WfKyfqP zr&>VnTL{R@Z(b+1A%nT7V$%%fm9i0WsQ@;M^N#NTcpXDcJ*qXY;J({FnA`c~ty}NB z=kBfBoSkvAFD`|t!hE4XXGiE)@8*PG2{&-N5!=!xp=mvpg*0%xvO(ZRR=*Alc(zP2 z?`dI?pOA5;-kN_WAbTD8L7YP!A8S!--Z><8ku9%_Ef~mol||zK36e*J^o(#I{m-m3F5t~WH~BNArQ>v|rEF!f2L%$%jW?g!!h1Ur zmWik^3Kph><8&Q8z?Q2#5;!(?Gs&)5dE}cRd9KMXsiz@)g8BCK9MR&Z{kC*>*=CAZfuc|9&7} zMFk1MhypwvP%k0vh`xBkfk%fUcH}M{L@8g5%C{1tzo)wc`KC?7Yp_^3ITP*mPS&;H zMS>K>MinnY?|k_yQSk~Aaz?>1Jd|d*NamN;z<11oobs1u+}$6bZ|&>p>X#_t~r%7pyP9>We$Qp)jcosE{1KAhs>c05|gbad0DpwIvet zRx@Nl{*a;*UfmIV9A*?56eFz%^%K6wdML(pcakTJ-NQpD#;ME4=$09}qOydZanBFW zxG#^KakI56`mAAEjbSiOsOH#$2c%h_v6eKBA=N+N)KZcMjLTlARjkRDi)v;54?K{h A2><{9 delta 26643 zcmZvF2Yij!`}lj#d1oSt3<*KZOe9w9S_z50XApbW9<8=C3EI+HxALgj(weQ6Hm`DT zC`!>%6fJ7CMXeSsYW|<+y!VFR-xr^I-}|2Z?B_YpIo|#*`t2Vp&WqH8Co2kXIlpW} zda80MddtA9Al5zaa!^#5*_nqaSeouCQgLlwaB6>+7ansrKBBLXfB8}RtK9JYbp@v$ zcHL@!-BmWVIO7mp(P|327O9P(-e#jIXKm&;DaBb5){5rL#IMTi41OjAZ^kiR{PQus zjc1eavm)z2HCPOOmfza>pIYoFbm%WaUFPghypMUF+1VVQFGlKNuFi87v!kxR=4{6o z)%;j!o@efBSm?U=Fx1sz!3uhNSPOMMUhoAz_6^4t1+LV9Yl>h$D!jHxV?T0NaGlb5 zl|TH0VY6}@!r-qpB3NWqE#jKHAjaj%t%0v^=XQmskIgRg+nZ)qt$+xti5Uy?H?m0A z#s#`-G}M!IaBx_rGAl1*={*gHyS7Uo7(Ceq6uB7!TuD{mejPO^OYy7$t*Vwh) zWRp;+y3-5|3cwKsucnA){5Sks*nz)-$XDo!P~ij#aXf%0P$vJi^s?Ljd&l z<9@E;9vs?P#ek);(0{uc5>|AtqJCfUdX1UyX4Q_0uu{xus~u#8AQ*>lXhgLiBEUQw zQmlTZBCHB#Nh`@&hPt7YoKv*vhhqgR?<-KG7msvR+*}hUQw!!@Wqx_Nn@5Qv!O8+f z*@qtwS)wcC%hrX(;0iS3^7ekYN##}#IMjtF=B@g=5BI0#At{J7?t)zv&CemeiSY}z zYUFxl(+J-J+?!He?``ajKP5^ZZcKF@{h}}al(f}sW9rsj^&~|S&f4zYKqW|vu7EvD z=utNjlY72qM&6{oa|~0k(~4-uXTz2sjUD#6y~3Yyx8j<6I1?*(Xk>-D_>ncfcAg_C zC9>+oc!9-BC`z%S$}olNrG>6QYERx0YaWf#Lg82!9$N`JQok*ctPTET)WksphE5nX zPMP_K59kt?><>k><{z>@5Pt{kOLc_F0=FR)5Qmj3MOd}XXxEmNk*>^5I<)G+lYCR^ zbSf6gw=+XSc#5yPtmFiusc539lCBNM8e-j|=hQIQ(_<;Fkw<&c5(o-skEU$>=>{jb zAHAz5x=Ni&WR|Pdsr(?`3W2{^VT;8Qenv@z;P1_1q7|Ej6kURG-U^C_S*_-`FXES# zSUJL~i{#SGb^Dug@U|bEVjz;Tqp}H9kHF>s{=l zg}JsLZ_Wo)fn}MjjB8jylq4!-_)R>*oUVxSyg!?cO;R`y#VJ%{epwmSBdkobiZue> z=BykfonXaXgU&R@{+O=IVc1c0UP!o;lsZtomXD*QQ^(OWq1~l{}rc^@2Z9 ziELTs(pqo9nM((0dPF*3|JJe093@v-uB=uzD)~wQlJa5Yq;gKVtlU!WDNmX1O89+1 z3B%B=0FyJ#`Q|a+?^xIB-(xs$>pJ{<5)*k(f7cDa0ME=@@J}fwvKrNKHMw8eRn?=r#^0|*-<*5t{ogyWt_#n? z{Y-E`JtW;V@g4~^$-WQnk))+H=6!vCEXlXqxuLFx4_D&T#fN*T+BiKdZ~LRcR95ck zE_@@|=-;QC>5-V_>u2dfJQ8W6BTkHUh!~p?ZYo(Zt(YK z%f0ikJ^ri*Bf34@QcZ;B0qlMHF*sjOw0R&BD6;Y%MKs|bz|k++5_{GJy)=W@LfAO$ zfEXE`hOks#Ee+c3VP&D9I->8d^Eyndz@q6vO&rRlrh-@V_1IE;JMymHsD@I`$Gduz zkC>rJC>!FP7Pv!Mx+U0AMKs{Em@ByTCL2XpFq_|Ag@xg)AFdW*4t^1g^xygj)>ULR zX#q)no=>8vE9`ttr@t^b*;0*x>N|O8(j;fR+&Gf6Axeg#>{0HsN-UGP*nakqC-VM0 zSDDKXDjEEVs0L-%ieyPppw?r_u)Hpdw+}?J+60Ql4ydsT?Jbzvc51pnlLXZ1g=LT zVEHAVfPgtMECb7=Abb+TQlVM`wrCp52IJd#U#MZ=iDjt)>}Tw`w8BdBYN9Fj{3Yy; zV;L|ho(;fyQU%|Pr=EW+?0H^-?3w5io+Ypp*)yvT(;D(}5~VPr5+l)fJKPMj3o5Y+ zh#6XA7~D%_Tk)CL0^CWgS)gc!!(UMN2LkX5%1ty*!0%PrW09566E^f@6=6yZmWWN{ zXzm)gjwRIzpU%{Z)?g__?;UaGZTXPy@X;n-)|K{aIfuNZ;gVmIHA2QB21K#A#EgL{ zwb+}~0};1hZAskDjB2y=$Y5nI;drick0nScu}@;TA72P%7ismMtIwLKK6MdYT?}Kc z)~6By>azeSu|-316AAa<6*cXIEOQEnpL5JaTW`RU3DXUlvM-&6jo{~!s)zuR1=51I3FJDyI?Uj1Cvj2D$jw|8^SOQXQRWVR3Qg%bxIDM%@yTyJr zEc3`Vj|g3q17ngt0`T4xphu~>p%hah+;D}X`2@UBp? zH`@l4`#4mJnh(C{!*XfiDBGc1CN3P2*B%$u8kS@-Gj4Sm4R2PSj5w07sK(o zT1;u)SW&lsSSEf@{O~~|#}Dd1XxtF3z$dix2eKQSS@zArEP^=`Nayc8pTOavEC@AV z{Fn%<29D7Pj*i7K>Q%NX6FA6P{dX`!sWn$oJ+~(vyO%qB{FrgW28-KO)1kfSFLJk?hMiYTb5^pk!IA5}D?r4S@Q8J6|9?iy49Wwl(={PAVBmy3cV_STc zJmc8}sw834Y!X|I&o?G9Pht6FHc5&b>LhKd>_m#5CsPT-QbpOGX{?kV{y4giSA?hU z$!W@w4$flVCkz_Tk@SHAHg_&tM}v_F^UP(bWCxc}wt+E^#rq+V zO%)%Do#H&=dn#<3$C`5GUHkDoHk3z}&=90-*r)*`Ck`BxG=5~iS8EMS8Zj_&=opyl zVr99q7(RBPkWyUsP8S{H)}PjlINeC{w+53@bMY{>zSuChe?(~np>Bg8I7 z#G;D&P7x{mT~txxQ+xALR-8f3ayAs(N@@9QIcp|rXQ?p%GK+y({@fp;YFnf+Z|4?+ z`-@q0S^k}(9-Q%|-?{-l#bnHLP~2^8R^2QCN4~(OA8%nr;Pn-(D)vlfAQY@%%^kXG z!x#DJYMFiHV;0ENtlC+oGvr_Mm_KN>#;ex~wXlxDqRGPp636*pclqgjpc zNAMh0LYr1vL#MC9Ru=Au9nAFBTQLsNJrQ!Zvhq%s8%JT6R7Ugt4WoqmX5=idvss2&6criSD-ifLdmCkF zZ7JRDLEBgj5k&?~jIv+Be`l53OlKvLP@AFR7|o`$k08d)T40mD_GmZj%-D1o`^YQ? zdpxWud)Il^3fNiZa-J_1u;~Gd9e3(D$?aU&^1Jb+y=xZ>W$^B9Hd1O3WPwV&v5rhq zEh|(~ER<>_MYN>Wqgfb4AitOgdV?8ajuem|Q= z)Jn?Ky`POC5nkmWJ4JT?OGCtbYhaKAmRhR8K!bV9ML?&&cV{8+RPrNnx0K*9#cP1pQ zoa3w++1zEw4PT&aQ%)!kkOI@uGMQ`cy)w4_ z6RXDv>k+QX9zRwGLQk>;e?QF%HTn2)19*&`ev)lV@0&%GuX#4{|7VHPCm;%BhTl|E%pO`Nz!otMP~57XlPVbL}gZ$Z$*^uh9Cb%qxR-) zHi~*DwKMvTV-b3=_-ojDhjpT{FoqR*m(9USjvaXytrk{CQI%OJx-eGsg<6IzXp)b1fydyf=yNJTJW1h6yagY`v-zj| zTMU~$6U9Wb=w?Tm{0Kw9HiaZV9$CJmhS8SiR>08Flat2dN8+#n11gn5X^x_Ov>y+z zP#N_@%KxaC*>v=P3F8K#M2>+^{dhSYRX}}g)ZULrpr!VkAFs-luK+EGVsIgsSy00t z2_B_>F|rg*HUcL4b2K)lOVwb1?$4V^E+>0Q3*`H8$&wf^1fu2aB{)wIM=$DtA`}dv zmQ=3s`3R?R@ODu?9JU7YcZrn9Q)-KGBC>YB!{WU#KZKK9k-^n8H5f{Ua->d2*oE?L zH0tJ-$iMO7ya1o2lz$Y?H`1smR%}lM?^S5ZRf*y@7L*Ir!r@VrtR&gUQ--I-g($rg zWiJYy%BY-O$~>>vDy@I=Sj8amk@!J+7F8YNzkUTR0lqJ3)>LbVVH~bs(i35Sq}dc+ zDQV7CrN6^wCpm?QPgx>YKC?5+X~$5};`#!4s=`xI@n`e3{1Cq?3_H9ce?(ZW0TRy`IR6pPzYf7wHm9& z3zRVk&H*q!ju*gf&|{$3+*zMFhD?nCeM_=N`RKYZIsCK z@L6gVCD0(Lag5QUaN>>PW-2c zRK6^Cn6U&&s1^KoLHq}enHC7W@^~AXDRHMKkEg&_TX{QtA@Nx}k2iydTO2OKFv_V_ zS}c6Dm6r-}0_M1JFb6&2dn_Kk?7+3+O^5dHB&uduuZ=q>ye03=$DrMt%h#bqKEf~Ye}ySZ+GQ&nAsoIQ zj4aS%Fz@?6C_Dz-gL&(imr1|U%daX!6Au#MaOZ1;{DP4~cw^_q82ARCqi|M5cmstu z;C+!H*C0dQ5v4?`7$ZOw2-vdOIFeU3wJoz74Ck8|++NJXSoi#q&brApfZ`o+v2905 zqL7qk)l4??( z1P7RjT*N`o>HHTMGK1HIBQy9>`=^<_GIQwRMikcISMn!tqY(ThC-FUq3fixNpWopz zP-1f7lx)vr-nLL0mYFK)PKtq3*O8M;O!Hw1om<|Jx|D`UP&Pm0d%i{6Nl4tZVowfF zD^(O_W}(>p5Owoo)Exu)B81>GC|g;qg1J|jzx`2|I*>!P2z3eUd|%Yaf0r9H5jl9t zIZ`YipTkd5*Qf++&s=Asl8MUX%85!Co~mKtL_toJqJAFWsiBCxV!VZdnzIidj|zSn z-wH(+A?H_J#BY*7tqf?K2jwoo*njWEypn^yH0LYb`D!7*>@$mb2>Txt7nQ%1QwUy~ zB4w8IuW_bQXK}uf1~xk~F@T=1byAPNtdU@Z$4{4^}ypK${K)t9VKP`vi62 z_loj8*F-WjdBA#rb&K~x!yBr8#;0Oq37u6-kRkv4jF(dRF=Zf%>Ao;@HJ^mVlFII^ zMuyf_^MN8Q>n#}7pI3(wYdA#}X=rp5u&1-I?u6a} z;Zqc1zu*(RT)OECRKWZXr7PCx=r<{AT7*>%qd2e7hFTQtdW)+NwUNgXENIIUwX#42 zK(!J^2K=&-52lHeh56xnT=kSqC`kfDrlPVFjQ`6UvlC*yC=W&J=;11Vq_oCC(pBDw z1?2^&1~TTx-)+T^Y?UIEWngFqFaN?onvzv5TaZ6g2I|zY>eEoDGan;9ZsCTy?YE0Z zO6u}%BKv10W(P;0!(PT}0ZsO!mHN$Y-hftsG@nfU*4e{LNw^2^;tddmC9JHt>=4?8 zE0a51gv0nfoHoEE19|q4>6MS{Wjok*RgzwF6{WK(&KB8m=AS&Are5S?Sp&BCHN(C95|rvn*sDlG~EfNA(=yeF>OS z#U_)>+x-}yjW49)x_^wz5cx_K_33dW*#M3n59_b6{(Od@RDj#?>}NgzLQkV5?#00J z(?r|5HYk;InIlFsV;VT}Lg}{u-j8_jL!p zG@pX&JjI{?jt+^A`&c3+FF18IgRM7tXBc#gx5r+ygjNl%psbNQ5~6N)6!cmz%`U190`ebEW0a0dSTn`c7U9X^~Ex(`u$@(*0-p*y^|${#Be zvCdfd?T$1$s@>(~Y4zO@_j+eDKD$PQm5dv|EFXO*Io=z0c_Q{hTxEOkS>O+(MheE> zo~j{k@AGhCJ88aL{ZA5rlziKM$3Ztf}N*ygNSQ>gqtjBmOsvy-kn#8#27v zw;WHjdp_lBSzv3V;ExsMV+_W278y9%x(i!~Zm@|9^hH4vqARp!g7Pw?e8ZFuc)$cP z32MDC7{f&)TG0b+xspA^&kkxhM0)4p zTyNHPe)|Aeu8TfYl>}x{Lo`$Q6G!k`SrxMzQvAF$WDXF;;j0!D=5#6!DaT{W@(mvM2O$m z=Du-Zqo3%E)1=rkMEDDmk&Z(57pgFMsj?J!*gpXHkVgP%#9RWsfQ*&JFYkAf=GpD<%A?X8hd1Uk!mJqH;=$rWNE0@QH_9IkwS%G z{1Gt#|a``BxSdS4z;oBs94nxZpKU6liX4%L8KHV=Rjr46w#i4 z!tS$WI8-f_-DGT4a4KV)@n=WBZrQCPtkPzv6%R)rqOuv&!3y`$KdMXrteEUp#J8m} z+T5hRS_~pOTGf4%h$tpv%kf|~*4cSQLl>>=#sJ#6t6q*oA>EF)LrMxd8>c9(lAN^fQUOJ?T)4*bxb#}o_WjNl<)VgSFx4aNZ@N&M z)omousC%Z64CppUA4TUCLa{*|Kcoqh0ZqglQkjh@9F&{iLc z$dz`Ql@*)agTzHN^c{lge^W=~*tZ%Hm&*~+e--U%YbUUY@JhtlBGLZ3sYu7L{!W~* zP?+EqMGFXi(_F$z;GBAZXeRn&2dJ$uOx6s4;npgVY=79f< zQ_P>ALoF7k@Ho*ZFF17%1NSbydl4n8dUkIzj-t^x>Nfx*Xj>}^+!J*K?vU9;jxT{s zS>Ngfm^r$-QJNqXCNb(s6DdIfqAfxK2X%?%_yy%s`Ehu!s@h0HIE%K>?^k2X{PqUn zsy}?}rqLwvvF7Q7d^)eA80f$b(Ty)iXUH8~Gc&ty1jcMa;mAW& zf_tJxl#ka?*91D521yD_;&ntjM8jb?+g)^0v!b&HK*kWXo!^Y{3U3s#O$6?Ey_Ee? zai+7@e(7lIIA+6wp3kYTNCwVtOdQ%V?J`79LLqS<3#7#iXm-4wBdQ{-aDUnMa5$e1f?L;1O)E?l33 zPh01RI-DEOw>+1lkAyY??Nao5i=oO~Q8Y^LLO|wSsJn;i(d*R4DQNd*2RG{5P4jGNqQg--!(VTO( z3HrFissMf;k-SOa9oZ()VSbYczzy@?z+5QSfR!uJDTU>*TrnR}`zRNwrRz$O0Ubef z!xu+!XU#4EloJ8$AMB|GPL{Wk1~>W~OR@IKWkhFE!1yi4eR=+{Fwxu?H8={QN5%#Gy5*?<$1d5sMtJQK8&j=2dZkq;4EY9Q zByVn%hEWHK;`(nw0q#g3+p|gJP)*!x`4Yb{`X2^e3r2#*I^6`#vd1N z;|mE!&v7z4E+Qrq72IcNDD}pEIPsI{0kcm?DI;h2=mgQtzqwD$PMD7li%tr%t|VO+ zIaR0=PB}V3#^Fwh{mf+Ryu0#AU95;G_9bpVCChEq=Lsln22JRfIA8$VVt@0CScOfN zdz;L!fVObX8Sw+!9v#k#QjRw^7j2Jazlt_wkz79~O3)Vc6Ge2uK*|%O6!btY2$lI~ zRmj0)?`mc_j6RJ$hj!=Y0wm#G391SuyNDQ^yIj`Yb4WSW2e6R9p5-0I7aErppHmwC z&PEk2+Z?<^ve6;5z~3Yeh~uE(H_=i@WBe0#1IImPV>TwHXXcb6O;R2r&yZrjA}wm^ z{1se5@wVc!6!;SV#uc=Rl5#MGeG_WtdY452l({V8eM9xSEK&lU@H~o#E_@}skHcKD z|Natr0_HBo?Axr^oM^JG%Q-&rnEmLUMRya@Cni%-%*4fYOb{V~`kBJTCFN9fM*QV8 zj(9>>X++7Z+s3-4 z#}BmA*oXE~d%XdX{HL!|uLBW5ISs3n+HH6|1M5$7|HSfho@;A|OTgpGj zApZCKEWg1Yiz1>FbbKHZ(R&&S!Pm`_@aTbv;21xA`+*4O7}=}*A0|z+0e1WU5V{;Y zeDfh@60@e@eki7fu$Dm@IwZ7pFLyRsD`@>ld=}7h?4#tl2s8{J{}#@DbIv-&EIsRzuk$DuxY;*habt6@S}DTe$=tQ3{WvA>GN#-p?r~T;3q)!Aa!U>d^(Vwj-Q9J zTjS@!?DqJ1IJ*;m9?9;CpFd{zz|UjZxIL?!$nJxmC*g3AS|c8xMzN9j$v$HvF?0Gg zobhxv3wKp=alR`tdtxIdk#1+ZpvZKi0H^#8j1E?tR8}75;4Tz$InDM_4o&w-4$hY; zIXGb^auV^=%pv6jVLnoP%Sde(S4wXQrJ+p#M4#t;O4aZGp4@P-*E zVctoF?XS#*s4=_+jQv2wdAEW6EZoycu`|YS%nrCR2lr4E11EDEq?u5pf=U4!=_Yt8s3~ezm7JQe{2&R^ zE|lyfRc~^nioyMt{JG+beVm(LQPnWttfE)pwEselFnI+>N5Qm>YO&Dr!5aD$QkXbt zW;_}?N2^si>ub-9R?&!L-C;@N?A(9ZV4)dm*lfvNnJ%Vr(`8hCG|s+ zr{7^z5wa?)H|a;ee^IF|ucG2PF7Kk+RaaNzv(!;m4fSJuUSC7)X>z_&5uNx-gv%dx z=l40{ad8RbRTP#f(5{S%9+_OZrwqX)KBxK%ddz)V_`%)+j5OYiQvIoO%pHe-GHTQO zIw}fO?*wbrQ!#hQ5ednOD(*hjSLf0T3Rb!6tLdn3F$mQVx0z_?f3Cf=sAkblSuuF4 zp*j+4O1t7oLzOnmrDAb6Qd8BehB-~(o7YjR_HBg0rYT)5uPDptY87g%q5ldzPILtE z6JBx5@1qI>LTWi5%AANj_d!w zktB7POw`4v3F9V?8j%&trPhRI!zqNK!@-t#gz}YE>K+JKt$PzwIy^=yjjyW(!0@`-QoCL~trGL@?$3L^ zck6<%)@nMg**O$vFK(^Mqloi6s@r_H=-o+ui*SM4IJuowRV~V+l{J2yRZgJNG4rT4 z`bHQmE(VRes`WW)?YFwB37q`^>$<89I9mxEb_ip1tu2)k&VBFD(eu^*>NogIoN0RoAeFX2*;Pq-sNhk)A}k8C zHJf5)=3vwhvrsQ=WV_fg_B*?WnYWR+_ui1V$IOlid?xN$uSXN+Fy`Ri!C-Z`h!gdo z>jRO1i<=1zhhz4U|1kBJkmth0Nxx>x;odN{*30i#QlpA5zyAy-4F6yC*MWPu+OyV6 z{cn2d&E}WhY=PDz{?}MvLiPx?XZ4rrZ-42{UT3c>Ua)xHS=2$Yw_&EKRl(KQhO4it zlrkhujMqk?MS%%#KHXCSc8rqjiewS1F@(LdOI>HQN+CiLBzD1Qbr2$0inZF~RC1GK zUw6i-HolhGeB;$EP&ojX%vY`ERQWiPEG{hQAX}pL?iw-+mFy#Vy~O7MlqN0b_R^kBs&~t#sR@nVQ7Ibf#MItV@wA#(3a8Cd z$@P<`H>}xGjYyz+W~*=OSkQ2RnvY)>aVHckmn?VOaF?1&W>_#q+gf7kV|9$TS(3~T z@xSq;a&^%`j1=#Pm0}MDVI~@95ZC806l1l5u?sPnceAe*q?L)&@Tjf)T{agvzR-rp zJgw`PG}$*-P3LT!y*^j1!=fmc+-J7;8#8Ep(wKhZ`i&pZZxlI)WAYcN$%W8wu~;P^ zT(a=v#d3*K;Cq&+X)P?9sJ;k-B|K&A4Ym^L(%wPg)(EuZsU zxLtG=eU74{qNpQUi@sulcvmbHo5f!7vmm$b7BQkd`|k_Hx;1oGW7vx-@usV z=wyywuEug+9u_TEPm^UZVTIZsEd%@K6>2q_z28dpqyukCA#-8^t5he=kg~z5tXHM) zM3%zC_39=s0ed#6xPOjYWw>)8I`dV^d`>P*v|xA8j;m<(z!2v2isU1dgp|+K!hrU+ ze~5Bkit(DSO3hZY`cSA^T5F4_>&LcWf9?D-rlUjoVhlA;eKCfLX4doyq(DnT=P%Uy zxCE)iBK}olV_&wj>hJUolNC3;ytD&x6G%qgGdHTq1OO7+ldyHt%dM3wHGRz;osSH) zRg~jPk+Ucm-WZQ$;#YmnDgPshL1q!lfJ#E6tytX1od&Bh0O$qaRj#F$2$&vA!SevC z+KVTeajP3z$5>^t6_I`}8UtncVf3cjHL15g&gV_((`ho7&VPKf)$|yc*IbJTd{HI8 zpo{arRAY&?q>!q=-Qho4=VRyD(KS2NfrXWduhwIs*KYNFYDHS<@os8Gek7*lt9^)Y zWN%6P9O|HG`Qv@EXNmyZ1^d+u*&C_1Du-3dvP5AgNsL6O=ZM<1u#@5k(57p9OkM1Z z0FRH!jz|PxGKBg$=7U|cN1jnvQ42g$^F4k^k@xpGIl3cc&a0hiL@81SFTxO7>&xmK zd?Dw5>$1ATSLnH-riBM7|0>Sr4U;)*{6qd7e+;Frs-Ge8d98+LSJgO~!A>?={abQY zlDr-}&o~L3;a}=7nwXSQ_3ub^FUNd%hq%RSW8YO>=v;UB2Yp!!HalNI|H@yZ{#7&a zE&16G)l|PO(~Hw_ z>v%k$jZ!-nj_6uZnDn387;8y^eegfEIg5bchiVe6exTOI_vap{U5f_rQ1r!GD-Vl}^M-WSO!r z6Dcd@;}{g(+pxbc{f100OVN!`jBd#7+5Vd-utA1mw*sV{yD)JTswKa{**!E)4C41;g~SjBwX921>xZdI`g1R!%*II z{=xhv;Sf_w>j6_sYv`T9FQv7vDr(ivxPtbO6r~|}5gnoMMwAvpOWS}2Yv6+@Ed^^M z{crWn$z4WEkD}v7g-4?w(a~t-zOsks@&mr3(VsXQqn*Du;y6DHnO%58SXn{q<3tfY zs7HCg+n`rxJ`Q*n-W&FJ;Y%R5E7ro3KJ)V{YF7vflG-1|XcS$R5^HCymZJZEq3*sj z&^ve~QF{j~Nhj;)L`+fl>a~I-Et7hZEZng+iiqyjwOn6KUPD_a0UT!Us-dN;Vcyf; zm6Sbn7FfBDhBclB?kp43MmS5l?PtZaCXiU&stJ9n;n~nt^|jtkviz;(iCTcO3-kj9 zJ|0xFYB>e1cO=>$R=377TWg?cG#|;KQ4KXW4o|r#x8$R9ka}k$O`bWDb&4h9oQF2m zW?)4+**=ke$E9e~MD=AuJY4(ev;l&BK-K2f zypE(c!WH;l3Z3EiT|oj9`)DJuIVPSQ#4o9$cV=o${Iepaw~erpP=D5kEnEwS8nIRd zFKo87(_$OGd?+^%jn?l={-&O->?B`cG_Hs8f%1*aVy4afZ=WBNhO%!n=kOd%#G}#h zu)lT=kfXifuN#vv{v7*>_6pA*sFfl_mL9B~@D8sJ(N^LcQj)fNh(;PhqO9(44SfIy zTVeeol-X;?;1gL{OO2Trthlq7UXs{f55mcuGYnWMEze$+4N)n~73l;9!7!@pBB z)Ui@QyWh};7m`)IsT!Hnn0$t=iQF#2bZ9MB;14PG9%RdLqzZNCXmX223iML5w4L5L z|22z9)Bb$6HkQVg#UCyp_Q};K4kKSY%GJoOmsWurwDd5UBymrfgXbl@QA9j!UOZiW z8y2+|H6i~jLi9l!3_IUDhT`k})*=wB$ryb5r?vRi*4pA$P5x3%Rg25Sr^2)+B|~1p z@Q5ctWI9F%=HZn$&`tI=(F}l9tF+#bFwtCV>yylf-h7$fJ#n|T3(VW5*UbM^iP-qUbGx=5pCt}YtwKx7Z-+)3e9{kc@6hBC zPYG13K>HR8?oT0T-`v70ajk6s)wH9UR5+K&gmwV6RFymiZGNP)o3yc1tt0{&$ zP^z_CONJBcFjpsGHy)%Ixm&9N@67QX#~9NaulCqio==3h;W(v(6?iSLe2&7CKr!EF zze37B+*aAQTN|FgSCa|8B&-tmX%tnH3%j{bBP$DK6`sZ3uZ{PDrGREkSM!J?+GmoG zk=BoB-Kbc~wK@xMQf*Ia@8Syy?`tPD3W880d*>;w9oCdwQT!;vpv5@?KK>4a2j__D zz3Sk+CJ)q0vfg$TD-OFvlF~8EBL2h|@xKwT$uVvBZyLsQ9H#WYtdV6ZEx0R}wY|=0 z_RcF>W+Ah=u3-l~?nv!@^Nw7nT(kSG*1Sjx8X5BxHc#a8AML<@v}^`>|7saD7Du}& z`5Mwpi+fsW(xxAvhB91z%U zLvKTC^$XE|z%QiDFt`z-lYDV5^a#~Iq2_oLgD@kpbjl(xAwrjGgC-N9$5Wk%Nc{kQ z;r$Cp=8i}zsizaRB^4AePoomWgu%`7I#LWtM%!IMZ%wUb*QH`~qM9=?ayC-pyJPiq zPxcS$<_bkh+q5Q-?MKTO{hl*>e295_OCL zN{PE8N$*J&C0OID>nNHfovAAk`F7arwWC|MhW*>b!wM9S3-jS*I zkR4G$++czwNlcPx4xtUvF$vVWLX;(xrXW}Q_i(3fq~67TIzkVpPKu4w4^hK$vOO)@ z_Kepvs69y$s4_{H3_j_F{wM2|1m5{%|2ai3&H~sC#7UGQl2{a9i;&+4?l<-RaQZEM z5)C|6??z1L#*%GnmYC5V-ipu@;N(=jx)(c0wV{wSO@9rm6WQDDX*veWoYlbTZ2d<> z%Z42N4$W8E=v!v#53n(*Y|m`{qJ%$LP+RBd_onyJI8&rP0Af*JL=oTm?w3N`|&3Hb-qPrN%auliTMZv7kEC^*F%*A zy@rYHzLShI>IqNuWB%B|Z29{Y-NcNj^Y}Xy2G!T7CTk~49w(TIFnP89yUwHJkqMoCf9+VpJFQ~lXeJjLO{tEqS= zD6(092i9-V9ZM$^#%$H`aKkx;&R*yFXBjgF(j{w^tTxa$&nN*?H|oJ~b*oN!5EnP< z(S&3tH?U!zj>e*tte!kQ4fmDyDYoC2`n$9RA{~2w`{-AC4Xy^EqXL0p=vY>GUmJ;~ zBOOIqbBYmX$A7KgqR6e3A(aXw{Yzy2SD=#%A{orHQ^!=5Lr8&P3ZActlb0zW1!7_+ zjQvK>#Ck`+(YtZ}yDv*+=5BouzLh;a+pVX_JUBXSq-NEdF#<*((BonB9-XXmsr@{A zP^bjZ-6hOV!P_a~MLGmu)jxqv-{~1J;($H`Ys!r5D+hE;y1VH;xiS5q{;IbR&<^P- z5Pwkb1uYKgeW||OTsVY1`(p#n>ngmiNY4vSJ;~tGQN0^{cueo@B~R^mVdLY}*a7Tu z9dn`@7-p=_+ugKux2|Wjgk&?XlwTup_D|!FFTTY7gln^#>;vq?3E;Su`8q|?6p_) zJH*w}%-VTP|JnO&yRYl%YPe(BKDTSh=fy1hQh)0WpOe@}f9s5h-s$7s3w_+ze=O`g z{GuKQzdV#`fNI+ANBW7v()_3T1>ev+p6TaFF*yDC8PDhBZ=h;oQ<0V4QusaPEhtjd z7z-VPjYYodK_Nz}Luh~M$1uNSrWIkeF;j4x7kQ~Ny!}vDVNQtAlh8 ztu8X3V2aKA3TKNHwurkf{<;uXIS;CqFouE=Y0UOjw<8Uy|3)KJN5G>Ut25v_gtFc5TjCsl!ss4N> zc95g69DW}=@b`A`PdQ%)6#)Szr!W>fcopuKH$L`uZF?#hHAtx6jxqkEcOgb}ew@LI z`0*_4bb^9+D7k_=)xnkR0X^9m1%D(PtDMFa;fq=bVkBAFt(EOJGd#i?W@hA8FcRVK zx`qK&Ya7Ie5^C<+Mza7m9?`K~L23L8{|6_N3LWbpxE9nkT+ZltHoKk?tnzGSFc$ZR zR`m>G8R;R*5*%(EmW=YxHfQ3Ur6bHv)?^62rdM|LJz&7B`bH6GSl{STIBIms1vE4+`kq4@ImnWuV>eid-m=b#tG62 z5}xi^g&l!=w$aRwFF;!QP+=d6yx`O*MsW-zs5szr3^L)IMspm%kHY?j=gu{tuwX9H z%_79$az$Crg~-Gjv5-8^7zWmSgYs-;2Rr9u2bf$v0*i;hparr-GD#_ykpg0YF#!6y zj44!~VDh2C%m988XJ#wR7EPrCl5NjhYQ!?Au-u^Vfb96`a%W~EvE!lsS@mY(iLFet z3&xaDO4Nj`6%MswpLj*ckJ#TDg{=_{mC0Ctq&?+hqY8s!D~(;g{vNJ;VWrS;l_L+Z zIf{ZGu=#R@Ef)>)f>Xm8o$r4WH~EcyzQzw$JB>%f*sqL{@X8uvnRh|h`WI|{Oku}F zDz@&yV9+|_Rd{8+K~6L+C3l0Lm4Wr6N zUR~z0SEdV_C&1fV45{L`8bein%M0m`wi>zK#v}{q{Nw=Wl4s0#KCb~duaOEH$&X{< zJxYTa4rjl5&Vcjs9r-o}i$}uod~s10x-dJMZ3aq=#7;`g9bWG(oG2!D6wEcLp{ha|%=0DgF|^4kI|Zd$i$B z#9;jrfobkd?937-XZhb7)sQEp+0g75VNp7{x6T+LU>!GJ^A2opKW<33Lt2MF{eUO< zTbz^tml_-66md}UltIA{+B&v9rwr*O$VpRV8D2ka%<`?G!I>8a3Ju~V(NxK#OH?y) zw_r7bwc&YVIMzfL2nRoJyej1kWqLQf zY^=kI6pwv$+4zDcNJ-nCD@Kd*m`Qh^!pImRpX;BV0?7uB8T8EA(;T8v3BxYIQhN-Q*Dpcgg3Q`#~1 z-j6g)PP3M1aZbd!*HXNva)~x9|AtYIq@Z*q+ukzP$Z2E#_+OH92++3sFGGfHq=CBa z0-|B~JtK=6(FX9dd$d-H2o%GktOj1XFSEobmyAjUPWg_sl_KvF?mgqoCG zAn7YD-1JV5f}Jvjz!PDn6|r~~#dr55l)cKvBVzJLdT+PL{@rB*dCi9o*aQSd$XzB^|U*Fv)h1Fcg){&+yrS zMkVtN8b*d8T2+^oWDu!NM~qR#*Dxts<3yEenCWGM@T%iNE3@$OPTEyb?$HH4uBv;< z;QUaY0EGs%%^I-#AVyXHscEi*yg7QT{UF(F#^CO1Bnqf)LLg0qA_XU^;N*ujH{bS+ z_oTT=Q9PU_LNv{k)-PT~^nl9gwn|ScQ{I{*g9Q!SI$$R1f7JE`M747&(OTh^jHss=E64B~NhThTJ14TZgSe znoDKUZ-+@3gyo{NJ+Cir&cUfFc+T-bKW$n5Yi4B$8p^w9JXHc&68)2@5;W4y^Gq|- zREt+u?7-K|Y!bpN9YQ`-oE>EO)6J^X1zrP(93p3=g(DLu%=W!c4tZ@cj-})1DJ%6lJy^Zs5Jg z{~gAysN4vnR{l~GQ&Vw9%jF0X)x~EJ8du0EIgGKwyi1j2>B#^ry|z+Tr#ag0Rp$6Y ztfZ_qS7S}dTTfOyBqJ-WF`KEBqMeABh2y`U@n){ac>JL(tIkqcSG>?~9D5rtgIkJM z<9&^hl~a)Txj6z?l0^A5I(+}RxjG?0!|OAASCLiuKfjv%pI_Z!=@%y65&0E-{e@Wx zPXyVQzc7z;CZNw2vlQ(5#Vi7sHk%hX9=qMU#jJ&wk=u{AnAOR4lyJv=H{`&4$$^q3 zpX56{BBOe?NupJTA{%cnd=JmIOPY|Z_qz)HCi=W?L$kvyD1!Qdm@y(Dl=%2cKnR(|yLI3xy=u zqb|vDrK;Rb2)5g@9FE@FaR_NsZ1 zc(Wn|UNc)_Yz}%}GoP3&CX%ka`SLq<}DSwk!i$IDG$ z!3OheQQZFOx+%ReG%zr#`W$qX|1!x|B<`_y{ACU_13Jo7xW(d2@sqfg7o1v!Lyw2% zd5z9BEOzdSngFjpGD~Pn@J*I;J6U~r=aKmu`!@fvxy2#BFqr%+UY&j8sU#{Q6N#!R z&&*68;GdaAWL?ZX4&+vM@3e5EI1J`+t3Sb+TL^ARSp~uhCp1W~Uezp8o|LuxhHj0+ z*HY~E3c{4EriL|;%Frz66KJK_PYg>_1M6cR!#YJ-$8!0n;$x)fc9de(64U^zF4m^a zlnwz_b3CsiPocwK0anKIUYc{!$#;+tZUST&E$@`B2U+0(>^$;U zErr$M4McMs4{r-8iq%}fRugKB)-(JUY~d-io4)wo!Vnsg{x^m$gmv$^MQ9#sVT{SK zw+rx@fFOlkgoRf?wGOw?;g*n~|8xcHa0|n44uQr+SY&>YS&aApS;Y%m{#nABg^kGW zd?(UE)&aa;It0OYBqpOfT2vT_Xbj*9HZXrSTX_Xmvb=!gZoc zMJ5yQqtIfj8cjEMhQf+yc`cG0@>aB!3fn8vZ32=DbEB=Mky*j-3QB|zu#*4(47%hb ct0GR3h~i{v3zH6C$}VcKNag}$Rc-zM0BKOu2><{9 diff --git a/CMakeLists.txt b/CMakeLists.txt index c51603d..1cb9ffd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required (VERSION 2.6.1) +cmake_minimum_required (VERSION 2.8.2) SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/3rdparty/CMakeModules" ${CMAKE_MODULE_PATH}) project (Slam6D) @@ -254,7 +254,7 @@ ENDIF(OPENMP_FOUND) IF(OPENMP_FOUND AND WITH_OPENMP) MESSAGE(STATUS "With OpenMP ") - SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${PROCESSOR_COUNT} -DOPENMP_NUM_THREADS=${NUMBER_OF_CPUS} ${OpenMP_CXX_FLAGS} -DOPENMP") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=${NUMBER_OF_CPUS} -DOPENMP_NUM_THREADS=${NUMBER_OF_CPUS} ${OpenMP_CXX_FLAGS} -DOPENMP") ELSE(OPENMP_FOUND AND WITH_OPENMP) MESSAGE(STATUS "Without OpenMP") SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DMAX_OPENMP_NUM_THREADS=1 -DOPENMP_NUM_THREADS=1") @@ -395,7 +395,7 @@ add_subdirectory(src/model) IF(EXPORT_SHARED_LIBS) ## Compiling a shared library containing all of the project add_library(slam SHARED src/slam6d/icp6D.cc) -target_link_libraries(slam scanlib_s ANN_s sparse_s newmat_s) +target_link_libraries(slam scan_s ANN_s sparse_s newmat_s show_s fbr_s) ENDIF(EXPORT_SHARED_LIBS) MESSAGE (STATUS "Build environment is set up!") diff --git a/CONTRIBUTORS b/CONTRIBUTORS index 6ffec73..e1921b1 100644 --- a/CONTRIBUTORS +++ b/CONTRIBUTORS @@ -25,16 +25,17 @@ Mohammad Faisal Abdullah m.faisal@jacobs-university.de Li Ming liming751218@whu.edu.cn Li Wei xpaulee@gmail.com Shams Feyzabadi sh.feyzabadi@gmail.co -Vladislav Perelmann v.perelman@jacobs-university.de +Vladislav Perelmann v.perelman@jacobs-university.de Chen Long lchen.whu@gmail.com -Remuas Dumitru r.dumitru@jaocbs-university.de +Remus Dumitru r.dumitru@jaocbs-university.de Billy Okal okal.billy@googlemail.com Razvan-George Mihalyi r.mihalyi@jacobs-university.de Johannes Schauer j.schauer@jacobs-university.de +Corneliu-Claudiu Prodescu c.prodescu@jacobs-university.de Further contributors Uwe Hebbelmann, Sebastian Stock, Andre Schemschat Hartmut Surmann Amuz Tamrakars, Ulugbek Makhmudov -Christof Soeger, Marcel Junker, Anton Fluegge, Hannes Schulz \ No newline at end of file +Christof Soeger, Marcel Junker, Anton Fluegge, Hannes Schulz diff --git a/README.scanserver b/README.scanserver index 0bebbfa..022fa73 100644 --- a/README.scanserver +++ b/README.scanserver @@ -45,6 +45,7 @@ If a great portion of the RAM is used for the cache data, swapping will usually * soft memlock unlimited * hard memlock unlimited +After adding these lines and rebooting the system the scanserver can be started without superuser rights. 5. Using the octtree serialization feature in show with scanserver diff --git a/include/shapes/ransac.h b/include/shapes/ransac.h index 7d8ec9a..1a02b0f 100644 --- a/include/shapes/ransac.h +++ b/include/shapes/ransac.h @@ -16,7 +16,6 @@ void Ransac(CollisionShape &shape, Scan *scan, vector *best_points = 0) { // create octree from the points DataXYZ xyz(scan->get("xyz reduced")); RansacOctTree *oct = new RansacOctTree(PointerArray(xyz).get(), xyz.size(), 50.0 ); - cout << "start 5000 iterations" << endl; for(int i = 0; i < 5000; i++) { ps.clear(); diff --git a/include/shapes/ransac_Boctree.h b/include/shapes/ransac_Boctree.h index db64a7e..d824b1f 100644 --- a/include/shapes/ransac_Boctree.h +++ b/include/shapes/ransac_Boctree.h @@ -108,7 +108,7 @@ void showbits(char a) if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center // check if leaf contains shape if ( shape.isInCube(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { - pointrep *points = children->points; + pointrep *points = children->getPointreps(); unsigned int length = points[0].length; T *point = &(points[1].v); // first point for(unsigned int iterator = 0; iterator < length; iterator++ ) { @@ -121,6 +121,7 @@ void showbits(char a) result += PointsOnShape( children->node, ccenter, size/2.0, shape); } ++children; // next child + //r++; } } @@ -142,7 +143,7 @@ void showbits(char a) if ( ( 1 << i ) & node.leaf ) { // if ith node is leaf get center // check if leaf contains shape if ( shape.isInCube(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { - pointrep *points = children->points; + pointrep *points = children->getPointreps(); unsigned int length = points[0].length; T *point = &(points[1].v); // first point for(unsigned int iterator = 0; iterator < length; iterator++ ) { @@ -169,7 +170,6 @@ void showbits(char a) void DrawPoints(vector &p, bitoct &node, unsigned char nrp) { bitunion *children; bitoct::getChildren(node, children); - unsigned char n_children = POPCOUNT(node.valid); unsigned char r = randUC(n_children); if (r == n_children) r--; @@ -217,8 +217,9 @@ void showbits(char a) if (leaf) { /* cout << "STOPPED" << endl; - return;*/ - pointrep *points = children[r].points; + return;*/ + + pointrep *points = children[r].getPointreps(); unsigned int length = points[0].length; if (length < nrp) return; if (length == nrp) { @@ -227,7 +228,6 @@ void showbits(char a) } return; } - // randomly get nrp points, we will not check if this succeeds in getting nrp distinct points for (char c = 0; c < nrp; c++) { int tmp = rand(points[0].length); diff --git a/include/show/show.icc b/include/show/show.icc index 2a389e3..91a79ec 100644 --- a/include/show/show.icc +++ b/include/show/show.icc @@ -5,11 +5,13 @@ @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. */ +#ifndef __SHOW_ICC__ +#define __SHOW_ICC__ /** sets the OpenGL point, (z axis is inverted in OpenGL) */ -void setGLPoint(GLdouble pX, GLdouble pY, GLdouble pZ) +inline void setGLPoint(GLdouble pX, GLdouble pY, GLdouble pZ) { // pZ *= -1.0; glVertex3d(pX, pY, pZ); @@ -20,9 +22,10 @@ void setGLPoint(GLdouble pX, GLdouble pY, GLdouble pZ) sets the OpenGL point, (z axis is inverted in OpenGL) */ -void setGLPoint(GLdouble* p) +inline void setGLPoint(GLdouble* p) { GLdouble pZ = 1.0 * p[2]; glVertex3d(p[0], p[1], pZ); } +#endif \ No newline at end of file diff --git a/include/show/show1.icc b/include/show/show1.icc index 461dbe4..940b03d 100644 --- a/include/show/show1.icc +++ b/include/show/show1.icc @@ -4,6 +4,8 @@ * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany. * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany. */ +#ifndef __SHOW1_ICC__ +#define __SHOW1_ICC__ #include "slam6d/globals.icc" @@ -119,3 +121,4 @@ inline void QuaternionToAxisAngle(const double *quat, double *axis, double &angl axis[1] = normalized_quat[1] / sin_a; axis[2] = -1.0 * normalized_quat[2] / sin_a; } +#endif \ No newline at end of file diff --git a/include/show/show_Boctree.h b/include/show/show_Boctree.h index 5057926..a9e687a 100644 --- a/include/show/show_Boctree.h +++ b/include/show/show_Boctree.h @@ -15,7 +15,7 @@ #include "show/colordisplay.h" #include "slam6d/scan.h" - +using namespace show; /** * @brief Octree for show diff --git a/include/show/viewcull.h b/include/show/viewcull.h index 964f99b..4adadec 100644 --- a/include/show/viewcull.h +++ b/include/show/viewcull.h @@ -7,14 +7,19 @@ #include #include #include +#elif __APPLE__ +#include +#include +#include #else -#include -#include +#include +#include #include #endif #ifndef __VIEWCULL_H__ #define __VIEWCULL_H__ +namespace show{ ///////////////////////// Variable declarations... /** The 6 planes of the viewing frustum */ @@ -161,5 +166,5 @@ void remViewport(); bool LOD(float x, float y, float z, float size); int LOD2(float x, float y, float z, float size); - +} #endif diff --git a/include/slam6d/ann_kd.h b/include/slam6d/ann_kd.h index 039185a..2c9c5f4 100644 --- a/include/slam6d/ann_kd.h +++ b/include/slam6d/ann_kd.h @@ -30,7 +30,7 @@ public: /** * Constructor using the point set pts and the num_of_pts n */ - ANNtree(double **_pts, int n); + ANNtree(PointerArray&_pts, int n); /** * destructor @@ -58,7 +58,7 @@ private: ANNidxArray nn_idx; //temporary ANNdistArray instance to use for storing the nearest neighbor double** pts; - + }; #endif diff --git a/include/slam6d/basicScan.h b/include/slam6d/basicScan.h index 3046f0c..98595db 100644 --- a/include/slam6d/basicScan.h +++ b/include/slam6d/basicScan.h @@ -10,6 +10,8 @@ class BasicScan : public Scan { public: + BasicScan() {}; + static void openDirectory(const std::string& path, IOType type, int start, int end); static void closeDirectory(); /* @@ -29,11 +31,13 @@ public: virtual void get(unsigned int types); virtual DataPointer create(const std::string& identifier, unsigned int size); virtual void clear(const std::string& identifier); - virtual unsigned int readFrames(); virtual void saveFrames(); virtual unsigned int getFrameCount(); virtual void getFrame(unsigned int i, const double*& pose_matrix, AlgoType& type); + + //! Constructor for creation of Scans without openDirectory + BasicScan(double * rPos, double * rPosTheta, std::vector points); protected: virtual void createSearchTreePrivate(); diff --git a/include/slam6d/globals.icc b/include/slam6d/globals.icc index 387ec4c..878323e 100644 --- a/include/slam6d/globals.icc +++ b/include/slam6d/globals.icc @@ -25,6 +25,21 @@ inline int gettimeofday (struct timeval* tp, void* tzp) #include #endif +#define _USE_MATH_DEFINES +#include + +#if defined(__CYGWIN__) +# ifndef M_PI +# define M_PI 3.14159265358979323846 +# define M_PI_2 1.57079632679489661923 +# define M_PI_4 0.78539816339744830962 +# define M_1_PI 0.31830988618379067154 +# define M_2_PI 0.63661977236758134308 +# define M_SQRT2 1.41421356237309504880 +# define M_SQRT1_2 0.70710678118654752440 +# endif +#endif + #include using std::min; using std::max; diff --git a/include/slam6d/kd.h b/include/slam6d/kd.h index 133f7c2..3545c97 100644 --- a/include/slam6d/kd.h +++ b/include/slam6d/kd.h @@ -10,6 +10,7 @@ #include "slam6d/kdparams.h" #include "slam6d/searchTree.h" +#include "slam6d/kdTreeImpl.h" #ifdef _MSC_VER #if !defined _OPENMP && defined OPENMP @@ -21,6 +22,15 @@ #include #endif + +struct Void { }; + +struct PtrAccessor { + inline double *operator() (Void, double* indices) { + return indices; + } +}; + /** * @brief The optimized k-d tree. * @@ -28,67 +38,13 @@ * capabilities (find nearest point to * a given point, or to a ray). **/ -class KDtree : public SearchTree { +class KDtree : public SearchTree, private KDTreeImpl { public: KDtree(double **pts, int n); virtual ~KDtree(); virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const; - -protected: - /** - * storing the parameters of the k-d tree, i.e., the current closest point, - * the distance to the current closest point and the point itself. - * These global variable are needed in this search. - * - * Padded in the parallel case. - */ -#ifdef _OPENMP -#ifdef __INTEL_COMPILER - __declspec (align(16)) static KDParams params[MAX_OPENMP_NUM_THREADS]; -#else - static KDParams params[MAX_OPENMP_NUM_THREADS]; -#endif //__INTEL_COMPILER -#else - static KDParams params[MAX_OPENMP_NUM_THREADS]; -#endif - - /** - * number of points. If this is 0: intermediate node. If nonzero: leaf. - */ - int npts; - - /** - * Cue the standard rant about anon unions but not structs in C++ - */ - union { - /** - * in case of internal node... - */ - struct { - double center[3]; ///< storing the center of the voxel (R^3) - double dx, ///< defining the voxel itself - dy, ///< defining the voxel itself - dz, ///< defining the voxel itself - r2; ///< defining the voxel itself - int splitaxis; ///< defining the kind of splitaxis - KDtree *child1; ///< pointers to the childs - KDtree *child2; ///< pointers to the childs - } node; - /** - * in case of leaf node ... - */ - struct { - /** - * store the value itself - * Here we store just a pointer to the data - */ - double **p; - } leaf; - }; - - void _FindClosest(int threadNum) const; }; #endif diff --git a/include/slam6d/kdManaged.h b/include/slam6d/kdManaged.h index dbc51b6..389bdeb 100644 --- a/include/slam6d/kdManaged.h +++ b/include/slam6d/kdManaged.h @@ -11,6 +11,7 @@ #include "slam6d/kdparams.h" #include "slam6d/searchTree.h" #include "slam6d/data_types.h" +#include "slam6d/kdTreeImpl.h" #ifdef _MSC_VER #if !defined _OPENMP && defined OPENMP @@ -25,6 +26,14 @@ #include #include +class Scan; + +struct ArrayAccessor { + inline double *operator() (const DataXYZ& pts, unsigned int i) { + return pts[i]; + } +}; + /** * @brief The optimized k-d tree. * @@ -32,69 +41,11 @@ * capabilities (find nearest point to * a given point, or to a ray). **/ -class KDtreeManagedBase : public SearchTree { -public: - KDtreeManagedBase(const DataXYZ& pts, unsigned int* indices, unsigned int n); - - virtual ~KDtreeManagedBase(); +class KDtreeManaged : + public SearchTree, + private KDTreeImpl - virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const { return 0; } - -protected: - /** - * storing the parameters of the k-d tree, i.e., the current closest point, - * the distance to the current closest point and the point itself. - * These global variable are needed in this search. - * - * Padded in the parallel case. - */ -#ifdef _OPENMP -#ifdef __INTEL_COMPILER - __declspec (align(16)) static KDParams params[MAX_OPENMP_NUM_THREADS]; -#else - static KDParams params[MAX_OPENMP_NUM_THREADS]; -#endif //__INTEL_COMPILER -#else - static KDParams params[MAX_OPENMP_NUM_THREADS]; -#endif - - /** - * number of points. If this is 0: intermediate node. If nonzero: leaf. - */ - int npts; - - /** - * Cue the standard rant about anon unions but not structs in C++ - */ - union { - /** - * in case of internal node... - */ - struct { - double center[3]; ///< storing the center of the voxel (R^3) - double dx, ///< defining the voxel itself - dy, ///< defining the voxel itself - dz, ///< defining the voxel itself - r2; ///< defining the voxel itself - int splitaxis; ///< defining the kind of splitaxis - KDtreeManagedBase *child1; ///< pointers to the childs - KDtreeManagedBase *child2; ///< pointers to the childs - } node; - /** - * in case of leaf node ... - */ - struct { - //! Content is an array of indices to be put into the dynamically aquired data array - unsigned int* p; - } leaf; - }; - - void _FindClosest(const DataXYZ& pts, int threadNum) const; -}; - -class Scan; - -class KDtreeManaged : public KDtreeManagedBase { +{ public: KDtreeManaged(Scan* scan); virtual ~KDtreeManaged() {} diff --git a/include/slam6d/kdMeta.h b/include/slam6d/kdMeta.h index 12514f8..3c53adb 100644 --- a/include/slam6d/kdMeta.h +++ b/include/slam6d/kdMeta.h @@ -11,6 +11,7 @@ #include "kdparams.h" #include "searchTree.h" #include "data_types.h" +#include "kdTreeImpl.h" #include #include @@ -30,6 +31,16 @@ class Scan; +struct Index { + unsigned int s, i; + inline void set(unsigned int _s, unsigned int _i) { s = _s; i = _i; } +}; + +struct IndexAccessor { + inline double* operator() (const DataXYZ* const* pts, const Index& i) const { + return (*pts[i.s])[i.i]; + } +}; /** * @brief The optimized k-d tree. @@ -38,78 +49,10 @@ class Scan; * capabilities (find nearest point to * a given point, or to a ray). **/ -class KDtreeMeta : public SearchTree { -protected: - class Index { - public: - unsigned int s, i; - inline void set(unsigned int _s, unsigned int _i) { s = _s; i = _i; } - }; -public: - KDtreeMeta(); - virtual ~KDtreeMeta(); - - void create(const DataXYZ* const* pts, Index* indices, unsigned int n); - - virtual double *FindClosest(double *_p, double maxdist2, int threadNum = 0) const { return 0; } - -protected: - /** - * storing the parameters of the k-d tree, i.e., the current closest point, - * the distance to the current closest point and the point itself. - * These global variable are needed in this search. - * - * Padded in the parallel case. - */ -#ifdef _OPENMP -#ifdef __INTEL_COMPILER - __declspec (align(16)) static KDParams params[MAX_OPENMP_NUM_THREADS]; -#else - static KDParams params[MAX_OPENMP_NUM_THREADS]; -#endif //__INTEL_COMPILER -#else - static KDParams params[MAX_OPENMP_NUM_THREADS]; -#endif - - /** - * number of points. If this is 0: intermediate node. If nonzero: leaf. - */ - int npts; - - /** - * Cue the standard rant about anon unions but not structs in C++ - */ - union { - /** - * in case of internal node... - */ - struct { - double center[3]; ///< storing the center of the voxel (R^3) - double dx, ///< defining the voxel itself - dy, ///< defining the voxel itself - dz, ///< defining the voxel itself - r2; ///< defining the voxel itself - int splitaxis; ///< defining the kind of splitaxis - KDtreeMeta *child1; ///< pointers to the childs - KDtreeMeta *child2; ///< pointers to the childs - } node; - /** - * in case of leaf node ... - */ - struct { - //! Content is an array of indices to be put into the dynamically aquired data array - Index* p; - } leaf; - }; - - inline double* point(const DataXYZ* const* pts, const Index& i) const { - return (*pts[i.s])[i.i]; - } - - void _FindClosest(const DataXYZ* const* pts, int threadNum) const; -}; - -class KDtreeMetaManaged : public KDtreeMeta { +class KDtreeMetaManaged : + public SearchTree, + private KDTreeImpl +{ public: KDtreeMetaManaged(const vector& scans); virtual ~KDtreeMetaManaged(); diff --git a/include/slam6d/scan.h b/include/slam6d/scan.h index 193984a..761bae7 100644 --- a/include/slam6d/scan.h +++ b/include/slam6d/scan.h @@ -103,8 +103,8 @@ public: }; // delete copy-ctor and assignment, scans shouldn't be copied by basic class - // Scan(const Scan& other) = delete; - // Scan& operator=(const Scan& other) = delete; + Scan(const Scan& other) = delete; + Scan& operator=(const Scan& other) = delete; virtual ~Scan(); @@ -256,10 +256,8 @@ protected: public: - /* Direct creation of reduced points and search tree */ - //! Apply reduction and initial transMatOrg transformation void toGlobal(); @@ -406,9 +404,10 @@ private: //! flag for openDirectory and closeDirectory to distinguish the scans static bool scanserver; +public: //! Mutex for safely reducing points and creating the search tree just once in a multithreaded environment - // it can not be compiled in wein32 use boost 1.48, therefore we remeove it temporarily -// boost::mutex m_mutex_reduction, m_mutex_create_tree; + // it can not be compiled in win32 use boost 1.48, therefore we remeove it temporarily + boost::mutex m_mutex_reduction, m_mutex_create_tree; }; #include "scan.icc" diff --git a/include/thermo/thermo.h b/include/thermo/thermo.h index a75c2da..1d7743e 100644 --- a/include/thermo/thermo.h +++ b/include/thermo/thermo.h @@ -6,14 +6,15 @@ #include #include using namespace std; -typedef vector > Float2D[1200][1600]; +//typedef vector > Float2D[1200][1600]; +typedef vector > Float2D[2592][3888]; void calcBoard(double point_array[][2], int board_n, double &x, double &y, double &cx, double &cy, bool pc); void sortBlobs(double point_array[][2], int board_n, int board_h, int board_w, bool quiet); IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2]); void drawLines(double point_array2[][2], int corner_exp, IplImage *image, bool color=false); IplImage* resizeImage(IplImage *source, int scale); -IplImage* detectCorners(IplImage *orgimage, int corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale=1); +IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale=1); void CalibFunc(int board_w, int board_h, int start, int end, bool optical, bool chess, bool quiet, string dir, int scale=1); void writeCalibParam(int images, int corner_exp, int board_w, CvMat* image_points, CvSize size, string dir); @@ -27,8 +28,8 @@ void calculateExtrinsicsWithReprojectionCheck(CvMat * points2D, CvMat * points3D, CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, CvMat * distortions, CvMat * instrinsics, int corners, int successes, string dir, bool quiet=true, string substring = "") ; void calculateExtrinsics(CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, int successes, string dir, bool quiet=true, string substring = "") ; -void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size); -void clusterSearch(float ** points, int size, double thresh1, double thresh2, fstream &outfile); +void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size, bool optical); +void clusterSearch(float ** points, int size, double thresh1, double thresh2, fstream &outfile, bool optical); void sortDistances(float ** points, int size); void ExtrCalibFunc(int board_w, int board_h, int start, int end, bool optical, bool chess, bool quiet, string dir, int scale=1); diff --git a/include/veloslam/veloscan.h b/include/veloslam/veloscan.h index 44f0247..12e2406 100644 --- a/include/veloslam/veloscan.h +++ b/include/veloslam/veloscan.h @@ -18,7 +18,7 @@ #include #include -#include "slam6d/scan.h" +#include "slam6d/basicScan.h" #include "veloslam/gridcell.h" #include "veloslam/gridcluster.h" @@ -36,7 +36,7 @@ class Trajectory /** * @brief 3D scan representation and implementation of dynamic velodyne scan matching */ -class VeloScan : public Scan { +class VeloScan : public BasicScan { public: VeloScan(); diff --git a/src/scanio/scan_io_velodyne.cc b/src/scanio/scan_io_velodyne.cc index 6bf4c63..5150144 100644 --- a/src/scanio/scan_io_velodyne.cc +++ b/src/scanio/scan_io_velodyne.cc @@ -363,11 +363,15 @@ int read_one_packet ( for ( c = 0 ; c < CIRCLELENGTH; c++ ) { -#ifdef _MSC_VER +#ifdef _MSC_VER fseek(fp , BLOCK_OFFSET, SEEK_CUR); #else +#ifdef __CYGWIN__ + fseek(fp , BLOCK_OFFSET, SEEK_CUR); +#else fseeko(fp , BLOCK_OFFSET, SEEK_CUR); #endif +#endif size=fread ( buf, 1, BLOCK_SIZE, fp ); if(size +#include + #include #include enum image_type {M_RANGE, M_INTENSITY}; -enum segment_method {THRESHOLD, ADAPTIVE_THRESHOLD, PYR_MEAN_SHIFT, WATERSHED}; +enum segment_method {THRESHOLD, ADAPTIVE_THRESHOLD, PYR_MEAN_SHIFT, PYR_SEGMENTATION, WATERSHED}; /* Function used to check that 'opt1' and 'opt2' are not specified at the same time. */ @@ -87,6 +90,7 @@ void validate(boost::any& v, const std::vector& values, if(strcasecmp(arg.c_str(), "THRESHOLD") == 0) v = THRESHOLD; else if(strcasecmp(arg.c_str(), "ADAPTIVE_THRESHOLD") == 0) v = ADAPTIVE_THRESHOLD; else if(strcasecmp(arg.c_str(), "PYR_MEAN_SHIFT") == 0) v = PYR_MEAN_SHIFT; + else if(strcasecmp(arg.c_str(), "PYR_SEGMENTATION") == 0) v = PYR_SEGMENTATION; else if(strcasecmp(arg.c_str(), "WATERSHED") == 0) v = WATERSHED; else throw std::runtime_error(std::string("segmentation method ") + arg + std::string(" is unknown")); } @@ -106,6 +110,23 @@ void validate(boost::any& v, const std::vector& values, } } +void segmentation_option_dependency(const po::variables_map & vm, segment_method stype, const char *option) +{ + if (vm.count("segment") && vm["segment"].as() == stype) { + if (!vm.count(option)) { + throw std::logic_error (string("this segmentation option needs ")+option+" to be set"); + } + } +} + +void segmentation_option_conflict(const po::variables_map & vm, segment_method stype, const char *option) +{ + if (vm.count("segment") && vm["segment"].as() == stype) { + if (vm.count(option)) { + throw std::logic_error (string("this segmentation option is incompatible with ")+option); + } + } +} /* * parse commandline options, fill arguments */ @@ -114,7 +135,8 @@ void parse_options(int argc, char **argv, int &start, int &end, fbr::projection_method &ptype, string &dir, IOType &iotype, int &maxDist, int &minDist, int &nImages, int &pParam, bool &logarithm, float &cutoff, segment_method &stype, string &marker, bool &dump_pano, - bool &dump_seg, double &thresh, int &maxlevel, int &radius) + bool &dump_seg, double &thresh, int &maxlevel, int &radius, + double &pyrlinks, double &pyrcluster, int &pyrlevels) { po::options_description generic("Generic options"); generic.add_options() @@ -169,7 +191,7 @@ void parse_options(int argc, char **argv, int &start, int &end, segment.add_options() ("segment,g", po::value(&stype)-> default_value(PYR_MEAN_SHIFT), "segmentation method (THRESHOLD, " - "ADAPTIVE_THRESHOLD, PYR_MEAN_SHIFT, WATERSHED)") + "ADAPTIVE_THRESHOLD, PYR_MEAN_SHIFT, PYR_SEGMENTATION, WATERSHED)") ("marker,K", po::value(&marker), "marker mask for watershed segmentation") ("thresh,T", po::value(&thresh), @@ -178,6 +200,13 @@ void parse_options(int argc, char **argv, int &start, int &end, "maximum level for meanshift segmentation") ("radius,R", po::value(&radius), "radius for meanshift segmentation") + ("links,l", po::value(&pyrlinks), + "error threshold for establishing the links for pyramid segmentation") + ("clustering,c", po::value(&pyrcluster), + "error threshold for the segments clustering for pyramid " + "segmentation") + ("levels,E", po::value(&pyrlevels)->default_value(4), + "levels of pyramid segmentation") ("dump-seg,D", po::bool_switch(&dump_seg), "output segmentation image (for debugging)"); @@ -206,6 +235,9 @@ void parse_options(int argc, char **argv, int &start, int &end, // display help if (vm.count("help")) { cout << cmdline_options; + cout << "\nExample usage:\n" + << "bin/scan2segments -s 0 -e 0 -f riegl_txt --segment PYR_SEGMENTATION -l 50 -c 50 -E 4 -D -i ~/path/to/bremen_city\n" + << "bin/scan2segments -s 0 -e 0 -f riegl_txt --segment PYR_SEGMENTATION -l 255 -c 30 -E 2 -D -i ~/path/to/bremen_city\n"; exit(0); } @@ -220,37 +252,36 @@ void parse_options(int argc, char **argv, int &start, int &end, else itype = M_INTENSITY; - // option dependencies on segmentation types - if (stype == WATERSHED) { - if (!vm.count("marker")) - throw std::logic_error("watershed segmentation requires --marker to be set"); - if (vm.count("thresh")) - throw std::logic_error("watershed segmentation cannot be used with --thresh"); - if (vm.count("maxlevel")) - throw std::logic_error("watershed segmentation cannot be used with --maxlevel"); - if (vm.count("radius")) - throw std::logic_error("watershed segmentation cannot be used with --radius"); - } - if (stype == THRESHOLD) { - if (!vm.count("thresh")) - throw std::logic_error("threshold segmentation requires --thresh to be set"); - if (vm.count("marker")) - throw std::logic_error("threshold segmentation cannot be used with --marker"); - if (vm.count("maxlevel")) - throw std::logic_error("threshold segmentation cannot be used with --maxlevel"); - if (vm.count("radius")) - throw std::logic_error("threshold segmentation cannot be used with --radius"); - } - if (stype == PYR_MEAN_SHIFT) { - if (!vm.count("maxlevel")) - throw std::logic_error("mean shift segmentation requires --maxlevel to be set"); - if (!vm.count("radius")) - throw std::logic_error("mean shift segmentation requires --radius to be set"); - if (vm.count("thresh")) - throw std::logic_error("mean shift segmentation cannot be used with --thresh"); - if (vm.count("marker")) - throw std::logic_error("mean shift segmentation cannot be used with --marker"); - } + segmentation_option_dependency(vm, WATERSHED, "marker"); + segmentation_option_conflict(vm, WATERSHED, "thresh"); + segmentation_option_conflict(vm, WATERSHED, "maxlevel"); + segmentation_option_conflict(vm, WATERSHED, "radius"); + segmentation_option_conflict(vm, WATERSHED, "links"); + segmentation_option_conflict(vm, WATERSHED, "clustering"); + segmentation_option_conflict(vm, WATERSHED, "levels"); + + segmentation_option_conflict(vm, THRESHOLD, "marker"); + segmentation_option_dependency(vm, THRESHOLD, "thresh"); + segmentation_option_conflict(vm, THRESHOLD, "maxlevel"); + segmentation_option_conflict(vm, THRESHOLD, "radius"); + segmentation_option_conflict(vm, THRESHOLD, "links"); + segmentation_option_conflict(vm, THRESHOLD, "clustering"); + segmentation_option_conflict(vm, THRESHOLD, "levels"); + + segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "marker"); + segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "thresh"); + segmentation_option_dependency(vm, PYR_MEAN_SHIFT, "maxlevel"); + segmentation_option_dependency(vm, PYR_MEAN_SHIFT, "radius"); + segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "links"); + segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "clustering"); + segmentation_option_conflict(vm, PYR_MEAN_SHIFT, "levels"); + + segmentation_option_conflict(vm, PYR_SEGMENTATION, "marker"); + segmentation_option_conflict(vm, PYR_SEGMENTATION, "thresh"); + segmentation_option_conflict(vm, PYR_SEGMENTATION, "maxlevel"); + segmentation_option_conflict(vm, PYR_SEGMENTATION, "radius"); + segmentation_option_dependency(vm, PYR_SEGMENTATION, "links"); + segmentation_option_dependency(vm, PYR_SEGMENTATION, "clustering"); // correct pParam and nImages for certain panorama types if (ptype == fbr::PANNINI && pParam == 0) { @@ -430,6 +461,60 @@ cv::Mat calculatePyrMeanShift(vector> &segmented_points, return res; } +///TODO: need to pass *two* thresh params, see: http://bit.ly/WmFeub +cv::Mat calculatePyrSegmentation(vector> &segmented_points, + cv::Mat &img, vector > > extendedMap, + double thresh1, double thresh2, int pyrlevels) +{ + int i, j, idx; + int block_size = 1000; + IplImage ipl_img = img; + IplImage* ipl_original = &ipl_img; + IplImage* ipl_segmented = 0; + + CvMemStorage* storage = cvCreateMemStorage(block_size); + CvSeq* comp = NULL; + + // the following lines are required because the level must not be more + // than log2(min(width, height)) + ipl_original->width &= -(1<height &= -(1< mapping; + unsigned int segments = comp->total; + for (unsigned int cur_seg = 0; cur_seg < segments; ++cur_seg) { + CvConnectedComp* cc = (CvConnectedComp*) cvGetSeqElem(comp, cur_seg); + // since images are single-channel grayscale, only the first value is + // of interest + mapping.insert(pair(cc->value.val[0], cur_seg)); + } + + segmented_points.resize(segments); + + uchar *data = (uchar *)ipl_segmented->imageData; + int step = ipl_segmented->widthStep; + for (i = 0; i < ipl_segmented->height; i++) { + for (j = 0; j < ipl_segmented->width; j++) { + idx = mapping[data[i*step+j]]; + segmented_points[idx].insert(segmented_points[idx].end(), + extendedMap[i][j].begin(), + extendedMap[i][j].end()); + } + } + + // clearing memory + cvReleaseMemStorage(&storage); + + cv::Mat res(ipl_segmented); + return res; +} + /* * calculate the adaptive threshold */ @@ -572,11 +657,13 @@ int main(int argc, char **argv) bool dump_pano, dump_seg; double thresh; int maxlevel, radius; + double pyrlinks, pyrcluster; + int pyrlevels; parse_options(argc, argv, start, end, scanserver, itype, width, height, ptype, dir, iotype, maxDist, minDist, nImages, pParam, logarithm, cutoff, stype, marker, dump_pano, dump_seg, thresh, maxlevel, - radius); + radius, pyrlinks, pyrcluster, pyrlevels); Scan::openDirectory(scanserver, dir, iotype, start, end); @@ -623,6 +710,8 @@ int main(int argc, char **argv) } else if (stype == PYR_MEAN_SHIFT) { res = calculatePyrMeanShift(segmented_points, img, fPanorama.getExtendedMap(), maxlevel, radius); + } else if (stype == PYR_SEGMENTATION) { + res = calculatePyrSegmentation(segmented_points, img, fPanorama.getExtendedMap(), pyrlinks, pyrcluster, pyrlevels); } else if (stype == ADAPTIVE_THRESHOLD) { res = calculateAdaptiveThreshold(segmented_points, img, fPanorama.getExtendedMap()); } else if (stype == WATERSHED) { diff --git a/src/shapes/plane.cc b/src/shapes/plane.cc index 5ec9108..c32d71d 100644 --- a/src/shapes/plane.cc +++ b/src/shapes/plane.cc @@ -276,7 +276,7 @@ int main(int argc, char **argv) if(!quiet) cout << nx << " " << ny << " " << nz << " " << d << endl; - /** + /* for (unsigned int i = 0; i < points.size(); i++) { cerr << points[i][0] << " " << points[i][1] << " " << points[i][2] << endl; } diff --git a/src/show/CMakeLists.txt b/src/show/CMakeLists.txt index 9ce328d..c3c2ded 100644 --- a/src/show/CMakeLists.txt +++ b/src/show/CMakeLists.txt @@ -1,4 +1,4 @@ -SET(SHOW_LIBS ${OPENGL_LIBRARIES} glui scan ANN) +SET(SHOW_LIBS glui scan ANN ${OPENGL_LIBRARIES}) IF(WIN32) IF( CMAKE_SIZEOF_VOID_P EQUAL 8 ) SET(SHOW_LIBS ${SHOW_LIBS} ${CMAKE_SOURCE_DIR}/3rdparty/windows/x64/freeglut.lib XGetopt) @@ -7,7 +7,7 @@ IF(WIN32) ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8 ) ENDIF(WIN32) IF (UNIX) - SET(SHOW_LIBS newmat dl ${GLUT_LIBRARIES} ${SHOW_LIBS}) + SET(SHOW_LIBS newmat dl ${SHOW_LIBS} ${GLUT_LIBRARIES}) ENDIF(UNIX) IF(WITH_GLEE) @@ -26,3 +26,8 @@ IF(WITH_WXSHOW) target_link_libraries(wxshow ${wxWidgets_LIBRARIES} wxthings ${SHOW_LIBS}) ENDIF(WITH_WXSHOW) +### EXPORT SHARED LIBS +IF(EXPORT_SHARED_LIBS) +add_library(show_s SHARED ${SHOW_SRCS}) +target_link_libraries(show_s ${SHOW_LIBS}) +ENDIF(EXPORT_SHARED_LIBS) diff --git a/src/show/compacttree.cc b/src/show/compacttree.cc index 44067b8..11318b0 100644 --- a/src/show/compacttree.cc +++ b/src/show/compacttree.cc @@ -38,6 +38,7 @@ using std::list; #include "show/scancolormanager.h" #include "show/viewcull.h" +using namespace show; compactTree::~compactTree(){ delete alloc; diff --git a/src/show/show_gl.cc b/src/show/show_gl.cc index 9b5bafd..13e043f 100644 --- a/src/show/show_gl.cc +++ b/src/show/show_gl.cc @@ -12,6 +12,7 @@ #include "show/viewcull.h" #include "show/scancolormanager.h" +using namespace show; bool fullydisplayed = true; // true if all points have been drawn to the screen bool mousemoving = false; // true iff a mouse button has been pressed inside a window, // but hs not been released diff --git a/src/show/viewcull.cc b/src/show/viewcull.cc index bc0966a..95a8b00 100644 --- a/src/show/viewcull.cc +++ b/src/show/viewcull.cc @@ -27,6 +27,7 @@ #endif #include "slam6d/globals.icc" +namespace show{ /** The 6 planes of the viewing frustum */ float frustum[6][4]; @@ -843,3 +844,4 @@ bool CubeInFrustum( float x, float y, float z, float size ) float minB[NUMDIM], maxB[NUMDIM]; /*box */ float coord[NUMDIM]; /* hit point */ +} diff --git a/src/slam6d/CMakeLists.txt b/src/slam6d/CMakeLists.txt index 7469d0c..98fee53 100644 --- a/src/slam6d/CMakeLists.txt +++ b/src/slam6d/CMakeLists.txt @@ -90,6 +90,7 @@ ENDIF(UNIX) IF(EXPORT_SHARED_LIBS) add_library(scan_s SHARED ${SCANLIB_SRCS}) #target_link_libraries(scan_s ${Boost_LIBRARIES} newmat) +target_link_libraries(scan_s newmat sparse ANN scanclient pointfilter scanio) ENDIF(EXPORT_SHARED_LIBS) ### SLAM6D diff --git a/src/slam6d/ann_kd.cc b/src/slam6d/ann_kd.cc index 29752fd..f9e32a9 100644 --- a/src/slam6d/ann_kd.cc +++ b/src/slam6d/ann_kd.cc @@ -39,9 +39,26 @@ using std::swap; * @param pts 3D array of points * @param n number of points */ -ANNtree::ANNtree(double **_pts, int n) +ANNtree::ANNtree(PointerArray&_pts, int n) { - pts = _pts; + /* + pts = new double*[n]; + for(unsigned int i = 0; i < n; i++) { + pts[i] = new double[3]; + pts[i][0] = _pts.get()[i][0]; + pts[i][1] = _pts.get()[i][1]; + pts[i][2] = _pts.get()[i][2]; + } + */ + pts = new double*[n]; + double* tpts = new double[3*n]; + for(unsigned int i = 0, j = 0; i < n; i++) { + pts[i] = &tpts[j]; + tpts[j++] = _pts.get()[i][0]; + tpts[j++] = _pts.get()[i][1]; + tpts[j++] = _pts.get()[i][2]; + } + annkd = new ANNkd_tree(pts, n, 3, 1, ANN_KD_SUGGEST); // links to the constructor of ANNkd_tree cout << "ANNkd_tree was generated with " << n << " points" << endl; nn = new ANNdist[1]; @@ -60,6 +77,8 @@ ANNtree::~ANNtree() delete annkd; //links to the destructor of ANNkd_tree delete [] nn; delete [] nn_idx; + delete [] pts[0]; + delete [] pts; } @@ -76,10 +95,11 @@ double *ANNtree::FindClosest(double *_p, double maxdist2, int threadNum) const #pragma omp critical annkd->annkSearch(_p, 1, nn_idx, nn, 0.0); + int idx = nn_idx[0]; - + if (Dist2(_p, pts[idx]) > maxdist2) return 0; - + return pts[idx]; } diff --git a/src/slam6d/basicScan.cc b/src/slam6d/basicScan.cc index 56179aa..0956967 100644 --- a/src/slam6d/basicScan.cc +++ b/src/slam6d/basicScan.cc @@ -66,6 +66,38 @@ void BasicScan::closeDirectory() Scan::allScans.clear(); } +BasicScan::BasicScan(double *_rPos, double *_rPosTheta, vector points) { + init(); + for(int i = 0; i < 3; i++) { + rPos[i] = _rPos[i]; + rPosTheta[i] = _rPosTheta[i]; + } + // write original pose matrix + EulerToMatrix4(rPos, rPosTheta, transMatOrg); + + // initialize transform matrices from the original one, could just copy transMatOrg to transMat instead + transformMatrix(transMatOrg); + + // reset the delta align matrix to represent only the transformations after local-to-global (transMatOrg) one + M4identity(dalignxf); + PointFilter filter; + if(m_filter_range_set) + filter.setRange(m_filter_max, m_filter_min); + if(m_filter_height_set) + filter.setHeight(m_filter_top, m_filter_bottom); + if(m_range_mutation_set) + filter.setRangeMutator(m_range_mutation); + + + double* data = reinterpret_cast(create("xyz", sizeof(double) * 3 * points.size()).get_raw_pointer()); + int tmp = 0; + for(unsigned int i = 0; i < points.size(); ++i) { + for(unsigned int j = 0; j < 3; j++) { + data[tmp++] = points[i][j]; + } + } +} + BasicScan::BasicScan(const std::string& path, const std::string& identifier, IOType type) : m_path(path), m_identifier(identifier), m_type(type) { @@ -274,7 +306,7 @@ void BasicScan::createSearchTreePrivate() kd = new KDtree(ar.get(), xyz_orig.size()); break; case ANNTree: - kd = new ANNtree(ar.get(), xyz_orig.size()); + kd = new ANNtree(ar, xyz_orig.size()); break; case BOCTree: kd = new BOctTree(ar.get(), xyz_orig.size(), 10.0, PointType(), true); diff --git a/src/slam6d/fbr/CMakeLists.txt b/src/slam6d/fbr/CMakeLists.txt index 1353805..18330dc 100644 --- a/src/slam6d/fbr/CMakeLists.txt +++ b/src/slam6d/fbr/CMakeLists.txt @@ -4,8 +4,8 @@ SET(FBR_IO_SRC scan_cv.cc) add_library(fbr_cv_io STATIC ${FBR_IO_SRC}) SET(FBR_PANORAMA_SRC panorama.cc) -add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC}) -#add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC} fbr_global.cc) +#add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC}) +add_library(fbr_panorama STATIC ${FBR_PANORAMA_SRC} fbr_global.cc) SET(FBR_FEATURE_SRC feature.cc) @@ -17,7 +17,8 @@ add_library(fbr_feature_matcher STATIC ${FBR_FEATURE_MATCHER_SRC}) SET(FBR_REGISTRATION_SRC registration.cc) add_library(fbr_registration STATIC ${FBR_REGISTRATION_SRC}) -add_library(fbr STATIC ${FBR_IO_SRC} ${FBR_PANORAMA_SRC} ${FBR_FEATURE_SRC} ${FBR_FEATURE_MATCHER_SRC} ${FBR_REGISTRATION_SRC} fbr_global.cc) +SET(FBR_SRC scan_cv.cc panorama.cc feature.cc feature_matcher.cc registration.cc fbr_global.cc) +add_library(fbr STATIC ${FBR_SRC}) IF(WITH_FBR) SET(FBR_LIBS scan ANN ${OpenCV_LIBS}) @@ -26,3 +27,10 @@ add_executable(featurebasedregistration feature_based_registration.cc fbr_global #target_link_libraries(featurebasedregistration fbr_cv_io fbr_panorama fbr_feature fbr_feature_matcher fbr_registration ${FBR_LIBS}) target_link_libraries(featurebasedregistration fbr ${FBR_LIBS}) ENDIF(WITH_FBR) + +### EXPORT SHARED LIBS + +IF(EXPORT_SHARED_LIBS) +add_library(fbr_s SHARED ${FBR_SRC}) +target_link_libraries(fbr_s scan ANN ${OpenCV_LIBS}) +ENDIF(EXPORT_SHARED_LIBS) \ No newline at end of file diff --git a/src/slam6d/icp6D.cc b/src/slam6d/icp6D.cc index 4baceb3..baf8cbd 100644 --- a/src/slam6d/icp6D.cc +++ b/src/slam6d/icp6D.cc @@ -144,6 +144,7 @@ int icp6D::match(Scan* PreviousScan, Scan* CurrentScan) #pragma omp parallel { int thread_num = omp_get_thread_num(); + Scan::getPtPairsParallel(pairs, PreviousScan, CurrentScan, thread_num, step, rnd, max_dist_match2, @@ -183,7 +184,6 @@ int icp6D::match(Scan* PreviousScan, Scan* CurrentScan) } } // end parallel - // do we have enough point pairs? unsigned int pairssize = 0; for (int i = 0; i < OPENMP_NUM_THREADS; i++) { @@ -330,6 +330,7 @@ void icp6D::doICP(vector allScans) vector < Scan* > meta_scans; Scan* my_MetaScan = 0; + for(unsigned int i = 0; i < allScans.size(); i++) { cout << i << "*" << endl; diff --git a/src/slam6d/kd.cc b/src/slam6d/kd.cc index f2bf4f0..cea3058 100644 --- a/src/slam6d/kd.cc +++ b/src/slam6d/kd.cc @@ -31,7 +31,8 @@ using std::swap; #include // KDtree class static variables -KDParams KDtree::params[MAX_OPENMP_NUM_THREADS]; +template +KDParams KDTreeImpl::params[MAX_OPENMP_NUM_THREADS]; /** * Constructor @@ -43,100 +44,11 @@ KDParams KDtree::params[MAX_OPENMP_NUM_THREADS]; */ KDtree::KDtree(double **pts, int n) { - // Find bbox - double xmin = pts[0][0], xmax = pts[0][0]; - double ymin = pts[0][1], ymax = pts[0][1]; - double zmin = pts[0][2], zmax = pts[0][2]; - for (int i = 1; i < n; i++) { - xmin = min(xmin, pts[i][0]); - xmax = max(xmax, pts[i][0]); - ymin = min(ymin, pts[i][1]); - ymax = max(ymax, pts[i][1]); - zmin = min(zmin, pts[i][2]); - zmax = max(zmax, pts[i][2]); - } - - // Leaf nodes - if ((n > 0) && (n <= 10)) { - npts = n; - leaf.p = new double*[n]; - memcpy(leaf.p, pts, n * sizeof(double *)); - return; - } - - // Else, interior nodes - npts = 0; - - node.center[0] = 0.5 * (xmin+xmax); - node.center[1] = 0.5 * (ymin+ymax); - node.center[2] = 0.5 * (zmin+zmax); - node.dx = 0.5 * (xmax-xmin); - node.dy = 0.5 * (ymax-ymin); - node.dz = 0.5 * (zmax-zmin); - node.r2 = sqr(node.dx) + sqr(node.dy) + sqr(node.dz); - - // Find longest axis - if (node.dx > node.dy) { - if (node.dx > node.dz) { - node.splitaxis = 0; - } else { - node.splitaxis = 2; - } - } else { - if (node.dy > node.dz) { - node.splitaxis = 1; - } else { - node.splitaxis = 2; - } - } - - // Partition - double splitval = node.center[node.splitaxis]; - - if ( fabs(max(max(node.dx,node.dy),node.dz)) < 0.01 ) { - npts = n; - leaf.p = new double*[n]; - memcpy(leaf.p, pts, n * sizeof(double *)); - return; - } - - double **left = pts, **right = pts + n - 1; - while (1) { - while ((*left)[node.splitaxis] < splitval) - left++; - while ((*right)[node.splitaxis] >= splitval) - right--; - if (right < left) - break; - swap(*left, *right); - } - - // Build subtrees - int i; -#ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas - omp_set_num_threads(OPENMP_NUM_THREADS); -#pragma omp parallel for schedule(dynamic) -#endif - for (i = 0; i < 2; i++) { - if (i == 0) node.child1 = new KDtree(pts, left-pts); - if (i == 1) node.child2 = new KDtree(left, n-(left-pts)); - } + create(Void(), pts, n); } KDtree::~KDtree() { - if (!npts) { -#ifdef WITH_OPENMP_KD - omp_set_num_threads(OPENMP_NUM_THREADS); -#pragma omp parallel for schedule(dynamic) -#endif - for (int i = 0; i < 2; i++) { - if (i == 0 && node.child1) delete node.child1; - if (i == 1 && node.child2) delete node.child2; - } - } else { - if (leaf.p) delete [] leaf.p; - } } /** @@ -152,45 +64,6 @@ double *KDtree::FindClosest(double *_p, double maxdist2, int threadNum) const params[threadNum].closest = 0; params[threadNum].closest_d2 = maxdist2; params[threadNum].p = _p; - _FindClosest(threadNum); + _FindClosest(Void(), threadNum); return params[threadNum].closest; } - -/** - * Wrapped function - */ -void KDtree::_FindClosest(int threadNum) const -{ - // Leaf nodes - if (npts) { - for (int i = 0; i < npts; i++) { - double myd2 = Dist2(params[threadNum].p, leaf.p[i]); - if (myd2 < params[threadNum].closest_d2) { - params[threadNum].closest_d2 = myd2; - params[threadNum].closest = leaf.p[i]; - } - } - return; - } - - // Quick check of whether to abort - double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, - fabs(params[threadNum].p[1]-node.center[1])-node.dy), - fabs(params[threadNum].p[2]-node.center[2])-node.dz); - if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2) - return; - - // Recursive case - double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis]; - if (myd >= 0.0) { - node.child1->_FindClosest(threadNum); - if (sqr(myd) < params[threadNum].closest_d2) { - node.child2->_FindClosest(threadNum); - } - } else { - node.child2->_FindClosest(threadNum); - if (sqr(myd) < params[threadNum].closest_d2) { - node.child1->_FindClosest(threadNum); - } - } -} diff --git a/src/slam6d/kdManaged.cc b/src/slam6d/kdManaged.cc index bf25b9c..562ac44 100644 --- a/src/slam6d/kdManaged.cc +++ b/src/slam6d/kdManaged.cc @@ -32,163 +32,13 @@ using std::swap; #include // KDtree class static variables -KDParams KDtreeManagedBase::params[MAX_OPENMP_NUM_THREADS]; - -/** - * Constructor - * - * Create a KD tree from the points pointed to by the array pts - * - * @param pts 3D array of points - * @param n number of points - */ -KDtreeManagedBase::KDtreeManagedBase(const DataXYZ& pts, unsigned int* indices, unsigned int n) -{ - // Find bbox - double xmin = pts[indices[0]][0], xmax = pts[indices[0]][0]; - double ymin = pts[indices[0]][1], ymax = pts[indices[0]][1]; - double zmin = pts[indices[0]][2], zmax = pts[indices[0]][2]; - for(unsigned int i = 1; i < n; i++) { - xmin = min(xmin, pts[indices[i]][0]); - xmax = max(xmax, pts[indices[i]][0]); - ymin = min(ymin, pts[indices[i]][1]); - ymax = max(ymax, pts[indices[i]][1]); - zmin = min(zmin, pts[indices[i]][2]); - zmax = max(zmax, pts[indices[i]][2]); - } - - // Leaf nodes - if ((n > 0) && (n <= 10)) { - npts = n; - leaf.p = new unsigned int[n]; - // fill leaf index array with indices - for(unsigned int i = 0; i < n; ++i) { - leaf.p[i] = indices[i]; - } - return; - } - - // Else, interior nodes - npts = 0; - - node.center[0] = 0.5 * (xmin+xmax); - node.center[1] = 0.5 * (ymin+ymax); - node.center[2] = 0.5 * (zmin+zmax); - node.dx = 0.5 * (xmax-xmin); - node.dy = 0.5 * (ymax-ymin); - node.dz = 0.5 * (zmax-zmin); - node.r2 = sqr(node.dx) + sqr(node.dy) + sqr(node.dz); - - // Find longest axis - if (node.dx > node.dy) { - if (node.dx > node.dz) { - node.splitaxis = 0; - } else { - node.splitaxis = 2; - } - } else { - if (node.dy > node.dz) { - node.splitaxis = 1; - } else { - node.splitaxis = 2; - } - } - - // Partition - double splitval = node.center[node.splitaxis]; - - if ( fabs(max(max(node.dx,node.dy),node.dz)) < 0.01 ) { - npts = n; - leaf.p = new unsigned int[n]; - // fill leaf index array with indices - for(unsigned int i = 0; i < n; ++i) { - leaf.p[i] = indices[i]; - } - return; - } - - unsigned int* left = indices, * right = indices + n - 1; - while(true) { - while(pts[*left][node.splitaxis] < splitval) - left++; - while(pts[*right][node.splitaxis] >= splitval) - right--; - if(right < left) - break; - swap(*left, *right); - } - - // Build subtrees - int i; -#ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas - omp_set_num_threads(OPENMP_NUM_THREADS); -#pragma omp parallel for schedule(dynamic) -#endif - for (i = 0; i < 2; i++) { - if (i == 0) node.child1 = new KDtreeManagedBase(pts, indices, left - indices); - if (i == 1) node.child2 = new KDtreeManagedBase(pts, left, n - (left - indices)); - } -} - -KDtreeManagedBase::~KDtreeManagedBase() -{ - if (!npts) { -#ifdef WITH_OPENMP_KD - omp_set_num_threads(OPENMP_NUM_THREADS); -#pragma omp parallel for schedule(dynamic) -#endif - for (int i = 0; i < 2; i++) { - if (i == 0 && node.child1) delete node.child1; - if (i == 1 && node.child2) delete node.child2; - } - } else { - if (leaf.p) delete [] leaf.p; - } -} - -/** - * Wrapped function - */ -void KDtreeManagedBase::_FindClosest(const DataXYZ& pts, int threadNum) const -{ - // Leaf nodes - if (npts) { - for (int i = 0; i < npts; i++) { - double myd2 = Dist2(params[threadNum].p, pts[leaf.p[i]]); - if (myd2 < params[threadNum].closest_d2) { - params[threadNum].closest_d2 = myd2; - params[threadNum].closest = pts[leaf.p[i]]; - } - } - return; - } - - // Quick check of whether to abort - double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, - fabs(params[threadNum].p[1]-node.center[1])-node.dy), - fabs(params[threadNum].p[2]-node.center[2])-node.dz); - if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2) - return; - - // Recursive case - double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis]; - if (myd >= 0.0) { - node.child1->_FindClosest(pts, threadNum); - if (sqr(myd) < params[threadNum].closest_d2) { - node.child2->_FindClosest(pts, threadNum); - } - } else { - node.child2->_FindClosest(pts, threadNum); - if (sqr(myd) < params[threadNum].closest_d2) { - node.child1->_FindClosest(pts, threadNum); - } - } -} +template +KDParams KDTreeImpl::params[MAX_OPENMP_NUM_THREADS]; KDtreeManaged::KDtreeManaged(Scan* scan) : - KDtreeManagedBase(scan->get("xyz reduced original"), prepareTempIndices(scan->size("xyz reduced original")), scan->size("xyz reduced original")), m_scan(scan), m_data(0), m_count_locking(0) { + create(scan->get("xyz reduced original"), prepareTempIndices(scan->size("xyz reduced original")), scan->size("xyz reduced original")); // allocate in prepareTempIndices, deleted here delete[] m_temp_indices; } diff --git a/src/slam6d/kdMeta.cc b/src/slam6d/kdMeta.cc index 4e7ab9e..8247522 100644 --- a/src/slam6d/kdMeta.cc +++ b/src/slam6d/kdMeta.cc @@ -32,166 +32,8 @@ using std::swap; #include // KDtree class static variables -KDParams KDtreeMeta::params[MAX_OPENMP_NUM_THREADS]; - -KDtreeMeta::KDtreeMeta() -{ -} - -/** - * Create a KD tree from the points pointed to by the array pts - * - * @param pts 3D array of points - * @param n number of points - */ -void KDtreeMeta::create(const DataXYZ* const* pts, Index* indices, unsigned int n) -{ - // Find bbox - double xmin = point(pts, indices[0])[0], xmax = point(pts, indices[0])[0]; - double ymin = point(pts, indices[0])[1], ymax = point(pts, indices[0])[1]; - double zmin = point(pts, indices[0])[2], zmax = point(pts, indices[0])[2]; - for(unsigned int i = 1; i < n; i++) { - xmin = min(xmin, point(pts, indices[i])[0]); - xmax = max(xmax, point(pts, indices[i])[0]); - ymin = min(ymin, point(pts, indices[i])[1]); - ymax = max(ymax, point(pts, indices[i])[1]); - zmin = min(zmin, point(pts, indices[i])[2]); - zmax = max(zmax, point(pts, indices[i])[2]); - } - - // Leaf nodes - if ((n > 0) && (n <= 10)) { - npts = n; - leaf.p = new Index[n]; - // fill leaf index array with indices - for(unsigned int i = 0; i < n; ++i) { - leaf.p[i] = indices[i]; - } - return; - } - - // Else, interior nodes - npts = 0; - - node.center[0] = 0.5 * (xmin+xmax); - node.center[1] = 0.5 * (ymin+ymax); - node.center[2] = 0.5 * (zmin+zmax); - node.dx = 0.5 * (xmax-xmin); - node.dy = 0.5 * (ymax-ymin); - node.dz = 0.5 * (zmax-zmin); - node.r2 = sqr(node.dx) + sqr(node.dy) + sqr(node.dz); - - // Find longest axis - if (node.dx > node.dy) { - if (node.dx > node.dz) { - node.splitaxis = 0; - } else { - node.splitaxis = 2; - } - } else { - if (node.dy > node.dz) { - node.splitaxis = 1; - } else { - node.splitaxis = 2; - } - } - - // Partition - double splitval = node.center[node.splitaxis]; - - if ( fabs(max(max(node.dx,node.dy),node.dz)) < 0.01 ) { - npts = n; - leaf.p = new Index[n]; - // fill leaf index array with indices - for(unsigned int i = 0; i < n; ++i) { - leaf.p[i] = indices[i]; - } - return; - } - - Index* left = indices, * right = indices + n - 1; - while(true) { - while(point(pts, *left)[node.splitaxis] < splitval) - left++; - while(point(pts, *right)[node.splitaxis] >= splitval) - right--; - if(right < left) - break; - swap(*left, *right); - } - - // Build subtrees - int i; -#ifdef WITH_OPENMP_KD // does anybody know the reason why this is slower ?? --Andreas - omp_set_num_threads(OPENMP_NUM_THREADS); -#pragma omp parallel for schedule(dynamic) -#endif - for (i = 0; i < 2; i++) { - if (i == 0) { - node.child1 = new KDtreeMeta(); - node.child1->create(pts, indices, left - indices); - } - if (i == 1) { - node.child2 = new KDtreeMeta(); - node.child2->create(pts, left, n - (left - indices)); - } - } -} - -KDtreeMeta::~KDtreeMeta() -{ - if (!npts) { -#ifdef WITH_OPENMP_KD - omp_set_num_threads(OPENMP_NUM_THREADS); -#pragma omp parallel for schedule(dynamic) -#endif - for (int i = 0; i < 2; i++) { - if (i == 0 && node.child1) delete node.child1; - if (i == 1 && node.child2) delete node.child2; - } - } else { - if (leaf.p) delete [] leaf.p; - } -} - -/** - * Wrapped function - */ -void KDtreeMeta::_FindClosest(const DataXYZ* const* pts, int threadNum) const -{ - // Leaf nodes - if (npts) { - for (int i = 0; i < npts; i++) { - double myd2 = Dist2(params[threadNum].p, point(pts, leaf.p[i])); - if (myd2 < params[threadNum].closest_d2) { - params[threadNum].closest_d2 = myd2; - params[threadNum].closest = point(pts, leaf.p[i]); - } - } - return; - } - - // Quick check of whether to abort - double approx_dist_bbox = max(max(fabs(params[threadNum].p[0]-node.center[0])-node.dx, - fabs(params[threadNum].p[1]-node.center[1])-node.dy), - fabs(params[threadNum].p[2]-node.center[2])-node.dz); - if (approx_dist_bbox >= 0 && sqr(approx_dist_bbox) >= params[threadNum].closest_d2) - return; - - // Recursive case - double myd = node.center[node.splitaxis] - params[threadNum].p[node.splitaxis]; - if (myd >= 0.0) { - node.child1->_FindClosest(pts, threadNum); - if (sqr(myd) < params[threadNum].closest_d2) { - node.child2->_FindClosest(pts, threadNum); - } - } else { - node.child2->_FindClosest(pts, threadNum); - if (sqr(myd) < params[threadNum].closest_d2) { - node.child1->_FindClosest(pts, threadNum); - } - } -} +template +KDParams KDTreeImpl::params[MAX_OPENMP_NUM_THREADS]; KDtreeMetaManaged::KDtreeMetaManaged(const vector& scans) : m_count_locking(0) @@ -217,7 +59,7 @@ KDtreeMetaManaged::~KDtreeMetaManaged() delete[] m_data; } -KDtreeMeta::Index* KDtreeMetaManaged::prepareTempIndices(const vector& scans) +Index* KDtreeMetaManaged::prepareTempIndices(const vector& scans) { unsigned int n = getPointsSize(scans); diff --git a/src/slam6d/scan.cc b/src/slam6d/scan.cc index f8e61a6..4badc29 100644 --- a/src/slam6d/scan.cc +++ b/src/slam6d/scan.cc @@ -146,7 +146,7 @@ void Scan::toGlobal() { void Scan::createSearchTree() { // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees a nullpointer) do the creation -// boost::lock_guard lock(m_mutex_create_tree); + boost::lock_guard lock(m_mutex_create_tree); if(kd != 0) return; // make sure the original points are created before starting the measurement @@ -157,7 +157,7 @@ void Scan::createSearchTree() #endif //WITH_METRICS createSearchTreePrivate(); - + #ifdef WITH_METRICS ClientMetric::create_tree_time.end(tc); #endif //WITH_METRICS @@ -166,7 +166,7 @@ void Scan::createSearchTree() void Scan::calcReducedOnDemand() { // multiple threads will call this function at the same time because they all work on one pair of Scans, just let the first one (who sees count as zero) do the reduction -// boost::lock_guard lock(m_mutex_reduction); + boost::lock_guard lock(m_mutex_reduction); if(m_has_reduced) return; #ifdef WITH_METRICS diff --git a/src/thermo/CMakeLists.txt b/src/thermo/CMakeLists.txt index 6b6b86d..8d5e7f8 100644 --- a/src/thermo/CMakeLists.txt +++ b/src/thermo/CMakeLists.txt @@ -2,47 +2,24 @@ IF (WITH_THERMO) find_package(OpenCV REQUIRED) include_directories(${CMAKE_SOURCE_DIR}/3rdparty/cvblob) - include_directories(${CMAKE_SOURCE_DIR}/3rdparty/gocr-0.48/src) - include_directories(${CMAKE_SOURCE_DIR}/3rdparty/gocr-0.48/include) include_directories(${CMAKE_SOURCE_DIR}/include/shapes/) include_directories(${CMAKE_SOURCE_DIR}/include/thermo/) include_directories(/usr/include/) include_directories(/usr/include/opencv) -# # Compile gocr library -# SET(GOCR_DIR ${CMAKE_SOURCE_DIR}/3rdparty/gocr-0.48/src/) -# add_library(Pgm2asc SHARED ${GOCR_DIR}gocr.c ${GOCR_DIR}pgm2asc.c ${GOCR_DIR}box.c ${GOCR_DIR}database.c -# ${GOCR_DIR}detect.c ${GOCR_DIR}barcode.c ${GOCR_DIR}lines.c ${GOCR_DIR}list.c -# ${GOCR_DIR}ocr0.c ${GOCR_DIR}ocr0n.c ${GOCR_DIR}ocr1.c ${GOCR_DIR}otsu.c -# ${GOCR_DIR}output.c ${GOCR_DIR}pixel.c ${GOCR_DIR}unicode.c ${GOCR_DIR}remove.c -# ${GOCR_DIR}pnm.c ${GOCR_DIR}pcx.c ${GOCR_DIR}progress.c ${GOCR_DIR}job.c) - -# add_executable(shapes shapes.cc geom_math.cc numberrec.cc scan_ransac.cc ) -# -# IF(WIN32) -# target_link_libraries(shapes scan XGetopt netpbm Pgm2asc) -# ENDIF(WIN32) -# -# IF (UNIX) -# target_link_libraries(shapes scan newmat dl netpbm Pgm2asc ) -# ENDIF(UNIX) -# - add_executable(caliboard caliboard.cc) -# add_executable(thermo thermo.cc) + add_executable(thermo thermo.cc) # add_executable(thermo thermo.cc src/cvaux.cpp src/cvblob.cpp src/cvcolor.cpp src/cvcontour.cpp src/cvlabel.cpp src/cvtrack.cpp) - add_executable(thermo thermo.cc) IF(UNIX) target_link_libraries(caliboard scan shape newmat dl ANN) target_link_libraries(thermo scan shape newmat dl ANN) -# target_link_libraries(thermo scan shape newmat dl ANN) target_link_libraries(thermo cv cvaux cxcore GL GLU highgui cvblob scan ANN) ENDIF(UNIX) IF (WIN32) target_link_libraries(caliboard scan shape newmat XGetopt ANN) -# target_link_libraries(thermo scan shape newmat XGetopt ANN) + target_link_libraries(thermo scan shape newmat XGetopt ANN) target_link_libraries(thermo cv cvaux cxcore GL GLU highgui cvblob scan ANN) ENDIF(WIN32) diff --git a/src/thermo/caliboard.cc b/src/thermo/caliboard.cc index 59ef4aa..ef7086f 100644 --- a/src/thermo/caliboard.cc +++ b/src/thermo/caliboard.cc @@ -42,6 +42,7 @@ using std::endl; #endif #include "shapes/hough.h" +#include "slam6d/basicScan.h" #include "shapes/shape.h" #include "shapes/ransac.h" @@ -114,6 +115,14 @@ void usage(char* prog) { } +void writeFalse(string output) { + ofstream caliout(output.c_str()); + + caliout << "failed" << endl; + caliout.close(); + caliout.clear(); +} + bool matchPlaneToBoard(vector &points, double *alignxf, int pattern, string output) { double rPos[3] = {0.0,0.0,0.0}; double rPosTheta[3] = {0.0,0.0,0.0}; @@ -139,6 +148,18 @@ bool matchPlaneToBoard(vector &points, double *alignxf, int pattern, s halfwidth = 19.0; halfheight = 38.0; break; + case 4: //Ostia + /* + halfwidth = 14.85; + halfheight = 21; + */ + halfwidth = 22.5; + halfheight = 30.5; + break; + case 5: // Ostia large + halfwidth = 22.5; + halfheight = 30.5; + break; } for(double i = -halfwidth; i <= halfwidth; i+=w_step) { @@ -147,16 +168,28 @@ bool matchPlaneToBoard(vector &points, double *alignxf, int pattern, s p[0] = i; p[1] = j; p[2] = 0.0; - //cout << p[0] << " " << p[1] << " " << p[2] << endl; + //cout << p[0] << " " << p[1] << " " << p[2] << " 1" << endl; boardpoints.push_back(p); } } int nr_points = boardpoints.size(); int nr_points2 = points.size(); - Scan * plane = new Scan(rPos, rPosTheta, points); - Scan * board = new Scan(rPos, rPosTheta, boardpoints); - board->transform(alignxf, Scan::INVALID, 0); + Scan * plane = new BasicScan(rPos, rPosTheta, points); + Scan * board = new BasicScan(rPos, rPosTheta, boardpoints); + + for(int i = 0; i < boardpoints.size(); i++) { + delete[] boardpoints[i]; + } + + plane->setRangeFilter(-1, -1); + plane->setReductionParameter(-1, 0); + plane->setSearchTreeParameter(simpleKD, false); + board->setRangeFilter(-1, -1); + board->setReductionParameter(-1, 0); + board->setSearchTreeParameter(simpleKD, false); + + board->transform(alignxf, Scan::ICP, 0); bool quiet = true; icp6Dminimizer *my_icp6Dminimizer = 0; @@ -166,24 +199,21 @@ bool matchPlaneToBoard(vector &points, double *alignxf, int pattern, s double mdm = 50; int mni = 50; my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, false, -1, false, 1, 0.00, false, false); - - plane->createTree(false,false); - board->createTree(false,false); - my_icp->match(plane, board); - delete my_icp; + mdm = 2; mni = 300; my_icp = new icp6D(my_icp6Dminimizer, mdm, mni, quiet, false, -1, false, 1, 0.00, false, false); my_icp->match(plane, board); - + delete my_icp; + delete my_icp6Dminimizer; double sum; double centroid_s[3] = {0.0, 0.0, 0.0}; double centroid_t[3] = {0.0, 0.0, 0.0}; vector pairs_out; - Scan::getPtPairs(&pairs_out, plane, board, 1, 0, 2.0, sum, centroid_s, centroid_t); + Scan::getPtPairs(&pairs_out, plane, board, 1, 0, 3.0, sum, centroid_s, centroid_t); int nr_matches = pairs_out.size(); cout << "Result " << nr_matches << " " << nr_points << " " << nr_points2 << endl; @@ -195,7 +225,7 @@ bool matchPlaneToBoard(vector &points, double *alignxf, int pattern, s } cout << endl << endl; - + cout << "Transform new: " << endl; for(int i = 0; i < 3; i++) { cout << pos[i] << " "; } @@ -204,7 +234,6 @@ bool matchPlaneToBoard(vector &points, double *alignxf, int pattern, s cout << deg(postheta[i]) << " "; } cout << endl; - vector * result = new vector(); cout << "Calipoints Start" << endl; ofstream caliout(output.c_str()); @@ -214,20 +243,49 @@ bool matchPlaneToBoard(vector &points, double *alignxf, int pattern, s } else { caliout << "Calibration" << endl; } - + + /** + * write FRAMES + */ + /* + string filename = "tmp.frames"; + + ofstream fout(filename.c_str()); + if (!fout.good()) { + cerr << "ERROR: Cannot open file " << filename << endl; + exit(1); + } + + // write into file + + //fout << "frames for scan" << endl; + //fout << plane->sout.str() << endl; + //fout << "frames for lightbulbs" << endl; + fout << board ->sout.str() << endl; + fout.close(); + fout.clear(); + */ + + /* + board->saveFrames(); + */ + /** + * end write frames + */ + switch(pattern) { // lightbulb case 0: for(double y = -25; y < 30; y+=10.0) { - //for(double x = -20; x < 25; x+=10.0) { for(double x = 20; x > -25; x-=10.0) { double * p = new double[3]; p[0] = x; p[1] = y; p[2] = 0.0; transform3(transMat, p); - result->push_back(p); + //result->push_back(p); caliout << p[0] << " " << p[1] << " " << p[2] << endl; + delete[] p; } } break; @@ -240,23 +298,22 @@ bool matchPlaneToBoard(vector &points, double *alignxf, int pattern, s p[1] = y; p[2] = 0.0; transform3(transMat, p); - result->push_back(p); caliout << p[0] << " " << p[1] << " " << p[2] << endl; + delete[] p; } } break; + // chessboard on wooden board pattern bottom case 3: for(double y = -4.1; y > -33.0; y-=5.2) { - //for(double y = -30.1; y < -0.0; y+=5.2) { - // for(double x = 7.8; x > -10; x-=5.2) { for(double x = -8.1; x < 10; x+=5.2) { double * p = new double[3]; p[0] = x; p[1] = y; p[2] = 0.0; transform3(transMat, p); - result->push_back(p); caliout << p[0] << " " << p[1] << " " << p[2] << endl; + delete[] p; } } break; @@ -269,25 +326,61 @@ bool matchPlaneToBoard(vector &points, double *alignxf, int pattern, s p[1] = y; p[2] = 0.0; transform3(transMat, p); - result->push_back(p); caliout << p[0] << " " << p[1] << " " << p[2] << endl; + delete[] p; } } break; + // Ostia + case 4: + for(double x = -12; x < 16; x+=8.0) { + for(double y = -20; y < 24; y+=8.0) { + double * p = new double[3]; + p[0] = x; + p[1] = y; + p[2] = 0.0; + transform3(transMat, p); + caliout << p[0] << " " << p[1] << " " << p[2] << endl; + delete[] p; + } + } + break; + break; + + //_|_|_|_|_|_|_ + //4: 1.5*8 = 12 + //6: 2.5*8 = 20 + // Universum + case 5: + for(double y = 20; y > -24; y-=8.0) { + for(double x = -12; x < 16; x+=8.0) { + double * p = new double[3]; + p[0] = x; + p[1] = y; + p[2] = 0.0; + transform3(transMat, p); + caliout << p[0] << " " << p[1] << " " << p[2] << endl; + delete[] p; + } + } + break; + } caliout.close(); caliout.clear(); cout << "Calipoints End" << endl; + delete board; + delete plane; return !(nr_matches < nr_points); } -int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int &end, int &pattern, int &maxDist, int &minDist, double &top, double &bottom, int &octree, IOType &type, bool -&quiet) { +int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int +&end, int &pattern, int &maxDist, int &minDist, double &top, double &bottom, int +&octree, IOType &type, bool &quiet) { bool reduced = false; int c; - // from unistd.h: extern char *optarg; extern int optind; @@ -335,7 +428,7 @@ int parseArgs(int argc, char **argv, string &dir, double &red, int &start, int & break; case 'p': pattern = atoi(optarg); - if(pattern < 0 || pattern > 3) { cerr << "Error: choose pattern between 0 and 3!\n"; exit(1); } + if(pattern < 0 || pattern > 5) { cerr << "Error: choose pattern between 0 and 3!\n"; exit(1); } break; case 'q': quiet = true; @@ -412,7 +505,6 @@ int main(int argc, char **argv) cout << "Parse args" << endl; parseArgs(argc, argv, dir, red, start, end, pattern, maxDist, minDist, top, bottom, octree, type, quiet); - Scan::dir = dir; int fileNr = start; string calidir = dir + "/cali"; @@ -447,21 +539,26 @@ int main(int argc, char **argv) int failures = 0; long calitime = GetCurrentTimeInMilliSec(); -#ifndef WITH_SCANSERVER + +//#ifndef WITH_SCANSERVER while (fileNr <= end) { - Scan::readScans(type, fileNr, fileNr, dir, maxDist, minDist, 0); + Scan::openDirectory(false, dir, type, fileNr, fileNr); + Scan::allScans[0]->setRangeFilter(maxDist, minDist); + Scan::allScans[0]->setHeightFilter(top, bottom); + Scan::allScans[0]->setSearchTreeParameter(simpleKD, false); string output = calidir + "/scan" + to_string(fileNr,3) + ".3d"; cout << "Top: " << top << " Bottom: " << bottom << endl; - Scan::allScans[0]->trim(top, bottom); - Scan::allScans[0]->toGlobal(red, octree); + Scan::allScans[0]->toGlobal(); double id[16]; M4identity(id); for(int i = 0; i < 10; i++) { Scan::allScans[0]->transform(id, Scan::ICP, 0); // write end pose } +/* #else //WITH_SCANSERVER + Scan::readScansRedSearch(type, start, end, dir, filter, red, octree); for(std::vector::iterator it = Scan::allScans.begin(); it != Scan::allScans.end(); ++it) { @@ -477,16 +574,22 @@ int main(int argc, char **argv) scan->transform(id, Scan::ICP, 0); // write end pose } #endif //WITH_SCANSERVER +*/ cout << "start plane detection" << endl; long starttime = GetCurrentTimeInMilliSec(); vector points; CollisionPlane * plane; plane = new LightBulbPlane(50,120); -#ifndef WITH_SCANSERVER +//#ifndef WITH_SCANSERVER Ransac(*plane, Scan::allScans[0], &points); +/* #else //WITH_SCANSERVER + + cout << "S" << endl; Ransac(*plane, scan, &points); -#endif //WITH_SCANSERVER + cout << "T" << endl; +//#endif //WITH_SCANSERVER +*/ starttime = (GetCurrentTimeInMilliSec() - starttime); cout << "nr points " << points.size() << endl; @@ -495,31 +598,73 @@ int main(int argc, char **argv) cout << "DONE " << endl; cout << nx << " " << ny << " " << nz << " " << d << endl; - double rPos[3]; - double rPosTheta[3]; - for(int i = 0; i < 3; i++) { - rPosTheta[i] = 0.0; - } - ((LightBulbPlane *)plane)->getCenter(rPos[0], rPos[1], rPos[2]); - double alignxf[16]; - EulerToMatrix4(rPos, rPosTheta, alignxf); - if(matchPlaneToBoard(points, alignxf, pattern, output)) { - successes++; - } else { + + if(std::isnan(d)) { + writeFalse(output); failures++; + } else { + + if(d < 0) { + nx = -nx; + ny = -ny; + nz = -nz; + d = -d; + } + + double tx, ty, tz; + tz = 0; + ty = asin(nx); + tx = asin(-ny/cos(ty)); + + double rPos[3]; + double rPosTheta[3]; + + for(int i = 0; i < 3; i++) { + rPosTheta[i] = 0.0; + } + + rPosTheta[0] = tx; + rPosTheta[1] = ty; + rPosTheta[2] = tz; + + //rPosTheta[1] = acos(nz); + // rotate plane model to make parallel with detected plane + + // transform plane model to center of detected plane + ((LightBulbPlane *)plane)->getCenter(rPos[0], rPos[1], rPos[2]); + cout << "Angle: " << deg(acos(nz)) << endl; + for(int i = 0; i < 3; i++) { + cout << rPos[i] << " "; + } + for(int i = 0; i < 3; i++) { + cout << deg(rPosTheta[i]) << " "; + } + cout << endl; + + double alignxf[16]; + EulerToMatrix4(rPos, rPosTheta, alignxf); + if(matchPlaneToBoard(points, alignxf, pattern, output)) { + successes++; + } else { + failures++; + } + } - for(int i = points.size() - 1; i > -1; i++) { + + + for(int i = points.size() - 1; i > -1; i--) { delete[] points[i]; } - + + delete plane; cout << "Time for Plane Detection " << starttime << endl; -#ifndef WITH_SCANSERVER +//#ifndef WITH_SCANSERVER delete Scan::allScans[0]; Scan::allScans.clear(); fileNr++; -#endif //WITH_SCANSERVER +//#endif //WITH_SCANSERVER } calitime = (GetCurrentTimeInMilliSec() - calitime); @@ -527,9 +672,11 @@ int main(int argc, char **argv) << " failures!" << endl; cout << "Time for Calibration " << calitime << endl; +/* #ifdef WITH_SCANSERVER Scan::clearScans(); ClientInterface::destroy(); #endif //WITH_SCANSERVER +*/ } diff --git a/src/thermo/thermo.cc b/src/thermo/thermo.cc index be22108..8c4ad8f 100644 --- a/src/thermo/thermo.cc +++ b/src/thermo/thermo.cc @@ -1,12 +1,3 @@ -/* - * thermo implementation - * - * Copyright (C) Dorit Borrmann - * - * Released under the GPL version 3. - * - */ - #include #include "thermo/thermo.h" #include "newmat/newmatap.h" @@ -35,9 +26,10 @@ using namespace cvb; Float2D data1; Float2D data2; -unsigned int BLOB_SIZE = 55; + +unsigned int BLOB_SIZE = 65; double AVG_THRES = 0.8; - +unsigned int GRAY_TH = 65; /** * Calculates the PCA of a two-dimensional point cloud * @param x x coordinate of the axis @@ -198,7 +190,8 @@ void sortBlobs(double point_array[][2], int board_n, int board_h, int board_w, b IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2]) { IplImage *gray_image = cvCloneImage(org_image); - cvThreshold(gray_image, gray_image, 140, 255, CV_THRESH_BINARY); + //cvThreshold(gray_image, gray_image, 100, 255, CV_THRESH_BINARY); + cvThreshold(gray_image, gray_image, GRAY_TH, 255, CV_THRESH_BINARY); IplImage *labelImg = cvCreateImage(cvGetSize(gray_image), IPL_DEPTH_LABEL, 1); // detect blobs @@ -277,7 +270,7 @@ IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int boa * Connects the detected calibration features in the image with lines. */ void drawLines(double point_array2[][2], int corner_exp, IplImage *image, bool color) { - for (int i = 0; i <= corner_exp - 2; i++) { + for (int i = 4; i <= corner_exp - 2; i++) { CvPoint pt1; CvPoint pt2; CvScalar s; @@ -312,7 +305,7 @@ void drawLines(double point_array2[][2], int corner_exp, IplImage *image, bool c } else { pt2.y = floor(point_array2[i + 1][1]) + 1; } - cvLine(image, pt1, pt2, s, 1, 8); + cvLine(image, pt1, pt2, s, 3, 8); } cvShowImage("Final Result", image); @@ -354,8 +347,9 @@ IplImage* resizeImage(IplImage *source, int scale) { /** * Detects the corners of the chessboard pattern. */ -IplImage* detectCorners(IplImage *orgimage, int corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale) { +IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale) { + cout << "Scale: " << scale << endl; IplImage *image = resizeImage(orgimage, scale); CvSize size = cvGetSize(image); CvPoint2D32f* corners = new CvPoint2D32f[corner_exp]; @@ -369,21 +363,24 @@ IplImage* detectCorners(IplImage *orgimage, int corner_exp, int board_h, int boa } int found = cvFindChessboardCorners(image, board_sz, corners, &corner_exp, - CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS); + CV_CALIB_CB_ADAPTIVE_THRESH + | CV_CALIB_CB_NORMALIZE_IMAGE | CV_CALIB_CB_FILTER_QUADS); cout << "found corners:" << corner_exp << endl; if (found != 0) {//if all corners found successfully + //if (corner_exp != 0) {//if all corners found successfully //Get Subpixel accuracy on those corners if(size.width > 400) { cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); } else { - cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(2, 2), cvSize(-1, -1), + cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(3, 3), cvSize(-1, -1), cvTermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); } } - + cout << "blub " << found << endl; + for (int i = 0; i < corner_exp; i++) { point_array2[i][0] = corners[i].x; point_array2[i][1] = corners[i].y; @@ -411,8 +408,10 @@ image_points, CvSize size, string dir, string substring) { j = i % corner_exp; CV_MAT_ELEM( *image_points2, float, i, 0) = CV_MAT_ELEM( *image_points, float, i, 0); CV_MAT_ELEM( *image_points2, float,i,1) = CV_MAT_ELEM( *image_points, float, i, 1); - CV_MAT_ELEM(*object_points2, float, i, 0) = (j / board_w) * 8; - CV_MAT_ELEM( *object_points2, float, i, 1) = (j % board_w) * 8; + CV_MAT_ELEM(*object_points2, float, i, 0) = (j / board_w) * 4; + CV_MAT_ELEM( *object_points2, float, i, 1) = (j % board_w) * 4; + //CV_MAT_ELEM(*object_points2, float, i, 0) = (j / board_w) * 4; + //CV_MAT_ELEM( *object_points2, float, i, 1) = (j % board_w) * 4; CV_MAT_ELEM( *object_points2, float, i, 2) = 0.0f; } for (int i = 0; i < images; ++i) { //These are all the same number @@ -465,10 +464,13 @@ image_points, CvSize size, string dir, string substring) { void CalibFunc(int board_w, int board_h, int start, int end, bool optical, bool chess, bool quiet, string dir, int scale) { cvNamedWindow("Original Image", 0); - cvResizeWindow( "Final Result", 320, 240 ); + cvNamedWindow("Final Result", 0); + cvResizeWindow( "Original Image", 480, 640 ); + cvResizeWindow( "Final Result", 480, 640 ); + /* cvNamedWindow("Final Result", 0); cvResizeWindow( "Final Result", 320, 240 ); - + */ int nr_img = end - start + 1; if (nr_img == 0) { cout << "ImageCount is zero!" << endl; @@ -487,19 +489,21 @@ chess, bool quiet, string dir, int scale) { int step = 0; for (int count = start; count <= end; count++) { - string t; + cout << "count : " << count << endl; + string t; string t1; if(optical) { - t = dir + "/photo" + to_string(count, 3) + ".ppm"; - t1 = dir + "/cimage" + to_string(count, 3) + ".ppm"; + //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; + t = dir + "/photo" + to_string(count, 3) + ".jpg"; + //t1 = dir + "/cimage" + to_string(count, 3) + ".ppm"; //t = dir + to_string(count, 3) + "/photo" + to_string(count, 3) + ".ppm"; //t1 = dir + to_string(count, 3) + "/cimage" + to_string(count, 3) + ".ppm"; } else { //t = dir + to_string(count, 3) + "/image" + to_string(count, 3) + ".ppm"; //t1 = dir + to_string(count, 3) + "/timage" + to_string(count, 3) + ".ppm"; t = dir + "/image" + to_string(count, 3) + ".ppm"; - t1 = dir + "/timage" + to_string(count, 3) + ".ppm"; + //t1 = dir + "/timage" + to_string(count, 3) + ".ppm"; } cout << t << endl; //loading images and finding corners @@ -515,41 +519,50 @@ chess, bool quiet, string dir, int scale) { double point_array2[corner_exp][2]; IplImage *image; + int tmp_corners = corner_exp; if(chess) { cout << "detect corners" << endl; - image = detectCorners(image1, corner_exp, board_h, board_w, quiet, point_array2, scale); + image = detectCorners(image1, tmp_corners, board_h, board_w, quiet, point_array2, scale); } else { cout << "detect blob" << endl; - int tmp_corners = corner_exp; image = detectBlobs(image1, tmp_corners, board_h, board_w, quiet, point_array2); } + /* for(int i = 0; i < corner_exp; i++) { cout << (float) point_array2[i][0] << " " << (float) point_array2[i][1] << endl; } + //drawing the lines on the image now + */ drawLines(point_array2, corner_exp, image); - - cout << "\nDo you want to use the data from this image ('y' or 'n'). 'x' aborts the calibration? : "; - int c = cvWaitKey(100); - if (c == 27) { - break; - } + //cvDrawChessboardCorners(image, size, point_array2, tmp_corners, true); + //cvShowImage("Final Result", image); char in; - cin >> in; - if (in == 'y') { - cvSaveImage(t1.c_str(), image); - size = cvGetSize(image); - step = successes * corner_exp; - //appending corner data to a generic data structure for all images - for (int i = step, j = 0; j < corner_exp; ++i, ++j) { - CV_MAT_ELEM(*image_points, float,i,0) = (float) point_array2[j][0]; - CV_MAT_ELEM(*image_points, float,i,1) = (float) point_array2[j][1]; + if(tmp_corners == corner_exp) { + cout << "\nDo you want to use the data from this image ('y' or 'n'). 'x' aborts the calibration? : "; + in = 'y'; + /* + int c = cvWaitKey(100); + if (c == 27) { + break; + } + cin >> in; + */ + if (in == 'y') { + //cvSaveImage(t1.c_str(), image); + size = cvGetSize(image); + step = successes * corner_exp; + //appending corner data to a generic data structure for all images + for (int i = step, j = 0; j < corner_exp; ++i, ++j) { + CV_MAT_ELEM(*image_points, float,i,0) = (float) point_array2[j][0]; + CV_MAT_ELEM(*image_points, float,i,1) = (float) point_array2[j][1]; + } + successes++; + } else if(in == 'x') { + break; } - successes++; - } else if(in == 'x') { - break; } cvReleaseImage(&image); cvReleaseImage(&image1); @@ -628,10 +641,19 @@ int realMedian(CvMat * vectors, int nr_vectors) { * Calculates the median of a set of vectors by iteratively calculating the * median and cropping outliers. */ -void filterMedian(CvMat * vectors, int nr_vectors, int threshold, CvMat * mean) { - +void filterMedian(CvMat * vectors, int nr_vectors, int thres, CvMat * mean) { + int threshold = thres; // calculate Median int min_pos = realMedian(vectors, nr_vectors); + if(thres == 4) { + (CV_MAT_ELEM(*mean,float,0,0)) = (CV_MAT_ELEM(*vectors,float,min_pos,0)); + (CV_MAT_ELEM(*mean,float,0,1)) = (CV_MAT_ELEM(*vectors,float,min_pos,1)); + (CV_MAT_ELEM(*mean,float,0,2)) = (CV_MAT_ELEM(*vectors,float,min_pos,2)); + (CV_MAT_ELEM(*mean,float,0,3)) = (CV_MAT_ELEM(*vectors,float,min_pos,3)); + (CV_MAT_ELEM(*mean,float,0,4)) = (CV_MAT_ELEM(*vectors,float,min_pos,4)); + (CV_MAT_ELEM(*mean,float,0,5)) = (CV_MAT_ELEM(*vectors,float,min_pos,5)); + return; + } // crop outliers double x1 = (CV_MAT_ELEM(*vectors,float,min_pos,0)); @@ -663,15 +685,18 @@ void filterMedian(CvMat * vectors, int nr_vectors, int threshold, CvMat * mean) count++; } } + cout << "min_pos " << min_pos << endl; // recurse - if(threshold < 3) { + if(thres < 3) { filterMedian(some_vectors, count, ++threshold, mean); cvReleaseMat(&some_vectors); // determine result } else { + /* x1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,0)); y1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,1)); z1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,2)); + */ double x2 = 0; double y2 = 0; double z2 = 0; @@ -727,16 +752,22 @@ void calculateExtrinsicsWithReprojectionCheck(CvMat * points2D, CvMat * for(int i = 0; i < successes; i++) { reprojectionError[i] = 0.0; } - - for(int i = 0; i < successes; i++) { + + cout << "reprojectionError" << endl; + for(int i = 0; i < successes + 4; i++) { reprojectionError[i] = 0.0; CvMat * rotation = cvCreateMat(1, 3, CV_32FC1); CvMat * translation = cvCreateMat(1, 3, CV_32FC1); - + if(i==successes) cout << "now other stuff" << endl; for(int k = 0; k < 3; k++) { CV_MAT_ELEM(*rotation, float, 0, k) = CV_MAT_ELEM(*rotation_vectors_temp, float, i, k); CV_MAT_ELEM(*translation, float, 0, k) = CV_MAT_ELEM(*translation_vectors_temp, float, i, k); + cerr << CV_MAT_ELEM(*translation, float, 0, k)<< " "; } + for(int k = 0; k < 3; k++) { + cerr << CV_MAT_ELEM(*rotation, float, 0, k)<< " "; + } + cerr << endl; for(int j = 0; j < successes; j++) { double tmp = 0; //calculate reprojection error @@ -760,19 +791,20 @@ void calculateExtrinsicsWithReprojectionCheck(CvMat * points2D, CvMat * reprojectionError[i] += tmp; cvReleaseMat(&point_3Dcloud); } + cout << reprojectionError[i]/successes/30 << endl; cvReleaseMat(&rotation); cvReleaseMat(&translation); } int maxindex = -1; double max = DBL_MAX; - for(int i = 0; i < successes; i++) { + for(int i = 0; i < successes + 4; i++) { if(reprojectionError[i] < max) { maxindex = i; max = reprojectionError[i]; } } - + cerr << "Reprojection error: " << max/successes << endl; CvMat * rotation = cvCreateMat(1, 3, CV_32FC1); CvMat * translation = cvCreateMat(1, 3, CV_32FC1); @@ -812,27 +844,59 @@ void calculateExtrinsics(CvMat * rotation_vectors_temp, CvMat * translation_vect for (int h = 0; h < successes; h++) { for(int t = 0; t < 3; t++) { - CV_MAT_ELEM(*rotation_vectors,float,h,t) =CV_MAT_ELEM(*rotation_vectors_temp,float,h,t); - CV_MAT_ELEM(*rotation_vector_mean,float,0,t) +=CV_MAT_ELEM(*rotation_vectors,float,h,t); + CV_MAT_ELEM(*rotation_vectors,float,h,t) = CV_MAT_ELEM(*rotation_vectors_temp,float,h,t); + CV_MAT_ELEM(*rotation_vector_mean,float,0,t) += CV_MAT_ELEM(*rotation_vectors,float,h,t); - CV_MAT_ELEM(*translation_vectors,float,h,t) =CV_MAT_ELEM(*translation_vectors_temp,float,h,t); - CV_MAT_ELEM(*translation_vector_mean,float,0,t) +=CV_MAT_ELEM(*translation_vectors,float,h,t); + CV_MAT_ELEM(*translation_vectors,float,h,t) = CV_MAT_ELEM(*translation_vectors_temp,float,h,t); + CV_MAT_ELEM(*translation_vector_mean,float,0,t) += CV_MAT_ELEM(*translation_vectors,float,h,t); - CV_MAT_ELEM(*vectors,float,h,t) =CV_MAT_ELEM(*translation_vectors_temp,float,h,t); - CV_MAT_ELEM(*vectors,float,h,t + 3) =CV_MAT_ELEM(*rotation_vectors_temp,float,h,t); + CV_MAT_ELEM(*vectors,float,h,t) = CV_MAT_ELEM(*translation_vectors_temp,float,h,t); + CV_MAT_ELEM(*vectors,float,h,t + 3) = CV_MAT_ELEM(*rotation_vectors_temp,float,h,t); + cout << CV_MAT_ELEM(*translation_vectors,float,h,t) << " "; } + cout << endl; } - + + // mean + cout << "Getting mean vector" << endl; for(int t = 0; t < 3; t++) { CV_MAT_ELEM(*rotation_vector_mean,float,0,t) /= successes; - CV_MAT_ELEM(*translation_vector_mean,float,0,t) /= successes; + CV_MAT_ELEM(*rotation_vectors_temp,float,successes,t) = CV_MAT_ELEM(*rotation_vector_mean,float,0,t); + CV_MAT_ELEM(*translation_vector_mean,float,0,t) /= successes; + CV_MAT_ELEM(*translation_vectors_temp,float,successes,t) = CV_MAT_ELEM(*translation_vector_mean,float,0,t); + cout << CV_MAT_ELEM(*translation_vectors_temp,float,successes,t) << " "; } + cout << endl; + // getting the median vectors + // median + cout << "Getting median vector" << endl; + filterMedian(vectors, successes, 1, median); + cout << "Got median vector" << endl; + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*translation_vector_median,float,0,t) = CV_MAT_ELEM(*median,float,0,t); + CV_MAT_ELEM(*rotation_vector_median,float,0,t) = CV_MAT_ELEM(*median,float,0,t+3); + CV_MAT_ELEM(*rotation_vectors_temp,float,successes + 1,t) = CV_MAT_ELEM(*rotation_vector_median,float,0,t); + CV_MAT_ELEM(*translation_vectors_temp,float,successes + 1,t) = CV_MAT_ELEM(*translation_vector_median,float,0,t); + } + + + // filtered median + cout << "Getting filtered median vector" << endl; + filterMedian(vectors, successes, 4, median); + cout << "Got filtered median vector" << endl; + for(int t = 0; t < 3; t++) { + CV_MAT_ELEM(*rotation_vectors_temp,float,successes + 2,t) = CV_MAT_ELEM(*median,float,0,t+3); + CV_MAT_ELEM(*translation_vectors_temp,float,successes + 2,t) = CV_MAT_ELEM(*median,float,0,t); + } + + // elementwise median // finding the median of rotation and translation // sorting the rotation vectors element by element + + sortElementByElement(vectors, 6, successes); /* - sortElementByElement(rotation_vectors, 3, successes); - sortElementByElement(translation_vectors, 3, successes); + sortElementByElement(translation_vectors, 3, successes); if(!quiet) { cout << "number of successes : " << successes << endl; @@ -849,18 +913,16 @@ void calculateExtrinsics(CvMat * rotation_vectors_temp, CvMat * translation_vect <> in; if(tmp_corners == corner_exp) { - if (in == 'y') { - size = cvGetSize(image); - //appending corner data to a generic data structure for all images - for (int j = 0; j < corner_exp; ++j) { - CV_MAT_ELEM(*image_points, float,j,0) = (float) point_array2[j][0]; - CV_MAT_ELEM(*image_points, float,j,1) = (float) point_array2[j][1]; - CV_MAT_ELEM(*object_points,float,j,0) = corners[j].x; - CV_MAT_ELEM(*object_points,float,j,1) = corners[j].y; - CV_MAT_ELEM(*object_points,float,j,2) = corners[j].z; + cout << tmp_corners << " " << corner_exp << endl; + drawLines(point_array2, corner_exp, image); + CvMat* image_points = cvCreateMat(corner_exp, 2, CV_32FC1); + CvMat* object_points = cvCreateMat(corner_exp, 3, CV_32FC1); + CvMat* Rotation = cvCreateMat(1, 3, CV_32FC1); + CvMat* Translation = cvCreateMat(1, 3, CV_32FC1); + + char in;// = 'y'; + size = cvGetSize(image); + //appending corner data to a generic data structure for all images + for (int j = 0; j < corner_exp; ++j) { + CV_MAT_ELEM(*image_points, float,j,0) = (float) point_array2[j][0]; + CV_MAT_ELEM(*image_points, float,j,1) = (float) point_array2[j][1]; + CV_MAT_ELEM(*object_points,float,j,0) = corners[j].x; + CV_MAT_ELEM(*object_points,float,j,1) = corners[j].y; + CV_MAT_ELEM(*object_points,float,j,2) = corners[j].z; + + /* + cout << (float)point_array2[j][0] << " "; + cout << (float)point_array2[j][1] << " "; + cout << corners[j].x << " "; + cout << corners[j].y << " "; + cout << corners[j].z << endl; + */ + } + cvFindExtrinsicCameraParams2(object_points, image_points, intrinsic, distortion, Rotation, Translation); + // append data to vectors + /* + if(CV_MAT_ELEM(*Translation, float, 0, 1) > -30) + if(CV_MAT_ELEM(*Translation, float, 0, 0) < -20) + if(CV_MAT_ELEM(*Translation, float, 0, 1) > -4 ) + if(CV_MAT_ELEM(*Translation, float, 0, 1) < -00) + if(CV_MAT_ELEM(*Translation, float, 0, 2) > -12) + if(CV_MAT_ELEM(*Translation, float, 0, 2) < -8 ) { + */ + if(!quiet) cout << "Rotation is:" << endl; + for (int row = 0; row < 1; row++) { + for (int col = 0; col < 3; col++) { + if(!quiet) cout << CV_MAT_ELEM( *Rotation, float, row, col ) << " "; + CV_MAT_ELEM( *rotation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Rotation, float, row, col ); + } + if(!quiet) cout << endl; + } + if(!quiet) cout << "Translation is:" << endl; + for (int row = 0; row < 1; row++) { + for (int col = 0; col < 3; col++) { + if(!quiet) cout << CV_MAT_ELEM( *Translation, float, row, col ) << " "; + CV_MAT_ELEM( *translation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Translation, float, row, col ); + } + if(!quiet) cout << endl; + } + + cout << "\nDo you want to use the data from this image ('y' or 'n'). 'x' aborts the calibration? : "; + int c = cvWaitKey(100); + cin >> in; + if (c == 27) { + break; + } + + if (in == 'y') { + for (int j = 0; j < corner_exp; ++j) { CV_MAT_ELEM(*points2D, CvPoint2D32f, successes, j).x = (float)point_array2[j][0]; CV_MAT_ELEM(*points2D, CvPoint2D32f, successes, j).y = (float)point_array2[j][1]; CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).x = corners[j].x; CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).y = corners[j].y; CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).z = corners[j].z; } - cvFindExtrinsicCameraParams2(object_points, image_points, intrinsic, distortion, Rotation, Translation); - // append data to vectors - if(!quiet) cout << "Rotation is:" << endl; - for (int row = 0; row < 1; row++) { - for (int col = 0; col < 3; col++) { - if(!quiet) cout << CV_MAT_ELEM( *Rotation, float, row, col ) << " "; - CV_MAT_ELEM( *rotation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Rotation, float, row, col ); - } - if(!quiet) cout << endl; - } - if(!quiet) cout << "Translation is:" << endl; - for (int row = 0; row < 1; row++) { - for (int col = 0; col < 3; col++) { - if(!quiet) cout << CV_MAT_ELEM( *Translation, float, row, col ) << " "; - CV_MAT_ELEM( *translation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Translation, float, row, col ); - } - if(!quiet) cout << endl; - } successes++; + // } } else if(in == 'x') { break; } - } + //cvReleaseImage(&image); + cvReleaseMat(&image_points); + cvReleaseMat(&object_points); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + } cvReleaseImage(&image); cvReleaseImage(&image1); - cvReleaseMat(&image_points); - cvReleaseMat(&object_points); - cvReleaseMat(&Rotation); - cvReleaseMat(&Translation); - }//for loop for imagecount + cvReleaseImage(&image2); + }//for loop for imagecount cvDestroyWindow("Original Image"); cvDestroyWindow("Final Result"); cout << "Number of successes: " << successes << endl; // Now calculating mean and median rotation and trans - //calculateExtrinsics(rotation_vectors_temp, translation_vectors_temp, successes, dir, quiet, substring); - calculateExtrinsicsWithReprojectionCheck(points2D, points3D, rotation_vectors_temp, translation_vectors_temp, distortion, intrinsic, corner_exp, successes, dir, quiet, substring); + calculateExtrinsics(rotation_vectors_temp, translation_vectors_temp, successes, dir, quiet, substring); + calculateExtrinsicsWithReprojectionCheck(points2D, points3D, rotation_vectors_temp, translation_vectors_temp, distortion, intrinsic, corner_exp, successes, dir, quiet, substring); cvReleaseMat(&intrinsic); cvReleaseMat(&distortion); cvReleaseMat(&translation_vectors_temp); - cvReleaseMat(&rotation_vectors_temp); + cvReleaseMat(&rotation_vectors_temp); cvReleaseMat(&points2D); cvReleaseMat(&points3D); } +//bool readFrames(char * dir, int index, double * rPos, rPosTheta) { +//bool readFrames(char * dir, int index, double * inMatrix, double * rPos) { +bool readFrames(const char * dir, int index, double * tMatrix, CvMat * inMatrix, CvMat * rPos) { + char frameFileName[255]; + snprintf(frameFileName,255,"%sscan%.3d.frames",dir,index); + ifstream pose_in; + pose_in.open(frameFileName); + + if (!pose_in.good()) return false; // no more files in the directory + + cout << "Reading frame " << frameFileName << "..." << endl; + double tmpMatrix[17]; + while(pose_in.good()) { + for (unsigned int i = 0; i < 17; pose_in >> tMatrix[i++]); + } + + M4inv(tMatrix, tmpMatrix); + + CV_MAT_ELEM(*inMatrix, float,0,0) = tmpMatrix[10]; + CV_MAT_ELEM(*inMatrix, float,0,1) = -tmpMatrix[2]; + CV_MAT_ELEM(*inMatrix, float,0,2) = tmpMatrix[6]; + CV_MAT_ELEM(*inMatrix, float,1,0) = -tmpMatrix[8]; + CV_MAT_ELEM(*inMatrix, float,1,1) = tmpMatrix[0]; + CV_MAT_ELEM(*inMatrix, float,1,2) = -tmpMatrix[4]; + CV_MAT_ELEM(*inMatrix, float,2,0) = tmpMatrix[9]; + CV_MAT_ELEM(*inMatrix, float,2,1) = -tmpMatrix[1]; + CV_MAT_ELEM(*inMatrix, float,2,2) = tmpMatrix[5]; + + CV_MAT_ELEM(*rPos, float,0,0) = tmpMatrix[14]; + CV_MAT_ELEM(*rPos, float,1,0) = -tmpMatrix[12]; + CV_MAT_ELEM(*rPos, float,2,0) = tmpMatrix[13]; + + return true; +} + +void writeGlobalCameras(int start, int end, bool optical, bool quiet, string dir, + IOType type, int scale, double rot_angle, double minDist, double maxDist, + bool correction, int neighborhood, int method) { + + string file; + int nr_img = end - start + 1; + if (nr_img < 1) { + cout << "ImageCount is zero!" << endl; + return; + } + string substring = optical? "Optical" : ""; + switch(method) { + case 0: + file = dir + "Rotation" + substring + ".xml"; + break; + case 1: + file = dir + "RotationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "RotationMean" + substring + ".xml"; + break; + } + CvMat *Rotation = (CvMat*) cvLoad(file.c_str()); + switch(method) { + case 0: + file = dir + "Translation" + substring + ".xml"; + break; + case 1: + file = dir + "TranslationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "TranslationMean" + substring + ".xml"; + break; + } + CvMat *Translation = (CvMat*) cvLoad(file.c_str()); + + double starttime = GetCurrentTimeInMilliSec(); + + stringstream outdat; + string outdir = dir + "/labscan-map"; +#ifdef _MSC_VER + int success = mkdir(outdir.c_str()); +#else + int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << outdir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << outdir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << outdir << " failed" << endl; + exit(1); + } + + for (int count = start; count <= end; count++) { + // filling the rotation matrix + + // reading the frame files 3D points and projecting them back to 2d + CvMat* inMatrix = cvCreateMat(3,3,CV_32FC1); + CvMat* rPos = cvCreateMat(3,1,CV_32FC1); + double * tMatrix = new double[17]; + readFrames(dir.c_str(), count, tMatrix, inMatrix, rPos); + + // TODO make sure correct points are printed + /* + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + */ + + // write colored data + int nrP360 = 9; + for(int p = 0; p < nrP360; p++) { + double angle = rot_angle * (p%nrP360); + int count0 = count * nrP360 + p; + string outname = outdir + "/image" + to_string(count0, 3) + ".mat"; + + fstream outfile; + outfile.open(outname.c_str(), ios::out); + + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + + cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + /* + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + */ + } + cout << endl; + + // transform into global coordinate system (inMatrix, rPos) + CvMat* t_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* r_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* rodglob = cvCreateMat(3,1,CV_32FC1); + cvRodrigues2(inMatrix, rodglob); + cvComposeRT(rodglob, rPos, rod_comI, t_comI, r_globalI, t_globalI); + //cvRodrigues2(r_globalI, rot_tmp); + + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_comI); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + cvReleaseMat(&t_comI); + cout << "Done projecting points" << endl; + + CvMat* rotmatrix = cvCreateMat(3,3,CV_32FC1); + cvRodrigues2(r_globalI, rotmatrix); + + //cvSave(outname.c_str(), rotmatrix); + + for(int i = 0; i < 3; i++) { + for(int j = 0; j < 3; j++) { + outfile << CV_MAT_ELEM(*rotmatrix,float,i,j) << " "; + } + + outfile << CV_MAT_ELEM(*t_globalI,float,i,0) << endl; + } + outfile << "0 0 0 1" << endl; + // checking whether projection lies within the image boundaries + cvReleaseMat(&rot_tmp); + + outfile.close(); + outfile.flush(); + + } + + } + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + +} /** - * Main function for projecting the 3D points onto the corresponding image and - * associating temperature values to the data points. + * Read scans + * Read frames */ -void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, -IOType type, int scale, double rot_angle, double minDist, double maxDist, -bool correction, int neighborhood, int method) { +void calculateGlobalCameras(int start, int end, bool optical, bool quiet, string dir, + IOType type, int scale, double rot_angle, double minDist, double maxDist, + bool correction, int neighborhood, int method) { int nr_img = end - start + 1; if (nr_img < 1) { @@ -1094,18 +1385,14 @@ bool correction, int neighborhood, int method) { } CvMat *Translation = (CvMat*) cvLoad(file.c_str()); CvMat* undistort = cvCreateMat(5,1,CV_32FC1); - for (int hh = 0; hh < 5; hh++) { - CV_MAT_ELEM(*undistort, float,hh,0) = 0; - } + for (int hh = 0; hh < 5; hh++) { + CV_MAT_ELEM(*undistort, float,hh,0) = 0; + } double starttime = GetCurrentTimeInMilliSec(); - // filling the rotation matrix - double rPosTheta[3] = {0.0, rad(rot_angle), 0.0}; - double rPos[3] = {0.0, 0.0, 0.0}; - double alignxf[16]; - EulerToMatrix4(rPos, rPosTheta, alignxf); - + stringstream outdat; + int pointcnt = 0; string outdir = dir + "/labscan-map"; #ifdef _MSC_VER int success = mkdir(outdir.c_str()); @@ -1121,178 +1408,215 @@ bool correction, int neighborhood, int method) { exit(1); } for (int count = start; count <= end; count++) { + // filling the rotation matrix CvMat* point_3Dcloud; CvMat* point_2Dcloud; CvMat* undistort_2Dcloud; - - CvMat* point_3Dcloud_2; - CvMat* point_2Dcloud_2; - CvMat* undistort_2Dcloud_2; - - // loading images - int count0; - if(rot_angle < 180 && rot_angle > 0) { - count0 = count % 9 == 8 ? count - 8 : count + 1; - } else { - count0 = count % 9 == 0 ? count + 8 : count - 1; - } - string t, t0; - if(optical) { - t = dir + "/photo" + to_string(count, 3) + ".ppm"; - t0 = dir + "/photo" + to_string(count0, 3) + ".ppm"; - } else { - t = dir + "/image" + to_string(count, 3) + ".ppm"; - t0 = dir + "/image" + to_string(count0, 3) + ".ppm"; - } - - IplImage* image = cvLoadImage(t.c_str(), -1); - if (!image) { - cout << "first image " << t << " cannot be loaded" << endl; - return; - } - CvSize size = cvGetSize(image); - - IplImage* image0; - if(fabs(rot_angle) > 1) { - image0 = cvLoadImage(t0.c_str(), -1); - - if (!image0) { - cout << "second image " << t0 << " cannot be loaded" << endl; - return; - } - image0 = resizeImage(image0, scale); - } - image = resizeImage(image, scale); - if(image) // reading the 3D points and projecting them back to 2d - Scan::readScans(type, count, count, dir, maxDist, minDist, 0); - Scan::allScans[0]->calcReducedPoints(-1, 0); - point_3Dcloud = cvCreateMat(Scan::allScans[0]->get_points_red_size(), 3, CV_32FC1); - point_2Dcloud = cvCreateMat(Scan::allScans[0]->get_points_red_size(), 2, CV_32FC1); - undistort_2Dcloud = cvCreateMat(Scan::allScans[0]->get_points_red_size(), 2, CV_32FC1); - for (int j = 0; j < Scan::allScans[0]->get_points_red_size(); j++) { - CV_MAT_ELEM(*point_3Dcloud, float,j,0) = Scan::allScans[0]->get_points_red()[j][2]; - CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -Scan::allScans[0]->get_points_red()[j][0]; - CV_MAT_ELEM(*point_3Dcloud, float,j,2) = Scan::allScans[0]->get_points_red()[j][1]; - } + Scan::openDirectory(false, dir, type, count, count); + Scan::allScans[0]->setRangeFilter(-1, -1); + Scan::allScans[0]->setSearchTreeParameter(simpleKD, false); + Scan::allScans[0]->setReductionParameter(-1, 0); - cout << "Number of points read: " << Scan::allScans[0]->get_points_red_size() << endl; - cvProjectPoints2(point_3Dcloud, Rotation, Translation, intrinsic, - distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); - - cvProjectPoints2(point_3Dcloud, Rotation, Translation, intrinsic, - undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + //Scan::readScans(type, count, count, dir, maxDist, minDist, 0); + DataXYZ reduced = Scan::allScans[0]->get("xyz reduced"); + int red_size = reduced.size(); + point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1); + cout << "Points: " << red_size << endl; + point_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + undistort_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + cout << "readScans done" << endl; + // read frames + //double * rPos = new double[3]; + //double * rPosTheta= new double[3]; + //readFrames(dir, count, rPos, rPosTheta); - // second image in case of overlap - if(fabs(rot_angle) > 1) { - point_3Dcloud_2 = cvCreateMat(Scan::allScans[0]->get_points_red_size(), 3, CV_32FC1); - point_2Dcloud_2 = cvCreateMat(Scan::allScans[0]->get_points_red_size(), 2, CV_32FC1); - undistort_2Dcloud_2 = cvCreateMat(Scan::allScans[0]->get_points_red_size(), 2, CV_32FC1); - for (int j = 0; j < Scan::allScans[0]->get_points_red_size(); j++) { - Point p(Scan::allScans[0]->get_points_red()[j]); - p.transform(alignxf); + CvMat* inMatrix = cvCreateMat(3,3,CV_32FC1); + CvMat* rPos = cvCreateMat(3,1,CV_32FC1); + double * tMatrix = new double[17]; + readFrames(dir.c_str(), count, tMatrix, inMatrix, rPos); + Scan::allScans[0]->transform(tMatrix, Scan::INVALID, 0); - CV_MAT_ELEM(*point_3Dcloud_2, float,j,0) = p.z; - CV_MAT_ELEM(*point_3Dcloud_2, float,j,1) = -p.x; - CV_MAT_ELEM(*point_3Dcloud_2, float,j,2) = p.y; - } + for (int j = 0; j < red_size; j++) { + Point p(reduced[j]); + // TODO make sure correct points are printed + + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + + } + int nr_points = red_size; + cout << "Number of points read: " << red_size << endl; + delete Scan::allScans[0]; + Scan::allScans.clear(); - cvProjectPoints2(point_3Dcloud_2, Rotation, Translation, intrinsic, - distortion, point_2Dcloud_2, NULL, NULL, NULL, NULL, NULL, 0); - - cvProjectPoints2(point_3Dcloud_2, Rotation, Translation, intrinsic, - undistort, undistort_2Dcloud_2, NULL, NULL, NULL, NULL, NULL, 0); - } // write colored data string outname = outdir + "/scan" + to_string(count, 3) + ".3d"; fstream outfile; outfile.open(outname.c_str(), ios::out); - //for counting how many points get mapped to first and second image file - int point_map1 = 0; // #points mapped to first image - int point_map2 = 0; // " " " second image - - // checking whether projection lies within the image boundaries - for (int k = 0; k < Scan::allScans[0]->get_points_red_size(); k++) { - float px = CV_MAT_ELEM(*undistort_2Dcloud,float,k,0); - float py = CV_MAT_ELEM(*undistort_2Dcloud,float,k,1); - if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) { - px = CV_MAT_ELEM(*point_2Dcloud,float,k,0); - py = CV_MAT_ELEM(*point_2Dcloud,float,k,1); - if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) { + + int nrP360 = 9; + for(int p = 0; p < nrP360; p++) { + //for(int p = 4; p < 5; p++) { + //double angle = rot_angle * (p%nrP360) + 2.0; + double angle = rot_angle * (p%nrP360); + cout << angle << endl; + // loading images + int count0 = count * nrP360 + p; + + string t, t0; + if(optical) { + //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; + //t = dir + "/photo" + to_string(count0, 3) + "_2.jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_90.jpg"; + t = dir + "/photo" + to_string(count0, 3) + ".jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_1.jpg"; + } else { + t = dir + "/image" + to_string(count0, 3) + ".ppm"; + } + + IplImage* image = cvLoadImage(t.c_str(), -1); + if (!image) { + cout << "first image " << t << " cannot be loaded" << endl; + return; + } + CvSize size = cvGetSize(image); + + image = resizeImage(image, scale); + + + /** TODO!!! + * Transform rPos and rPosTheta into OpenCV (Rodrigues) + * cvComposeRT with rod_comI and t_comI + */ + // rotate Rotation and Translation + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + cout << "tmp" << endl; + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + cout << "tmp2" << endl; + CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + cout << "tmp3" << endl; + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + cout << endl; + cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + } + cout << endl; + + cvRodrigues2(rod_comI, rot_tmp); + + // transform into global coordinate system (inMatrix, rPos) + CvMat* t_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* r_globalI = cvCreateMat(3,1,CV_32FC1); + CvMat* rodglob = cvCreateMat(3,1,CV_32FC1); + cvRodrigues2(inMatrix, rodglob); + cvComposeRT(rodglob, rPos, rod_comI, t_comI, r_globalI, t_globalI); + //TODO verify order + cvRodrigues2(r_globalI, rot_tmp); + + //cvComposeRT(rod_comI, t_comI, rodglob, rPos, r_globalI, t_globalI); + + + cvProjectPoints2(point_3Dcloud, r_globalI, t_globalI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + cvProjectPoints2(point_3Dcloud, r_globalI, t_globalI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + + + // END TODO + // Project Points + /* + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + */ + + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_comI); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + cvReleaseMat(&t_comI); + cout << "Done projecting points" << endl; + + //for counting how many points get mapped to first and second image file + int point_map1 = 0; // #points mapped to first image + int point_map2 = 0; // " " " second image + int point_map3 = 0; // " " " second image + + // checking whether projection lies within the image boundaries + cout << "Now project points" << endl; + for (int k = 0; k < nr_points; k++) { + float px = CV_MAT_ELEM(*undistort_2Dcloud,float,k,0); + float py = CV_MAT_ELEM(*undistort_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5 ) { point_map1++; - int ppx = 0; - int ppy = 0; - if (px - int(px) < .5) { - ppx = int(px); - } else { - ppx = int(px) + 1; - } - if (py - int(py) < .5) { - ppy = int(py); - } else { - ppy = int(py) + 1; - } - - CvScalar c; - c = cvGet2D(image, ppy, ppx); - // check for overlap - if(correction) { - vector temp_vec; - float p_id = 1; // 1 for pixel, 0 for neighboring pixel - temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1))); - temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2))); - temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0))); - temp_vec.push_back(c.val[2]); - temp_vec.push_back(c.val[1]); - temp_vec.push_back(c.val[0]); - temp_vec.push_back(p_id); - if(neighborhood > 1) { - int limit = neighborhood / 2; - - int lower_y = ppy - limit > 0 ? ppy - limit : 0; - int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1; - int lower_x = ppx - limit > 0 ? ppx - limit : 0; - int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1; - - for (int y = lower_y; y < upper_y; y++) { - for (int x = lower_x; x < upper_x; x++) { - if(x == ppx && y == ppy) { - temp_vec[6] = 1; - } else { - temp_vec[6] = 0; - } - data1[y][x].push_back(temp_vec); - } - } - - } else { - data1[ppy][ppx].push_back(temp_vec); - } - temp_vec.clear(); - } else { - // write all the data - outfile << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; - outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; - outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; - outfile << (c.val[0] - 1000.0)/10.0 << endl; - //outfile << c.val[2] <<" "<< c.val[1]<<" "< 1) { - // check for overlap - px = CV_MAT_ELEM(*undistort_2Dcloud_2,float,k,0); - py = CV_MAT_ELEM(*undistort_2Dcloud_2,float,k,1); - if (px < image0->width - .5 && px >= 0 && py >= 0 && py < image0->height - .5) { - px = CV_MAT_ELEM(*point_2Dcloud_2,float,k,0); - py = CV_MAT_ELEM(*point_2Dcloud_2,float,k,1); - if (px < image0->width - .5 && px >= 0 && py >= 0 && py < image0->height - .5) { + px = CV_MAT_ELEM(*point_2Dcloud,float,k,0); + py = CV_MAT_ELEM(*point_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) { point_map2++; + CvMat* tmp1 = cvCreateMat(1, 1, CV_32FC3); + CvMat* tmp2 = cvCreateMat(1, 1, CV_32FC3); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).x = CV_MAT_ELEM(*point_3Dcloud,float,k,0); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).y = CV_MAT_ELEM(*point_3Dcloud,float,k,1); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).z = CV_MAT_ELEM(*point_3Dcloud,float,k,2); + + cvTransform(tmp1, tmp2, rot_tmp); + cvReleaseMat(&tmp1); + + //double tmpz = CV_MAT_ELEM(*t_globalI, CV_32FC1, 2,0); + double tmpz = -CV_MAT_ELEM(*t_globalI, float, 2,0); + //double tmpz = CV_MAT_ELEM(*TranslationI, float, 2,0); + + if(CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z < tmpz) { + cvReleaseMat(&tmp2); + continue; + } + + //cout << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).x << " " + // << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).y << " " + // << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).z << endl; + + cvReleaseMat(&tmp2); + /* + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,1) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,2) << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).x << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).y << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z << endl; + */ + point_map3++; int ppx = 0; int ppy = 0; - if (px - int(px) < .5) { ppx = int(px); } else { @@ -1303,97 +1627,505 @@ bool correction, int neighborhood, int method) { } else { ppy = int(py) + 1; } + CvScalar c; + c = cvGet2D(image, ppy, ppx); + // check for overlap + /* + if(correction) { + vector temp_vec; + float p_id = 1; // 1 for pixel, 0 for neighboring pixel + temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0))); + temp_vec.push_back(c.val[2]); + temp_vec.push_back(c.val[1]); + temp_vec.push_back(c.val[0]); + temp_vec.push_back(p_id); + if(neighborhood > 1) { + int limit = neighborhood / 2; - c = cvGet2D(image0, ppy, ppx); - if(correction) { - vector temp_vec; - float p2_id = 1; - temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1))); - temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2))); - temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0))); - temp_vec.push_back(c.val[2]); - temp_vec.push_back(c.val[1]); - temp_vec.push_back(c.val[0]); - temp_vec.push_back(p2_id); - if(neighborhood > 1) { - int neighbors = 3; - int limit = neighbors / 2; + int lower_y = ppy - limit > 0 ? ppy - limit : 0; + int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1; + int lower_x = ppx - limit > 0 ? ppx - limit : 0; + int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1; - int lower_y = ppy - limit > 0 ? ppy - limit : 0; - int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1; - int lower_x = ppx - limit > 0 ? ppx - limit : 0; - int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1; + for (int y = lower_y; y < upper_y; y++) { + for (int x = lower_x; x < upper_x; x++) { + if(x == ppx && y == ppy) { + temp_vec[6] = 1; + } else { + temp_vec[6] = 0; + } + data1[y][x].push_back(temp_vec); + } + } - for (int y = lower_y; y < upper_y; y++) { - for (int x = lower_x; x < upper_x; x++) { - if(x == ppx && y == ppy) { - temp_vec[6] = 1; - } else { - temp_vec[6] = 0; - } - data2[y][x].push_back(temp_vec); - } - } + } else { + data1[ppy][ppx].push_back(temp_vec); + } + temp_vec.clear(); + } else { + */ + // write all the data - } else { - data2[ppy][ppx].push_back(temp_vec); - } - temp_vec.clear(); + outdat << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outdat << c.val[2] <<" "<< c.val[1]<<" "< 100) { + outfile.write(outdat.str().c_str(), outdat.str().size()); + pointcnt = 0; + outdat.clear(); + outdat.str(""); + } + + /* + outfile << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outfile << c.val[2] <<" "<< c.val[1]<<" "< 1) { + if(size.width > 0 && size.height > 0) { + CorrectErrorAndWrite(data2, outfile, size, optical); } } } - } - // write data with overlap correction - if(correction) { - CorrectErrorAndWrite(data1, outfile, size); - if(fabs(rot_angle) > 1) { - if(size.width > 0 && size.height > 0) { - CorrectErrorAndWrite(data2, outfile, size); + cout << "Done correction" << endl; + // clean up + outfile.flush(); + + double endtime = GetCurrentTimeInMilliSec(); + double time = endtime - starttime; + time = time/1000.0; + cout<<"runtime for scan " << count0 << " in seconds is: " << time << endl; + + cout << point_map1 << " " << point_map2 << " " << point_map3 << endl; + cvReleaseImage(&image); + if(correction) { + for (int i = 0; i < size.height; i++) { + for (int j = 0; j < size.width; j++) { + data1[i][j].clear(); + if(fabs(rot_angle) > 1) { + data2[i][j].clear(); + } + } } } + cvReleaseMat(&rot_tmp); } - - // clean up - outfile.flush(); outfile.close(); - delete Scan::allScans[0]; - Scan::allScans.clear(); - - double endtime = GetCurrentTimeInMilliSec(); - double time = endtime - starttime; - time = time/1000.0; - cout<<"runtime for scan " << count << " in seconds is: " << time << endl; - - cvReleaseImage(&image); cvReleaseMat(&point_2Dcloud); cvReleaseMat(&point_3Dcloud); cvReleaseMat(&undistort_2Dcloud); - if(fabs(rot_angle) > 1) { - cvReleaseImage(&image0); - cvReleaseMat(&point_2Dcloud_2); - cvReleaseMat(&point_3Dcloud_2); - cvReleaseMat(&undistort_2Dcloud_2); - } - for (int i = 0; i < size.height; i++) { - for (int j = 0; j < size.width; j++) { - data1[i][j].clear(); - data2[i][j].clear(); - } - } + } + cvReleaseMat(&intrinsic); + cvReleaseMat(&distortion); + cvReleaseMat(&Rotation); + cvReleaseMat(&Translation); + cvReleaseMat(&undistort); + +} + +/** + * Main function for projecting the 3D points onto the corresponding image and + * associating temperature values to the data points. + */ +void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir, + IOType type, int scale, double rot_angle, double minDist, double maxDist, + bool correction, int neighborhood, int method) { + + int nr_img = end - start + 1; + if (nr_img < 1) { + cout << "ImageCount is zero!" << endl; + return; + } + string substring = optical? "Optical" : ""; + string file = dir + "Intrinsics" + substring + ".xml"; + CvMat *intrinsic = (CvMat*) cvLoad(file.c_str()); + file = dir + "Distortion" + substring + ".xml"; + CvMat *distortion = (CvMat*) cvLoad(file.c_str()); + switch(method) { + case 0: + file = dir + "Rotation" + substring + ".xml"; + break; + case 1: + file = dir + "RotationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "RotationMean" + substring + ".xml"; + break; + } + CvMat *Rotation = (CvMat*) cvLoad(file.c_str()); + switch(method) { + case 0: + file = dir + "Translation" + substring + ".xml"; + break; + case 1: + file = dir + "TranslationMedian" + substring + ".xml"; + break; + case 2: + file = dir + "TranslationMean" + substring + ".xml"; + break; + } + CvMat *Translation = (CvMat*) cvLoad(file.c_str()); + CvMat* undistort = cvCreateMat(5,1,CV_32FC1); + for (int hh = 0; hh < 5; hh++) { + CV_MAT_ELEM(*undistort, float,hh,0) = 0; } + double starttime = GetCurrentTimeInMilliSec(); + + stringstream outdat; + int pointcnt = 0; + string outdir = dir + "/labscan-map"; +#ifdef _MSC_VER + int success = mkdir(outdir.c_str()); +#else + int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO); +#endif + if(success == 0) { + cout << "Writing scans to " << outdir << endl; + } else if(errno == EEXIST) { + cout << "Directory " << outdir << " exists already. CONTINUE" << endl; + } else { + cerr << "Creating directory " << outdir << " failed" << endl; + exit(1); + } + for (int count = start; count <= end; count++) { + // filling the rotation matrix + CvMat* point_3Dcloud; + CvMat* point_2Dcloud; + CvMat* undistort_2Dcloud; + + // reading the 3D points and projecting them back to 2d + Scan::openDirectory(false, dir, type, count, count); + Scan::allScans[0]->setRangeFilter(-1, -1); + Scan::allScans[0]->setSearchTreeParameter(simpleKD, false); + Scan::allScans[0]->setReductionParameter(-1, 0); + + //Scan::readScans(type, count, count, dir, maxDist, minDist, 0); + DataXYZ reduced = Scan::allScans[0]->get("xyz reduced"); + int red_size = reduced.size(); + + point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1); + cout << "Points: " << red_size << endl; + point_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + undistort_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1); + cout << "readScans done" << endl; + for (int j = 0; j < red_size; j++) { + Point p(reduced[j]); + // TODO make sure correct points are printed + + CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z; + CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x; + CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y; + + } + int nr_points = red_size; + cout << "Number of points read: " << red_size << endl; + delete Scan::allScans[0]; + Scan::allScans.clear(); + + // write colored data + string outname = outdir + "/scan" + to_string(count, 3) + ".3d"; + fstream outfile; + outfile.open(outname.c_str(), ios::out); + + + int nrP360 = 9; + for(int p = 0; p < nrP360; p++) { + //for(int p = 0; p < 9; p++) { + //double angle = rot_angle * (p%nrP360) + 2.0; + double angle = rot_angle * (p%nrP360); + cout << angle << endl; + // loading images + int count0 = count * nrP360 + p; + + string t, t0; + if(optical) { + //TODO t = dir + "/photo" + to_string(count, 3) + ".ppm"; + //t = dir + "/photo" + to_string(count0, 3) + "_2.jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_90.jpg"; + t = dir + "/photo" + to_string(count0, 3) + ".jpg"; + //t = dir + "/photo" + to_string(count0, 3) + "_1.jpg"; + } else { + t = dir + "/image" + to_string(count0, 3) + ".ppm"; + } + + IplImage* image = cvLoadImage(t.c_str(), -1); + if (!image) { + cout << "first image " << t << " cannot be loaded" << endl; + return; + } + CvSize size = cvGetSize(image); + + image = resizeImage(image, scale); + + // rotate Rotation and Translation + CvMat* RotationI = cvCreateMat(3,1,CV_32FC1); + CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod40 = cvCreateMat(3,1,CV_32FC1); + cout << "Angle: " << angle << " " << rad(angle) << endl; + CV_MAT_ELEM(*rod40,float,0,0) = 0.0; + CV_MAT_ELEM(*rod40,float,1,0) = 0.0; + CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle); + cout << "tmp" << endl; + CvMat* t40 = cvCreateMat(3,1,CV_32FC1); + CV_MAT_ELEM(*t40,float,0,0) = 0.0; + CV_MAT_ELEM(*t40,float,1,0) = 0.0; + CV_MAT_ELEM(*t40,float,2,0) = 0.0; + cout << "tmp2" << endl; + CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1); + CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* t_comI = cvCreateMat(3,1,CV_32FC1); + CvMat* rod_com = cvCreateMat(1,3,CV_32FC1); + CvMat* t_com = cvCreateMat(1,3,CV_32FC1); + cout << "tmp3" << endl; + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w); + CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w); + } + cout << endl; + cout << "Final Rotation" << endl; + + cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI); + for(int w = 0; w < 3; w++) { + CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0); + CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0); + cout << CV_MAT_ELEM(*RotationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod40,float,w,0) << " "; + cout << CV_MAT_ELEM(*t40,float,w,0) << " "; + cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " "; + cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl; + } + cout << endl; + + cvRodrigues2(rod_comI, rot_tmp); + + // Project Points + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0); + + cvReleaseMat(&rod40); + cvReleaseMat(&RotationI); + cvReleaseMat(&TranslationI); + cvReleaseMat(&t40); + cvReleaseMat(&rod_comI); + cvReleaseMat(&rod_com); + cvReleaseMat(&t_com); + cvReleaseMat(&t_comI); + cout << "Done projecting points" << endl; + + //for counting how many points get mapped to first and second image file + int point_map1 = 0; // #points mapped to first image + int point_map2 = 0; // " " " second image + int point_map3 = 0; // " " " second image + + // checking whether projection lies within the image boundaries + cout << "Now project points" << endl; + for (int k = 0; k < nr_points; k++) { + float px = CV_MAT_ELEM(*undistort_2Dcloud,float,k,0); + float py = CV_MAT_ELEM(*undistort_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5 ) { + point_map1++; + px = CV_MAT_ELEM(*point_2Dcloud,float,k,0); + py = CV_MAT_ELEM(*point_2Dcloud,float,k,1); + if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) { + point_map2++; + CvMat* tmp1 = cvCreateMat(1, 1, CV_32FC3); + CvMat* tmp2 = cvCreateMat(1, 1, CV_32FC3); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).x = CV_MAT_ELEM(*point_3Dcloud,float,k,0); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).y = CV_MAT_ELEM(*point_3Dcloud,float,k,1); + CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).z = CV_MAT_ELEM(*point_3Dcloud,float,k,2); + + cvTransform(tmp1, tmp2, rot_tmp); + cvReleaseMat(&tmp1); + + + if(CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z < 0) { + cvReleaseMat(&tmp2); + continue; + } + cvReleaseMat(&tmp2); + /* + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,1) << " " + << CV_MAT_ELEM(*point_3Dcloud,float,k,2) << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).x << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).y << " " + << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z << endl; + */ + point_map3++; + int ppx = 0; + int ppy = 0; + if (px - int(px) < .5) { + ppx = int(px); + } else { + ppx = int(px) + 1; + } + if (py - int(py) < .5) { + ppy = int(py); + } else { + ppy = int(py) + 1; + } + + CvScalar c; + c = cvGet2D(image, ppy, ppx); + // check for overlap + /* + if(correction) { + vector temp_vec; + float p_id = 1; // 1 for pixel, 0 for neighboring pixel + temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2))); + temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0))); + temp_vec.push_back(c.val[2]); + temp_vec.push_back(c.val[1]); + temp_vec.push_back(c.val[0]); + temp_vec.push_back(p_id); + if(neighborhood > 1) { + int limit = neighborhood / 2; + + int lower_y = ppy - limit > 0 ? ppy - limit : 0; + int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1; + int lower_x = ppx - limit > 0 ? ppx - limit : 0; + int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1; + + for (int y = lower_y; y < upper_y; y++) { + for (int x = lower_x; x < upper_x; x++) { + if(x == ppx && y == ppy) { + temp_vec[6] = 1; + } else { + temp_vec[6] = 0; + } + data1[y][x].push_back(temp_vec); + } + } + + } else { + data1[ppy][ppx].push_back(temp_vec); + } + temp_vec.clear(); + } else { + */ + // write all the data + + outdat << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outdat << c.val[2] <<" "<< c.val[1]<<" "< 100) { + outfile.write(outdat.str().c_str(), outdat.str().size()); + pointcnt = 0; + outdat.clear(); + outdat.str(""); + } + + /* + outfile << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" "; + outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" "; + + if(optical) { + outfile << c.val[2] <<" "<< c.val[1]<<" "< 1) { + if(size.width > 0 && size.height > 0) { + CorrectErrorAndWrite(data2, outfile, size, optical); + } + } + } + + cout << "Done correction" << endl; + // clean up + outfile.flush(); + + double endtime = GetCurrentTimeInMilliSec(); + double time = endtime - starttime; + time = time/1000.0; + cout<<"runtime for scan " << count0 << " in seconds is: " << time << endl; + + cout << point_map1 << " " << point_map2 << " " << point_map3 << endl; + cvReleaseImage(&image); + if(correction) { + for (int i = 0; i < size.height; i++) { + for (int j = 0; j < size.width; j++) { + data1[i][j].clear(); + if(fabs(rot_angle) > 1) { + data2[i][j].clear(); + } + } + } + } + cvReleaseMat(&rot_tmp); + } + outfile.close(); + + cvReleaseMat(&point_2Dcloud); + cvReleaseMat(&point_3Dcloud); + cvReleaseMat(&undistort_2Dcloud); + + } cvReleaseMat(&intrinsic); cvReleaseMat(&distortion); cvReleaseMat(&Rotation); @@ -1424,7 +2156,7 @@ void sortDistances(float ** points, int size) { * Performs clustering on all points that are projected onto one pixel. * Writes only the points from the largest closest cluster. */ -void clusterSearch(float ** points, int size, double thresh1, double thres2, fstream &outfile) { +void clusterSearch(float ** points, int size, double thresh1, double thres2, fstream &outfile, bool optical) { int position = 0; int cluster_count = 0; @@ -1442,9 +2174,10 @@ void clusterSearch(float ** points, int size, double thresh1, double thres2, fst double * tmp = new double[4]; tmp[0] = position; tmp[1] = j - 1; - tmp[2] = sum / (j - position); + // tmp[2] = sum / (j - position); // weird heuristic ;-) (clustersize/(rank of the cluster)) - tmp[3] = (double)(j - position) / (clusters.size() + 1.0); + tmp[2] = (double)(j - position) / (clusters.size() + 1.0); + tmp[3] = (double)(j - position); if(tmp[3] > max_cluster) { max_position = clusters.size(); max_cluster = tmp[3]; @@ -1453,11 +2186,22 @@ void clusterSearch(float ** points, int size, double thresh1, double thres2, fst position = j; } + /* + max_position = 0; + for(int p = clusters.size() - 1; p > -1; p--) { + max_position = p; + break; + } + */ + for(int p = clusters[max_position][0]; p <= clusters[max_position][1]; p++) { if(points[p][6] == 1) { outfile << points[p][0] << " " << points[p][1] << " " << points[p][2] << " "; - //outfile << points[p][3] << " " << points[p][4] << " " << points[p][5] << endl; - outfile << (points[p][5] - 1000.0)/10.0 << endl; + if(optical) { + outfile << points[p][3] << " " << points[p][4] << " " << points[p][5] << endl; + } else { + outfile << (points[p][5] - 1000.0)/10.0 << endl; + } } } @@ -1466,10 +2210,11 @@ void clusterSearch(float ** points, int size, double thresh1, double thres2, fst } } -void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size) { +void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size, bool optical) { double thresh1 = 4; double thresh2 = 5; - + + cout << size.height << " " << size.width << endl; // getting points mapping to one pixel for (int i = 0; i < size.height; i++) { for (int j = 0; j < size.width; j++) { @@ -1486,8 +2231,7 @@ void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size) { //sorting the points now in ascending order wrt distance sortDistances(points, tmp_size); //look for clusters - clusterSearch(points, tmp_size, thresh1, thresh2, outfile); - + clusterSearch(points, tmp_size, thresh1, thresh2, outfile, optical); for (int k = 0; k < tmp_size; k++) { delete[] points[k]; } @@ -1607,7 +2351,7 @@ int parseArgs(int argc, char **argv, string &dir, int &start, int &end, double if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); } if (end < start) { cerr << "Error: cannot be smaller than .\n"; exit(1); } break; - case 'f': + case 'f': try { type = formatname_to_io_type(optarg); } catch (...) { // runtime_error @@ -1718,7 +2462,11 @@ int main(int argc, char** argv) { // either mapping if(mapping) { if(!quiet) cout << "Starting projecting and mapping image data to point cloud..." << endl; - ProjectAndMap(start, end, optical, quiet, dir, type, scale, rot_angle, minDist, maxDist, correction, neighborhood); + //TODO ProjectAndMap(start, end, optical, quiet, dir, type, scale, rot_angle, minDist, maxDist, correction, neighborhood); + + //calculateGlobalCameras(start, end, optical, quiet, dir, type, scale, + writeGlobalCameras(start, end, optical, quiet, dir, type, scale, + rot_angle, minDist, maxDist, correction, neighborhood, 0); if(!quiet) cout << "\nDONE" << endl; return 0; } diff --git a/src/veloslam/veloscan.cc b/src/veloslam/veloscan.cc index e20459d..dea4d44 100644 --- a/src/veloslam/veloscan.cc +++ b/src/veloslam/veloscan.cc @@ -109,7 +109,7 @@ Trajectory::Trajectory() * default Constructor */ VeloScan::VeloScan() - : Scan() +// : BasicScan() { isTrackerHandled =false; } @@ -131,9 +131,11 @@ int VeloScan::DeletePoints() /** * Copy constructor */ +/* VeloScan::VeloScan(const VeloScan& s) - : Scan(s) + : BasicScan(s) { } +*/ int VeloScan::TransferToCellArray(int maxDist, int minDist) { diff --git a/src/veloslam/veloslam.cc b/src/veloslam/veloslam.cc index 2432596..991aed4 100644 --- a/src/veloslam/veloslam.cc +++ b/src/veloslam/veloslam.cc @@ -1018,7 +1018,7 @@ void MatchTwoScan(icp6D *my_icp, VeloScan* currentScan, int scanCount, bool eP ) //////////////////////ICP////////////////////// if (scanCount > 0) { - PreviousScan =Scan::allScans[scanCount-1]; + PreviousScan = Scan::allScans[scanCount-1]; // extrapolate odometry // 以前一帧的坐标为基准 if (eP) currentScan->mergeCoordinatesWithRoboterPosition(PreviousScan); @@ -1111,7 +1111,7 @@ int main(int argc, char **argv) exit(0); } - VeloScan::openDirectory(scanserver, dir, type, start, end); + Scan::openDirectory(scanserver, dir, type, start, end); if(VeloScan::allScans.size() == 0) { cerr << "No scans found. Did you use the correct format?" << endl;