From 35076b0cf0dd97810efeac563ecb9903ec3ef0e5 Mon Sep 17 00:00:00 2001 From: Jan Bednar Date: Fri, 24 Apr 2020 15:30:16 +0200 Subject: [PATCH 01/17] specified Open CV version to 3.4.6. as docs require --- ov_core/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index ae72dafce..d708c34c6 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sen # Include libraries find_package(Eigen3 REQUIRED) -find_package(OpenCV 3 REQUIRED) +find_package(OpenCV 3.4.6 REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) # display message to user From ea8a38cade8dd033fd2bb0ad04acff2531fea4cf Mon Sep 17 00:00:00 2001 From: uav35 Date: Tue, 13 Jul 2021 10:13:33 +0200 Subject: [PATCH 02/17] Working Open-VINS fork --- ov_msckf/launch/display.rviz | 2 +- ov_msckf/src/core/RosVisualizer.cpp | 28 +-- ov_msckf/src/fisheye_mask_752x480.jpg | Bin 0 -> 14153 bytes ov_msckf/src/ros_subscribe_msckf.cpp | 13 +- ov_repub/CMakeLists.txt | 205 ++++++++++++++++++++++ ov_repub/launch/bluefox_test_real.launch | 110 ++++++++++++ ov_repub/launch/simulation_bluefox.launch | 100 +++++++++++ ov_repub/launch/simulation_gneva.launch | 98 +++++++++++ ov_repub/package.xml | 65 +++++++ 9 files changed, 601 insertions(+), 20 deletions(-) create mode 100644 ov_msckf/src/fisheye_mask_752x480.jpg create mode 100644 ov_repub/CMakeLists.txt create mode 100644 ov_repub/launch/bluefox_test_real.launch create mode 100644 ov_repub/launch/simulation_bluefox.launch create mode 100644 ov_repub/launch/simulation_gneva.launch create mode 100644 ov_repub/package.xml diff --git a/ov_msckf/launch/display.rviz b/ov_msckf/launch/display.rviz index a0c8e1579..0c6012fae 100644 --- a/ov_msckf/launch/display.rviz +++ b/ov_msckf/launch/display.rviz @@ -309,7 +309,7 @@ Visualization Manager: Global Options: Background Color: 44; 44; 44 Default Light: true - Fixed Frame: global + Fixed Frame: world Frame Rate: 30 Name: root Tools: diff --git a/ov_msckf/src/core/RosVisualizer.cpp b/ov_msckf/src/core/RosVisualizer.cpp index ab41a1497..fd9f5a55b 100644 --- a/ov_msckf/src/core/RosVisualizer.cpp +++ b/ov_msckf/src/core/RosVisualizer.cpp @@ -31,7 +31,7 @@ RosVisualizer::RosVisualizer(ros::NodeHandle &nh, std::shared_ptr ap // Setup pose and path publisher pub_poseimu = nh.advertise("/ov_msckf/poseimu", 2); - ROS_INFO("Publishing: %s", pub_poseimu.getTopic().c_str()); + ROS_INFO("Publishing: %s", pub_poseimu.getTopic().c_str()); pub_odomimu = nh.advertise("/ov_msckf/odomimu", 2); ROS_INFO("Publishing: %s", pub_odomimu.getTopic().c_str()); pub_pathimu = nh.advertise("/ov_msckf/pathimu", 2); @@ -174,7 +174,7 @@ void RosVisualizer::visualize_odometry(double timestamp) { // Our odometry message nav_msgs::Odometry odomIinM; odomIinM.header.stamp = ros::Time(timestamp); - odomIinM.header.frame_id = "global"; + odomIinM.header.frame_id = "map"; // The POSE component (orientation and position) odomIinM.pose.pose.orientation.x = state_plus(0); @@ -290,7 +290,7 @@ void RosVisualizer::publish_state() { geometry_msgs::PoseWithCovarianceStamped poseIinM; poseIinM.header.stamp = ros::Time(timestamp_inI); poseIinM.header.seq = poses_seq_imu; - poseIinM.header.frame_id = "global"; + poseIinM.header.frame_id = "map"; poseIinM.pose.pose.orientation.x = state->_imu->quat()(0); poseIinM.pose.pose.orientation.y = state->_imu->quat()(1); poseIinM.pose.pose.orientation.z = state->_imu->quat()(2); @@ -326,7 +326,7 @@ void RosVisualizer::publish_state() { nav_msgs::Path arrIMU; arrIMU.header.stamp = ros::Time::now(); arrIMU.header.seq = poses_seq_imu; - arrIMU.header.frame_id = "global"; + arrIMU.header.frame_id = "map"; for (size_t i = 0; i < poses_imu.size(); i += std::floor(poses_imu.size() / 16384.0) + 1) { arrIMU.poses.push_back(poses_imu.at(i)); } @@ -340,7 +340,7 @@ void RosVisualizer::publish_state() { // NOTE: a rotation from GtoI in JPL has the same xyzw as a ItoG Hamilton rotation tf::StampedTransform trans; trans.stamp_ = ros::Time::now(); - trans.frame_id_ = "global"; + trans.frame_id_ = "map"; trans.child_frame_id_ = "imu"; tf::Quaternion quat(state->_imu->quat()(0), state->_imu->quat()(1), state->_imu->quat()(2), state->_imu->quat()(3)); trans.setRotation(quat); @@ -402,7 +402,7 @@ void RosVisualizer::publish_features() { // Declare message and sizes sensor_msgs::PointCloud2 cloud; - cloud.header.frame_id = "global"; + cloud.header.frame_id = "map"; cloud.header.stamp = ros::Time::now(); cloud.width = 3 * feats_msckf.size(); cloud.height = 1; @@ -440,7 +440,7 @@ void RosVisualizer::publish_features() { // Declare message and sizes sensor_msgs::PointCloud2 cloud_SLAM; - cloud_SLAM.header.frame_id = "global"; + cloud_SLAM.header.frame_id = "map"; cloud_SLAM.header.stamp = ros::Time::now(); cloud_SLAM.width = 3 * feats_slam.size(); cloud_SLAM.height = 1; @@ -478,7 +478,7 @@ void RosVisualizer::publish_features() { // Declare message and sizes sensor_msgs::PointCloud2 cloud_ARUCO; - cloud_ARUCO.header.frame_id = "global"; + cloud_ARUCO.header.frame_id = "map"; cloud_ARUCO.header.stamp = ros::Time::now(); cloud_ARUCO.width = 3 * feats_aruco.size(); cloud_ARUCO.height = 1; @@ -520,7 +520,7 @@ void RosVisualizer::publish_features() { // Declare message and sizes sensor_msgs::PointCloud2 cloud_SIM; - cloud_SIM.header.frame_id = "global"; + cloud_SIM.header.frame_id = "map"; cloud_SIM.header.stamp = ros::Time::now(); cloud_SIM.width = 3 * feats_sim.size(); cloud_SIM.height = 1; @@ -581,7 +581,7 @@ void RosVisualizer::publish_groundtruth() { geometry_msgs::PoseStamped poseIinM; poseIinM.header.stamp = ros::Time(timestamp_inI); poseIinM.header.seq = poses_seq_gt; - poseIinM.header.frame_id = "global"; + poseIinM.header.frame_id = "map"; poseIinM.pose.orientation.x = state_gt(1, 0); poseIinM.pose.orientation.y = state_gt(2, 0); poseIinM.pose.orientation.z = state_gt(3, 0); @@ -600,7 +600,7 @@ void RosVisualizer::publish_groundtruth() { nav_msgs::Path arrIMU; arrIMU.header.stamp = ros::Time::now(); arrIMU.header.seq = poses_seq_gt; - arrIMU.header.frame_id = "global"; + arrIMU.header.frame_id = "map"; for (size_t i = 0; i < poses_gt.size(); i += std::floor(poses_gt.size() / 16384.0) + 1) { arrIMU.poses.push_back(poses_gt.at(i)); } @@ -612,7 +612,7 @@ void RosVisualizer::publish_groundtruth() { // Publish our transform on TF tf::StampedTransform trans; trans.stamp_ = ros::Time::now(); - trans.frame_id_ = "global"; + trans.frame_id_ = "map"; trans.child_frame_id_ = "truth"; tf::Quaternion quat(state_gt(1, 0), state_gt(2, 0), state_gt(3, 0), state_gt(4, 0)); trans.setRotation(quat); @@ -701,7 +701,7 @@ void RosVisualizer::publish_loopclosure_information() { // PUBLISH HISTORICAL POSE ESTIMATE nav_msgs::Odometry odometry_pose; odometry_pose.header = header; - odometry_pose.header.frame_id = "global"; + odometry_pose.header.frame_id = "map"; odometry_pose.pose.pose.position.x = _app->get_state()->_clones_IMU.at(active_tracks_time1)->pos()(0); odometry_pose.pose.pose.position.y = _app->get_state()->_clones_IMU.at(active_tracks_time1)->pos()(1); odometry_pose.pose.pose.position.z = _app->get_state()->_clones_IMU.at(active_tracks_time1)->pos()(2); @@ -745,7 +745,7 @@ void RosVisualizer::publish_loopclosure_information() { // Construct the message sensor_msgs::PointCloud point_cloud; point_cloud.header = header; - point_cloud.header.frame_id = "global"; + point_cloud.header.frame_id = "map"; for (const auto &feattimes : active_tracks_posinG) { // Get this feature information diff --git a/ov_msckf/src/fisheye_mask_752x480.jpg b/ov_msckf/src/fisheye_mask_752x480.jpg new file mode 100644 index 0000000000000000000000000000000000000000..5caf51f5516f1ea7484038bba35528169679180f GIT binary patch literal 14153 zcmd6Oc|4SB`1e$HiL6;N6>Y^g}c9&?b;BuCjgnCGOjh7gj&RFY(w?1fDB zgk;~%mVQ?Y^)3+OO;W4rho%gozw6wJ?RP zTL*)!gMMI~QP?5a`gND`2DqR{RS>>9$x5w1)%{IBG8%} zHb5(JL(4#;!=dLeuFc${+xHpqh@Eib-Qh2;aXINOpMr5oy@X9SRZ-LZLIl5nq|_E^ z8Ks@OcJEQv($>-4e?afhVG~m`bBiMHYFhfOjNA8e?&m(pdzk;Iw5+_Mvg*mx>W0Rq=9bnMZ7+Lz`}zk4hlWQc zN#r+E(=%`1&C=d4efUUUW~_YTjKKI-*@lFHz}tJVyTAN>wt?==P6fOdM-@m}dG%Jq z7m4_2gNvk4yehs-OOPjX{&6kMh@P3TKcQ#XGg~P%BV&7Fht0yBY5cuKhLRUcMuE8( z^GYTtlZFBX_W9?roEX>84=oG~C41E$7;b*-u&_P3|GJ;J=By1#QbYB*WO2Hc;7Hq4 z43@69mYhHD;5vjE2!JKC!juK-f8f9-)fgYC0t8S3XcGu21iLH>T`DWG20P-B86&3*)m&C00cKRo69)>SEX zz`ncILT<+{be0Y+$!1nH<#`8>VsuAIN74})ZR)2rjQW$ZzcY|6%z;Iy0`;|oat@3o zq1)^VXVj336s)Oc&w2aQOv`B86XMnVY5mjd0!hf^>y<;uhG-aJA4|% z0zWM>cG=G-a$sCz3~f)U7yUf@omfA=Vn&*?1J;qIcq7B*d?2FPBs4#`OMmzN zgzcJU0wy93gJa*K6NhBya$ugQ;a2(!hGQ`GBt3_rKFNW}G29nB{N_6`^=FtWU0jdD zAB-GVEPv{dmufRDX_2$EB`5y?ZaNMWbm72w>cWE9zapXCd0P!N5SrkkAis|*TrWL( zp#qWVs7(-9F?2e0u+O>>b*@tXI*Bv_A8Sfs-&HW@ZX(hriV@4*rpsJhaZ*oXrCMt>NRj7d z0UVeB%bQx9{f>x~x8&d4CGTW4Uu4;C^M0tNOyXx5OGiZ?_l@!)YRFXe(LA~&I|on9 zf|m!CG1b97`HkR98c%9WAhnMJKjL?T61ud@1ecWl+?)pS;Oc(vBY~PvfJ7PAwTI@_p z+n*EvCMjw2Ex;L1J(T$Sjuh4F^ZK2_Gt9C99| zU68cQS8((c5am+^7yoYWUo*G5pCBJl4Ak%Dz`WoLg#A2Oz1}K52tVw)t*3Sl2MYmP zx=-V&oye$|=az?o>yu3zM=-TPi$$LZ(xF>7TzUQWj8;Qct;n(pM>MbZ? zOc%Cnrg8jGv#SQp?v6^is^HNdPCrTE&WPK`FK!|;Jtz@tu@MZMLMDX$ZTk#@!!R(GCp#ab!3d>?v9X5ov+~Nc^w^4Zo_D>S z+&3sS-BRgrj5fb_*go0fzGuR2@+HO7htI$V7Fi->kk6lr2Kfc4J|ByZZH?Z41oBP|feawf*fuI z1^Su$+o>nXE;zb@axR*XHI;BY_Q z0Q2?G_lGIqW|u*sPDoq$Ik3ipTK-_BIp*~;0nL~pW1-;aH9HcRaCh1v3?+o}uoeE2pMtHs$ zi`mK8L$fpn+iA!vEThnSA&EzsKb~YKgFV3!B)EoT$eUr6^EF8s&eAqUJWs!6XNYAc zC}ajPKniHZu_QLo2N>EMm|;2|7T_C%S{`Q|QcZy#pAx#7z=16dOu2JliHO^5@ZZRM zbqszRKzxg2Y!7DX;2PSH=hcVih=cT~7Q)6$0QHtnwPKB0nA=^G4f&(zO0bo)cB4Ou zOeQ<}4w&bJqFotd>$X7waF_ir%MqXrG9A5Y%zacC{!TJp2N@4Lm zx?q8e%1(1FwBrfEc0|n&%jV$;?V@x!`c0DR zooEY#9%bG1jT#5G972tFvBV1KLaUPF8e=E1b}^O1q`Lz?#F_Mx0MWThC;!TnrrYf% zG81RZ3fd4WW!0w@%t^Zkc8f&cjko`17WQ{8kU=CA#EE$^c{0h^Eb+DIZOC(76 zOt-tduF%@4AN^k1LE{GPNJ75iMbmag8`EUd7jFxZaq5gcgDkOL5|$;(q_@$`=HqFW zC&5AR#d|-41EgC4`4m;i|DD>&3>~s@16dl6wDPw$KWjp$)Sb9U{AZqhfIK}8KPai>IL)Z(qYTsmD2m(Oo4Y7eb@$8XF7))Md2`lS~ap+vD5*S4u1lD4Kp*6H%meBzfS)=oTMW6W({}-y$0QFnyqL8=$|`#@ z(>ngDaJ~=LGjh+dzA7F1qirTe+p8mo50|to@q&l{X*mQ1Qs-@6EYU;<$Q}#9F()7* z>==3~OoO^>xYmefOeghF8!!D-wOh)}qA2!~uI`g$t)1Q(+hb=#I^zAN@O1ru<%s^^ zrv3Y74vZHnFm``T9O|OCRkJG%w>qs5HevKaA{IcBKH9Pstfy^1eEXAN%#?;HW)v~z z`kX8JxEjgaM&Bfub>|=0hWNyWOyxYDcpFJwuwv|zzruk*?)54MRtD~5S--EfrEQ?3 z$=gn)XKrC`N>$JB6>36xRwAaIkKx;DpVSRCpO#quPN4g{Mxgcs^^Q^w-eQn)Ir$~I(|cFQ4zJGIMN#QvMV%eqM#=bCb;%H` zX4myJyF4OlYlxS1ci=NA>)VZv-4*CvmDP`{uXS7)?G*pjz$T|aK>_yir4cN)>+74x zTR~*>0Z?RT=_Inmg9gcq@d1mv^-uxZMhfM?2)@kY)jdeMHhy7YM}|%(*I-RU`)087 zy{Ajn24p|w@B8|WI3-v_Nb{#xQ!j=cerxR>4vbrk%9B-P&WL}*@F4LnmQA56EA!@c z;k8fB_?7$6DUvVT%E0p)&{a=Wt+~-a!2He~YAI8CFar$*)-$ z(Tox?9Cn*>b-YxP`1qa8O4{o&6S}i|ooD$dE3-Y0a^21d1^l~+_~&N-e-R&Ie$@bl z=Vc#4Y&8Oi4+)#<2@_}cR!fKr?f5B&k17$BFNQi={iW1Kg z+ijm|Xq``bf}5}W(^q_*yKA1}rYylt6YML2@L@D4;G#=Wn50NqRZ#s}eUVJ`aTm?w zRpki;$MOWha62(M8!4y=ufiu!k*{0sd_Z1Klq&{QzUr~SbUd*GH{@)_aAwMe;OOsZ z!B-6MVyqK1t`W0q73j3GOnKSfG1KX>dl{5kiiw|S$eW*)z|M02Equuq%!A)izCVTc z=P>&jUe;E1rl2{i}&1uGO{@}+0c!!J7#9Op;Jjx7fT<_sQkb+xa`P07$#1|%@0xqpF^GpdpS_Zc=0fm_sWMZ zp|^uh<@ri|1C=@@BTY+M-qAfbV=N^O+ce#Hw14ZhFL(A8s{Y*c=SRRMfNGOCEPR3; z!GZZ9WAUX2r_$~UnL;RL~C;YsN5rQI{vcB z*P1nm4;&aMq|Ck!wT>P@4&~7&zZPM!kYEaC3)BK(V#UUvEk_!TyXh2cW6EcqUplaP z@}HOdZuJZz`U4m%C}#+V5-KqH$nNVU?z)nX_#^H&-0o)wenK;4*ph3HUfu3YXv6Oz< zMaPaBb{3DB(TnVt8yszy&x(yB7si@&-3?4dgn@tQ3jSxHIMs~;oQ;Uyd;aGCZ+*9e)l2kbi{=-8d z#*oC@a9F_NNIEDWMYW=b(S%p)Z>L?cb-TUOTz9`NZ+fC}%A0U5rb1Hn+~JbLX=s1N z6ooyp`zMaQ5P1xezoBgZ^awP-6Q{a<)eD|qfr8n6Mtd&A;f1@CY_VpiYl*St51v_G zkHfV0O@eF5x>ZmH!3Vpl@ zm3C0+g<}A?5bQkGZ3!r64V^S)8dB!okeR41LR8l<-B!- zV77Jt&ACAOEx}w(a0t3bvUKw+6zg~ zcBE2#UAoQf(cYHpJx&2eegD~jOfdVQ@HYpFQHy3+Vt$i(2{;4Iy!N7Z1UXd|E%q5p zMRb9)a~9k-nKW_0d?M)xy|Vc2eMCjM(&c@{$PAgk*zo5C{vm;(DY)8-oMDMxX_je? z4%2+-;Dm-+x!R#@fmwPRDe**wi>5^dI$HWyvBPSr#oN^~PAG~#2v%74iSQ*m)>!RQQpEmkQ09zD9yJ)n{B&qravUO%>iI3JFwjOO%={c}s zvp;L-nvEH4YVB+vA?xRP!hyXMFysNopk_h7VbOn0aaL!QAi}A3>MJJc6jjK9aA0J0 z_Pz;w8u!{2>7$Up387-Ig-Jmdci5Q$0B?wwRgdpZ27EGZr6{0XF+#Vxx>t3fxLDv0 ztX;dQr&9r&@g{XV=8Pg6zGlMj>VO>(XRvD$CC{)wNmWm%)y$i7+F6m;=~no5q$2s$ z2w|in@__Gj><=~$S<*>Km+kDfMF;`w1J=w7_~EG*1Za!%y`Q&8B(V1l58~IT^uLfH zm3PfHe)q#!9N0}qOed1=2MTduy<4+F|5L5-$E*PCE7bdAsYkJrAqA~?>M{KAQmd;T zP3LYA`jAfoYWm(sL`J^|!)Y4z8RP_DJg^u}oOU-?X$>{%3MgM6Su?U73 z%`48pmQ0X1i_xPE##9KQod+uTaUZk1?4E^Y7BKUFYg29Nd0`2)E;P$ZR63fHEehAP zb8wVpkYAqz+g-}O4a^f*vK-h0eZA#1i04RRAqpaM#Ces> z7Stz?S{zAW?TWL(;Kor#E*T#Ur<$`Z8!&2vn87@TESab5UWgN6zQl}<;8)dRTTG6{ z${Zb^l}cW?Q6{YKt-o0UuwTobf1>BF`;05#rbDaiL3J?oFfK7$g%MZ`1)#H1785#ieW2LL z?EF$%AVO;RL%~(<4OO>jLU%3o0Ne8K(z|8=pogS}Nwwew=J0ULI?St}Uody73D?5l zMIkBH)yayIrB1U|b_12~WO>y*?YoQPB2BDIjctD`<6`YwLpyjOyHHOIm*eHcBU-p5esoe#7oZBqz{GRvSzgx@3)C%>1Gq%KfFDtl|3?10=AdF3Ax4=dKcctUxkO z%;%F@%)NI@sLyUSZJ<)}@!e$SnA{ty=Z0Qh?h>{ZitA7faEc%e*NiI9YbD5Gy0i6q zKQNE^wSs&5SQdpN@Aer4JswB7SOh8?m+DtH$=eA?k4O9}xbF?^!y0lVKA%7S22@OE z(F&mM#v`!ZDSjclnoy70a%!~E&C~7YWWeKWspUhbu`Wc4ch4T6);KBT#B+q-${ z$Zhvo%~r+hB{8b6w@#()K_)8y>4z}R0RWH)>A`RRonW}z`x3R;XS13VJKP%FgE?rc%kZ0UC z9hohB+6Xg}c)dmmz7)|H1MF##opJxd#%V2%;-O{?Es>r}{kVV)z84bJBI?X=<-m?U zeo~RJY+90V5PAIRzEfCjrLAbA$13H%ScYVbDB}7JtobSfN%u+sn z`y^|vXmQa4>)!KpU@#u>@ z*sbh7&9Zy<2fEpG1e8R`{rClYK7S17z@WXfgChiLGl6O^41MzUhWqsxs*opJaBoId zs&oh==|Omp+j5{-S%#R=aDQ|u`kB2`RcT$OM@-h~N=x$(uQL*MImW-%?`=QJ`ugz^Zhjf$ zbAcocrPoovK`G6CmNad->6b(-*;h6=(ohMUIZp|CwZC>WF>Qj+Y~*g*$nt3c_b5uX zxqG*cA$~2dzjdb;ptIN|VNeSk0za_h>$YQtfX(N?)s&-gG1%|HrQ}w29-vStsbhHB zy5lwb-hy+80a;eXHTV|UeIL9D-i(cP{oDa-BFKPy(D>gwVCsSR&9i{#?;WtEv13yW z`KukU(X+~rW&~g`d*oV|58Xi(K}iTy`?ACZ8}Oxrl$m1iC2DhC?|gt&qEmQQWM<)b z>EVG-VD@>%Udo80SG*|VYISWDU%pD|xP_+-_r{g~>P9~M5GdqFTn@2^SlqbC*co+s zrgw4SgOD*&F67g!j>YNT;@cCUE;>zG7gIJqn3!H5d7rtuFCpB}oPd6UjL`a;8j9OR zU1d3vaO*+}j?xA%haG$9T|E)^@DPxL);l&g>f+#HOfoOto~9wc=S07Yi@B#Kmxu}A ziC@L)?8dVUW7h0_a|r(ByJWRj)yt2iS{G8W5C)YwSMb zjp+_nt(EtWPgB?L@GsetNU|a?w=VvUd=LR&1Lg20)jyCAeu3k`Cx3>J59@4=AFd)F zl3F*Og^&+Ea%ZDsa}xhS5!rAk=Q;x&!QV!*#H{F~lzOD(LM<*5B@L9ZkfeganzD+h z!+7W4d_FZd4Xz6sb!Ok2JrI>8TBv*Bc!OP4f(}^qpP~oo4GNHYv<)c6oL%I?tLafY+V+agtz0{|y*Hur<<(T(loqLIZf>$M7P@}{N&ZSZ z@ps<}*yE|_8I}Ydl;>s2no$MGgUwwOTs&To)$WU8`kh<~M2Wn0?w$Xo=FDyuLA}#? z&u?1q7poG`F&icxEcX6~$_POgW~dFaB!nCAR7>oTW)nf=10d@|M~Tjd*Jq8ZsK85Mo{(Oh#|+AgLAbz;b50SK_hvWmiU~ld z9(Fb(9h1(D$7$sclT!`yyZ3I*xhNhd{+8=;OFR1kbi`baZ|H)KnEk0JiW&8!9TOF4 zaB-3uZhw?uAbJcvt-h$LW)PzoulCbT3$E^hs&p-l?FSCIUjC{#`P$9ZUV`H!YF&u^ zN!nmS?XkSK)jQ+zI)TDDy^2|9ry-|?NQ?73(~L!TCrM;wMcdjc-rxpgP*kTyhVl#q zG9VVgE05>VQYfz5dNFcuGafUXiACoZc7iRBhh^IJ&GuFmp4OU9wEvGorhi0Tby z$;?2ZYlLP`DVcP}4w=!r85wyKEE{SuR*YpxlPn4&pQP1g$Q<#Cb)PFGed<&&^$F`T zoaWzHBwJC@-+?XZ`>I1-dvo2|syfsz zk#g=1t*pQ6kQ^MMv*~%_>uyWniSt`i-g{1{rF-|=9O8vps@egac^0hl* z(Af$1DM;$Ym{&-u1!}kj6%WWP;48w69<2=2!KmEvBdq`l{lQjAvoo@zh}_Ru zGfz#Q<=2s`ShKj)m!%qJDt>Y$W;*~B9)fE@HaSm*_I8&2n>%BWYBR?~G|AYfprLy( zluKbcD2)$v-~2*wX4JCOG~%7Dou?_cN7^1@+`E6}WD!s|{yY|fT|yFy2*AA3>!KJj ztwXw)h{jKL+e=F;Y?C=KCw!pCCaed)=hA}b(9o$z_wFaZpf!sp?|RSu7uO*Gq8)BT z?{}`3)*^E+{m%7K3kYY$>j`naF1({g0phytgVWZM`|m~Y)&2A>)3a>q_ZePt!fz zEWmX(!*fYpU!|zXb6aoTG&WvY z)|ijatM;_ZNczEn6w9|`CE`1-5HW;w^MfP`xpGLN-6Oh?MAaFPM4M#n+@6(I+a)(T zJq|jlgAM2NT%zO-Jv~*Rpm?E$)-HZw*J`VsE9Xx$vcnCxsK*1`YWP)sy`)ozq;h{8 zX#*xRm+iL8{kn=LRrFR%wLSCWPj&~~v-#@Q9%8Mr2q+nU2k{Mpy6!TYCl`W!@Cx3~ z27%0Vmqj3K9o0V;EZgmUN8g)vz%~@swhJ-{%GS>Yk(0|9 zq!mL3(fG+e3r!S;IgFR5$xA;DvN^#oI5XB>s+0XL)#;4MI*-`h+v27L*3j}LBk96J z&JcBf;E&^()|hrs^Rv?2@;g_Y`Wb(GvonuiEWL_9R__0mXU zH5>w@O3FUJT~QSoAj~i_+{r+PO(-%vAI>2zX*ft5n>=(JyL2MFbZ8-7(M?86Y_Elg zO4|D_`ge2NKM=14SZD!JdkFDLMzz%7C@7J$b%D5F4c2)Cl@J)W*%B6q6?Kz*B^e{3 z59}Ol`?!-Ge~Lg&@j}YBYO1SyD?*hA;AKGVaR?*~99b#~TSpX>txRrg)Y`4tsg`;=gvZ%D9V5?!4pnImC4xC)jc4$<@5Iv85Gq*%Zqi`? z1`tz$5dappE*EGBr@itM&MeYx{8_>m(O70V#m@McVfSP5L7Mp>Kbp@c*Zj4{eY>WZcxU)Jz}U(Ug_wSnmi(5 z?y*`zA66Q8sAD-vEOgz3Zc)E;CSfn8XUatNg! zqTB2g^6YiR_`bFTl0mAc^7B;;RjZAx~N!M!rfFP<1%zi!| zDQ8O^?haV_{dmL@?JVeE{rPyr*hF+kSy_p|qx;FQ)pB$7s~@~)=j3(3UUmkan3Ao; zRFBj~3TL_iuL2sEmQaRHO_@Re8rxuegBV>mjAV( zXArJ~AL?Cj>#<7#Vmd*50ka45f*=M(WMl={4v!5xknfl~kxCLoUutUO?yup%M$MaO zTK1cBo94v3FBMy@0v@>vp<*rt|5F!Zk6P9PhE$88VlOBX=78^0ERULOjnjTko834l z(uihb^9}YIg{n-G6rHUVsx?&V-9pG>$hYCYmr-APM{NP>tsw=dK|O|$h%*h<6Nxlo z(itoX7i;GfS{F}EYz%I2IZm4MtZ9~*Ybo8CB9_~yd8x|Tk+>w!w;*ls06Yz>BE6j! zM|Xh&-Q=gh viz; // Callback functions void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg); -void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0); +void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0, const cv::Mat mask_image); void callback_stereo(const sensor_msgs::ImageConstPtr &msg0, const sensor_msgs::ImageConstPtr &msg1, int cam_id0, int cam_id1); // Main function int main(int argc, char **argv) { + cv::Mat mask_image = cv::imread("/home/mrs/fisheye_mask_752x480.jpg",0); // Launch our ros node ros::init(argc, argv, "run_subscribe_msckf"); @@ -112,7 +113,7 @@ int main(int argc, char **argv) { std::string cam_topic; nh.param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); // create subscriber - subs_cam.push_back(nh.subscribe(cam_topic, 5, boost::bind(callback_monocular, _1, i))); + subs_cam.push_back(nh.subscribe(cam_topic, 5, boost::bind(callback_monocular, _1, i, mask_image))); ROS_INFO("subscribing to cam (mono): %s", cam_topic.c_str()); } @@ -147,7 +148,7 @@ void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg) { viz->visualize_odometry(message.timestamp); } -void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0) { +void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0, const cv::Mat mask_image) { // Get the image cv_bridge::CvImageConstPtr cv_ptr; @@ -161,8 +162,10 @@ void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0) { // Create the measurement ov_core::CameraData message; message.timestamp = cv_ptr->header.stamp.toSec(); - message.sensor_ids.push_back(cam_id0); - message.images.push_back(cv_ptr->image.clone()); + message.sensor_ids.push_back(cam_id0); + cv::Mat masked_image; + cv_ptr->image.copyTo(masked_image, mask_image); + message.images.push_back(masked_image.clone()); // send it to our VIO system sys->feed_measurement_camera(message); diff --git a/ov_repub/CMakeLists.txt b/ov_repub/CMakeLists.txt new file mode 100644 index 000000000..411688a2c --- /dev/null +++ b/ov_repub/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ov_repub) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ov_repub +# CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/ov_repub.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/ov_repub_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ov_repub.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/ov_repub/launch/bluefox_test_real.launch b/ov_repub/launch/bluefox_test_real.launch new file mode 100644 index 000000000..ba853f0a5 --- /dev/null +++ b/ov_repub/launch/bluefox_test_real.launch @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [0.0,0.0,9.81007] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [752, 480] + + [218.62328342963636,218.84544019573178, 374.186243114997, 203.57046644527676] + [-0.007054279635535503,-0.013010999471709818,0.007672805064263297,-0.0034608152598251778] + + + + [ + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0.007, + 0, 0, 0, 1 + ] + + + + + + + + \ No newline at end of file diff --git a/ov_repub/launch/simulation_bluefox.launch b/ov_repub/launch/simulation_bluefox.launch new file mode 100644 index 000000000..70b503ca3 --- /dev/null +++ b/ov_repub/launch/simulation_bluefox.launch @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [0.0,0,9.81007] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [752, 480] + + + [227.4010064226358,227.35879407313246,375.5302935901654,239.4881944649193] + [0.019265981371039506, 0.0011428473998276235, -0.0003811659324868097, 6.340084698783884e-05] + + [ + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 + ] + + + + + + + + + + + + \ No newline at end of file diff --git a/ov_repub/launch/simulation_gneva.launch b/ov_repub/launch/simulation_gneva.launch new file mode 100644 index 000000000..95ff243cb --- /dev/null +++ b/ov_repub/launch/simulation_gneva.launch @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [0.0,0.0,9.81] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [752, 480] + + [227.4010064226358,227.35879407313246,375.5302935901654,239.4881944649193] + [0.019265981371039506,0.0011428473998276235,-0.0003811659324868097,6.340084698783884e-05] + + + + [ + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 + ] + + + + + + + + \ No newline at end of file diff --git a/ov_repub/package.xml b/ov_repub/package.xml new file mode 100644 index 000000000..af1e51ea4 --- /dev/null +++ b/ov_repub/package.xml @@ -0,0 +1,65 @@ + + + ov_repub + 0.0.0 + The ov_repub package + + + + + rakshith + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + roscpp + rospy + roscpp + rospy + + + + + + + + From bb923a50a2409de52ecf51c0bf74d5fa89fc0365 Mon Sep 17 00:00:00 2001 From: uav35 Date: Tue, 13 Jul 2021 10:35:38 +0200 Subject: [PATCH 03/17] Changed from 'map' to 'world' --- ov_msckf/src/core/RosVisualizer.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/ov_msckf/src/core/RosVisualizer.cpp b/ov_msckf/src/core/RosVisualizer.cpp index fd9f5a55b..57c67713e 100644 --- a/ov_msckf/src/core/RosVisualizer.cpp +++ b/ov_msckf/src/core/RosVisualizer.cpp @@ -174,7 +174,7 @@ void RosVisualizer::visualize_odometry(double timestamp) { // Our odometry message nav_msgs::Odometry odomIinM; odomIinM.header.stamp = ros::Time(timestamp); - odomIinM.header.frame_id = "map"; + odomIinM.header.frame_id = "world"; // The POSE component (orientation and position) odomIinM.pose.pose.orientation.x = state_plus(0); @@ -290,7 +290,7 @@ void RosVisualizer::publish_state() { geometry_msgs::PoseWithCovarianceStamped poseIinM; poseIinM.header.stamp = ros::Time(timestamp_inI); poseIinM.header.seq = poses_seq_imu; - poseIinM.header.frame_id = "map"; + poseIinM.header.frame_id = "world"; poseIinM.pose.pose.orientation.x = state->_imu->quat()(0); poseIinM.pose.pose.orientation.y = state->_imu->quat()(1); poseIinM.pose.pose.orientation.z = state->_imu->quat()(2); @@ -326,7 +326,7 @@ void RosVisualizer::publish_state() { nav_msgs::Path arrIMU; arrIMU.header.stamp = ros::Time::now(); arrIMU.header.seq = poses_seq_imu; - arrIMU.header.frame_id = "map"; + arrIMU.header.frame_id = "world"; for (size_t i = 0; i < poses_imu.size(); i += std::floor(poses_imu.size() / 16384.0) + 1) { arrIMU.poses.push_back(poses_imu.at(i)); } @@ -340,7 +340,7 @@ void RosVisualizer::publish_state() { // NOTE: a rotation from GtoI in JPL has the same xyzw as a ItoG Hamilton rotation tf::StampedTransform trans; trans.stamp_ = ros::Time::now(); - trans.frame_id_ = "map"; + trans.frame_id_ = "world"; trans.child_frame_id_ = "imu"; tf::Quaternion quat(state->_imu->quat()(0), state->_imu->quat()(1), state->_imu->quat()(2), state->_imu->quat()(3)); trans.setRotation(quat); @@ -402,7 +402,7 @@ void RosVisualizer::publish_features() { // Declare message and sizes sensor_msgs::PointCloud2 cloud; - cloud.header.frame_id = "map"; + cloud.header.frame_id = "world"; cloud.header.stamp = ros::Time::now(); cloud.width = 3 * feats_msckf.size(); cloud.height = 1; @@ -440,7 +440,7 @@ void RosVisualizer::publish_features() { // Declare message and sizes sensor_msgs::PointCloud2 cloud_SLAM; - cloud_SLAM.header.frame_id = "map"; + cloud_SLAM.header.frame_id = "world"; cloud_SLAM.header.stamp = ros::Time::now(); cloud_SLAM.width = 3 * feats_slam.size(); cloud_SLAM.height = 1; @@ -478,7 +478,7 @@ void RosVisualizer::publish_features() { // Declare message and sizes sensor_msgs::PointCloud2 cloud_ARUCO; - cloud_ARUCO.header.frame_id = "map"; + cloud_ARUCO.header.frame_id = "world"; cloud_ARUCO.header.stamp = ros::Time::now(); cloud_ARUCO.width = 3 * feats_aruco.size(); cloud_ARUCO.height = 1; @@ -520,7 +520,7 @@ void RosVisualizer::publish_features() { // Declare message and sizes sensor_msgs::PointCloud2 cloud_SIM; - cloud_SIM.header.frame_id = "map"; + cloud_SIM.header.frame_id = "world"; cloud_SIM.header.stamp = ros::Time::now(); cloud_SIM.width = 3 * feats_sim.size(); cloud_SIM.height = 1; @@ -581,7 +581,7 @@ void RosVisualizer::publish_groundtruth() { geometry_msgs::PoseStamped poseIinM; poseIinM.header.stamp = ros::Time(timestamp_inI); poseIinM.header.seq = poses_seq_gt; - poseIinM.header.frame_id = "map"; + poseIinM.header.frame_id = "world"; poseIinM.pose.orientation.x = state_gt(1, 0); poseIinM.pose.orientation.y = state_gt(2, 0); poseIinM.pose.orientation.z = state_gt(3, 0); @@ -600,7 +600,7 @@ void RosVisualizer::publish_groundtruth() { nav_msgs::Path arrIMU; arrIMU.header.stamp = ros::Time::now(); arrIMU.header.seq = poses_seq_gt; - arrIMU.header.frame_id = "map"; + arrIMU.header.frame_id = "world"; for (size_t i = 0; i < poses_gt.size(); i += std::floor(poses_gt.size() / 16384.0) + 1) { arrIMU.poses.push_back(poses_gt.at(i)); } @@ -612,7 +612,7 @@ void RosVisualizer::publish_groundtruth() { // Publish our transform on TF tf::StampedTransform trans; trans.stamp_ = ros::Time::now(); - trans.frame_id_ = "map"; + trans.frame_id_ = "world"; trans.child_frame_id_ = "truth"; tf::Quaternion quat(state_gt(1, 0), state_gt(2, 0), state_gt(3, 0), state_gt(4, 0)); trans.setRotation(quat); @@ -701,7 +701,7 @@ void RosVisualizer::publish_loopclosure_information() { // PUBLISH HISTORICAL POSE ESTIMATE nav_msgs::Odometry odometry_pose; odometry_pose.header = header; - odometry_pose.header.frame_id = "map"; + odometry_pose.header.frame_id = "world"; odometry_pose.pose.pose.position.x = _app->get_state()->_clones_IMU.at(active_tracks_time1)->pos()(0); odometry_pose.pose.pose.position.y = _app->get_state()->_clones_IMU.at(active_tracks_time1)->pos()(1); odometry_pose.pose.pose.position.z = _app->get_state()->_clones_IMU.at(active_tracks_time1)->pos()(2); @@ -745,7 +745,7 @@ void RosVisualizer::publish_loopclosure_information() { // Construct the message sensor_msgs::PointCloud point_cloud; point_cloud.header = header; - point_cloud.header.frame_id = "map"; + point_cloud.header.frame_id = "world"; for (const auto &feattimes : active_tracks_posinG) { // Get this feature information From 57e25fa9bc58c17bfec2b07918bdef1533dc4f25 Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Mon, 29 Aug 2022 13:04:16 +0200 Subject: [PATCH 04/17] testing some config params --- ov_repub/launch/bluefox_test_real.launch | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ov_repub/launch/bluefox_test_real.launch b/ov_repub/launch/bluefox_test_real.launch index ba853f0a5..62a14aae3 100644 --- a/ov_repub/launch/bluefox_test_real.launch +++ b/ov_repub/launch/bluefox_test_real.launch @@ -4,7 +4,8 @@ - + + @@ -28,7 +29,8 @@ - + + [0.0,0.0,9.81007] @@ -36,6 +38,7 @@ + @@ -107,4 +110,4 @@ - \ No newline at end of file + From f1ebbb21ccb51185786a839bf9ba1f9c22ce7e8e Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Mon, 29 Aug 2022 16:42:15 +0200 Subject: [PATCH 05/17] added updated launch file and config for bluefox --- config/bluefox_imu/estimator_config.yaml | 126 ++++++++++++++++ config/bluefox_imu/kalibr_imu_chain.yaml | 31 ++++ config/bluefox_imu/kalibr_imucam_chain.yaml | 41 ++++++ ov_repub/launch/bluefox_update.launch | 151 ++++++++++++++++++++ 4 files changed, 349 insertions(+) create mode 100644 config/bluefox_imu/estimator_config.yaml create mode 100644 config/bluefox_imu/kalibr_imu_chain.yaml create mode 100644 config/bluefox_imu/kalibr_imucam_chain.yaml create mode 100644 ov_repub/launch/bluefox_update.launch diff --git a/config/bluefox_imu/estimator_config.yaml b/config/bluefox_imu/estimator_config.yaml new file mode 100644 index 000000000..6811a0779 --- /dev/null +++ b/config/bluefox_imu/estimator_config.yaml @@ -0,0 +1,126 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) +use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: false # if we have more than 1 camera, if we should try to track stereo constraints between pairs +max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) + +calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI +calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) +calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized + +max_clones: 11 # how many clones in the sliding window +max_slam: 100 # number of features in our state vector +max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch +max_msckf_in_update: 50 # how many MSCKF features to use in the update +dt_slam_delay: 3 # delay before initializing (helps with stability from bad initialization...) + +gravity_mag: 9.81 # magnitude of gravity in this location + +feat_rep_msckf: "GLOBAL_3D" +# feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +# feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_slam: "ANCHORED_FULL_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: false +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 50 +zupt_max_disparity: 1.5 # set to 0 for only imu-based +zupt_only_at_beginning: true + +# ================================================================== +# ================================================================== + +init_window_time: 2.0 # how many seconds to collect initialization information +# init_window_time: 2.0 # how many seconds to collect initialization information +init_imu_thresh: 0.3 # threshold for variance of the accelerometer to detect a "jerk" in motion +init_max_disparity: 2.0 # max disparity to consider the platform stationary (dependent on resolution) +init_max_features: 75 # how many features to track during initialization (saves on computation) + +init_dyn_use: false # if dynamic initialization should be used +init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) +init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE) +init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in +init_dyn_mle_max_threads: 6 # how many threads the MLE should use +init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced) +init_dyn_min_deg: 10.0 # orientation change needed to try to init + +init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by +init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by +init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by +init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by +init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion + +init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess +init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess + +# ================================================================== +# ================================================================== + +record_timing_information: false # if we want to record timing information of the method +record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame + +# if we want to save the simulation state and its diagional covariance +# use this with rosrun ov_eval error_simulation +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching +num_pts: 400 # number of points (per camera) we will extract and try to track +# num_pts: 200 # number of points (per camera) we will extract and try to track +# fast_threshold: 20 # threshold for fast extraction (warning: lower threshs can be expensive) +fast_threshold: 15 # threshold for fast extraction (warning: lower threshs can be expensive) +grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking) +# grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking) +grid_y: 3 # extraction sub-grid count for vertical direction (uniform tracking) +# min_px_dist: 10 # distance between features (features near each other provide less information) +min_px_dist: 20 # distance between features (features near each other provide less information) +knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches +track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz) +downsample_cameras: false # will downsample image in half if true +num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + + diff --git a/config/bluefox_imu/kalibr_imu_chain.yaml b/config/bluefox_imu/kalibr_imu_chain.yaml new file mode 100644 index 000000000..431a30b72 --- /dev/null +++ b/config/bluefox_imu/kalibr_imu_chain.yaml @@ -0,0 +1,31 @@ +%YAML:1.0 + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + # accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + # accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + # gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + # gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + # model: calibrated + # rostopic: /imu0 + # time_offset: 0.0 + # update_rate: 200.0 + + accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + # accelerometer_noise_density: 2.0000e-2 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + # accelerometer_random_walk: 3.0000e-2 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + # gyroscope_noise_density: 1.6968e-03 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + # gyroscope_random_walk: 1.9393e-04 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + # rostopic: /uav35/vio_imu/imu_raw + rostopic: /uav35/vio_imu/imu_filtered + time_offset: 0.0 + # update_rate: 200.0 + update_rate: 1000.0 diff --git a/config/bluefox_imu/kalibr_imucam_chain.yaml b/config/bluefox_imu/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..feb4a78d9 --- /dev/null +++ b/config/bluefox_imu/kalibr_imucam_chain.yaml @@ -0,0 +1,41 @@ +%YAML:1.0 + +cam0: + T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI + - [1, 0, 0, 0] + - [0, 1, 0, 0] + - [0, 0, 1, 0.007] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.007054279635535503,-0.013010999471709818,0.007672805064263297,-0.0034608152598251778] + distortion_model: equidistant + intrinsics: [218.62328342963636,218.84544019573178, 374.186243114997, 203.57046644527676] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /mv_25003659/image_raw +# cam0: +# T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI +# - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] +# - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768] +# - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] +# - [0.0, 0.0, 0.0, 1.0] +# cam_overlaps: [1] +# camera_model: pinhole +# distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] +# distortion_model: radtan +# intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv +# resolution: [752, 480] +# rostopic: /cam0/image_raw +# cam1: +# T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI +# - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556] +# - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024] +# - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038] +# - [0.0, 0.0, 0.0, 1.0] +# cam_overlaps: [0] +# camera_model: pinhole +# distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] +# distortion_model: radtan +# intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv +# resolution: [752, 480] +# rostopic: /cam1/image_raw diff --git a/ov_repub/launch/bluefox_update.launch b/ov_repub/launch/bluefox_update.launch new file mode 100644 index 000000000..cfd4d4414 --- /dev/null +++ b/ov_repub/launch/bluefox_update.launch @@ -0,0 +1,151 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 08bddc030df120912f170e4b007741ca3f6d2f8c Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Mon, 17 Oct 2022 18:19:15 +0200 Subject: [PATCH 06/17] added launch file and config for t265 stereo --- config/t265/estimator_config.yaml | 126 +++++++++++++++++++++++++++ config/t265/kalibr_imu_chain.yaml | 20 +++++ config/t265/kalibr_imucam_chain.yaml | 28 ++++++ ov_repub/launch/t265.launch | 52 +++++++++++ 4 files changed, 226 insertions(+) create mode 100644 config/t265/estimator_config.yaml create mode 100644 config/t265/kalibr_imu_chain.yaml create mode 100644 config/t265/kalibr_imucam_chain.yaml create mode 100644 ov_repub/launch/t265.launch diff --git a/config/t265/estimator_config.yaml b/config/t265/estimator_config.yaml new file mode 100644 index 000000000..e7097cb6c --- /dev/null +++ b/config/t265/estimator_config.yaml @@ -0,0 +1,126 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) +use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs +max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) + +calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI +calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) +calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized + +max_clones: 11 # how many clones in the sliding window +max_slam: 100 # number of features in our state vector +max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch +max_msckf_in_update: 50 # how many MSCKF features to use in the update +dt_slam_delay: 3 # delay before initializing (helps with stability from bad initialization...) + +gravity_mag: 9.81 # magnitude of gravity in this location + +feat_rep_msckf: "GLOBAL_3D" +# feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +# feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_slam: "ANCHORED_FULL_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: false +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 50 +zupt_max_disparity: 1.5 # set to 0 for only imu-based +zupt_only_at_beginning: true + +# ================================================================== +# ================================================================== + +init_window_time: 2.0 # how many seconds to collect initialization information +# init_window_time: 2.0 # how many seconds to collect initialization information +init_imu_thresh: 0.3 # threshold for variance of the accelerometer to detect a "jerk" in motion +init_max_disparity: 2.0 # max disparity to consider the platform stationary (dependent on resolution) +init_max_features: 75 # how many features to track during initialization (saves on computation) + +init_dyn_use: false # if dynamic initialization should be used +init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) +init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE) +init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in +init_dyn_mle_max_threads: 6 # how many threads the MLE should use +init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced) +init_dyn_min_deg: 10.0 # orientation change needed to try to init + +init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by +init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by +init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by +init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by +init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion + +init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess +init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess + +# ================================================================== +# ================================================================== + +record_timing_information: false # if we want to record timing information of the method +record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame + +# if we want to save the simulation state and its diagional covariance +# use this with rosrun ov_eval error_simulation +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching +num_pts: 400 # number of points (per camera) we will extract and try to track +# num_pts: 200 # number of points (per camera) we will extract and try to track +# fast_threshold: 20 # threshold for fast extraction (warning: lower threshs can be expensive) +fast_threshold: 15 # threshold for fast extraction (warning: lower threshs can be expensive) +grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking) +# grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking) +grid_y: 3 # extraction sub-grid count for vertical direction (uniform tracking) +# min_px_dist: 10 # distance between features (features near each other provide less information) +min_px_dist: 20 # distance between features (features near each other provide less information) +knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches +track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz) +downsample_cameras: false # will downsample image in half if true +num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + + diff --git a/config/t265/kalibr_imu_chain.yaml b/config/t265/kalibr_imu_chain.yaml new file mode 100644 index 000000000..c7768789e --- /dev/null +++ b/config/t265/kalibr_imu_chain.yaml @@ -0,0 +1,20 @@ +%YAML:1.0 + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + # accelerometer_noise_density: 2.0000e-2 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + # accelerometer_random_walk: 3.0000e-2 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + # gyroscope_noise_density: 1.6968e-03 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + # gyroscope_random_walk: 1.9393e-04 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /uav35/vio_imu/imu_filtered + time_offset: 0.0 + update_rate: 200.0 diff --git a/config/t265/kalibr_imucam_chain.yaml b/config/t265/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..2c4d47a24 --- /dev/null +++ b/config/t265/kalibr_imucam_chain.yaml @@ -0,0 +1,28 @@ +%YAML:1.0 + +cam0: + T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI + - [-0.99988, 0.00102315, 0.0154821, 0.0106987114995718] + - [-0.00100478, -0.999999, 0.00119407, 1.07511405076366e-05] + - [0.0154833, 0.00117837, 0.999879, -0.000165671401191503] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.00413605011999607, 0.0385973118245602, -0.0369558110833168, 0.00608701910823584] + distortion_model: equidistant + intrinsics: [284.378601074219,285.364898681641, 423.223510742188, 402.276000976562] #fu, fv, cu, cv + resolution: [848, 800] + rostopic: /uav35/t265/fisheye1/image_raw +cam1: + T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI + - [-0.999979, -0.0022358, 0.00603111, -0.0537273213267326] + - [0.00226225, -0.999988, 0.00438268, -0.000109005901322234] + - [0.00602124, 0.00439623, 0.999972, 0.00031432809191756] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.00312783196568489, 0.0387581698596478, -0.0367337986826897, 0.00606365082785487] + distortion_model: equidistant + intrinsics: [285.629211425781, 286.654602050781, 433.148193359375, 403.567413330078] #fu, fv, cu, cv + resolution: [848, 800] + rostopic: /uav35/t265/fisheye2/image_raw diff --git a/ov_repub/launch/t265.launch b/ov_repub/launch/t265.launch new file mode 100644 index 000000000..e530ce2b1 --- /dev/null +++ b/ov_repub/launch/t265.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 0ad1b2392b8f6b6df01e61e7718da0ca7298d6be Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Tue, 18 Oct 2022 13:53:55 +0200 Subject: [PATCH 07/17] added updated simulation config --- .../estimator_config.yaml | 126 ++++++++++++++++++ .../kalibr_imu_chain.yaml | 31 +++++ .../kalibr_imucam_chain.yaml | 41 ++++++ ov_repub/launch/bluefox_test_real.launch | 113 ---------------- ov_repub/launch/simulation_bluefox2.launch | 51 +++++++ 5 files changed, 249 insertions(+), 113 deletions(-) create mode 100644 config/bluefox_imu_simulation/estimator_config.yaml create mode 100644 config/bluefox_imu_simulation/kalibr_imu_chain.yaml create mode 100644 config/bluefox_imu_simulation/kalibr_imucam_chain.yaml delete mode 100644 ov_repub/launch/bluefox_test_real.launch create mode 100644 ov_repub/launch/simulation_bluefox2.launch diff --git a/config/bluefox_imu_simulation/estimator_config.yaml b/config/bluefox_imu_simulation/estimator_config.yaml new file mode 100644 index 000000000..02a77421d --- /dev/null +++ b/config/bluefox_imu_simulation/estimator_config.yaml @@ -0,0 +1,126 @@ +%YAML:1.0 # need to specify the file type at the top! + +verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT + +use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) +use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it +use_rk4int: true # if rk4 integration should be used (overrides imu averaging) + +use_stereo: false # if we have more than 1 camera, if we should try to track stereo constraints between pairs +max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) + +calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI +calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) +calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized + +max_clones: 11 # how many clones in the sliding window +max_slam: 100 # number of features in our state vector +max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch +max_msckf_in_update: 50 # how many MSCKF features to use in the update +dt_slam_delay: 3 # delay before initializing (helps with stability from bad initialization...) + +gravity_mag: 9.81 # magnitude of gravity in this location + +feat_rep_msckf: "GLOBAL_3D" +# feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +# feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_slam: "ANCHORED_FULL_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH" + +# zero velocity update parameters we can use +# we support either IMU-based or disparity detection. +try_zupt: false +zupt_chi2_multipler: 0 # set to 0 for only disp-based +zupt_max_velocity: 0.1 +zupt_noise_multiplier: 50 +zupt_max_disparity: 1.5 # set to 0 for only imu-based +zupt_only_at_beginning: true + +# ================================================================== +# ================================================================== + +init_window_time: 2.0 # how many seconds to collect initialization information +# init_window_time: 2.0 # how many seconds to collect initialization information +init_imu_thresh: 0.1 # threshold for variance of the accelerometer to detect a "jerk" in motion +init_max_disparity: 2.0 # max disparity to consider the platform stationary (dependent on resolution) +init_max_features: 75 # how many features to track during initialization (saves on computation) + +init_dyn_use: false # if dynamic initialization should be used +init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) +init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE) +init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in +init_dyn_mle_max_threads: 6 # how many threads the MLE should use +init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced) +init_dyn_min_deg: 10.0 # orientation change needed to try to init + +init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by +init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by +init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by +init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by +init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion + +init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess +init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess + +# ================================================================== +# ================================================================== + +record_timing_information: false # if we want to record timing information of the method +record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame + +# if we want to save the simulation state and its diagional covariance +# use this with rosrun ov_eval error_simulation +save_total_state: false +filepath_est: "/tmp/ov_estimate.txt" +filepath_std: "/tmp/ov_estimate_std.txt" +filepath_gt: "/tmp/ov_groundtruth.txt" + +# ================================================================== +# ================================================================== + +# our front-end feature tracking parameters +# we have a KLT and descriptor based (KLT is better implemented...) +use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching +num_pts: 400 # number of points (per camera) we will extract and try to track +# num_pts: 200 # number of points (per camera) we will extract and try to track +# fast_threshold: 20 # threshold for fast extraction (warning: lower threshs can be expensive) +fast_threshold: 15 # threshold for fast extraction (warning: lower threshs can be expensive) +grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking) +# grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking) +grid_y: 3 # extraction sub-grid count for vertical direction (uniform tracking) +# min_px_dist: 10 # distance between features (features near each other provide less information) +min_px_dist: 20 # distance between features (features near each other provide less information) +knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches +track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz) +downsample_cameras: false # will downsample image in half if true +num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +# aruco tag tracker for the system +# DICT_6X6_1000 from https://chev.me/arucogen/ +use_aruco: false +num_aruco: 1024 +downsize_aruco: true + +# ================================================================== +# ================================================================== + +# camera noises and chi-squared threshold multipliers +up_msckf_sigma_px: 1 +up_msckf_chi2_multipler: 1 +up_slam_sigma_px: 1 +up_slam_chi2_multipler: 1 +up_aruco_sigma_px: 1 +up_aruco_chi2_multipler: 1 + +# masks for our images +use_mask: false + +# imu and camera spacial-temporal +# imu config should also have the correct noise values +relative_config_imu: "kalibr_imu_chain.yaml" +relative_config_imucam: "kalibr_imucam_chain.yaml" + + + + diff --git a/config/bluefox_imu_simulation/kalibr_imu_chain.yaml b/config/bluefox_imu_simulation/kalibr_imu_chain.yaml new file mode 100644 index 000000000..32c7cba56 --- /dev/null +++ b/config/bluefox_imu_simulation/kalibr_imu_chain.yaml @@ -0,0 +1,31 @@ +%YAML:1.0 + +imu0: + T_i_b: + - [1.0, 0.0, 0.0, 0.0] + - [0.0, 1.0, 0.0, 0.0] + - [0.0, 0.0, 1.0, 0.0] + - [0.0, 0.0, 0.0, 1.0] + # accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + # accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + # gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + # gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + # model: calibrated + # rostopic: /imu0 + # time_offset: 0.0 + # update_rate: 200.0 + + accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + # accelerometer_noise_density: 2.0000e-2 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + # accelerometer_random_walk: 3.0000e-2 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + # gyroscope_noise_density: 1.6968e-03 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + # gyroscope_random_walk: 1.9393e-04 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + model: calibrated + rostopic: /uav1/vio/imu + # rostopic: /uav35/vio_imu/imu_filtered + time_offset: 0.0 + # update_rate: 200.0 + update_rate: 1000.0 diff --git a/config/bluefox_imu_simulation/kalibr_imucam_chain.yaml b/config/bluefox_imu_simulation/kalibr_imucam_chain.yaml new file mode 100644 index 000000000..95da2ec8a --- /dev/null +++ b/config/bluefox_imu_simulation/kalibr_imucam_chain.yaml @@ -0,0 +1,41 @@ +%YAML:1.0 + +cam0: + T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI + - [1, 0, 0, 0] + - [0, 1, 0, 0] + - [0, 0, 1, 0.000] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [0.019265981371039506, 0.0011428473998276235, -0.0003811659324868097, 6.340084698783884e-05] + distortion_model: equidistant + intrinsics: [227.4010064226358,227.35879407313246,375.5302935901654,239.4881944649193] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /uav1/vio/camera/image_raw +# cam0: +# T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI +# - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] +# - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768] +# - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] +# - [0.0, 0.0, 0.0, 1.0] +# cam_overlaps: [1] +# camera_model: pinhole +# distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] +# distortion_model: radtan +# intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv +# resolution: [752, 480] +# rostopic: /cam0/image_raw +# cam1: +# T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI +# - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556] +# - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024] +# - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038] +# - [0.0, 0.0, 0.0, 1.0] +# cam_overlaps: [0] +# camera_model: pinhole +# distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] +# distortion_model: radtan +# intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv +# resolution: [752, 480] +# rostopic: /cam1/image_raw diff --git a/ov_repub/launch/bluefox_test_real.launch b/ov_repub/launch/bluefox_test_real.launch deleted file mode 100644 index 62a14aae3..000000000 --- a/ov_repub/launch/bluefox_test_real.launch +++ /dev/null @@ -1,113 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [0.0,0.0,9.81007] - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [752, 480] - - [218.62328342963636,218.84544019573178, 374.186243114997, 203.57046644527676] - [-0.007054279635535503,-0.013010999471709818,0.007672805064263297,-0.0034608152598251778] - - - - [ - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0.007, - 0, 0, 0, 1 - ] - - - - - - - - diff --git a/ov_repub/launch/simulation_bluefox2.launch b/ov_repub/launch/simulation_bluefox2.launch new file mode 100644 index 000000000..c1b900331 --- /dev/null +++ b/ov_repub/launch/simulation_bluefox2.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 76864598b6eafec05dd1a48c38ffb3e74f3d7180 Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Tue, 18 Oct 2022 16:46:03 +0200 Subject: [PATCH 08/17] changed TFs to be able to connect them to MRS TFs, updated simulation config --- config/bluefox_imu_simulation/estimator_config.yaml | 2 +- ov_msckf/src/ros/ROS1Visualizer.cpp | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/config/bluefox_imu_simulation/estimator_config.yaml b/config/bluefox_imu_simulation/estimator_config.yaml index 02a77421d..7b55a9887 100644 --- a/config/bluefox_imu_simulation/estimator_config.yaml +++ b/config/bluefox_imu_simulation/estimator_config.yaml @@ -42,7 +42,7 @@ zupt_only_at_beginning: true init_window_time: 2.0 # how many seconds to collect initialization information # init_window_time: 2.0 # how many seconds to collect initialization information init_imu_thresh: 0.1 # threshold for variance of the accelerometer to detect a "jerk" in motion -init_max_disparity: 2.0 # max disparity to consider the platform stationary (dependent on resolution) +init_max_disparity: 5.0 # max disparity to consider the platform stationary (dependent on resolution) init_max_features: 75 # how many features to track during initialization (saves on computation) init_dyn_use: false # if dynamic initialization should be used diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index e51809593..003ebef34 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -301,10 +301,12 @@ void ROS1Visualizer::visualize_odometry(double timestamp) { auto odom_pose = std::make_shared(); odom_pose->set_value(state_plus.block(0, 0, 7, 1)); tf::StampedTransform trans = ROSVisualizerHelper::get_stamped_transform_from_pose(odom_pose, false); - trans.frame_id_ = "global"; - trans.child_frame_id_ = "imu"; + /* trans.frame_id_ = "global"; */ + /* trans.child_frame_id_ = "imu"; */ + tf::Transform trans_inv = trans.inverse(); + tf::StampedTransform tf_temp(trans_inv, trans.stamp_, "imu", "global"); if (publish_global2imu_tf) { - mTfBr->sendTransform(trans); + mTfBr->sendTransform(tf_temp); } // Loop through each camera calibration and publish it From d9f13c3b452d5924e891db1a61ad63a6f9f071f2 Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Tue, 31 Jan 2023 11:34:15 +0100 Subject: [PATCH 09/17] updated simulation config --- config/bluefox_imu_simulation/estimator_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/bluefox_imu_simulation/estimator_config.yaml b/config/bluefox_imu_simulation/estimator_config.yaml index 7b55a9887..47f87c309 100644 --- a/config/bluefox_imu_simulation/estimator_config.yaml +++ b/config/bluefox_imu_simulation/estimator_config.yaml @@ -41,8 +41,8 @@ zupt_only_at_beginning: true init_window_time: 2.0 # how many seconds to collect initialization information # init_window_time: 2.0 # how many seconds to collect initialization information -init_imu_thresh: 0.1 # threshold for variance of the accelerometer to detect a "jerk" in motion -init_max_disparity: 5.0 # max disparity to consider the platform stationary (dependent on resolution) +init_imu_thresh: 0.05 # threshold for variance of the accelerometer to detect a "jerk" in motion +init_max_disparity: 2.0 # max disparity to consider the platform stationary (dependent on resolution) init_max_features: 75 # how many features to track during initialization (saves on computation) init_dyn_use: false # if dynamic initialization should be used From caac88a0ccf495c2f1cacb364e6ee4a5f291321d Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Wed, 8 Feb 2023 17:16:52 +0100 Subject: [PATCH 10/17] added t265_serial.launch --- ov_repub/launch/t265_serial.launch | 52 ++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 ov_repub/launch/t265_serial.launch diff --git a/ov_repub/launch/t265_serial.launch b/ov_repub/launch/t265_serial.launch new file mode 100644 index 000000000..d3c6500d7 --- /dev/null +++ b/ov_repub/launch/t265_serial.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From b68c95b3e8299541d0e48993f912afbb6320816b Mon Sep 17 00:00:00 2001 From: vasek Date: Thu, 9 Feb 2023 11:04:36 +0100 Subject: [PATCH 11/17] removed t265_serial.launch --- ov_repub/launch/t265_serial.launch | 52 ------------------------------ 1 file changed, 52 deletions(-) delete mode 100644 ov_repub/launch/t265_serial.launch diff --git a/ov_repub/launch/t265_serial.launch b/ov_repub/launch/t265_serial.launch deleted file mode 100644 index d3c6500d7..000000000 --- a/ov_repub/launch/t265_serial.launch +++ /dev/null @@ -1,52 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 1bb6989b15db45015f7b4a0f9ea7e4341546b598 Mon Sep 17 00:00:00 2001 From: vasek Date: Thu, 9 Feb 2023 11:06:15 +0100 Subject: [PATCH 12/17] added mutex to the state class --- ov_msckf/src/state/State.h | 5 ++++- ov_msckf/src/state/StateHelper.cpp | 3 ++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ov_msckf/src/state/State.h b/ov_msckf/src/state/State.h index 0a46550f5..9c0678291 100644 --- a/ov_msckf/src/state/State.h +++ b/ov_msckf/src/state/State.h @@ -63,6 +63,7 @@ class State { * @return timestep of clone we will marginalize */ double margtimestep() { + std::lock_guard lock(_mutex_state); double time = INFINITY; for (const auto &clone_imu : _clones_IMU) { if (clone_imu.first < time) { @@ -72,6 +73,8 @@ class State { return time; } + std::mutex _mutex_state; + /** * @brief Calculates the current max size of the covariance * @return Size of the current covariance matrix @@ -120,4 +123,4 @@ class State { } // namespace ov_msckf -#endif // OV_MSCKF_STATE_H \ No newline at end of file +#endif // OV_MSCKF_STATE_H diff --git a/ov_msckf/src/state/StateHelper.cpp b/ov_msckf/src/state/StateHelper.cpp index d5da933d2..40ce8bc3e 100644 --- a/ov_msckf/src/state/StateHelper.cpp +++ b/ov_msckf/src/state/StateHelper.cpp @@ -614,6 +614,7 @@ void StateHelper::augment_clone(std::shared_ptr state, Eigen::Matrix state) { if ((int)state->_clones_IMU.size() > state->_options.max_clone_size) { double marginal_time = state->margtimestep(); + std::lock_guard lock(state->_mutex_state); assert_r(marginal_time != INFINITY); StateHelper::marginalize(state, state->_clones_IMU.at(marginal_time)); // Note that the marginalizer should have already deleted the clone @@ -634,4 +635,4 @@ void StateHelper::marginalize_slam(std::shared_ptr state) { it0++; } } -} \ No newline at end of file +} From 4cb1848f0a287052a9eed8edcff524947d22d78f Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Mon, 12 Jun 2023 16:14:17 +0200 Subject: [PATCH 13/17] added install script for ceres --- installation/install.sh | 131 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100755 installation/install.sh diff --git a/installation/install.sh b/installation/install.sh new file mode 100755 index 000000000..b66e1009c --- /dev/null +++ b/installation/install.sh @@ -0,0 +1,131 @@ +#!/bin/bash + +default=n + +# get the path to this script +MY_PATH=`dirname "$0"` +MY_PATH=`( cd "$MY_PATH" && pwd )` +cd "$MY_PATH" + +CERES_PATH=$MY_PATH/../lib +CERES_VERSION=1.14.0 + +# IMPORTANT: These variables should match the settings of your catkin workspace +PROFILE="RelWithDebInfo" # RelWithDebInfo, Release, Debug +BUILD_WITH_MARCH_NATIVE=false +if [ ! -z "$PCL_CROSS_COMPILATION" ]; then + BUILD_WITH_MARCH_NATIVE=$PCL_CROSS_COMPILATION +fi +CMAKE_STANDARD=17 + +#Ceres Solver +while true; do + [[ -t 0 ]] && { read -t 5 -n 2 -p $'\e[1;32mInstall Ceres-Solver(required for OpenVINS)? [y/n] (default: '"$default"$')\e[0m\n' resp || resp=$default ; } + response=`echo $resp | sed -r 's/(.*)$/\1=/'` + + if [[ $response =~ ^(y|Y)=$ ]] + then + cd ~/git + + echo "Installing ceres solver" + # Ceres installation - dependencies + + # CMake + sudo apt-get -y install cmake + # google-glog + gflags + sudo apt-get -y install libgoogle-glog-dev + # BLAS & LAPACK + sudo apt-get -y install libatlas-base-dev + # Eigen3 + sudo apt-get -y install libeigen3-dev + # SuiteSparse and CXSparse (optional) + # - If you want to build Ceres as a *static* library (the default) + # you can use the SuiteSparse package in the main Ubuntu package + # repository: + sudo apt-get -y install libsuitesparse-dev + # - However, if you want to build Ceres as a *shared* library, you must + # add the following PPA: + # sudo add-apt-repository ppa:bzindovic/suitesparse-bugfix-1319687 + # sudo apt-get update + # sudo apt-get install libsuitesparse-dev + + # Build with march native? + if $BUILD_WITH_MARCH_NATIVE; then + echo "Building ceres optimizer with -march=native" + CMAKE_MARCH_NATIVE="-march=native" + else + echo "Building ceres optimizer without -march=native" + CMAKE_MARCH_NATIVE="" + fi + + # Profile-dependent flags + if [[ "$PROFILE" == "RelWithDebInfo" ]]; then + BUILD_FLAGS_PROFILE=( + -DCMAKE_CXX_FLAGS_${PROFILE^^}="-O2 -g" + -DCMAKE_C_FLAGS_${PROFILE^^}="-O2 -g") + elif [[ "$PROFILE" == "Release" ]]; then + BUILD_FLAGS_PROFILE=( + -DCMAKE_CXX_FLAGS_${PROFILE^^}="-O3" + -DCMAKE_C_FLAGS_${PROFILE^^}="-O3") + else + BUILD_FLAGS_PROFILE=( + -DCMAKE_CXX_FLAGS_${PROFILE^^}="-O0 -g" + -DCMAKE_C_FLAGS_${PROFILE^^}="-O0 -g") + fi + + # Defaults taken from mrs_workspace building flags + BUILD_FLAGS_GENERAL=( + -DCMAKE_EXPORT_COMPILE_COMMANDS=ON + -DCMAKE_CXX_STANDARD=$CMAKE_STANDARD + -DCMAKE_BUILD_TYPE=$PROFILE + -DCMAKE_CXX_FLAGS="-std=c++$CMAKE_STANDARD $CMAKE_MARCH_NATIVE" + -DCMAKE_C_FLAGS="$CMAKE_MARCH_NATIVE" + -DBUILD_TESTING=OFF + -DBUILD_DOCUMENTATION=OFF + -DBUILD_BENCHMARKS=OFF + -DBUILD_EXAMPLES=OFF + -DSCHUR_SPECIALIZATIONS=ON + ) + + # download ceres solver + echo "Downloading ceres solver" + [ ! -d $CERES_PATH ] && mkdir -p $CERES_PATH + cd $CERES_PATH + + if [ ! -d $CERES_PATH/ceres-solver-$CERES_VERSION ] + then + # unpack source files + wget -O "$CERES_PATH/ceres-solver-$CERES_VERSION.tar.gz" http://ceres-solver.org/ceres-solver-$CERES_VERSION.tar.gz + tar zxf ceres-solver-$CERES_VERSION.tar.gz + rm -f ceres-solver-$CERES_VERSION.tar.gz + fi + + # install ceres solver + echo "Compiling ceres solver" + cd $CERES_PATH/ceres-solver-$CERES_VERSION + [ ! -d "build" ] && mkdir build + cd build + cmake "${BUILD_FLAGS_GENERAL[@]}" "${BUILD_FLAGS_PROFILE[@]}" ../ + + [ -z "$GITHUB_CI" ] && N_PROC="-j$[$(nproc) - 1]" + [ ! -z "$GITHUB_CI" ] && N_PROC="-j$[$(nproc) / 2]" + + echo "building with $N_PROC processes" + + make ${N_PROC} + sudo make install + + echo "Done installing prerequisities for OpenVINS" + + # remove the ceres solver source and build files + rm -rf $CERES_PATH + + break + elif [[ $response =~ ^(n|N)=$ ]] + then + break + else + echo " What? \"$resp\" is not a correct answer. Try y+Enter." + fi +done + From 8e7ef27a69aea5872b7bed37add8a2f1783c29be Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Tue, 13 Jun 2023 15:01:22 +0200 Subject: [PATCH 14/17] using UAV_NAME namespace for imu and camera topic, setting these topics only in launch files, cleaned up launch files --- config/bluefox_imu/kalibr_imu_chain.yaml | 16 +- config/bluefox_imu/kalibr_imucam_chain.yaml | 26 --- .../kalibr_imu_chain.yaml | 16 +- .../kalibr_imucam_chain.yaml | 28 +--- config/t265/kalibr_imu_chain.yaml | 6 +- config/t265/kalibr_imucam_chain.yaml | 4 +- ov_msckf/src/ros/ROS1Visualizer.cpp | 8 +- ov_repub/launch/bluefox.launch | 22 +++ ov_repub/launch/bluefox_update.launch | 151 ------------------ ov_repub/launch/simulation_bluefox.launch | 101 ++---------- ov_repub/launch/simulation_bluefox2.launch | 51 ------ ov_repub/launch/simulation_gneva.launch | 98 ------------ ov_repub/launch/t265.launch | 42 +---- 13 files changed, 49 insertions(+), 520 deletions(-) create mode 100644 ov_repub/launch/bluefox.launch delete mode 100644 ov_repub/launch/bluefox_update.launch delete mode 100644 ov_repub/launch/simulation_bluefox2.launch delete mode 100644 ov_repub/launch/simulation_gneva.launch diff --git a/config/bluefox_imu/kalibr_imu_chain.yaml b/config/bluefox_imu/kalibr_imu_chain.yaml index 431a30b72..fcd0bee12 100644 --- a/config/bluefox_imu/kalibr_imu_chain.yaml +++ b/config/bluefox_imu/kalibr_imu_chain.yaml @@ -6,26 +6,12 @@ imu0: - [0.0, 1.0, 0.0, 0.0] - [0.0, 0.0, 1.0, 0.0] - [0.0, 0.0, 0.0, 1.0] - # accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - # accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - # gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - # gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - # model: calibrated - # rostopic: /imu0 - # time_offset: 0.0 - # update_rate: 200.0 accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - # accelerometer_noise_density: 2.0000e-2 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - # accelerometer_random_walk: 3.0000e-2 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - # gyroscope_noise_density: 1.6968e-03 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - # gyroscope_random_walk: 1.9393e-04 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) model: calibrated - # rostopic: /uav35/vio_imu/imu_raw - rostopic: /uav35/vio_imu/imu_filtered + # rostopic: /uav35/vio_imu/imu_filtered time_offset: 0.0 - # update_rate: 200.0 update_rate: 1000.0 diff --git a/config/bluefox_imu/kalibr_imucam_chain.yaml b/config/bluefox_imu/kalibr_imucam_chain.yaml index feb4a78d9..762402492 100644 --- a/config/bluefox_imu/kalibr_imucam_chain.yaml +++ b/config/bluefox_imu/kalibr_imucam_chain.yaml @@ -13,29 +13,3 @@ cam0: intrinsics: [218.62328342963636,218.84544019573178, 374.186243114997, 203.57046644527676] #fu, fv, cu, cv resolution: [752, 480] rostopic: /mv_25003659/image_raw -# cam0: -# T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI -# - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] -# - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768] -# - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] -# - [0.0, 0.0, 0.0, 1.0] -# cam_overlaps: [1] -# camera_model: pinhole -# distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] -# distortion_model: radtan -# intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv -# resolution: [752, 480] -# rostopic: /cam0/image_raw -# cam1: -# T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI -# - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556] -# - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024] -# - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038] -# - [0.0, 0.0, 0.0, 1.0] -# cam_overlaps: [0] -# camera_model: pinhole -# distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] -# distortion_model: radtan -# intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv -# resolution: [752, 480] -# rostopic: /cam1/image_raw diff --git a/config/bluefox_imu_simulation/kalibr_imu_chain.yaml b/config/bluefox_imu_simulation/kalibr_imu_chain.yaml index 32c7cba56..d7a2e0828 100644 --- a/config/bluefox_imu_simulation/kalibr_imu_chain.yaml +++ b/config/bluefox_imu_simulation/kalibr_imu_chain.yaml @@ -6,26 +6,12 @@ imu0: - [0.0, 1.0, 0.0, 0.0] - [0.0, 0.0, 1.0, 0.0] - [0.0, 0.0, 0.0, 1.0] - # accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - # accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - # gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - # gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - # model: calibrated - # rostopic: /imu0 - # time_offset: 0.0 - # update_rate: 200.0 - accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - # accelerometer_noise_density: 2.0000e-2 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - # accelerometer_random_walk: 3.0000e-2 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - # gyroscope_noise_density: 1.6968e-03 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - # gyroscope_random_walk: 1.9393e-04 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) model: calibrated - rostopic: /uav1/vio/imu - # rostopic: /uav35/vio_imu/imu_filtered + # rostopic: /uav0/vio/imu time_offset: 0.0 # update_rate: 200.0 update_rate: 1000.0 diff --git a/config/bluefox_imu_simulation/kalibr_imucam_chain.yaml b/config/bluefox_imu_simulation/kalibr_imucam_chain.yaml index 95da2ec8a..0f936d661 100644 --- a/config/bluefox_imu_simulation/kalibr_imucam_chain.yaml +++ b/config/bluefox_imu_simulation/kalibr_imucam_chain.yaml @@ -12,30 +12,4 @@ cam0: distortion_model: equidistant intrinsics: [227.4010064226358,227.35879407313246,375.5302935901654,239.4881944649193] #fu, fv, cu, cv resolution: [752, 480] - rostopic: /uav1/vio/camera/image_raw -# cam0: -# T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI -# - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] -# - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768] -# - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] -# - [0.0, 0.0, 0.0, 1.0] -# cam_overlaps: [1] -# camera_model: pinhole -# distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] -# distortion_model: radtan -# intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv -# resolution: [752, 480] -# rostopic: /cam0/image_raw -# cam1: -# T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI -# - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556] -# - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024] -# - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038] -# - [0.0, 0.0, 0.0, 1.0] -# cam_overlaps: [0] -# camera_model: pinhole -# distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] -# distortion_model: radtan -# intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv -# resolution: [752, 480] -# rostopic: /cam1/image_raw + # rostopic: /uav0/vio/camera/image_raw diff --git a/config/t265/kalibr_imu_chain.yaml b/config/t265/kalibr_imu_chain.yaml index c7768789e..6a2d4d31f 100644 --- a/config/t265/kalibr_imu_chain.yaml +++ b/config/t265/kalibr_imu_chain.yaml @@ -10,11 +10,7 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - # accelerometer_noise_density: 2.0000e-2 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - # accelerometer_random_walk: 3.0000e-2 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - # gyroscope_noise_density: 1.6968e-03 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - # gyroscope_random_walk: 1.9393e-04 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) model: calibrated - rostopic: /uav35/vio_imu/imu_filtered + # rostopic: /uav35/vio_imu/imu_filtered time_offset: 0.0 update_rate: 200.0 diff --git a/config/t265/kalibr_imucam_chain.yaml b/config/t265/kalibr_imucam_chain.yaml index 2c4d47a24..babd79efe 100644 --- a/config/t265/kalibr_imucam_chain.yaml +++ b/config/t265/kalibr_imucam_chain.yaml @@ -12,7 +12,7 @@ cam0: distortion_model: equidistant intrinsics: [284.378601074219,285.364898681641, 423.223510742188, 402.276000976562] #fu, fv, cu, cv resolution: [848, 800] - rostopic: /uav35/t265/fisheye1/image_raw + # rostopic: /uav35/t265/fisheye1/image_raw cam1: T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI - [-0.999979, -0.0022358, 0.00603111, -0.0537273213267326] @@ -25,4 +25,4 @@ cam1: distortion_model: equidistant intrinsics: [285.629211425781, 286.654602050781, 433.148193359375, 403.567413330078] #fu, fv, cu, cv resolution: [848, 800] - rostopic: /uav35/t265/fisheye2/image_raw + # rostopic: /uav35/t265/fisheye2/image_raw diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 99d31cee5..7dc457d6d 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -153,7 +153,7 @@ void ROS1Visualizer::setup_subscribers(std::shared_ptr pars // Create imu subscriber (handle legacy ros param info) std::string topic_imu; _nh->param("topic_imu", topic_imu, "/imu0"); - parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); + /* parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); */ sub_imu = _nh->subscribe(topic_imu, 1000, &ROS1Visualizer::callback_inertial, this); PRINT_INFO("subscribing to IMU: %s\n", topic_imu.c_str()); @@ -164,8 +164,8 @@ void ROS1Visualizer::setup_subscribers(std::shared_ptr pars std::string cam_topic0, cam_topic1; _nh->param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); _nh->param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); - parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); - parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); + /* parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); */ + /* parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); */ // Create sync filter (they have unique pointers internally, so we have to use move logic here...) auto image_sub0 = std::make_shared>(*_nh, cam_topic0, 1); auto image_sub1 = std::make_shared>(*_nh, cam_topic1, 1); @@ -183,7 +183,7 @@ void ROS1Visualizer::setup_subscribers(std::shared_ptr pars // read in the topic std::string cam_topic; _nh->param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); - parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); + /* parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); */ // create subscriber subs_cam.push_back(_nh->subscribe(cam_topic, 10, boost::bind(&ROS1Visualizer::callback_monocular, this, _1, i))); PRINT_INFO("subscribing to cam (mono): %s\n", cam_topic.c_str()); diff --git a/ov_repub/launch/bluefox.launch b/ov_repub/launch/bluefox.launch new file mode 100644 index 000000000..82c5ac3b8 --- /dev/null +++ b/ov_repub/launch/bluefox.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/ov_repub/launch/bluefox_update.launch b/ov_repub/launch/bluefox_update.launch deleted file mode 100644 index cfd4d4414..000000000 --- a/ov_repub/launch/bluefox_update.launch +++ /dev/null @@ -1,151 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ov_repub/launch/simulation_bluefox.launch b/ov_repub/launch/simulation_bluefox.launch index 70b503ca3..9163ed284 100644 --- a/ov_repub/launch/simulation_bluefox.launch +++ b/ov_repub/launch/simulation_bluefox.launch @@ -1,100 +1,21 @@ - - - + - - - + + + - - + - - - - - - - - - - - - - - - - - - - - - - [0.0,0,9.81007] - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [752, 480] - - - [227.4010064226358,227.35879407313246,375.5302935901654,239.4881944649193] - [0.019265981371039506, 0.0011428473998276235, -0.0003811659324868097, 6.340084698783884e-05] - - [ - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1 - ] - + + + - - - - - - - - \ No newline at end of file + diff --git a/ov_repub/launch/simulation_bluefox2.launch b/ov_repub/launch/simulation_bluefox2.launch deleted file mode 100644 index c1b900331..000000000 --- a/ov_repub/launch/simulation_bluefox2.launch +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ov_repub/launch/simulation_gneva.launch b/ov_repub/launch/simulation_gneva.launch deleted file mode 100644 index 95ff243cb..000000000 --- a/ov_repub/launch/simulation_gneva.launch +++ /dev/null @@ -1,98 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [0.0,0.0,9.81] - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [752, 480] - - [227.4010064226358,227.35879407313246,375.5302935901654,239.4881944649193] - [0.019265981371039506,0.0011428473998276235,-0.0003811659324868097,6.340084698783884e-05] - - - - [ - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1 - ] - - - - - - - - \ No newline at end of file diff --git a/ov_repub/launch/t265.launch b/ov_repub/launch/t265.launch index e530ce2b1..2d366d399 100644 --- a/ov_repub/launch/t265.launch +++ b/ov_repub/launch/t265.launch @@ -1,51 +1,21 @@ + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + From 5807b58447219e7d6e876754a64078237cdd0295 Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Tue, 13 Jun 2023 15:34:39 +0200 Subject: [PATCH 15/17] updated readme --- ReadMe.md | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/ReadMe.md b/ReadMe.md index d1698022a..6d88b790a 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -1,8 +1,11 @@ -# OpenVINS +# OpenVINS (fork) -[![ROS 1 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml) -[![ROS 2 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml) -[![ROS Free Workflow](https://github.com/rpng/open_vins/actions/workflows/build.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build.yml) +## Changes from the original repository +* using `/UAV_NAME/` namespace for IMU and camera topic names, setting these topic names in launch files +* flipped the published TF tree to be compatible with the [MRS system TF tree](https://ctu-mrs.github.io/docs/system/frames_of_reference.html) +* custom configs and launch files + +## Original readme Welcome to the OpenVINS project! The OpenVINS project houses some core computer vision code along with a state-of-the art filter-based visual-inertial From 23a82fb32bb04c95447e35186fd01a01e5f076c7 Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Mon, 9 Oct 2023 11:37:24 +0200 Subject: [PATCH 16/17] updated simulation config --- config/bluefox_imu_simulation/kalibr_imu_chain.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/bluefox_imu_simulation/kalibr_imu_chain.yaml b/config/bluefox_imu_simulation/kalibr_imu_chain.yaml index d7a2e0828..85bb27a4d 100644 --- a/config/bluefox_imu_simulation/kalibr_imu_chain.yaml +++ b/config/bluefox_imu_simulation/kalibr_imu_chain.yaml @@ -14,4 +14,4 @@ imu0: # rostopic: /uav0/vio/imu time_offset: 0.0 # update_rate: 200.0 - update_rate: 1000.0 + update_rate: 125.0 From cbc468eae1c68907673b45b31e77b1c89eb6cf10 Mon Sep 17 00:00:00 2001 From: Vaclav Pritzl Date: Mon, 9 Oct 2023 13:24:55 +0200 Subject: [PATCH 17/17] updated configs for bluefox and t265 --- config/bluefox_imu/estimator_config.yaml | 14 ++++---- config/bluefox_imu/kalibr_imu_chain.yaml | 30 ++++++++++++++++- config/bluefox_imu/kalibr_imucam_chain.yaml | 2 +- .../estimator_config.yaml | 14 ++++---- .../kalibr_imu_chain.yaml | 30 ++++++++++++++++- config/t265/estimator_config.yaml | 15 ++++----- config/t265/kalibr_imu_chain.yaml | 32 ++++++++++++++++++- 7 files changed, 111 insertions(+), 26 deletions(-) diff --git a/config/bluefox_imu/estimator_config.yaml b/config/bluefox_imu/estimator_config.yaml index 6811a0779..7f036cd87 100644 --- a/config/bluefox_imu/estimator_config.yaml +++ b/config/bluefox_imu/estimator_config.yaml @@ -3,15 +3,15 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) -use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: false # if we have more than 1 camera, if we should try to track stereo constraints between pairs max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized +calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix) +calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated max_clones: 11 # how many clones in the sliding window max_slam: 100 # number of features in our state vector @@ -22,10 +22,10 @@ dt_slam_delay: 3 # delay before initializing (helps with stability from bad init gravity_mag: 9.81 # magnitude of gravity in this location feat_rep_msckf: "GLOBAL_3D" -# feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" -# feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" -feat_rep_slam: "ANCHORED_FULL_INVERSE_DEPTH" -feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" +# feat_rep_slam: "ANCHORED_FULL_INVERSE_DEPTH" +# feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. diff --git a/config/bluefox_imu/kalibr_imu_chain.yaml b/config/bluefox_imu/kalibr_imu_chain.yaml index fcd0bee12..2003100d0 100644 --- a/config/bluefox_imu/kalibr_imu_chain.yaml +++ b/config/bluefox_imu/kalibr_imu_chain.yaml @@ -11,7 +11,35 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated # rostopic: /uav35/vio_imu/imu_filtered time_offset: 0.0 update_rate: 1000.0 + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoGYRO: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoACC: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] diff --git a/config/bluefox_imu/kalibr_imucam_chain.yaml b/config/bluefox_imu/kalibr_imucam_chain.yaml index 762402492..514058be0 100644 --- a/config/bluefox_imu/kalibr_imucam_chain.yaml +++ b/config/bluefox_imu/kalibr_imucam_chain.yaml @@ -12,4 +12,4 @@ cam0: distortion_model: equidistant intrinsics: [218.62328342963636,218.84544019573178, 374.186243114997, 203.57046644527676] #fu, fv, cu, cv resolution: [752, 480] - rostopic: /mv_25003659/image_raw + # rostopic: /mv_25003659/image_raw diff --git a/config/bluefox_imu_simulation/estimator_config.yaml b/config/bluefox_imu_simulation/estimator_config.yaml index 47f87c309..dad761b22 100644 --- a/config/bluefox_imu_simulation/estimator_config.yaml +++ b/config/bluefox_imu_simulation/estimator_config.yaml @@ -3,15 +3,15 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) -use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: false # if we have more than 1 camera, if we should try to track stereo constraints between pairs max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized +calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix) +calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated max_clones: 11 # how many clones in the sliding window max_slam: 100 # number of features in our state vector @@ -22,10 +22,10 @@ dt_slam_delay: 3 # delay before initializing (helps with stability from bad init gravity_mag: 9.81 # magnitude of gravity in this location feat_rep_msckf: "GLOBAL_3D" -# feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" -# feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" -feat_rep_slam: "ANCHORED_FULL_INVERSE_DEPTH" -feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" +# feat_rep_slam: "ANCHORED_FULL_INVERSE_DEPTH" +# feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. diff --git a/config/bluefox_imu_simulation/kalibr_imu_chain.yaml b/config/bluefox_imu_simulation/kalibr_imu_chain.yaml index 85bb27a4d..438336eb6 100644 --- a/config/bluefox_imu_simulation/kalibr_imu_chain.yaml +++ b/config/bluefox_imu_simulation/kalibr_imu_chain.yaml @@ -10,8 +10,36 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated # rostopic: /uav0/vio/imu time_offset: 0.0 # update_rate: 200.0 update_rate: 125.0 + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoGYRO: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoACC: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] diff --git a/config/t265/estimator_config.yaml b/config/t265/estimator_config.yaml index e7097cb6c..0f0079c12 100644 --- a/config/t265/estimator_config.yaml +++ b/config/t265/estimator_config.yaml @@ -3,15 +3,15 @@ verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT use_fej: true # if first-estimate Jacobians should be used (enable for good consistency) -use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it -use_rk4int: true # if rk4 integration should be used (overrides imu averaging) - +integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking) calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion) calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized +calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix) +calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated max_clones: 11 # how many clones in the sliding window max_slam: 100 # number of features in our state vector @@ -22,10 +22,10 @@ dt_slam_delay: 3 # delay before initializing (helps with stability from bad init gravity_mag: 9.81 # magnitude of gravity in this location feat_rep_msckf: "GLOBAL_3D" -# feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" -# feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" -feat_rep_slam: "ANCHORED_FULL_INVERSE_DEPTH" -feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH" +feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" +feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" +# feat_rep_slam: "ANCHORED_FULL_INVERSE_DEPTH" +# feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. @@ -40,7 +40,6 @@ zupt_only_at_beginning: true # ================================================================== init_window_time: 2.0 # how many seconds to collect initialization information -# init_window_time: 2.0 # how many seconds to collect initialization information init_imu_thresh: 0.3 # threshold for variance of the accelerometer to detect a "jerk" in motion init_max_disparity: 2.0 # max disparity to consider the platform stationary (dependent on resolution) init_max_features: 75 # how many features to track during initialization (saves on computation) diff --git a/config/t265/kalibr_imu_chain.yaml b/config/t265/kalibr_imu_chain.yaml index 6a2d4d31f..9f737d229 100644 --- a/config/t265/kalibr_imu_chain.yaml +++ b/config/t265/kalibr_imu_chain.yaml @@ -10,7 +10,37 @@ imu0: accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) - model: calibrated # rostopic: /uav35/vio_imu/imu_filtered time_offset: 0.0 update_rate: 200.0 + # three different modes supported: + # "calibrated" (same as "kalibr"), "kalibr", "rpng" + model: "kalibr" + # how to get from Kalibr imu.yaml result file: + # - Tw is imu0:gyroscopes:M: + # - R_IMUtoGYRO: is imu0:gyroscopes:M: + # - Ta is imu0:accelerometers:M: + # - R_IMUtoACC not used by Kalibr + # - Tg is imu0:gyroscopes:A: + Tw: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoGYRO: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Ta: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + R_IMUtoACC: + - [ 1.0, 0.0, 0.0 ] + - [ 0.0, 1.0, 0.0 ] + - [ 0.0, 0.0, 1.0 ] + Tg: + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + - [ 0.0, 0.0, 0.0 ] + +