From ff7823c83f2b69845bf5a06ba025e18c71ed12a4 Mon Sep 17 00:00:00 2001 From: caroline Date: Mon, 25 Apr 2022 16:34:21 +0200 Subject: [PATCH] kdtree needs to be altered --- cfg/config.cfg | 19 +++ pcl_methods.odt | Bin 0 -> 15164 bytes src/initial_processing.cpp | 260 +++++++++++++++++++++++++++++++++---- 3 files changed, 251 insertions(+), 28 deletions(-) create mode 100644 pcl_methods.odt diff --git a/cfg/config.cfg b/cfg/config.cfg index 39c120c..a6c6309 100755 --- a/cfg/config.cfg +++ b/cfg/config.cfg @@ -14,6 +14,25 @@ gen.add("ybneg", double_t, 0, "Min bound of Y - Axis cropping", -5, -5, 5) gen.add("zbpos", double_t, 0, "Max bound of Z - Axis cropping", 5, -5, 5) gen.add("zbneg", double_t, 0, "Min bound of Z - Axis cropping", 0, -5, 5) +gen.add("rotx", double_t, 0, "Rotation of cloud around X - Axis", 0, 0, 180) +gen.add("roty", double_t, 0, "Rotation of cloud around Y - Axis", 45, 0, 180) +gen.add("rotz", double_t, 0, "Rotation of cloud around Z - Axis", 0, 0, 180) +gen.add("movex", double_t, 0, "Move along X - Axis", 0, -10, 10) +gen.add("movey", double_t, 0, "Move along Y - Axis", 0, -10, 10) +gen.add("movez", double_t, 0, "Move along Z - Axis", 0, -10, 10) + +gen.add("passx_min", double_t, 0, "Start value for Passthrough Filtering for X - Axis", 0.0, -5.0 , 5.0) +gen.add("passx_max", double_t, 0, "End value for Passthrough Filtering for X - Axis", 5.0, -5.0 , 5.0) +gen.add("passy_min", double_t, 0, "Start value for Passthrough Filtering for Y - Axis", -5.0, -5.0 , 5.0) +gen.add("passy_max", double_t, 0, "End value for Passthrough Filtering for Y - Axis", 5.0, -5.0 , 5.0) +gen.add("passz_min", double_t, 0, "Start value for Passthrough Filtering for Z - Axis", 0.0, -5.0 , 5.0) +gen.add("passz_max", double_t, 0, "End value for Passthrough Filtering for Z - Axis", 5.0, -5.0 , 5.0) + +gen.add("voxel_size", double_t, 0, "Size of the Leafs for the Voxel Filter", 0.02, 0.0 , 0.1) + +gen.add("Pseg_dist", double_t, 0, "Point distance for planar segmentation", 0.025, 0.0 , 0.05) + +gen.add("green_thresh", int_t, 0, "Green threshold for color filtering", 150, 0 , 255) exit(gen.generate(PACKAGE, "pcl_tutorial", "config")) diff --git a/pcl_methods.odt b/pcl_methods.odt new file mode 100644 index 0000000000000000000000000000000000000000..b00a98573729585054bdc03f7fe84e0586144eef GIT binary patch literal 15164 zcmc(`Wmp}_);7Fxmk=a41PH<1A$V|?;2PZB-95OwySr}O-QC??0=#6-H)m$fGjqQ8 z`Sq@S?Q5^vUF)vZ)z#Ijs@E+o1_q7_06+r(GggW68ol)JqyPZm*Z1u&fSHk*o~@Im zp0=f>sgbU>t&xQ}jlKDIY71=}BO7W9OFi@N7P@w3dgiv&wt9}X(tiPav;Vhzc+>bT z%=L{7?5ux-v7x82wXiVN(YB`f-&yIKT4>wq{co+_T(tY|ZDIY5hn5zWc9y?P|BH_} ze`BYutE*?K_jaNd|IN?8Yw=f~SZbT=nf@`PD%9yd918){0X#pEcK5L+4Xn zXS=DMi#4JctrYrGsl#Rtr`Np~>akbM&GH(10aKpQY=g%ido`E%3PR=5Vo=aEcZQP? zAOL_57y$6Ed*JW7@3-4f&sLk-(aba~dL%rV7S-?04OYMaj=7f*_%U=^aHdDTbXxw9 z+WMHIn`*v4fza&<2r3yJF3na7*(iz)BGd1L%L7c zYl=tz@KG^JN6tGlh(!oA*3%osm5NSLW%K838z%mOqo#@weqA^L+He^!Vh2t~xpB8^ z(!Pb38`a{(jWFYcT{OmF7=7b$8}Q}|Q`XsrFR`MpPBG}YM#26on6Ww=hqG~DBoN*x zqc_;7e(#c$b%#OIrRZ)s>Uk#`H1@S`+Ns2=v6B+o${{6xx~82r&T(S1qoX$*1!bh; zZG*o@(Zc#ML){swgKp7D9A?ommZA}@ z&wnnLsFEshfH*cWardzd+{~9MCD-H0`ytj^`EvOcdk(+SO4QNk^gh#!F1TxphT^kDOKL;7@^av^}KkbCjY7VD14At&Srvi zB`eu>C!3T13=huvT6g#L?TSM|pBz4V4Zn?m+5b8Me%(MedbYMk<_0#uMnS8}ip?S` zvPXOS$1WGq45Kny5qAF=e{@i=oW!H#3XE~VIL;`X_4amk%*mMbn8p1x0MVIoyZaNN zy1Pox<~B^thNy;u@!2RjDFHv46G5A#E7%q8!-lCW?)B zx!4qZY>JgD;Y?Lw!bTpMGJKvR^5)tn6e(KxFGN#cn?47_h&~%#HvzFP0`+Env!!19RZ~{ISpIh$% zwgg;6xm<8s<;FU8KON4ds@$1L9!J$sjC?b2USt?kjP4o)uW1KlvzPHbc z1^6i*q_n1^O-sy$^70rG9vZr?Yae;RTSXvCXz4*Eh#2r`?~f@rPEi6yUqn-|LF{;Cb(sxY;@tP}^v zd;G&mq%Hi#m(c#<e&Lp(6>zLKS5L&jkGXaN6%v2f=D`L(Q5%2a;Hxu>Qngx`aKPpE>-6R;^<)#6RkLv5Et(Fx!{Ine^gyGu8+Joa$i-|T%A}SWjC&A}!c8WH*y_DU~!Zc zw@A1O^7wS=7p-qm_dtcv-7$)qpzm1$v7OlBqPE{Kbyzd*CHy~+em9*19ZqfX!W8q1 z0Ks*tMvyJ0*SbL+N)WgsVSyhCxk8T@FGmV$=ORZsp3egGt=;yYs|(u~7He>DTTH{^ zp*7)s^fn9;$vq!I&~a5U|DnQee++yd=nmt0`smo-s(PNMh*wR3 zn}PrQ-C*l7w?s_%r(gludzR!7fc&!~wv_j+roTDxwFh|^>Gr*Fi`Hg+k_);C?K1}7 zk%QKO>5X5nlSNGc$s|IYIAix(-k#O4fH|hLxt7oQV>SE-VR<~`QLy4Rr;bnIS#DXC zd6`yTTJE4Qt(2Ceg33!F8W)_dOzI0;y=x%YsRb_V>4&2v=#68#hvIc**NL#qck2Es z%Aud(^DNE}eD>`I9kiGNBYrX^f2lmK+5AU$THlPgsa91pn8f9_qBP2E& zXD_31!9%Anu{|RWgCAofWt%x1xZqaQIEve8yy)t2me55G7RP{}H#ik!G~hhH#G~#l zaE;71ZIK{*tojmk=O{m&l~Kpw%(;%4gCKoZ{k%oA_%uwdl3-3o(){V96hJ29B3l{+ zt?%y%LPNzj*W(wzCS0vHYYp^DVxm*lBL9{1xoi!_gIw zgJcU50N|ndJv{n-l_KYLE4q&c0Q~y?T3E;%+L`H?Ya5x`(AfU2Qd^oE1W8K@A;4n6 zzEwdG5f+er`>h24KmgEhk6S?JbKd~~0QxK~E-%PP!^X?ZtSI+QQ`cDC;BBFxX{E2C zr>&*)-O$v`RL989$lTVz#@)=u&0b&EUf;;l$k^W8%G%1@&DzY`+TPaA$<@xr+0N0~ z+0Mnu#l$(-(bd<()7RBE#3L}w*7Jvpf0RpLv|C7$YglHWqlurBoxg{dpO1T}mrIPl zi;uUjuWyiFU|5K+XIP+rNN|XMNKC|!VBg4;&>zvE(Mdo2J!3+AqQXNHLcF6RB4eV$ zQlkBn-;PAbM8_t@CZ;CFMkmK7rzFHCCnpEQ<-{asMWz)cr(`Cl7Nlk*rDo=&WEaI` zRi)-urWaHu7d7O>1?D8jWThq*r$uI@Wn`omS7UdS?<`xzgdF1qRM*z*!0vq4&cg8F zeDA^1@aD?=_QuNb=Ir72^7+Q-`OeJc?!w*v%*FolY*4F0k!S=!N!OqtH-oeqq z&e74)-pS?8`OVhF)5-bO@%7{J{nO>~_QmPZ!`aT&`N`wO_QUnb)5G!A)z#(g!|lV> z^~2Ns?d8vh>z9|8XD1T@001RVM1WV`aq%P>!4!MWt)m$cdl7X(C|YX-K3nrbKi_Ca z**K*kFXz6bD%V&^Hke*z3P<8Hb|>b2|Fm|H5Pb`lJt`9dlTg$UYw859n1B8_DmJY% z&cIa>BRPwl@H;AHj7b%SGh)8{L~qodFOQB$9HOG83)EN-*+JuXJUC~oeHRW9uVc*) z_?<_Uu7T3;jrk-BV3;yLP5UPOt3X56$9x)#0cOplFayD%8AYMc;&z%Ao&S1C9JjM5 z^uQzgy1korZZMD?5$1H{OG`@EF^ucW^o%6C%)19DDbD3jp z*nN6_s%VloATy{a`)GF&G-B0wynZ?~qTR%m@Vyli-w7L9OVFP6J%glkt;e}6L( zlyqQzP}vNSk~>+cNIj`#Oo%~D5Ao+gDpe0Bt%wO~b?eY!;;r*>$^o9*`ls43dNQs6 znW6BqC1OE3>KUA)QM{&c%)YC&mM5xa#_R7_ry1N+PH-ws7+a$$+QSXGW%B~%;jxi_7nNp1XfnctO3&gf$8 z$Pw45BO%o`un>(+$8~1BkN-S8taglu!DeFQgaCn`pjQ#NOE#Zq z$*lt?Z{f6~A`f4aba)mmdX=_Jf1uU7SH{R*7%~mV{U5TZaz^svIL|$Nu=7^(Jc1wa z@X+?INJb&_!wNU2RSJZxYfli>X;=BI&gZATNEg82^;Sj8?$2HG+8<5CV`7NAZgsqJ zLZsN|cbPGq&QWv>X!Z8crW)DhJosL?-hoA^)=z27?E|#PPoWxDIFN3Z`YQF;Uh8|N zz|*{5-J2$kU8m;`9|gZ6Wy03`cBBHpokz_rp!ryb0KuwowcLu${gd!0k+(q17%D>Q zJXGlzns#9)|@;4y=&Bo;Lp%J zn5uKnttU~kR0&`I0Pr@-KwN9lVOSVK2zhc+aG`QP_Ny6likAF(sOi*2!?iKD89pW` z5BHkWfODDoXI*I352_fDucc?c&537-9=-5Zv>1wz^Qu0kE34raxb9$`r{RjbW?aj2 zC*Jl)^QZf!}yyjh8G|(Jd7*0am1I2=+>x_Gl{FOILGz>i0&vk$hW; zp#&8LpSoL66BT;MBnYP9TtBfm<80_8(d6h<`mqlB$3ej?UcV%N{@yosiP`Pg(6bb$1!eUl3ETx-~)d9%!O`!YUsQ}T+GJ{+9 z!Ggltei8tXqSw|xd(d=;dA4<<=rb{$XTqrqVRpmMd)Ec|_>#`OwdRaG)ogFSrp=OD z3s~(;dBIk(kls%Pj9)|Usdvz>wWhZfHR8SFV;mFghDK#$`OX@*2byyDI-w^2wA%?> zc;_#4-2W2rUfeSkCKWXMr30Gty%V*sG6t!y?lQsN4Wu`(afJgk6c}$E{Xw=l3b7GRFTe_EK+v37yF39qUokEUD|x7OftjztLL z!lmj7Kj(hYR6j)?Be%SS^w7OGk7Ns(RYP5BI;hdk9>YL{>l2&2axTwRb-b8csX#AN ze8psa5^noAeYRSTjBqIz5Rmy^q+O#S$rHn zx?5B@-JfGD9!Ok;ytCrUK*3#aYW2|wcc@+8p06OC$FP3j(raHCyWmaj!zeHDQb9AM zs(8LYil})>ZLI#X2brQ@T~6sII-rkrVwt_|moUi2$BXKc zDY^2Gv0V{%-8xnr1dv7nk6w>fEVLY|C8>4wrsZxZ0G~)jqpQ49MUsYr2J8f?BwhN# z6R7Y89xw>z`WUYs!}h{P3c!$% zAtCn+N<)`u?gHuvX+=mHt+ne*k@L>W4tkAgSGQ6bP(6c(ma1vn>+MOlAu+dD!IKQ> zMsgSRX3)$~5(PLe0V-J~RI9=kcB398l~`h$lbG1MA%D~w-$0?AdXH<97L$d{bx;qxnx!Jtj z@^LNnow}mU3-s#xBO=)Et zX?H%(UwreV?dy^Tw?Fw^$il#c9O&dlk@@?{D+-7Z!&djGAsTEApm3+5`8jC_o3JH( zn0Y}UqzXJ6Qi~?V`90S8J@9$umBp9`=U{a0l5N9gnzL1y+fK;#y%8);EskjmXf8~%2f@Y8ASUkjDgTlaseq*+-zJQE=n0JG%x?re(=#O ze2*X6Dvl5b#GV8Q83oO3qVRY;O*UG5zR|748N#X&=$D--V49};o}UOul%I1US&#pt z=7vsJq!r=L;aGlKM5(A(Ws4=NN@r#^{_5P zBBgTfw|p`wi@zh|r|+S-HQGGl^eLd^esD;+f_i9sTfGwNEZ}_QlU?_DMv?3!uok*@qLYfM26o>=YDd`Xi>lz$F+Nt)s*_u z;#1ZM$5}12IoK?s-{gtmGWbD2sj8{sa~p0wzWmwj&2i4B690f<|0YOGrNg68{U^Gv z{7q_c<*=~eo8-_F#4Z@Ie3xu8&x6$T_PZ(P^OA#dwV2c4^OZZ$@jLbGQRY2Hkgac{ z6Vtn65+zn;9=m$2CB+VS6(s?<4lbB9JNH6LmJ%MTIu4aC!zma~K19wP;m)myaoFBp zx;G_f1*^-e%xSm9aw>)x3{kYlOzd3-eF*o?dR~ZVD$Q2j`y}#$OKH{5Ff1np2qN_$ zR#kr?g4-CUO{v=wE^i+SAzWH;FuP?}1|C*XDAI(t%}m_U-bP_4N{+lB#(vf;j*m16 zp`uSJkdSP-OLybeZ6iSV5SK?;GSDB6n|;G9g;8FG8}N*~=KSCz=7!oCvE0)kvHz+4 zsk;1OL&)47A=L+$<@q2W{TU#qsc~=j18umQ<{NQ}tHIs~MH!Z|BnyOjW~ndXvGO>xK-{rPCW(eVhv= zu08QJPSa{DJdgL|<+kJscWEzi zv@VZ7_U%}?hP`oZdD;gKtyJeB(@wbDv;eFvn)yZ?Crsc+1VfXQsy{GEU0z^FSn; z4|+dwwOXv@<-1qIapH&eh2f^(o!X_bZyVCr*Dm^`GmMY)HIHa6ngWJ_qnS>)JMF0u zyGxkWwYpCyJT_MA&V6%x?v{~GVH%Gw>pfyq^5(V7-#uJ#*zvEA+_B?#6XEfu@!;525!7hw zTpg^mMVt1i9r5-g<{hEJTcOBfvxA~=JKJzpEFLPv>3r}e*oN~b_sjVpiQc6XofM}z z9a0FOhCUE~Ty}Hv4RMxP7d%nTX@nQ`@8%rJ6|gOMN z$8|HNLx9zRRZSD1D|9nA)A;(0L8{9k*%q%y3B%U~Ufq!c4L&Wy&0IAaL$#0%8W&p` zaPcgq^r~GM?cYpTH!Y|Q*CXHj9uiu947c>XisZ4@={&=bj}R5r+vZ@qt~Kvb?MD)-3&~2dkKTj4J707bPcW!keYT7Ba+b`r{u&rzK(RIBg^nQUHh&D zKQ?+BLs2QVXFit`>ky3@C6>i)pidb>q=f_`T(AOdE*2@sI3@kTeLvC-_x*@N z#gMVMhj9P88KMh9hAH+l23!0*5JQ@vmwPS*PK|Of3=I^TM29tUN2bW(G->Ct^N4ir zi_S)szwtwpQ8pz71f?UYgo3M8I!#**vDCzuJ-oiE&vP0s6XZo8wKeS}Gvu||SIE_a zSvEy=QDb{ais-fCjX!FvfTzHw$Y_fXyA=~M^CJk?mX3P41Tl?AM*NU!%^+^0-mhCZp7{LB0PXykGp^-zrLZ%t zv3}zye%M2>RO18v5YpJ!tWOY#LQ+*hSFHxhc>pa#L)vv&^Muwm|18mVj%h+ER_H%( zw)`G|{LgKe|93}Nihgp(n1Hi%b9!C8?cf4L1SJK^__VzL`Q`bqtzR2kCsV!OUYwq& zs75c6eZ1JDZcWKI<;wF4e zWz(i1$k3t=y8}~z+p^}Akv1>LJE987J>K7PYO-yp?L%g)8#U(FWEe;zFTXnd+^|f* zH;2F7@<31WM`bJW6WUV(G@x%5;bwfmA$C_S?N)|}~RzD~ldoG{ljY51Db(GN- ztPk}Sm-RxDYc!<%K$}8MZUu(bg z8kg?xid8;$TiUYroILh!J-hR?W5A(@9CYCV%@pEdEeua``su6B^6G^@kP_{^$3@QOrf%qkGo9 zM=0PvKP~Pfbp^p^h=+%3Bm(%k$*7BvX)v(1+(ck(upIZtt2$88P$p#G0gEMQn~&tU zJeo8|jDfCx^;2>^o?Q!i>ZU;n$4KYT%5`2z0UjQ!f_A)|^CV~r|%kaJF z$OK^uV2G*K-U?r$cJD$Qco;oaTg{@?QRS$LDpNb-p;aWPlhIdT^ff}a&07HAA6-(< zCrTz*=NZ3EVAu2Ot)k+OsO~u9AzG zr=Ms}CeGM>CnBb4yWDPA$v1sDM~abznqa&D-ToGBK^J#1(qI;uibpnJIg-@7 zU}a9c0;9IW7=%;z5Jo=#7$}X3WmcLh@9pt%e3cfld6b2ver;uA$uRHk+dLNLRm`_y zyEq;^eODWRV&Jzy)S@V&=jOzZHYt~2phlS~8C(1{DQm<%1Q*jp_%eu^*5-}^q6o?2FRAwOpzwbx6Lprw<#bD|- zEYCPX@#ipt!Pvry@^o{)>{AJ#GScn8f0sL9ZYcrI!Au<>*DQEAgG;Y5dPZ(xN*b8$ zK8l+g+5Zl2Exo)n0zMQ6D#Y5t3aR$fKGkU<+X>B?2zP6Qije(bXN=AU7rr{Y_Bl8P za&E3`pkf`W^m*%RY+cKe!6i< zskvsy@YR04`8Y1O(qo3=z)%P(R*(4;<<#TR5(zB4;czyBj~7;4Y-+7ZniFZJD9#CA z2ML!DmX8$}3m^O}S3Qyz;mtQJ{K4c_ykUoLF9K0=-h*KB17#V}Ya<*H5UI6CJ12fO z9lG$xCP$XF8`eysv)P6vZ!!R%;7pcs(y(Tc^^TqrSiydh98$|9J~+cMW^(%>^n%EN`=GZ32daTAolOL^RG=-%#!zsc8Y8$Syh|7zFau2 z=S47Wgs%p3E7CySBq%;&MRMK;K~=^93z+ff%A{1iYrmQqYl<9XGI}A@>D;3dPf`+d zlS-tgcx5j*JQ*9`CeD5&wTm!bE%N3 z?!HUb-TD@aK+7$B12Wg|aV#P<>HG^e9O(SpqS{{_=xrMxANb>@^OKUKANvSJl+}I;pAY8yR5FfKL(=+Z>mb?)tvG_0f$j!Te$6J1cHZ zwI;OnX?=b)fE8gnuTc{{i`dbY`|4Z)E&?K#POJtAG%f()nd(Io&*Kb)k5Mbwjs2YX z38}X5W>iu&=J(pd@ox2y4XPiR1;?V#>MzFdu|=?c)%j7zJNs6HNq^{- zD8yDIGf}iG(6_E(c)!ftT!;&V392vDCf5T=nbJJm|F+aQLRsQ^VjQt4Xi~Mv$qRNb z=2Xj%k^8PNa@L{AH%Gh=Jf{nk{dVzVDLDU7Xqx{CTzez5X896SJ>e0fxC#YrRH6UZ zA%O&>k2jL|t~lBawQBZP>1;oa7;MZ`SoAJ++Pv_*gmeQ_1%eG~^g& z?K>wtZ-mT_$*c2XCZD9Kp1UgPT>@Jl@%>6ocn_J^-S%Jav0s|m(yiaJ_#*%P9{cao z{NUaq6p2B)+X8Sdm7Jm&JMd*tNFwnPtCRNW`B7HFPG3CBfn_LbKP%wD7)jNn z(N}CAS2Um^`?aaVekPA<;=41Kad0ocY#>~59{wmCN+BkWWW+%ixsTxCK$)_Qp|hV# z6v32|i0&a4O}EFq8{sWyp__V=%-e$29^iAYRItfb5Te$l6<%NDJj`_+R!F~PY88O=80pzYxf$>DA^)OS83@&_w91Z@`%sIViW zo{}_On5A&HJi8p4Yg(a^h-1-I@)u;A_A=@~zP7FpV#ZK8rrkS^GIPOE5Yj`3oN-V9 z?8KDr&>=OI4H*Y|lIC}%Y7ja0Eusx2xF|tK1j##7-StfJD^as1ZEY7K zoMDl61Mm}+2F$eY9tN%V6Wid@wFUUr{Bz{=D*pG`|UwPBZQ6Y;GHanM?s9UY#%{sEJ~ zPAQ=W3~3b9KLA&!p&O?AB-z#pEkee*Jj!zF0q*;{JK7?A5e;gc8hH-T*u>II08NUQ zAX|(-Cwq58t*dM1_2D~2>#iSt4QgiPn`y)>UXU~{dx2^ zTUc)KJBv|Q-`1c|j|#W2UJ*B~PhMtbA{%5AAFb5iiXX$bZX>#52dBf2wDaM~xv^}V zl1+bB$mO*S>^$$}&uH0;4h?uuC}gc@*^`eHdMBzafEDj(*$)mCdRvve`$Pp*EoUx? z7Ro`OxYU0%Byi@ zn~t38bNCZ0*&5s6o+3#{XT9`E3IkrIv3pB z1(GIDzLo&KVA?-$hC;7j?fhd0GyIn4W2AAMLgEU_fay^H9vo;LL76sKmZ+dkvs@C6 zIUyM(_3j*3StXc72cYt6>R0Lytp^`kPh*(|zw!;cVF7=pQ8Lb+%lpG;2do$ejW5**{Il99aK>9F>&>x2r!vGQD@<&m@-5tIf7F%spx(7ccJ)ZgitmBQHE1i zTsDLJ8}4H(oQg@!r@62VQ3bq^Z$=H0uez289%_&HWB4UvuUnz(P1oF`(7)8--&Dw7 zYRPXZ>>m~2N^K52lw6+h?NPu5%=y&3&ZSgL!8E9%i;nQOmTh02#@0sBhh{l&jdrm* zWM>WvpoYp{zvwEdoEb4f<`w13ASHM_@0ofTwYp4vh$}knsm`w?-Ix1yOzPq|sN^PG zKv$`V9c3)7H!b6C{6j*~YF9OB)oe3-n0DbIJ*Nwni?=(Uu2#__%vf5#${7x$e{Pk{ z`LWY6j59bJeKQxR&6(6?HAr$}@#fb;gL~M^2w#Dt^xLy9;??W2@1Pv@Dg+=IKYoRD z3wI5ENcSdqNB=zN3wtTki+4GO%sC3H_GPiZ`C3;cqV9^}=6#yGinhn))=KMj;acN! z_h-t5dJr)+TOqUX2l8qZd}$oiFU2!iU*H%NxRqpCx~wcr5nQs1^2TS!f(wW@yhMWW zP>=fz9e!#9R&@!tL#w;f2_qn7?@%Kn_du=@lRe2a?k|~~VVb-B{VJXPSYZ}$K78y| zE5bkj$VT*eR@vG%ZW~_AmyOpmD1&+e6trPTf0;zhR9quxvIPDK)yVUry^;iGoF=77 z?t3f$7quL7c1jdlK`f{+MdN8mZ7)M##*tRVZ{QpZjB{Ttk#in?h*CUuP8hmb+cnlOnt+%vG94HVbcA79lkI-Il40*s>VLOB2^&)Zm~@oJ_5B>qkGP%?)2!E35X$4BPZ`UIO8adT9Ie6jY%8F(giJ)iWFvK(YdEj@kxX z1sNMLPzuH0`pXSxuVoZ!9uj5hd~ozkW~`N4nj#KE7jl%o&Q4pO=k3OM5Uc8UoJ?pi z#ChS%*eqCFqcqS4joiUR{-dqoLT_Arq**am*3QUIFq?wlbt};Xy_Ma3-(H*$BesZ;d{^Ag4d4%TW=V}r|@81yiY+|1SBECO@HPyT5yvx zO7wna98cC8Xg`bo;SgYA5Da(k8ApB#TvG;-UxF+woxeLM{`o!NS$~d<4}oy-KziE& z^;EkjiquG`8aIVJbsL>0Rr!F-TkbJX`WKicfftNdW+aBKliI^1KuVr3n9S zKddjKo;0d^bW2(~VFB4BVP=;^%%(lji-hmSO0RrdG*D)Pq#Tx69$_}rZsG*L8S}{d z%s~Cr*3n^N8~D5{SDHenR5S(^NTC@hrogU z&p!Ge%(;Jo{XS* +#include +#include + #include "ros/ros.h" #include "std_msgs/String.h" #include "std_msgs/Bool.h" #include "std_msgs/Float32.h" #include +#include +#include +#include + #include - - -#include -#include -#include - -#include +#include +#include // used for passthrough filtering #include #include #include #include -#include -#include -#include +#include +#include // for plane segmentation +#include // for plane segmentation +#include +#include +#include // for color filtering +#include // to use the kdtree search in pcl +#include + + /* TBD: get to know typedef such as stringstream @@ -25,7 +35,9 @@ TBD: get to know typedef such as stringstream // define publisher ros::Publisher pub; -ros::Publisher pub_2; +ros::Publisher pub2; +ros::Publisher marker_pub; +ros::Publisher marker_pub2; ros::Publisher visual; float xbpos = 5.0; @@ -34,6 +46,21 @@ float ybpos = 5.0; float ybneg = -5.0; float zbpos = 5.0; float zbneg = 0.0; +float rotx = 0; +float roty = 45; +float rotz = 0; +float movex = 0.0; +float movey = 0.0; +float movez = 0.0; +float passx_min = 0.0; +float passx_max = 5.0; +float passy_min = -5.0; +float passy_max = 5.0; +float passz_min = 0.0; +float passz_max = 5.0; +float voxel_size = 0.02; +float Pseg_dist = 0.025; +int green_thresh = 150; typedef pcl::PointXYZRGB PointT; @@ -44,49 +71,224 @@ void callback(pcl_tutorial::configConfig &config, uint32_t level) ybpos = config.ybpos; ybneg = config.ybneg; zbpos = config.zbpos; - zbneg = config.zbneg; + zbneg = config.zbneg; + rotx = config.rotx; + roty = config.roty; + rotz = config.rotz; + movex = config.movex; + movey = config.movey; + movez = config.movez; + passx_min = config.passx_min; + passx_max = config.passx_max; + passy_min = config.passy_min; + passy_max = config.passy_max; + passz_min = config.passz_min; + passz_max = config.passz_max; + voxel_size = config.voxel_size; + Pseg_dist = config.Pseg_dist; + green_thresh = config.green_thresh; } void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){ - - // create pointer to Point Cloud in pcl + // create pointer to Point Clouds in pcl pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); + //pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); + + // publish unprocessed point cloud + sensor_msgs::PointCloud2 input; + input = *cloud_msg; + pub.publish(input); // convert ROS message data to point cloud in pcl pcl::fromROSMsg(*cloud_msg, *cloud); - - // Create filtering object + //Perform a translation and rotation on the object cloud using rqt: dynamic reconfigure + Eigen::Affine3f transform = Eigen::Affine3f::Identity(); + transform.translation() << movex, movey, movez; + transform.rotate(Eigen::AngleAxisf((rotx*M_PI) / 180, Eigen::Vector3f::UnitX())); + transform.rotate(Eigen::AngleAxisf((roty*M_PI) / 180, Eigen::Vector3f::UnitY())); + transform.rotate(Eigen::AngleAxisf((rotz*M_PI) / 180, Eigen::Vector3f::UnitZ())); + pcl::transformPointCloud(*cloud, *cloud, transform); + + // Create filtering objects for all 3 coordinates pcl::PassThrough passx; passx.setInputCloud (cloud); passx.setFilterFieldName ("x"); - passx.setFilterLimits (0.0, 5.0); + passx.setFilterLimits (passx_min, passx_max); passx.filter (*cloud); pcl::PassThrough passy; passy.setInputCloud (cloud); passy.setFilterFieldName ("y"); - passy.setFilterLimits (-5.0, 5.0); + passy.setFilterLimits (passy_min, passy_max); passy.filter (*cloud); pcl::PassThrough passz; passz.setInputCloud (cloud); passz.setFilterFieldName ("z"); - passz.setFilterLimits (0.0, 5.0); - passz.filter (*cloud); + passz.setFilterLimits (passz_min, passz_max); + passz.filter (*cloud); +/* // Visualizing the bound of the passthrough filter in RVIZ + visualization_msgs::Marker bounding_box; + bounding_box.id = 0; + bounding_box.type = visualization_msgs::Marker::CUBE; + bounding_box.color.g = 1.0; + bounding_box.color.a = 0.5; + bounding_box.header.frame_id = "zed_base_link"; + bounding_box.action = visualization_msgs::Marker::ADD; + bounding_box.pose.orientation.w = 1.0; + bounding_box.pose.position.x = (passx_max+passx_min)/2; + bounding_box.pose.position.y = (passy_max+passy_min)/2; + bounding_box.pose.position.z = (passz_max+passz_min)/2; + bounding_box.scale.x = abs(passx_max-passx_min); + bounding_box.scale.y = abs(passy_max-passy_min); + bounding_box.scale.z = abs(passz_max-passz_min); + marker_pub.publish(bounding_box); */ -/* pcl::VoxelGrid vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well + visualization_msgs::Marker line_list; + line_list.id = 1; + line_list.type = visualization_msgs::Marker::LINE_LIST; + line_list.scale.x = 0.05; + line_list.color.g = 1.0; + line_list.color.a = 1.0; + line_list.header.frame_id = "zed_base_link"; + line_list.action = visualization_msgs::Marker::ADD; + line_list.pose.orientation.w = 1.0; + geometry_msgs::Point p1; geometry_msgs::Point p2; geometry_msgs::Point p3; geometry_msgs::Point p4; + geometry_msgs::Point p5; geometry_msgs::Point p6; geometry_msgs::Point p7; geometry_msgs::Point p8; + p1.x = passx_min; p1.y = passy_min; p1.z = passz_min; + p2.x = passx_min; p2.y = passy_max; p2.z = passz_min; + p3.x = passx_max; p3.y = passy_min; p3.z = passz_min; + p4.x = passx_max; p4.y = passy_max; p4.z = passz_min; + p5.x = passx_min; p5.y = passy_min; p5.z = passz_max; + p6.x = passx_min; p6.y = passy_max; p6.z = passz_max; + p7.x = passx_max; p7.y = passy_min; p7.z = passz_max; + p8.x = passx_max; p8.y = passy_max; p8.z = passz_max; + line_list.points.push_back(p1); line_list.points.push_back(p2); + line_list.points.push_back(p1); line_list.points.push_back(p3); + line_list.points.push_back(p4); line_list.points.push_back(p2); + line_list.points.push_back(p4); line_list.points.push_back(p3); + line_list.points.push_back(p1); line_list.points.push_back(p5); + line_list.points.push_back(p2); line_list.points.push_back(p6); + line_list.points.push_back(p3); line_list.points.push_back(p7); + line_list.points.push_back(p4); line_list.points.push_back(p8); + line_list.points.push_back(p5); line_list.points.push_back(p6); + line_list.points.push_back(p5); line_list.points.push_back(p7); + line_list.points.push_back(p8); line_list.points.push_back(p6); + line_list.points.push_back(p8); line_list.points.push_back(p7); + marker_pub2.publish(line_list); + + pcl::VoxelGrid vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well vox.setInputCloud (cloud); // Set input cloud - vox.setLeafSize (0.02, 0.02, 0.02); // Set the size of the voxel - vox.filter (*cloud); */ + vox.setLeafSize (voxel_size, voxel_size, voxel_size); // Set the size of the voxel [m] + vox.filter (*cloud); + + // Planar segmentation + // coefficients: used to show the contents of inliers set together with estimated plane parameters + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + // Create the segmentation object + pcl::SACSegmentation seg; + // Optional + seg.setOptimizeCoefficients (true); + // Mandatory: set model and method type + // distance threshold defines how close a point has to be to the model to be considered an inlier + seg.setModelType (pcl::SACMODEL_PLANE); + seg.setMethodType (pcl::SAC_RANSAC); // ransac is a simple and robust method + seg.setDistanceThreshold (Pseg_dist); + seg.setInputCloud (cloud); + seg.segment (*inliers, *coefficients); + if (inliers->indices.size () == 0){ + PCL_ERROR ("Could not estimate a planar model for the given dataset."); + std::cout << "Could not estimate a planar model"; + //return (-1); + } + //Extraction step of the inliers + pcl::ExtractIndices extract; + extract.setInputCloud (cloud); + extract.setIndices (inliers); + extract.setNegative (true); + //extract.filter(*cloud); + pcl::PointCloud cloudF; + extract.filter(cloudF); + cloud->swap(cloudF); + + // color filtering + // Building the condition for points to remain in cloud + pcl::ConditionAnd::Ptr color_condition (new pcl::ConditionAnd ()); + pcl::PackedRGBComparison::Ptr green_condition(new pcl::PackedRGBComparison("g", pcl::ComparisonOps::GT, green_thresh)); + color_condition->addComparison(green_condition); + // Building the filter + pcl::ConditionalRemoval color_filter; + color_filter.setCondition (color_condition); + color_filter.setInputCloud(cloud); + // apply the filter + color_filter.filter(*cloud); + + + /* // Creating the KdTree object for the search method of the extraction + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance (0.2); // 2cm + ec.setMinClusterSize (50); + ec.setMaxClusterSize (25000); + ec.setSearchMethod (tree); + ec.setInputCloud (cloud); + ec.extract (cluster_indices); + ROS_INFO_STREAM(cluster_indices.size()<<" clusters extracted "); + + Eigen::Vector4f centroidL; + Eigen::Vector4f minL; + Eigen::Vector4f maxL; + Eigen::Vector4f centroidR; + Eigen::Vector4f minR; + Eigen::Vector4f maxR; + bool first_Lcentroid_detected = false; + bool first_Rcentroid_detected = false; + Eigen::Vector4f centroidCENT; + Eigen::Vector4f minCENT; + Eigen::Vector4f maxCENT; + bool first_centroid_detected = false; + + for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it){ + pcl::PointCloud::Ptr cloud_clusterL (new pcl::PointCloud); + for (const auto& idx : it->indices){ + cloud_clusterL->push_back ((*cloud)[idx]); //* + cloud_clusterL->width = cloud_clusterL->size (); + cloud_clusterL->height = 1; + cloud_clusterL->is_dense = true; + Eigen::Vector4f centroid; + Eigen::Vector4f min; + Eigen::Vector4f max; + pcl::compute3DCentroid (*cloud_clusterL, centroid); + pcl::getMinMax3D (*cloud_clusterL, min, max); + } + + if (first_centroid_detected){ + if (abs(centroid[1]) < abs(centroidCENT[1])){ + centroidCENT = centroid; + minCENT = min; + maxCENT = max; + } + } + else{ + centroidCENT = centroid; + minCENT = min; + maxCENT = max; + first_centroid_detected = true; + } + } */ // convert to ROS data type sensor_msgs::PointCloud2 output; pcl::toROSMsg(*cloud, output); - pub.publish(output); + pub2.publish(output); + } @@ -104,11 +306,13 @@ int main(int argc, char **argv){ server.setCallback(f); // create subscriber to receive point cloud - ros::Subscriber sub = n.subscribe("/zed/zed_node/point_cloud/cloud_registered", 1, cloud_cb); + ros::Subscriber sub = n.subscribe("/zed2i/zed_node/point_cloud/cloud_registered", 1, cloud_cb); // create publisher for output processed point cloud - pub = n.advertise("processed_cloud", 1); - pub_2 = n.advertise("unprocessed_cloud", 1); + pub = n.advertise("unprocessed_cloud", 1); + pub2 = n.advertise("processed_cloud", 1); + marker_pub = n.advertise("visualization_marker", 10); + marker_pub2 = n.advertise("visualization_marker2", 10); // create publisher for visualising marker visual = n.advertise ("marker", 1);