From c1ac1388908abe93a0e5797dbbfcbe9a7b3ec4de Mon Sep 17 00:00:00 2001 From: gfuse Date: Thu, 26 Aug 2021 20:27:01 +0200 Subject: [PATCH] calibrate orintation --- GPU_version/vio/include/vio/depth_filter.h | 4 +- GPU_version/vio/include/vio/matcher.h | 2 +- GPU_version/vio/include/vio/ukf.h | 51 ++++++++----------- GPU_version/vio/out.txt | Bin 5883 -> 119709 bytes GPU_version/vio/param/px30.yaml | 10 ++-- GPU_version/vio/param/vo_fast.yaml | 8 +-- GPU_version/vio/src/depth_filter.cpp | 37 ++++---------- GPU_version/vio/src/frame_handler_mono.cpp | 10 ++-- GPU_version/vio/src/matcher.cpp | 2 +- GPU_version/vio/src/sparse_img_align_gpu.cpp | 2 + GPU_version/vio/src/visualizer.cpp | 6 +-- GPU_version/vio/src/vo_node.cpp | 6 +-- 12 files changed, 56 insertions(+), 82 deletions(-) diff --git a/GPU_version/vio/include/vio/depth_filter.h b/GPU_version/vio/include/vio/depth_filter.h index 04190a9..8c9eee2 100644 --- a/GPU_version/vio/include/vio/depth_filter.h +++ b/GPU_version/vio/include/vio/depth_filter.h @@ -123,7 +123,7 @@ public: std::list>& getSeeds() { return seeds_; } /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty - static void updateSeed( + static bool updateSeed( const float x, const float tau2, Seed* seed); @@ -150,7 +150,7 @@ protected: double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds. double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds. Matcher matcher_; - uint seeds_cont_=0; + //uint seeds_cont_=0; /// Initialize new seeds from a frame. void initializeSeeds(FramePtr frame); diff --git a/GPU_version/vio/include/vio/matcher.h b/GPU_version/vio/include/vio/matcher.h index c7c1c6e..0a79204 100644 --- a/GPU_version/vio/include/vio/matcher.h +++ b/GPU_version/vio/include/vio/matcher.h @@ -87,7 +87,7 @@ public: max_epi_search_steps(1000), subpix_refinement(true), epi_search_edgelet_filtering(true), - epi_search_edgelet_max_angle(0.7) + epi_search_edgelet_max_angle(0.3) {} } options_; diff --git a/GPU_version/vio/include/vio/ukf.h b/GPU_version/vio/include/vio/ukf.h index 066e25c..8cd09ec 100644 --- a/GPU_version/vio/include/vio/ukf.h +++ b/GPU_version/vio/include/vio/ukf.h @@ -18,22 +18,16 @@ public: virtual ~Base(){ }; void predict(double dx,double dz,double dpitch,double time){ - if(t_==0.0){ - t_=time; - return; - } - double dt=time-t_; - t_=time; - state_h_(2)=state_(2)+dt*dpitch; - state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2)); - state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2)); + state_h_(2)=state_(2)+dt_angular*dpitch; + state_h_(1)=state_(1)+dt_liner*(dz*cos(state_(2))-dx*sin(state_(2))); + state_h_(0)=state_(0)+dt_liner*(dx*cos(state_(2))+dz*sin(state_(2))); Eigen::Matrix R; R< G; - G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)), - 0,1.0,-dz*sin(state_(2))-dx*cos(state_(2)), + G<<1.0,0,dt_liner*(-dx*sin(state_(2))+dz*cos(state_(2))), + 0,1.0,dt_liner*(-dz*sin(state_(2))-dx*cos(state_(2))), 0,0,1.0; cov_h_=G*cov_*G.transpose()+R; predict_up=true; @@ -43,33 +37,30 @@ public: Eigen::Matrix Q; Eigen::Matrix E; Eigen::Matrix k; - double dt=time-t_; - t_=time; - H<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))), - 0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))), + H<<1.0,0,pow(0.03*dt_liner,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))), + 0,1.0,-pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))), 0,0,1.0; Q< R; R< G; - G<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))), - 0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))), + G<<1.0,0,pow(0.03*dt_liner,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))), + 0,1.0,-pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))), 0,0,1.0; cov_h_=G*cov_*G.transpose()+R; - yaw_t_1_=state_(2); } void correct(double x, double z,double pitch,size_t match,double time){ @@ -77,7 +68,6 @@ public: Eigen::Matrix Q; Eigen::Matrix E; Eigen::Matrix k; - t_=time; ++match; H<<1.0,0,0, 0,1.0,0, @@ -89,17 +79,16 @@ public: k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); state_=state_h_+(k*E); cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_; - yaw_t_1_=state_(2); predict_up=false; } Eigen::Matrix cov_; Eigen::Matrix state_; bool predict_up=false; private: - double t_=0.0; Eigen::Matrix cov_h_; Eigen::Matrix state_h_;//state{x,z,pitch} - double yaw_t_1_=0.0; + double dt_liner=0.2; + double dt_angular=0.2; }; @@ -128,16 +117,16 @@ public: while(lock)usleep(5); lock=true; pitch=pitch+M_PI; - if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec()); + if(filter_->predict_up)filter_->correct(-z,-x,pitch,match,1e-9*time.toNSec()); lock=false; double pitch_f; pitch_f=filter_->state_(2)+M_PI; - return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f)); }; std::pair,vio::SE2_5> get_location(){ double pitch_f; pitch_f=filter_->state_(2)+M_PI; - return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f)); } private: Base* filter_= nullptr; diff --git a/GPU_version/vio/out.txt b/GPU_version/vio/out.txt index ab3b540fa51ae81430eba7a1f2fcb09d85adb652..72bd21ab33034c08024eeafa88184b2d30ba7b7e 100644 GIT binary patch literal 119709 zcmeI5+j1PqnV5a&7`}pT%6F1&HtT$#tqTgGvj^~%251t!19U^(4RS_L z@9(cuXBINE5~x~$u>nPBR|Jring98mzxye?`YCwx-Cy4QJ@_fgag0q3_h>7FV0S{mS5KAhl~Fid_KLrS-lMMH2fW3dnW(K z{K+y&$~X*NpS)PDKV2O9Z>HtwH>)z^+^%n4ZO>Pi{U=}j@ansfb5>ay=9%-Gcf5KT zhEU?Bf9w&go$btLM+ZUg9>+|KF@ASt>Mwq3{i!6t`O(2sm%F&ir z>&^0fy@%b`ha>pL_zvBC>%RP&w@_KW*UhVe% zm&e3W6(&VoR=Eut*W3MSdwp16Ee{vF)&3~xQa9eJD2>xNvmSVV*xj5RZg%a*uTFnm zULTIf&PL!-C1nwog;A6UJTA9CpDs5ySIafBx(CMCN7D?(kBrPDAN}Ryes#GztEmIx zI11=+5k@26D59juLm~fLibOsw_unkppg(IocGxP6iXyfG_#Zd0*WtICl0PbtjYhwl z$GL;j`tNe$^WFA(seZh2eLP+u{t{$sZaws?lau9UyWStUIb65|i+nN-qpCFPK7s6C z|LxtaPlir()gR4^FeyZrFx|M9y#%aUCA{a?R*_b2TS zAQp6cTBfD)SFiu|^~sMvaOCC5tM6a^>CLxqet7fxgTB-2fBoV0cXBO$es;P1024_3b( zR-5zH`S6d{XXoADUvAHw-&22Y`Wz=&T`teIS68Q-^W{HoRyV8V$J47d>Z3kW^ZWhH z&!2YNn``AqyVXB#);rGD{jOHOa;(9U<^E#3J9OvWU$1w^KRP`-LzM1-f%3!i-TL!t zhu%H?wAt>#b4xbk3YZM zZa#H~USBS+ciT_c{Kqd`t*(|Z+Zo$($R+>W?tbeQbe~!FAV2(e-2hbRrT_J2yIJ0B z*22}x&)Y*!GW{PwESK9O@LDt=$&Ty9Ho3qVS)bk7eSdND@#E#~ueLv5?pBBtfPHJ> z+2ze1jR|)7@6B2N2OqEVboj>#O$c&7FV9z>*MMceK8LzLpDnMpB7T-DP+37lbGA?YJ=LG_d8`NJmdN4F>2CSk&FAAz*R>(1|IBgkJAml){PXGN zOiphCqfir|%u}Nb$G`Q-Ji{)D8uS9T8Hvk2{RnF z*Xy(6w@=VB-{I2Z0IhxlKc5^sWQ#0`%c!g>>%rGoH^Uu1tTy}YuHJ*+x<8-dRT45W z`c(A=FmDDIIO9j3SC{VZ7HJU|NhlWi`_~^{|KRxRd=cV(g=G{A7~kN;;c3?9`szIR zxZdp#!AFVRES%rKq{frSqwc?P{cCWFRuUP}JA}b{{}MxmI8l)X|My~jN#Lgp1Dr-6 zbrp0PauEdo=i48G*4YcNCY*L>zDSBP!Ip}}^KNcn-5mmtk}95a_Z*iBTRNAp$H?x* zLafjx_3Gx!hY#}yw=pJBG&K!mLqAKrs zW?ZT&O`@!*Bbox8u`_4gPE*cYm5U+{os8lDbOLE6SrS%x#}#9TE@nA2-fBJp(!|n| ztjrQ$>gaJg3DV+X$M=VHQWC%}s$Tw#LAqz2iHDA1qYi0ze!R9hP8{4^7Vl z>0*JUcJDLiVVS2%mHP_3dCm-V^dGu2n6V159+TQQeK;^<%|RUZhI3J-MV7|3;rMWe zMw=2jz4xKZvLNe1IP*aXI){<6`%-jThjfb+`gq_&v*UefnnRa)R`G$((jAj`{Z5nQ zUA`#N=)UBU!oz8TI}|@Gc@%g#MSf43P8TtdMo`yzKOaV%$M7f?DenKh4~;rZaArea znx4ffjWDmX=>CjIMpJy(%=gf7u+pQFGoT5A(>^T0|I>(y7yEaeqagiB3$C2uHy4)V;|t?a;}hO75u=vjr-V3Y0kYC69TeM_Mh?dwW~i zf*fxirToVe(DO)-j6=I!m*D+sO%P_Z{)x|yBATF7HM92MAYMgv$`V!cip`NemNiQ!Z9xc>)lgo1NlQ2 zi`;d0VlrVuHI2vJGh_lwy$JqBPJF!&>h!0iP#23PdG+S7 z2*PiI4>uH1?DyXUo7ESp8P;*|#~@l1VNoQj{}JZj1eBCX)zBXUk>6n)Ka+)#Wnoej z_?X%vBxl|%ioXf|a=O0cg8FXoH3eGV1h02H3OUvrslC{*&KJQ4vVB2aH@N;MM_<%i z+s=$pnl1+CB@vW3&zlk}S#U1plKAvxe_kVW&p1P4j`Z`>2uKtoNf~E8Nyf&nWXR;`L&ij5vb<4j~(I~|eZ%?m-&G%bM2PoUh zif=Y2tINxmLTH<}r-!qP6**7-A%%}pmk|8CI(%8JHo@V`w#lsWdvsrw$B`4;Bn6VZ z7*J@A#a*<-W~0zJt%$xVz`(aii1Ims5oc+=Xc~p0h9d!Kj|XrUlMAc2=DbGc0C{AK zeDL66=prp)YGv!Fkn~lzvysg6G9>1!u7SU*)J--e71}~;LE@EhyqN!RA(gA+&~89wD0w)5QlEF5F4kg@y+a#8u|RU78Y|P>-7~qzk2V9ycu_ zmW*T0b=Vww7IN)roLpxaqZnJQVWe>!^!$-z4Opf5qdA^Uvn+D*0Jmr+p}V1HT_Xcg zjzl$v0t*y3-f^QRY;re_l0=ltG&H0XVQBbA_e5Eb1>Ib zNxYzNLOU)l1OX5?c!@({AS&P$U07vq@S=qazw^#7pNLdOt;DdBS|lYIH5dn z;{!k!!U}W@=Fs8JVR=_|8wDSi*VL2o@`WLQ~0Y2^xV0w}lrKq()SUgmMWfkL-|V zrfQUwCUheLAiY2`FG0c&_6f7Bm12C%vM3?(tL-Fhwe{IJvyreO{6arT;$#F0AT%wD zjgXdZP@;=UO(Rf-Rc7rX8(ooDsx`tyKA@^jJ1+KT8s=_KhfYphP6fK~LD5=EHqn$( zH>=4r)?i!I7(o{qu}G3ZvsbW`)7JI`@kS(OIE{Yp-|XC0kqe^FuD+3#+eo~Gx7~2+ zsS>yGM(Y&TumW=7Hu{=7ZZiTSLfg+E+G%*yu#LSq?EDxop*K$B@9NO^b>h2whzK27)$47sUzP z0t(oX)RfyawucQ@5n@hXLg1~rl}*zTEt$hyDH7@~l&yg$3AE<4pw!Ge6?Xe%mjO+v zOMB=uAxrN>328+LQaLa52`9TzLIH583wNo2XpwJ2NtWifETfzVKz%$LQiyxk`G#?U zHa{oe>QewyAJ1xj0QJG5$U?6h|=O8`ZhGRregeH(603sA4q(&d7W;n8_Vi-zIHISMS zf)PDXeh_pa58BKIR72!A?yY&_)){4A(+ReukxS!Byhz+EH!A}vJ;;vr%xU_#lJUF+ ze!ZW-pPkq;JmaKc1-n$EY`DBdYZU{(dLA-bRZ)r`tD0gW*2o`rV$BE(oGJu^592w> zMxq+3-o=8A$z0W2+YHAP=Ry+{q}U4D`JCtFLZ1GH>%6FzI?d9@_k?U{Ozh5U1Qqp1 zwF(^~y zZt46do8TsCO@M6b9ZB&ITsKq*0->X5qr}K*CehdykxMkPV?gs7;;Z@^A`LPKNBGTl zM}F_=p3crX#OY8XBO+IdXl}DkUQ(>;1YlGZx>XGqklS(sFrvpoBPu#6rOavTyIzl&L=$2xHVstT$kf4XVw?B;- zk8+Zx(`^DeueHblZa|WZ1X>BJgDpizAazl@z0@_H7yh21NQy#FeQPXXN=n`EC1KAb z0Oh=5Sz<$X62N#~^oXZ}H#@OW4Ud*?WR0EJvfB~U9JU&fg-os-x(?y7Ud*UW1iqT- zAw?$?9M|Bp@hVm5^`db*%q5UgdWtkXhRI4#mRn&mqo&Ts^s)upnNIy?5dz7GZmMz0 z>7E&t0fXp#9oyGq+qxss5i%XtXrr z45=cGio>)mW3c%Ud5UGB$w4|;x`(FzK}p*0F7+%X@Ln>E6xZOuSa zJp@1!0281OQW`;(B|}$-X{)URVuKXg>(SziO!n>C1MFlYW5;fh4WXj3!sL*!zRGtP zg(SUd*jyR_AZ8l)mP349~RH81qa}Nthjd3%LZnyG* zWb9PACyTmRH!w0H96p#q$5Ku*G(IW5II0Na*3zxygE^PgTPt@p28X{w4BZXgEXRpd zxnGL>ao(L$2H@6VgB~SPmx*@oq>S;3d6w2i=#N02d8)^XW!V7M^O6av+sCiK}ReKF;X+e*i?Uqz2=TiS>8B0wpS zV}6`d0)zd5K?pTuBkmQ&laJ)#VAV7CsJ@YvV(2U?Z$}o7#rZaanpz@$)iVzmp+K(J z>qf*S{E0Zz6X}J)l$y7~s3Rgqy`O1R_0rm}In&5U4Ee@Bu<0TKrJgZ2C!r;T5H#Tg z2{9ASO=#4fE6}ZjHuVDAS*41^FC;d*-lUR`vcX1F*vX7K@?m@sbtY|+PCz75prGnN zY#zMGMw;0aC#8>i(p921bm-SRp*W2sLiv`iq9qc^+@5-o&4eUzGi)wl?tbe#L+Zwz z$Y<%+lq4hbmU(b&&5M$z)TQS*F{HcH4di8Bgx5ASJMyv(uOW4fY#Py%;zvp@vZq~3 z&my2*nbF`xb}Gl!4gXl4|EL$)D4&60GiV`N{PgZJf~!=lHCx+cAT{H+LSAJR_jZR7 zyo8HA9n;x4Rce}eL>^41Dg>4klmT|AGm>Y_D2jnYLxpM^ zTeI;g+%uF2)LpmCZR67j+jKx#Jdn{Q&#ZjPA1Fc?F6~WrxYUT;hVmgs;;+jVWVT4j4&KwHVvG_Bq^t8cCs=Y zj{wfVa2RNsR1tq1bI+*?vXTk{mCz(pNu5up42)cgaP%?nu9O(jAS16H7VvMsx|aw^F`jk$@Lw$y0lQ(j`w#3?M7F!jAg6ntt# zRK(kY)BSJeWvLW`6Gy3vTf?DSV=WmA?~oJksLCkum|>~c7f8=5s;FLMgJi80_0tiS zY!Pkcw~PuH#Md-Qgz=?IPWUa$+pv#K5|O*n?I`#SZ^q4UQk(8~-r4yL`U$Brs&QcD zHy}|A8)Y%UF!!5#*?_?tgmo#G;waI$-}b_TR5MNE>(3CH;IYO5fyw}Pw;JKm0-Xn? z=80NQH(4`CVyLjlDKt{WO4d>~qmwnGbVtaV$VqK(PbF(or>brjH4+~SyNtX`%68yh z;g>Rvw}XrklXcU$7aPt_)@1N_bFT?aSjn0PV3J{kR>&GZx4xH^tP99mg&>RMk=G6w zMhFuA)8}!q2t(aXCX{LGJ^`|YF?Wr~v`IfiYOUs`kqIM!hFeFtkv(LL7*7*&Y0&(% zt;tsD10bMK;@kb!M)=mDXfW2x8X@bkfCcYnc*>gjDHK%VZqzhmr%>qXb^rjAu9etU z3Z>d_K%p{jsgH&jAs8_eOS3PrK(MKrBiaNRMT7c`f|{(`)SZ<=aa;WKoinV(gDskf zN0pHgcH4loo9iN;MR*rI?;6RPs)yR2pGelyQwc~^3Z-eqeO}1A_SMxzJW|tR(s*8i zk+r2Ye%|@X8t8TteXL}S*o$r(@Vp7r29xb%jc=$&pqZS1toXE$HCb3y)xvl*3iM{D z5rW82#K%-YMspsqHwEJmTgtplr*@&HMJE#?7d+I~kcrJ`qO5fWN1&c=0jI0P7SsOX zJTHBKdhITPm%66jXfncAo`v^=@B9=B3Gr&WTkz)O)Nc1p6QOHsCae@%7B$`U5R^I= zMS;sziAd)>jo;+1E$I!ZGF zkYM4kj0Io1tGi~r8mZOB^0!F^B6PY#%OW0%Z;ogsYFb+~ zuO0eKd9=2^7j$5hC1f}1zW!-M!i0Si#sm7KQ_fAF)KE^3ib&2z*%<0%8eel~n?@6y zsY@hicXT9-U=pW6U^@<`*{M_9Wm>k~)QQ`b4J!ww8Lz82vw?zMC%s!tf(#^O>fNl$ zy=Z15j|0OcSTG88!7Xk}Ys1<^8a5$`ejhZrb|!(K_W{-kgprw@nT)38T)hYuO6vxF z*&vl+l|7P8h}7Zf(8eXO3N=(nIgEmed`d?q-AiM=1`HO_rsg(~$zIJ7#6h_aR3YVu z$06`W_QgY~y_RVdiv-pPKO)dCSyt(`_?!#8t=^}z$I zAwt|3l2-E$VWX%Yj62@ysPAO8?NadKWm3DeC1V`*V2H#dyJ;Lyh?zK%27cIfZB3Pm z9_OkRb#GM~1xDTbf;V>5(`0qYGIdhMcw9!Yc-j@S6DN|2srU`*?RpxIdf1dKD!0Cr zjY~<-3@H;5KUR`2=8pQfFcvcG`~M%eS{{F$kojnOnwrGc#?26)$BXhS&Z(xJE{$ucsUoCtR9r%yvTWTPXGe=$h4k6;>+ z$CGQfGMkKFZ`7X6(4?Btv+~HrP>deen*}iIp+C3AWx}LB|4N80X?sTzny^jYZ!~oyd${ zxyK@Upc>jjv>ItE;^tb_E5{3|uBdn71m;CDP?=-&B8X35xi^cDGG>zsCs!7YBiKcd z@PZyLlb|*A)lM8B;fJNY1*rtL%Z<0HLx!D5NjHmZIH2`<*dM^)m$tyg=Ahuhdd@|2 zK?Vt?D_yt_>KtQL8lDEq1d{0kpGNe<-;kDqekh#mXrpC;j1m2WfIK7Yv-3B_Bo#sS znnbJylJr#%OU7y+cBDOtAiB;A;w^W9Lz}Rpg`l34Igu9_t)qz+Q6?ijkFdh9JW-xj zb=z}Tm--xhTm^cZ+|~mfueIMb?T#{4R;4s(dZer-CornYTVaDPXKlj=-twbWi{U}x zk+_Ld9t{~Lia4>uJHgYP%4QiP%4RMrX=}klsCz<0BOQ)b- zLX|9>9v81+T^sUvZ?+yaJ?*@i)X1qi4ZTR-a728TbiAqOCDOz!kQyf(*DO8Dp_~?O zYSpw&ub@_L$?5J<4x4Ic*1O&m0Vnhd45l(7A2Ik?L3gn!Xa*nQ1eE3NR85Gm8DWG$ z0s=FK$Z|uCHcY`Ox}IZEq6FQI=q0GBCuYQ;Vnlp>JR7PA%j%I(G2Lj6sux+UFnJhF z_32o|=!Aw@__^hH3^~?2Sg5~k10uw4&5PtgS2ehcVT8fg7+rkKvIxbf${rSxa&DLd zo-r??0ag)LEQxu1yFsl@btBPqr=PP{VS6Q+pJ3UknWd`Our%d%*-tA;@MiJ=5@aY^ zfg0ItAIj-H=k2%*Jm#e7l!ZY`_@+@EwreuVMxSSix?y~wTSW0BobDjA6B4R7eT|ZX zmXSIeGEofkP`E+>h%BVLo40AwyVcFZKwGy-lOl38M6wgpw|WoDctxCUAmQV}%x=Tt zry;zX z0!*Kh=2xc*U>>mE!(Mv;Z6&MC5oagQA|97C!rjz8T}&V18g54%>q3W&Wabb@^(AdM zpkhv+ZU?KDQP}Kc6S>4LJquuGXH_&r-z{%tRZ@DzZC0)53K_BJ8B(s9shAgN=Mh6j z`k__+7MrB&G_gUD2R~}3Z<;*k@gqG6MxPr)_!^CFQO&d(sm6xnrReW<(g-sn^g&Z> z!#mr(I5$c#7OCbFONwqXwUtX5ouSA{mOz{h`PwXMI<7q}YB#&xR&vy&x|P4o9neIc zD7n!W;3b*JWybM7JLk|v14_mMHOTRyFO0=U@0>d@Krd|FI;B zKuEPGE2EXr*`wnCbRnD?KYb*G#0hx*#mza)kkqnuEW+Fyzbsdg6=IfXRSyy(U-~0iW6l%v1~pRS>aAeV)`P zQ|P!KupDZL+-tp)P1M4tk7HF*0LoM2V^*a!Pv5dCCA2(ZRff=&3O_<@bX51mXl5Rs zG@c<#fMcWyKI|frQq9&@15wg~YA^>2tV@yF1O!gc&@-k`pf&Yu=%yZdhWSP0*mVH` zc@{K^xM`YB4b3TMQ3YSti)@#qd>Z&E+|DB6%nkN!0)iyM;88=oL%MwGonUAq_5qA% zTzq!jBLjz%u|i=|Lh&&A1Xa@$*N8qFX+<$Qm0$Mql0}e-+Y4vh-%$Br$F*rx8`yEM z^lD9Z>Y7SzQ0Kk!89es{!$P?sRw`MyUP2hr4I|9aeT$u^F+0XCG2szDVB(H?TrrTl zsclw1kjM1^Vvj{x)V-%{V|KJm9km%(V6&4n<`m(;k$`H-p#Nds3vOq);|NL9t2`iS zCYBxhM~vDLO4A`Ia+&a5jVlL9>yyxA5pT$JlEktTK|zd)9#^u6M~v!4{M@o=5ciTr zUF$L$+Op_2ASiZfc0&CbDIbAQ&%qq03a5=o7r!Q$g&sS>Y?DO^U1^#J3Aco0E6r`B z&-h0@cp{6svFJ9$G4yu06A_o=L@gsYF-L~gSSYo&_uqy%sDT41*ai@c9&T`8LmcH2 zIx+HzC~gP(*bv9@X6V+ULg{vDhTyncW-(1!xgts!?si*K2q(|V%h`nwN%z)SvBy9d zZa?(o2o^B2-5nnwPOGODS%iV#$3To67q=6Qr*Rynupa-X9E$jAuWXE4!;y9A__VEg zR#DgNa3sT?-8552=~SCm7IihqFuU=*Eb{CdnVo9T$|U@oJ6Y+54%+;7?2I=~$KpzO zsG7LMz-Lh-C2YWe$LiSlMC1)4?8JJ>hWB)16oph&=?*B1umBt8u$`AFT%Pk6T$+V< zCk9RYD4K4}rcJX*XhaoI9RBz`fRWNj?Ra|k^HUn;LDja7TPdxODwo@?FFMwZxtIdt z0i`iIyp3$xbSxRR;cRRY0LQW_BaNW%Rlh=tLU1gcrV)~`u2k<$Bcj11wwfq_Ee=C6 z+7nYQvVwXEnJP3w$!Eqp%x=9$nTux1q1@>>?b+4{7p5}m&J%7gPZ2hdvD)f*?o&fe zJQq6fd!;z8?S!&&4y{C*F^y-zgz!cVS~)$%m&b=0mrPdX$%=0_C#%cLmjRn>-ku)L zE>`=O0mnRF?+>S&vsLi(>hNW?+60F$+u-Bw^h$m&aatO!Yphv#=Q)*W2{Yu?06P=+ z2%z=4-6#=gg!CJK;3PAsELKGko~TrmcVuST8I?kLDqt7~r(^3nv{R@-FWQfY5p_G8 zXcfbRM2pJB7z~)EJ@$=@qy)E8P2QOJ7A#utWFc^(y4;apj^ELPWFQ`eW}pKn%|s=U z(FOx{lA)H8r+-xWKy&=@HAW-e5MDGI`rOHZH^42UwbY$UEWMG@4P{P##S5IStx3dD zSEOu>fkqgtk;fK^L12~lTlHD+mGrswQqcq}+#Nc8V2nq=Kc!uSgB#|z5CCdbM5O`= z-io2dz^NW;NHEe#S%r&<71^_o2@+x?5qR){gadWdZ1H@Lx;1GNsHSIk)7B|rrm6eV z1G`97(Mq8rB-30Ok}>T|#+fJ|znp-4jDSvIfWxE07-N2#Yb3tvQW#{rqb{apFA6Ye?g8aG)OX%-9PR^^8lzWbVW7CKR9$$$5F9 zz_m4nYZOf=kYf0g#1ouKhH)19d1=Ss5{@e0HdcA^b;>37}PVr ziBY7g7b9O;lu3+o(U2<`HZ8)Cr3OS@gMCcEzJ@N}GTo8aJg7+WB5^Cp9EFTnMMo*a zKb$7UakQF6!93rbZj|eUPU4#Kw;oqRbok#)TAbP{NXykIF@;w(p+HEqY@?Y#&V(#^ zdaEwDog@m;cvYzJ0)&H2*3gM%Yj_a5$49a_Bq0KVe6oU3B3t4Ux!4~s5dQJ97d@~D z=lEkWqIa!hA1fLWxf*lJ>sVq8;+*Z)zXt1laCW)fuY$ui*snLAZZ1!E|1J1%*j@+k zt`F<0^*{OH!UX`{`Q;F>?>N%$z9-i*5*lTSVg9gtQm09F5%L?qrxnrnRbGTJY3y#4 zfd$jgp;aWM#<5JQ5|wv%uv?gnhmt7u^s;jAzNZD%vC#O#Vj4q%E}X|PQzXPeiv1oq zv%rroUV^VaR68Ham~fBOdDuO3a)W+WxRpDPcEX*MsfQRAw=+*w&5|>kDs8Q08@Dk- zYI^9M2WSYcsbYbsC4!<8=_W%Bfleqw4R}!}g0JT^?j?XkKHdlG4i!w>H>M>J>7&W= z@=Is9^K;XAX*i<8K#c>_R&^{wGHQ?nXVB}gR&u$oFekbs-JvdHk*VX9q^tD83ltP) zJKX_!q{2@)JK!&Qd5YZ>_{X>*Zm;<{u@9o1D*Ty$iUg>yh#WN{fFLEF(^PVyN}JXk z<+yYtRzRvFZ`0NiwXB14kD$q^VW1+-8S0kDQ$umWcj$dUy@R6t{BIU!=v+Y?) z)M;Wi>E0-7cyqS9Pb3rQl&v zCJ+2RFv`l1Sm7fzVa?8E2^Ccj%w;rxc(!fL#$}YtM^S->Sf!X%NyI^{hmo)y5 z!kHs?CB@Qp>&!84Q%|qC`}HkZ8|I)%W$J^XnR+4;MGw1YJ$lhE2s=NTdDJs^mT;1e z95f2c=qP&M?h(p-}D?Kra;iB!Y+^?iK(IHMDiwEn}!KA2IJNTcA1%=E*W1PPDcJ+wxvJBA;r=%n+|98oe_O0mqKsXj_p&;>*D{5{+{ zTlj<_jf~;*F~nvQZ$-fLYg1i*fxKeEqJn7N(UsW? z^C7yTlJJ3JR(0OpFJ?6xZBd`m^mL9pP*yRl#L4hb{M!|H!%iw4U5I}C{`dO^l%%DJ zHYY~0xkm_1Y1(&)!drl|V(;mKmYHEs7`#}=!5@QLq;wauyUX7`ktk#B(0E4)%0Ha* zL!LDu+c%Pzb zcS_4W*`iOj=#wq_WQ#u8qEqtuPg3+riatrvCn@?QMW3XoBcK1ICZ5#9lbU!^6HjX5 zNliSdiPhO*z1;+FPp^Z`_uKX6DY9*U2Kgzn{S?`LiflhcwvP>RPsMokWAaZ4(5D3G z*}RhRsmkK1%Hqiu{ZC+v^3LSuq}CF5>TdP4wb48ZpE!iZJ^yg_U?HQyqqJB(51oy> z_dKol{jJvuJ7nESrH8zS$JoB~>%7yE0vSAT><(l^fwMcXyOi@Sz(+&WvHI*zY}H=M zWy3$(mD02$zB^s1;4RcN{%SdD8ls}Fl%?_xRd-@3W+kNMe^OM1dGq=%>|>g_@n~Gd zzh675`dcx^s=fw!2D8QNfe)jWn~+Qp8ity#L}eNo;;7#XOAuuqHIql`(UH#gX5lgtutiFe*!# zR{uCKijpj2ble=m#+&3AxrTeK0Eu^=8R6-C9i7LZd6yS?KVgK|z1b}ed9a|-EL`$o z?8cH8fy-2d5-+*O@=Ai>%EoR{5i=a{aiA3PyyZg+r6{Zr!pN5+!-WY0i5qLg0;RM7 zj(&6N=NC$0Sz-x3EW?2qgs^5Tg)y&HIB6+O<$=6M65UbGi62-j)uJ#|jio9r8O`is zxFidcMfgZS3Nx7ffi)b@*(8fd1yU3eEO0Yip5!pxBMEOTUfx!JXof4oMaoR-qga9m zoeD0^5{9lmj^yTSgy$X@L?N$g5cN<%6r*Q(oy4Wz!jM-d5$hAIfMcWuX#2+`A|A;t z?xNmin_ddbRm2W_EV4R&p7CJa0|&?=4EI#zcn>9>G)GT6-J7DqczzrWnMAw=T|G3Ac*~e49aJEd z&`IcwCgeQxxk$Zu+)7GEe!G)kOA{XQ3d?&8mBtq`McFb!Ob~zk9+&|EQ%?er^hz zJja=cV=X|PL{ZFC@%l^zGE4%7k@aDbc||!KaV?lAk3+t4JaCpdtxs|eJW=Q+W#-0I zQY6tLDdrQ>5treMK1O8=LlY)=+LYMkT`*z#O2l(IQ=deyTAjeCF&-j_GaVtH(&Kd(Ho$J@v*=QD<3@RTQR9g#9gydB3^T z0+}%>-#lqZ2B4b6OA2!(er}qRWC~v0(asx+Kh$VXi++m>^o20(5!qmp=XyF4H(pZw zC3&T~UzCv(joJ*m<&t8==YEfrIxbmo-ZWArQxpK<85jtWq zPg8*M{+_mT!AnriI1X}QQALy0A0APbs=^JC9#L@8=?asu0G6IFJkE}K1RIp;^dqr- zGEMv8(13cU?Ss}r0;*O(66ZqSF2(RXt{@nBm#rt5Jj`JCQ{$w_bTG-#EDm!z&$ngErkhjUQ+R|T!~aLl}%3Ke2zO7^Rm0L ziaa}&WIc5nnKVuzU^Kxr!4yRVBUK#45i&7$sxPxYRTMuljTSq0uF=a2B zOf zEO9}x4GfE@m_W++^-Qs_5M}d(FQMTi7qxT5(IU@retJvRNN{20m)dfPoeBk_3J{X< zB5YmQPBJWLzfgo#fZQAEl<1<>T}sEB)VW0-rmCVrg%oAhle$Ud@+W1g`B}V)n&w&C z*1=*raTSNW3?_{Hf} zE+3N7&*0z=HKyC+^pptOiBv((g8Eg}nom=a$Gz+@VN2j1=IWxTBAMQkh3UAMSS68S zpW|AQb#KFg0Q8(`;vCo4^Da)64;(`;o$vZenvxCjB7^@BxJC`ur&8ST1nrfmh;= zM~$$9kWH|QiAB~9&LkB`(+53YnwBBhU$)L6KR0ci5Vjx^?t?Z5!2tb1*oq1)e&nR@ ziaJAT!WBrxWCXy9i@+liGJa86JC!9}b%&Ii-+|1~Vov{H(o;7M=-&FS$JIc$|IH-B zmL4rcS43F4-|hF z#bjc0s)8|*RoM_ll>ayhKC5q}Trr9Yw>T%dgE7J`q`gv%*QUy~~ z{23BKBchdTc>d zikBRP>Y4;klmam)rg9SB!Vlw8$bqRc7fh*$swhr4wiTG@iaN&H`tOk&QTtB1!+F{W ztuV1d{=STuh8dD>o)AouOA&(_e@f}Xh}@++1%3tv914^Z$`3{|C6eW<+pH(yHzK=+ zMXhWdrvbyMbG#wbG z?hYeDwbHVLFuS|9rX)2tsJ==|FnsH|Y;T{FUR6`P|n5VE53x*`4Ds3?xNpCmg zZBfAmE=m=tiV!l%$Z)$LR*APEiQjRaa9|kgcwjLQw80+fSlDPQ<)K)Ey{; zNl_{CreZil*FS1e4k%z)@o|K0L}sWhk(nxMsZbpR`U^6%O8Uo0+6DG9%5v>dLoT|aUyJywWspXt zbuVDpFuILO6XFG8TTVk2G?Hp*IGJYP?*4{9qyxZbXDg(00ah3^!t^TkORrXB+$MvW?v?8Az6T0$9GV=xi|*S>THjWVGDnYcB6+}RNv zGtqXgAe%VlQe53Tadb|Ca3@D#50pB*a+*nrYt%%6jx(_lZM>vz)tyA*C2f%_KR3xd zG%%qXA0RuVEs-Ww+MsjTxDR%l&MJO=p_|5(S`==}I28BtaR>q*Cu>hPnc<0sOI2}C z4^lBi`F+RlMl4}<)REe8UT~qQE*es|#%ZKbrQ6wrsGO?ZSSx_3XmA|!&QakJ@_hWX zvvV$JVlZKSqp66Zj0J}Ydh0PIO<&BsaKWn}|4a%=T_ok{iBmFTQX?L{0Omk%M6t9U zOrUdxNSKom$ZO!dQ@@Uo_@2tL}h!qP{ZmVY?2iX~n5L^)j; zzLoBF0BzFWdR*>KcO(76@HW@g=>5V&((GEype*j4MH3H`<)KSb<$DBaMC+jo$A|{X zmC+jGOzZ$J`Du)9)li|?h^nMs`?(Ag* zd*EK2RDZ%wr~$?3bzy%Ju{yAH>$@J8yAxsK(o^o>x*Ec|7qiYkOhf8-q#VBg^Fk&? z<_+LDm5Bpz(-Doz8s^86SaOO~I_g`%7p8qGSVk{ZO*3l1Tj>v7nTbw3 zFHSy<-pS)o6rtTwc%bJr^LcU5weEgwc`VIFWdJbKd8H7pl)8%X^IGZXzMfY>lf9EX z{OL$4eYAnT3^t%_!%0U41b|>vfn3)8*-l9sVh;HY6msxrC5aR%%EIyG+>x&@-f}3);v9o z^N@5}JJ#O<n|A}yAadwm%$f-A0WEyFpufr}=jH<>myjV3aZP7MSt zJRi|&=ovehpXbHsnJcB5$N+qmAqZ!5cL%{Hc_*cfl#Tfiw}XE(!IjqALMaW-f>Yk4 zXa@Q`HC%=CzdGeJ5ohNAfZ#`v&vY$|c~{flRAx-1$5`vyOvJk?oDgF^Ni(*$XW=MM z35@iS1*6QPB7>m`Dwv6!cZI|jnHNQnl2k&CgV{kANRL7J8TA8f-Cxo~kr65AiQSO* zRlo#phzQIR`eZ-`=v0|8ra`Rxx>ei~ew2Jlk6=PrGH=$Yf~7KHa_c#%4C2(v&Jrf< zipea(0~VK>Qy(g^3eKaee76Ez)R6S9&ccl3Bw->=5>b>3f4Wo+CiGxBIT@+$^$nY* zNu?PBoi>TBiJ7aunE3WR#S4k{SMa&~Ona&6kseHwwA_xqp zOrbxxk+$QPcZf5(@`_q0_S{J^7PTiXDyc`YYn_*v=9n8JN)AhHVleo=e8toa=0nZG zR&}XbsfG)g9wIhf2QZWP=eZ#nr{zPZ4pc=IfoCmJNyER`lik)PO<5q#QZ1fCI7dp0 z>P0q|mU=utV|RAG;&BmD4Gk$xHAqOVP{pj8378g?$2j>4%@b33H5FO3X4kI~9QmqX z_P#soR==wv^GA=1$_nag&TB=Myx8Jp*36D9M!O=X=HV=H)rnIpmq*S>t6E3iM$W4I zMh!dIigC}#aTVwZkHd{9%1qS85lu9ENa)lx^SLNxf4{LJ}e1G83NU)3bO)A>!`|vD;ZFv&z$2YVzYJ# zHj!d&I6hL84K&Usajq>zzrZjXfdZF=~JVOS`ICSYbcZq13aWjoo#KPN;0 zA8nG&H`6IcSkW)zV~xx;JQ z?r^=}_Z}sD^lb{ z1(Rw9#YDBjw&crlM;MqyiK<;C!-&!cE{_Pk7g zPM?=&#ACB`wJKL7OZJziH=DDI=bt{_>{t8+GelYa?Brr~_KSR&e^3sxTb&)YyWfJ3 z+np4L{1V*kPd}{|!4DVfeQ_xc=o%$$N#Ij+V{KdzjKZk@85lRvEQA& z_`KdO_n)^f{`&oo%g?Lbe!bnu&t5b)dhz|QNWJEN|JP@~`zgBGA5M1wF6b{E>{mM$ z1{a6J^~)D8Y7o3kO5(~de)m&&_3YV)AK$)x_5RP??L8~l)6q>N^_eHmP>F1P3Fv-)rOaeLO6f!JmE3-SrM$E%Z*_Qrw?1F)*Z<@r1bw=nt~cw$62e(> zr0dh&`taM!pfvn?y}4Wi$(I2+-dk%AyVJ8@R_B~MJpT5VkNednSdp*GedUw3FiZLbgOtL5PW8oJ;hi{o$qadUdf(U-FJ ze7C(`emt|T>bJ<5YvEPsOjwVYI*jn zT#K*^)@gm#{ujUzT!kf0r}m5UP`*NIEMi42*Dd(idPiFN0SeYK0g3m zx_6F0hR(XhXZ-KatIPg(>#LjocOO=p{dQM>njiOnvOV8kv8`P6)9Ub!fAznr8L$0a z{lAspY_I!YY_Fd^`|jPJU&BUI;XeqA4)?JsT)2l>JI_RA({zbOB{+QPEi9WuU_^J9M(oNceJ QFIR_E4Wx6VY`yLO2N~?a&Hw-a literal 5883 zcmb_gTXW(#7T&k5;xF`z9=o>|Bgv9%GdmA7O=qYfkbYDbV~gO8ZFy{y%l`U3 zvOz!^aHcA$A_cm9=OUeRzN1gKIK4&V>G#SpQ=bE|3ptgvX*@`xE@SD0bED_JB~>_!d%BWhxr^w)yiNBhFc&h z=bppyImnfaA6AvYWNsU#O{ho7Sv8k01`k_I*uthQirNlmE@kHwUB2#ty3`hew16_( zb20JGfrbZ~dJYr31(ajrIL_IV5BD`?p39is1}ZqQ-LvLL*qBc&OoZ#SVLI4xs6MB~ zj~R-yxQc@${-2D_?&BCU#ysvG^IG|&dJjb$`6b{Ob?oyu1{_8~U3T;wG@@3`Pvgvw z;u7L0u6FKghxTj;%cG7vfUb%le3X&Vncz01ITsM-C*6>%QmVX)Q@>gjvfKqFYjLlG zIi})hNNSDCs#u5By4ZlH!Hb_)rO}(Ws10^8j%|AC^b&O+_}M!3W5`towH1`RggGZc zAD5*}WLO^}oZ&8+V7RE~-9c;u3fo7+NxMVapZOrb=t|p^Wx^GM9t?+mrs8rJbl5tds5o(5&KN%F z*^57BKSAQwL-n)+tgqAeSeM)+sFtco z5nShcD7@pWlF8@!7c{)OLGyJ6u_+NOK(kzC!^!9C%P(k<=Sds}Rjjh|Z=Lfn%{KJg zH*`@1`6>=!$8h3*8~qp$NB(qhIqD-L^JZ@u5)`9b{K7q`lKXpRSEDH)4E-OY`C>eq z_BX1bA+jxu_LqLW8BebL@zno)G@p(pi$1zfRQL#_LhJ6!608d<>BrjWUz)ifdJ3{o zMn5eIR}!|qYw3L)#RY^i2niB@f8)M(!`bE4cru#%gCB$OWbkzYt{{8PnaqZRiGMk| z^cUm*z?|UR7JKs-liAmU1)eJ$3haluC-{+TF-l$H+Tv(nYtB_xL9t6cJu`GB0cliQ zFoNBq7qEd)xw(BCP?fh}GKgk;T2mFN^bk{zoX3Q^j9r4ovBbV*oR!Msa0fh8g`5Hj zg_12_{WyI%DFH~IM)HL2Yi{Wv>1%+B* zF!C1^Jjo(>kjPLN6qkL7YL|d|=-;b2fgZrd2xty?Dn*6-a}9wOSqS|;xkej$g8In8 z;%J@f0Cqw|972Z3fdzS5qkF}k6j>jptHF;`I@1*AmwWveg zkF=dSLV!tB-{3as9E`U=^xSRe5Gb%{{toHFf|c|mZHJZsoGh6KWnN$E-#wvRr9q*lNE29qKko>k(wsk+gxDMtFv5ZadUY&nE%oFb@SH& zmM|L3(bZrM1nU}jQV0D5)j~U+H-jKWcN>N3qf{m;io<#>3~zJ=QeqKiIrXr%!_eqH zf?H*VHzZRp$7ay*W4#uLv6(OpuukY^6!Q9;CieLBCewo~yxaV?fvQFBBpx(vFlU6C z?x>IGZuD(y+DB~9u48=)Ftpuvvj94B=wY;`M&R-8w2%1SlBNaF&TKrqWhdc_HjCA7 zEH$8+cx@*1JITRBgzF>GZyg8Y%Lb`_S2~=ik(#%mgNf#m3moA#if%>MZ*DU5KqrSxK0|6LA#>Exn-?mCta957L7mx# z^K)FSP##C{28vyAx8Wxwg*4$`4i*rl{>^;+>n6ydPYIHhDyu$$?tYWmKOn_ylgJzV zP3DnRrA_J;>&ypDAtk;iJsVTDwnOqz>#D13{4wi2r?GxJ7Xrnub6gxsbgPp+?7lnj zCOE#UUrx4L_MAyDZ{Ti?So9bti8e%KNWR2XXL}kyO_Zy3V{)XPGqllP?@Se>B2_@` Y>gf8_who0V%@cUKuC3){ - initializeSeeds(frame); - seeds_cont_=0; - }else if(seeds_cont_==0){ - initializeSeeds(frame); } - ++seeds_cont_; + initializeSeeds(frame); } void DepthFilter::initializeSeeds(FramePtr frame) @@ -185,9 +180,6 @@ void DepthFilter::updateSeedsLoop() if(new_keyframe_set_) { new_keyframe_set_ = false; - if(seeds_cont_>3){ - seeds_updating_halt_ = false; - } clearFrameQueue(); frame = new_keyframe_; } @@ -198,12 +190,6 @@ void DepthFilter::updateSeedsLoop() } } updateSeeds(frame); - - if((frame->isKeyframe() && seeds_cont_>3) ||(frame->isKeyframe() && seeds_cont_==0)){ - initializeSeeds(frame); - seeds_cont_=0; - } - } } @@ -211,7 +197,7 @@ void DepthFilter::updateSeeds(FramePtr frame) { // update only a limited number of seeds, because we don't have time to do it // for all the seeds in every frame! - size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size(); + size_t n_updates=0, n_failed_matches=0; lock_t lock(seeds_mut_); std::list::iterator it=seeds_.begin(); @@ -224,11 +210,6 @@ void DepthFilter::updateSeeds(FramePtr frame) if(seeds_updating_halt_) return; - // check if seed is not already too old - if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) { - it = seeds_.erase(it); - continue; - } if(it->ftr->frame == NULL){ it = seeds_.erase(it); continue; @@ -241,7 +222,7 @@ void DepthFilter::updateSeeds(FramePtr frame) T.rotation_matrix()(1,0),0.0,T.rotation_matrix()(1,1); SE3 T_ref_cur(R,Vector3d(T.translation()(0),0.0,T.translation()(1))); const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) ); - if(xyz_f.z() < 0.0 && xyz_f.y() < 0.08) { + if(xyz_f.z() < 0.0) { ++it; // behind the camera continue; } @@ -271,7 +252,11 @@ void DepthFilter::updateSeeds(FramePtr frame) double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau)); // update the estimate - updateSeed(1.0/z, tau_inverse*tau_inverse, &*it); + if(!updateSeed(1.0/z, tau_inverse*tau_inverse, &*it)){ + ++it; + ++n_failed_matches; + continue; + } ++n_updates; if(frame->isKeyframe()) @@ -315,12 +300,11 @@ void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list& seeds) seeds.push_back(*it); } } - -void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed) + bool DepthFilter::updateSeed(const float x, const float tau2, Seed* seed) { float norm_scale = sqrt(seed->sigma2 + tau2); if(std::isnan(norm_scale)) - return; + return false; boost::math::normal_distribution nd(seed->mu, norm_scale); float s2 = 1./(1./seed->sigma2 + 1./tau2); float m = s2*(seed->mu/seed->sigma2 + x/tau2); @@ -339,6 +323,7 @@ void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed) seed->mu = mu_new; seed->a = (e-f)/(f-e/f); seed->b = seed->a*(1.0f-f)/f; + return true; } double DepthFilter::computeTau( diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 576b57d..1b4de63 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -171,11 +171,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; reprojector_.reprojectMap(new_frame_, overlap_kfs_); -/* + std::cout<<"Reprojection Map nPoint: "<T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';*/ + (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; if( reprojector_.n_trials_ < 10)return RESULT_FAILURE; size_t sfba_n_edges_final=0; double sfba_thresh, sfba_error_init, sfba_error_final; @@ -183,16 +183,14 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() Config::poseOptimThresh(), Config::poseOptimNumIter(), false, new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 || - fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){ + fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI || + sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ return RESULT_FAILURE; } auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0), new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_); new_frame_->T_f_w_ =result.second; new_frame_->Cov_ = result.first; - if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ - return RESULT_FAILURE; - } optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); // select keyframe diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index 0b9b9f7..3816fd8 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -249,7 +249,7 @@ bool Matcher::findEpipolarMatchDirect( return false; } - size_t n_steps = epi_length_/0.7; // one step per pixel + size_t n_steps = epi_length_/0.3; // one step per pixel Vector2d step = epi_dir_/n_steps; if(n_steps > options_.max_epi_search_steps) diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index a64adf5..1ce97e2 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -129,6 +129,8 @@ int SparseImgAlignGpu::solve() x_ = Eigen::Matrix(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2])); if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2])) return 0; + double norm=x_.norm(); + if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1); return 1; } void SparseImgAlignGpu::update() diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index 0145baa..e04a74c 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -55,8 +55,8 @@ void Visualizer::publishMinimal( Quaterniond q(AngleAxisd(odom.second.pitch(), Vector3d::UnitZ())); geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped); msg_pose_with_cov->header = header_msg; - msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(1); - msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(0); + msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(0); + msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(1); msg_pose_with_cov->pose.pose.position.z = 0.0; msg_pose_with_cov->pose.pose.orientation.x = q.x(); msg_pose_with_cov->pose.pose.orientation.y = q.y(); @@ -71,7 +71,7 @@ void Visualizer::publishMinimal( pub_pose_with_cov_.publish(msg_pose_with_cov); if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){ publishPointMarker( - pub_points_, Eigen::Vector3d(odom.second.se2().translation()(1),odom.second.se2().translation()(0),0.0), "trajectory", + pub_points_, Eigen::Vector3d(odom.second.se2().translation()(0),odom.second.se2().translation()(1),0.0), "trajectory", ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5)); } } diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index ef818eb..7f0bbd4 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -154,7 +154,7 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) { }; imu_in[0] = 0.5*imu_[0]+0.5*imu->linear_acceleration.x; imu_in[1] = 0.5*imu_[1]+0.5*imu->linear_acceleration.y; - imu_in[2] = 0.5*imu_[2]+0.5*imu->angular_velocity.z; + imu_in[2] = imu->angular_velocity.z; memcpy(imu_, imu_in, static_cast(3*sizeof(double))); vo_->UpdateIMU(imu_in,imu->header.stamp); imu_time_=imu->header.stamp; @@ -184,8 +184,8 @@ bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) { res.header.frame_id = "/world"; res.header.seq = trace_id_; auto odom=vo_->ukfPtr_.get_location(); - res.x=odom.second.se2().translation()(1); - res.y=odom.second.se2().translation()(0); + res.x=odom.second.se2().translation()(0); + res.y=odom.second.se2().translation()(1); res.yaw=odom.second.pitch(); res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2), odom.first(1,0),odom.first(1,1),odom.first(1,2),