From fae39ceaa50a456ba4147fc132b52fd33263b0a4 Mon Sep 17 00:00:00 2001 From: cho-jang-hyun Date: Wed, 12 Jun 2024 16:51:17 +0900 Subject: [PATCH 1/5] add Translation and saving --- sliced_pretransformed_map.png | Bin 0 -> 20193 bytes src/pointcloud_to_2dmap.cpp | 93 ++++++++++++++++++++++++++++++---- 2 files changed, 82 insertions(+), 11 deletions(-) create mode 100644 sliced_pretransformed_map.png diff --git a/sliced_pretransformed_map.png b/sliced_pretransformed_map.png new file mode 100644 index 0000000000000000000000000000000000000000..c44bcf535155d6607cfd2329d21e025f7a002893 GIT binary patch literal 20193 zcmeHvdt6gjx;7VqDg@+W(1@TYSTslwBE$U2Fhl`G5f z_kG^yz3vr$Wyg@iF*9&8OiWBL>;DqA(Zs~!9`w%};DO1!ZH}Lrn9Mg>9~KgqZTdrx zbwOI+*|eT-In%In;{O_r-SWjkX>y_H@9b}WOkb`xcT{0sMz;G(_d8mvd?m%~_+xB0 zp6tUY^>sV;3l1JL0CJhyZfPjf6vy*H(2bqTy^U&knm z_Q1+gOEHe^?vBCdyjmBH{R61CX{ST#t(sP{YP}|cXX+7t#s|60%(+6cER$BmxtE&G(GTw03SBC*?9*5)T&W%S^TU4@ZMtxuCC z5x^}5a907&s+9vj5ASc%EQg+biP%>;9!cpuWl(Z0?S5KW+X4ufg=70d$f_&1oVt9a z&afE;>@Sb8KjUkb2@d#HEWncOf#K@lYO|N4vtYwxN+6o@e;!sF@o*;<$n*gm17>LM z@29!9dK$@ZRgS=Wp%vvLpop)tu_x?@YZALNcF%mJo9!VEVdZXInHyIbq8AT@O{~S_ zUej4=zZnXMX++jy2>l*nJIyr|_--Drv;_`=VJF~L`#OMUz7YnVSR8d8-D!XiL$X>S z{T4n7;ep~OK89J`JUpogiJO(X<8qm5?wDr^f^}zEpW`lS$%~oRo+;s;IkbZs{6P&t zS_gS8_o08xV7&HDW`E7d%DpGr_CfeM=-vdZ^1d4vU;!tquxT)mWx#Ww87CWM1@PbtrJ1|kLKI4+z^JBNYj{f?5j_RDhyw(DXjbZYWTLzeK9{Tn9$U;9Rjj@5N zV@jF6=7+&NX~dpl)m^-85&3##h@Hw=Z?%1HdRg#5N%Heyu~Tvy|AA=uQ-8eI5DBaAU5p8^TzEpvaKAAPQy8zRNFVBcvoT=fkq$2e#t17{Ncxs}Ar$ zJqV_V_n*_8ZA=gJ6VPzveX-iN^k5iDH_@;-b0R|C?D`@uma=!&a<>mq!X$D@aI878 zJ2R)Pc;IP6Ac5#7(9GAb4oViU@}tV>O}0M01G675MBTB6sO~za*(D%(oL<>|AwP)4 zJ$}FuO@^Q()9|4nn;+>-H=Uky1MItE#mSJjK`hrjTT2<-ceJ5jHtf87mpkk;$)JRc znT!>?sbwx3YJvt!f+9bB8CQYId-skwn67p%Gqtvk9=Tv|De=0m&w%Jnq2>1bXfH1u zu;16zB(E+#iMztlr8J9*p~i^#?XoVRtu8u%rbny%h=PAmN}9cBI||8al;VqCL301e zt(aWNN_i%EbF3fpWu!cRgrbX6C=0O<9@&_0ns=Z+uhonfU=ODRa5U@kz;6CZKdZsp z%|Qp7rm3Qkv<5Y0sbIVWAHAi77uw@w{?7d{ByFqN(VJ6E-&TyAPWe9aI^|j?TN5LG z0n-!UnA|`!c_OptdL8AhK-rN?1gnz70fHxyzE00$F{lDfB!#s^O(oy87c~17*~+ck1@swDV@8Z$M|89G z2&l=t(aDB0+&;ju?#V3{C3SCcyQr+e@@vEMPz9h;r%VT%?WyHOpYtip2aGJ-DIMJy ziiu%zdsY^atMyT!Tw0@&4rG+IlKim>DB?GXlYOCx2QF>2WR&?nju8gRjQM`smq;7j z{q?QzeTNKuU-^PZ`bgLlnlGy^eMT{d%6#@;y>e`ooSGnDXJ>Z3sS)l5{t|u)p?3i2 z%YEkm0B#D^Y@Zl&0jYf7<)*6xf|zohY~`^g4XLz^LZO7$q`Y*R59Fh+q>?G-@w=O5 zT(U9}eG!-Q$R_nv5XyD-B3@mg21h;k<{&LUmMXU~qJ%|cQ3eI3EqNkWRBVO6v_FKE zk7HzmAdEUyOVKN=STx3*nf&mhAsaELB&7*wq_f;Fm40eY{Y^qfI$@W4&JATh4{CGO z?6z1N1&u0?ahXZGAJ(ZDO@azNs9T%ch0VhsTMB)9K@K$)cV$2g7dF)AJJ%PxH^j$7 zo}h^9;7;=wiq=K)NHqL_EcT+GdcWdBGRO+;u>tMhGp|6dQo|cIHiVjw< zW=6L&Um@D)Riq4Zen4cNUs5G|o2$X6q|S`0;U4 zsn728>~_siYcWMB1=d;g-P15nN-YD-|0umx!czAi|4 zlVNG(^45VEn=avL+-^I|mv0z7pUyeS=MOl6uu4a_OBVh-{Am7_tC{3NK?~#SlAh>Gqn%!~qlGR|l1WvEu{foQz_eh(CaJ zL-QzFK?Om(xw)r6tZ?CFTBFZF1>SzoJnnV|Bk-U;P8Rc(ix*r;>`~C@s3(=|Ce8ZN zHoAJ(zL~SBM3fq2XQZ#7K}86~7_|9Iqu*k+;jTQ$ocPXZ0j5F>*$rvBvZrtG^8P5wX7ZS%Pb>+;uelzlzwRKs+ zaSOfl@5R6bHa~O=RvLG^MJ)@@rFz3EnS zbC9TT)IY;AKs+)))ZWO2HFKUJLT6j~01?w6y11gcYXoVZJ&h^3lS zlQK5U&NnLNZ}}2k_;yd1%&fWE8E%*R*W%z5=w$&ks03dYWu0^CDOJoGs7C2V3D9yB z_CA6qCSqWJg)1@}$t8CK<(a}IA-;E-K`fShshbq)9{EphGADM{@I*Yy{BFZSvRKsI z6V>tg*VpEQVGX>=fwu-f^(?Y;jVt3`>YPjcAA#X+g|39wt&&f6E35se@6|O!!+RH# zKZz=M?C%D`>+1p>hfkr131FPeec}%XF=RrrZ_S)~%iR_9uRk%8_o%s{*beLSwBhRV zmG7h96%~v`~2h@)pOoyN)i#c17HSk#&A1d zT60zTk&sU+Is=-KftL}oc}ep!%NetyuNyzEs8Ll6(oFX~Y;E-t9p&MSB~_lXJi|{R z|LkOSH2)Qk^eLT*Pum%yyp6F$CurD5eKi@0$^Bt3wP8T8<|FbxqO`%zXt7RoRVn7} z@gtV};uIQnJN16(PMtB)+tocQDIVVwq%o{p4RQl$5_v2N<93E(UcQ~X;#OR>-pZ7= z;^4jffwh=-Z~B)wP%S8|qu?!ujzrzWW+G^wpnjDET@XYw%UT;r^Nl!lw=UaLS4&w_ za*-V68`F6Wg`b>WHhCi3$FOrJHy9jt>fevL6wI?V#*vQol zDjSU#YeDvmc5w3mPd~`*2pspYl>tPSl|d&;ICzKb=k?9ZZ7I$rt%CqcCB^XVL={wU zAP$O*bb^Aj9<_&H9|4W2$bCfoaX=sh7!;r5(2NCLaqKRGkZ^uJhg>LlKrE=hQA)RbtfMZ34gv932}~KOaT{FultPn z0>=Id0AnzYbKyG{$fkHZgMIOKxuW996!;ni29eu5ybEymJ5rpD1aQC9lOhFIX(&NP zEHCq1f!r}Ia@^zFPDjOa*cjBVt&uwowxo#R3m%BHDbyGoxXkkkG+n|sSpOT+{}+M( zPm8W=8SeUbN&KJ2qEE2@rt2dx52SDo{QL3o|Ep=>78+!qaq>u#1LcNnOZjtY@c-0I z?fp!+N6_3sO@&+zHzOn;Yg72(eOmV!>VlxKaM;i*eBWa_Qs5%B+c3FjY@=_BAG8e5 z)orG^rs@xZ>WUr&rEm>iCj_bgP>F=gBA91D=}ytzc*Erb=`}FXx?TfHw^Vnb3}eM2 zJrKq>wMx*jQ8hZy_p;!sYKy6+=r1)vAqx554FZ(*G< zs~@tnec{s%>J&)*Qx7Gaf%hUL9QgRjQfJshKK4Ga!Q1{LK)Fr%>)`0=<^Q*N>kiv_ zJe389I<9jq+JY=0z4^m_e?=$uLj)H>k6f9e5dP3WgQka@3$c!RA$r_pw(1cGrE zr1C3yauPQYjmiDHAr-(74dxnW6-k#6#x{UolYHRc4`HR+2HeEi;*&UNz|cunA0&@~ z0SQe)bBrR!Trs+e$VxRl9MCpewiG9?io#6x8Px7h2lvj&&G{6=BkXRzGbcHo73i<9 zSL9jfQ}iUb_qO6VTc2aV9|i>3dQR$(XlSN_p2pea>%D6fw5DhSMT)f;@InY~@9Raw z{_)+J3_`!&H9q9KF$8K7g+P>=fS1MrYPo@C;jnYboFJ!Ede6wf+EcFEWK7i9SaCyp zpa(%2Og1MfOsc?-tvb(;uNUBk{GH;e(?xu{l8cOPLu_CL=~q``6pENQbE;pAxzyee zLInfenfJaOS-CPQs+>CLB#xl6jmQ|v47{a?uUQs2KzusvEZDol2#2{-YC<-JII@2`mp?+NDk#ceJeX^gH*Nheq9^I1e3@L17z4RLV`c8h6x3rF^1G6F+;Vr~&Bkr+W1`Da>Fmcbiz9=LSzD=(T$uE?|v zogQe>tB;J0RHjO(>NcVJ2}sXBjR`Z0lxc|^XsI8WnKnAE1h~H>)whxAtcE&?TCXiued?I-xCLF z=8#h_2}Dl_7HgjJ2L|8X$M-8Ywbu7BR#2U=zmRK?i~N{#l}7S?oMM!$z+7tYuBP zf%<0KgAeD3dXka`1^qR(;@w9DdvW^M?>Lkx`e1cbxqUh#og5W%w`OF(b%!C_xFdYN zC`c-sP5s$x-iGY@qtwHCTR!F&G?t$u6Pb(|Y4hG3?9Mc{^M(9A|%dl3VtfP}S$cjGJCjWH>8t$5&99XGmO* zDE739Cc;+IXSZNgE{)V3& zSIye!2k;DaObcy>a9vcaHOlctVpW0%?GtQ_HG>{yX;*1lNjF8Evoj(a_>M!;T+Q+p zT6EMi21PxEC=K5@V3QL%7^X~ul^xTB+3mtSo2X~dTYWXPETBUrlvb1S5*3Zuj%>Z1 z$s!BcMsJ#}HDgej%TS-v3rsBJD>;d-c&=DUx9#Sbc5u!0K~|+<*oxkcQa53-vJh`> zA*s8!CUh^i)RR~G&U8+jDlx`dHB?Q~q}X23E81el@lg%=Fh|pET^aINvW+s)H|omC>XSFd*`I-T^Obhux}jZ8B}*xS{UTtHh6rB?M6ud)MhoAX<|_N$5NhA zZkQPiz2Aa0fK+c|=IMRvfH@ziD=!&z3uHh8zVrL zySqk)odjtonbo0b#aj6xr`#wUABK8{Zi(6vSjT=0>SNVsP1v^Xu)UB~+R0{VvuxMl zOO}&%j1cb|hjCbD)Z4T0jJUJzN8Eu1bx23{0z`>&vQYY!tZ9?|=a7)cB5AO^r!O7^ z%~LvJV9;N&lYUlxI_+H>IsH;Z%LQ2vcyWO(!KBYk{mH7a=Lg2Db}x`C?ENAO)%?aD zWMk(yE`~4KmPGBxSOSMxYKd5@&S08!(6EnYVswDk;O}z_ON$c_4{3W`Vp_0Fnyoud zXR@2zca9UDIrFxTz6l|=V+IyzfJ;5F{-%qz-vx+36|7<^%ou|OBw0vUA;OjBEGe%* zI@d@>J+`AeHJ?v8nDR* zS+@-y2rC3U>0%gTK<59O%?t;qeR}`tgSuw(8!ZKiodbBNU0?LT0Y{3BK8HsA{<$kd zz04325VT@GJP9!H&~YRVPCPdFrLhO~SyK5ry@8)67DBASk@kLvrQX^f7#M??L7b8N z4SOM7aPa=HLEEj|uh&+!L$7UfqZrlp4(t|0fyYYyW+P}un_qzj>bEY%fTBe29D@Pu z;By)d>Z8w%_d(#0v{d;u|ZTn-@Y+Gn^PR1bjTv=(?G4_`yH2XU=Y)DO4`yJ8t zSrQuLc7T|#NFK24XZ=XPk0y=nlGp|8-}xyAdn9#qa_s876l z8m^5xd*MPj6<5K{Qags)d1_CcFNNmgDFWuVocu2EH#atTqQ%EJ(%3HyCHz&GG_tM~ zw}ypA>G=W~Y3q)p?kldws21sJ+2>G~Mhk2!ydNp)t(Y9oCLIFB!QCQ zpsVW0XI=R)B4-C-v99B6ZtSViwXh_1GCUrppdM_xGX!R1t3QGwsZ%Idy^dA|sC+1q zUh1XH(Fnb_t3lb(S-2BqwSL!ax?VY`2s%2d9VQ#GlWDJAdA%6*yB&H9DUD*(!YB~$ zI`^*tH2{s3j~@S-&DnJ+rmw}Ww(ZdT{S{T}LXrkEW}j*+dg$9Hx^y*7mNM&%*(Uq8 zNY4}O^+43mR6~DC(iGm^*3tb6&qzl9D=zJ7 zk^0DcoL{vD>HRx5Xg7H0Tz{?Li6rkFLs}rkzfyMt;AKRwF$tw?eHwl8u)0E!U;z5y~$Y1>lykj zB;k5!Y=sXZ*C{~iaw{mnkm+cJN6to$8+a8qdIIGK(j&p@F58K#fEmRiXsVo<9KFTqwxlNkxJo@7QW^r{A4kA>@Mq{yZ)f|ZP}Zt_$w zPg61-?5dozVmX)Msafi){KD*YL<^?xX7zrv$WB|HrbaT?Z_`=P7h59}BrTt48tG{%=3moezP1+o==hM{d&L&E3 z@CyT6E8W;ujy|EDL5od0(tWWVv?C;;)P+R_@H)AP3cgsoHjjN5I+?fR?Li?48&9-P zmW6blNjS%d)s~w$wn1mrO^iYAX(6%CJ&2tky1ry090lX*X+t}bC^J>KZ`vr)GJ}fpTCnL?{VpNQ2;JQu}sj> zQARX>7t(rMnB8eJ+hii?=1SuD+iRl`pS$8+!8K(&!@e%e8*g*?dLfE?mr5jcN*;6% z%1>l%=TSSs^^T6f)}rtec&^R4(rEgfc&2aP!*ac6cpvjqO{s+%Cx-eTXME|iN_8Qk zUf14v2i#_u0X-q!DwEYB6O;#&tc?Yin!v^4{GjrJ?N=X`8vuY?=6b9;0r)ANIKDxT zShNA$ET^_wU?0dsIz2g)zR74Ls;`Q+ClHIMS+=VA+y3Fw$rBeYaYaeU_KP6eE0ooj zu(=dYb_fXAMAymy)cb4OH5_RL2>Uaeq6Qz*G4*IVRmNJEfxD3*YYy;5biJn?dV)nP zv4|(!O<7K}RXTFBOo<0(q7=zpfUC_IYJwGWW?i>SXS0Yp-!-z+21>xf5Q0?B+htT%`;3I*b|KYKy)H@}?fJ_h zNBa+PmCrV*ofBLYLaCeFVbaTS%W-z;uj~qfoznIEt*wz)5~UTJ@(JzW4;el?wFA7` zm1~-?vkyd0oG=-CV1K@J-t{oak7+?0X!j%X#-F_bAOGe$i%WRs(XHKoi0zH6M{-{{ z*r|dSM($ki>He-Dfd&4$+k=LkoI_6mcY%9W4L&@5=zx!@s!O+Hu`GQ0*fuj+ZaDYL zXm>$lu^Fl3u`byI5hqy z-5BEZv6EE)H-4)k-C0=|xpOUm2mv^64Om(+-_ib4N7d;=U!5ehn?fu=s;^A*tc^yu z`NURc2kBQNc;z@iKvr6h#rFOXA#;Z}F1oLHDAvn3R`0p_jq^Uf9QfQZ6J`r4 z!16*aPIe&Z_=ed(ydK>RCT+S-jlagD#+jpxE--p0YE zlouJHx_SuyvTC}_NMLF{uyTbbV)A@+XJMjr;ursJU-@*HwEYecgQ>kRou{Tj z+)jDqpicBF5XffC{O+BR2=2=6t@8vG27I{7Zgok#1W+qPdaf zQ>(pTX;8aL{%nlcx1Iwjdo{Hh8`{7OwpBFi^?aHvProX8^H6-k|$QPa}5 zAh}9TRm)vs!0g3A3q|D)A@vNuTSWZ{+|z%9r1$>gEmCiY8Olw+2(EGk)4*gpXU{NW16(SNW%Ku%V1P(1PB@ z==p`d;f=n5`uL8nKCXuX+aQ8<4`=iZMDI5a=S!iIxN17V?NdvGFOY9+zOGuMTh;p@?-g1ImG&y8b{mgObl1DPcf0XfXfei>~!l zLapZ;oVBi?LA_@S(h~N=jp$$e0{RuyQ%>o658OC_Ni*obcKfWo`Z9Z{!NMztE`>og0aEv=gUUgVcLO&9YND_q|K0&K1vd3=b(dY#wZ*f))pxPO z5-E=!G7r${5x1TooCL!Gl0ndMZSIdf*6*1YpK$5sg%*@7vL)SK87$q= zXM_08Wapue`QL5ffA@lcz29a%x>5dr2hfAh+=jzWzZ_ z?YQ`-ETZ+ZyB68a>Ep#JyBlqKQLT@{b%NfDKR=q2ly{O*B}UJ>+(9*-OCSC}mrZ;I z@g=An(wTAAp-*9*E_c#~e?(Rfu0B8dJ|d*nEg}2!W!(|P8}$APd}k7j{F8Lu&XYvz zT`-T`Y(3}DReMl5o+WxV9OCRoL; zTlfG(MPd4-W?02s{~H$aWN!!whW}lGstwj+DSt0ytZQ}cxQ}($T6iT7P?FQItQ+3n z>!Yy91|sScG)0$9fK+FH>#i7NUq!}0*=NN9R9Npby0{nWM3tv|f>!0NH_V_k_lT$7ED zPLE4`jW#96G@sr}QW2@c9!N1#BtH(B1WVrVCSER`oR#XtjS^jnXeG9c4lJfTN zxbn%*dM`mtH3Vk~vsG-=|@jpQ|M!@C;cyjhl{`4C#_V;-mko}9qK5&Y~6yw-a! z6oo@2a4`OF{O8K)>t@Mt>ahC9>@|sSYOLbpM>}Bn`~AT(h=jQFtmyOICt+L<2JoXF z!>cd+ty#^m1#9H!w*Im4Ihu{InCjXJ*1(uO84*u$Mjl(_#3QShb@E2Vh**Wi6%gQ{ zEYw!?ci)Ad>6XwP&(%87YoMTWtE8N_Gaq(2C;2;(>f2N5IwM4I;`E|F#19f7h*FM-FR!i&pSo1c9cu literal 0 HcmV?d00001 diff --git a/src/pointcloud_to_2dmap.cpp b/src/pointcloud_to_2dmap.cpp index 9d4b754..c77d23b 100644 --- a/src/pointcloud_to_2dmap.cpp +++ b/src/pointcloud_to_2dmap.cpp @@ -8,6 +8,9 @@ #include #include +#include +#include + class MapGenerater { public: @@ -30,8 +33,10 @@ class MapGenerater { continue; } - int x = point.x * m2pix + map_width / 2; - int y = -point.y * m2pix + map_width / 2; + // 2D 맵에서 3D 맵의 시작점 설정 + // 현재는 (1500 x 1500) 이미지의 (1125, 1125) (3/4 지점)를 PCD 데이터의 (0,0,0)을 맞춘다 + int x = point.x * m2pix + map_width / 4 * 3; + int y = -point.y * m2pix + map_width / 4 * 3; if(x < 0 || x >= map_width || y < 0 || y >= map_height) { continue; @@ -46,6 +51,35 @@ class MapGenerater { return map; } + pcl::PointCloud::Ptr transformPointCloud(const pcl::PointCloud::Ptr& cloud, float theta[], const Eigen::Vector3f& pivot) { + // 회전 변환 행렬 생성 + Eigen::Affine3f transform = Eigen::Affine3f::Identity(); + + // 입력 받은 변환 각도 + float theta_x, theta_y, theta_z; + + theta_x = M_PI / 360 * theta[0]; + theta_y = M_PI / 360 * theta[1]; + theta_z = M_PI / 360 * theta[2]; + + // 기준점으로 평행 이동 + transform.translation() = -pivot; + pcl::PointCloud::Ptr shifted_cloud(new pcl::PointCloud); + pcl::transformPointCloud(*cloud, *shifted_cloud, transform); + + // 회전 변환 적용 + transform.translation() = Eigen::Vector3f::Zero(); + transform.rotate(Eigen::AngleAxisf(theta_x, Eigen::Vector3f::UnitX())); + transform.rotate(Eigen::AngleAxisf(theta_y, Eigen::Vector3f::UnitY())); + transform.rotate(Eigen::AngleAxisf(theta_z, Eigen::Vector3f::UnitZ())); + pcl::PointCloud::Ptr rotated_cloud(new pcl::PointCloud); + pcl::transformPointCloud(*shifted_cloud, *rotated_cloud, transform); + pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud); + pcl::transformPointCloud(*rotated_cloud, *transformed_cloud, transform); + + return transformed_cloud; +} + public: const double resolution; // meters per pixel const double m2pix; // inverse resolution (pix/m) @@ -65,12 +99,12 @@ int main(int argc, char** argv) { description.add_options() ("help", "Produce help message") ("resolution,r", po::value()->default_value(0.1), "Pixel resolution (meters / pix)") - ("map_width,w", po::value()->default_value(1024), "Map width [pix]") - ("map_height,h", po::value()->default_value(1024), "Map height [pix]") + ("map_width,w", po::value()->default_value(1500), "Map width [pix]") // default: 1024 + ("map_height,h", po::value()->default_value(1500), "Map height [pix]") // default: 1024 ("min_points_in_pix", po::value()->default_value(2), "Min points in a occupied pix") ("max_points_in_pix", po::value()->default_value(5), "Max points in a pix for saturation") - ("min_height", po::value()->default_value(0.5), "Min height of clipping range") - ("max_height", po::value()->default_value(1.0), "Max height of clipping range") + ("min_height", po::value()->default_value(1), "Min height of clipping range") // default: 0.5 + ("max_height", po::value()->default_value(2), "Max height of clipping range") // default: 1.0 ("input_pcd", po::value(), "Input PCD file") ("dest_directory", po::value(), "Destination directory") ; @@ -100,23 +134,60 @@ int main(int argc, char** argv) { return 1; } - MapGenerater generater(vm); - cv::Mat map = generater.generate(*cloud); + // MapGenerater 인스턴스 생성 + MapGenerater transformation_map_generater(vm); + MapGenerater map2d_generater(vm); + cv::Mat map; + + // 모드 설정 + bool transformation_mode = true; // true: pcd 변환 및 별도 저장과 2d map 저장, false: 2d map 저장 + + /* + 아래 if 문은 pcd 파일을 translation 하고 다른 파일로 저장하는 부분 + */ + if (transformation_mode){ + + // rotation(회전) 각도 설정: (x,y,z 순으로 degree 단위) + float theta[3] = {5, -20, 15.5}; // 임의로 찾은 최적의 값 (5, -20, 15.75도) + + // translation(평행 이동) 설정: x는 위아래, y는 좌우 + // 맵을 만드려고 하는 pointcloud의 가장 왼쪽 아래 영역으로 + Eigen::Vector3f pivot(2.6, -7.2, 0); + + // 변환 함수 + pcl::PointCloud::Ptr transformed_cloud = transformation_map_generater.transformPointCloud(cloud, theta, pivot); + + // transform 결과를 별도의 pcd 저장, 필요하지 않은 경우 주석 처리 + std::string output_filename = "transformed2_9F.pcd"; // 출력 PCD 파일 경로 + std::cout << "PointCloud transform start!" << std::endl; + pcl::io::savePCDFileASCII(output_filename, *transformed_cloud); // 별도의 PCD 파일 저장 + std::cout << "transformed PointCloud saved!" << std::endl; + + //cv::Mat + map = map2d_generater.generate(*transformed_cloud); + } + + else{ + //cv::Mat + map = map2d_generater.generate(*cloud); + } std::string destination = vm["dest_directory"].as(); if(!boost::filesystem::exists(destination)) { boost::filesystem::create_directories(destination); } - cv::imwrite(destination + "/map.png", map); + // 파일 이름 지정 + cv::imwrite(destination + "/sliced_pretransformed_map.png", map); std::ofstream ofs(destination + "/map.yaml"); ofs << "image: map.png" << std::endl; - ofs << "resolution: " << generater.resolution << std::endl; - ofs << "origin: [" << -generater.resolution * generater.map_width / 2 << ", " << -generater.resolution * generater.map_height / 2 << ", 0.0]" << std::endl; + ofs << "resolution: " << map2d_generater.resolution << std::endl; + ofs << "origin: [" << -map2d_generater.resolution * map2d_generater.map_width / 2 << ", " << -map2d_generater.resolution * map2d_generater.map_height / 2 << ", 0.0]" << std::endl; ofs << "occupied_thresh: 0.5" << std::endl; ofs << "free_thresh: 0.2" << std::endl; ofs << "negate: 0" << std::endl; + std::cout << "map image saved!" << std::endl; return 0; } \ No newline at end of file From 424c981ddbdada3cfd2acf59479f229514d54b35 Mon Sep 17 00:00:00 2001 From: Jang Hyun Cho <69357083+cho-jang-hyun@users.noreply.github.com> Date: Tue, 5 Nov 2024 16:30:49 +0900 Subject: [PATCH 2/5] Update README.md --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 8b32772..5380e8e 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,6 @@ -# pointcloud_to_2dmap +# Pointcloud_Transformation_and_Convert_2dmap + +this repository is from kenji koide ```bash # Usage From a461c07c5d73867d7466c21f01ec87ef3931215f Mon Sep 17 00:00:00 2001 From: Jang Hyun Cho <69357083+cho-jang-hyun@users.noreply.github.com> Date: Tue, 5 Nov 2024 16:55:29 +0900 Subject: [PATCH 3/5] Update README.md --- README.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 5380e8e..415e341 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,13 @@ # Pointcloud_Transformation_and_Convert_2dmap -this repository is from kenji koide +reference: https://github.com/koide3/pointcloud_to_2dmap + +## Requirement +- Eigen +- OpenCV +- PCL + + ```bash # Usage From 750cc82efb871d3b886960b1c7f9c61333993e71 Mon Sep 17 00:00:00 2001 From: Jang Hyun Cho <69357083+cho-jang-hyun@users.noreply.github.com> Date: Tue, 5 Nov 2024 16:57:49 +0900 Subject: [PATCH 4/5] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 415e341..7f66235 100644 --- a/README.md +++ b/README.md @@ -11,6 +11,8 @@ reference: https://github.com/koide3/pointcloud_to_2dmap ```bash # Usage +make +./pointcloud_to_2dmap