From 03f296b3eb0f564aab35c5bd6b5d28fe855005fa Mon Sep 17 00:00:00 2001 From: nico Date: Tue, 19 Jun 2018 15:09:13 +0800 Subject: [PATCH] fix-size eigen --- Examples/ROS/ORB_VIO/CMakeLists.txt | 8 + .../ROS/ORB_VIO/launch/testmynteye.launch | 6 + Examples/ROS/ORB_VIO/src/.ros_vio.cc.swp | Bin 16384 -> 0 bytes Examples/ROS/ORB_VIO/src/mynt_vio.cc | 305 ++++++++++++++++++ Examples/ROS/ORB_VIO/src/ros_vio.cc | 6 +- config/.euroc.yaml.swp | Bin 16384 -> 0 bytes config/mynteye.yaml | 116 +++++++ include/Frame.h | 8 +- include/KeyFrame.h | 8 +- include/System.h | 2 +- include/Tracking.h | 6 +- src/Frame.cc | 4 +- src/IMU/imudata.h | 6 + src/KeyFrame.cc | 6 +- src/LocalMapping.cc | 6 +- src/System.cc | 2 +- src/Tracking.cc | 6 +- 17 files changed, 473 insertions(+), 22 deletions(-) create mode 100644 Examples/ROS/ORB_VIO/launch/testmynteye.launch delete mode 100644 Examples/ROS/ORB_VIO/src/.ros_vio.cc.swp create mode 100644 Examples/ROS/ORB_VIO/src/mynt_vio.cc delete mode 100644 config/.euroc.yaml.swp create mode 100644 config/mynteye.yaml diff --git a/Examples/ROS/ORB_VIO/CMakeLists.txt b/Examples/ROS/ORB_VIO/CMakeLists.txt index 9158a8e..b49b8fa 100644 --- a/Examples/ROS/ORB_VIO/CMakeLists.txt +++ b/Examples/ROS/ORB_VIO/CMakeLists.txt @@ -63,4 +63,12 @@ target_link_libraries(VIO ${LIBS} ) +rosbuild_add_executable(MYNT_VIO +src/mynt_vio.cc +src/MsgSync/MsgSynchronizer.cpp +src/MsgSync/MsgSynchronizer.h +) +target_link_libraries(MYNT_VIO +${LIBS} +) diff --git a/Examples/ROS/ORB_VIO/launch/testmynteye.launch b/Examples/ROS/ORB_VIO/launch/testmynteye.launch new file mode 100644 index 0000000..f9a759d --- /dev/null +++ b/Examples/ROS/ORB_VIO/launch/testmynteye.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/Examples/ROS/ORB_VIO/src/.ros_vio.cc.swp b/Examples/ROS/ORB_VIO/src/.ros_vio.cc.swp deleted file mode 100644 index 14ff76a2692ab317a89fe33adb5198b2d3bbdf97..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 16384 zcmeHOU2G#)6&|(}`b#T*1Oi0wx*|BKGm~VyVRzOWn$0G=7Wu2~?6O@|)_8nvPuHF? z^W($`+tQ*|qDcIx54374@c>VNRsx>-Py_`kBnkql73~8&Rq7+H1UyhwzH{$fdlEa@ zE<)m=WA)p~+~0G*bMCq4T)Wk&bA=W5dHrq;pW8L?a0jZ z6s`2m%whJC4{y8|7M-p!WgulBWgulBWgulBWgulBWgulBW+3cbr#*zf55@0M{CwY@ z=R5I7rP19z_1DGk=i}$Ad+OuY^pP@Hc$piz$|bS zV89)~Ex=8{A>hTEH0?LQH-N{1OF$E70B3+jAOqYE+y?ynMos%C@C2|8G=VB$0f&H} z-k@ne0UiS$1ug>>;5_hY;Pnq`+DpI{;0eG2E&^|SK+_%uc7P&q2k;r-uh(nZ^S~p( z!@vW81Iz+bz)ir7z+c|4X}<-Y0iFiBKmqs!@DE(LyaK!oJPSMqtN>?#x1r0|f#-oX zPzFf9S>V6^u*9bwG=iRM>rUHh@CB>Ys#^A?Y#?68PiU&8Q;*9IpIqHoULNmP1l$cg zze2;9WSCJSrR=yYRwJB4L(w{m0dq_hm%WZ--y3nimksI9U>xfeg2-3B69vtR(2vuO zL|IJ8huf_N2JtnPKVORKJfo}5}Z-rHV{n`V()wRlOY?v&MFs2>(89wZ@CToTVlc7$IL zRnE^BSF@R7%jNh}9~i z(^AW7kS>KzARq*WK{G!)JvB9@bS2H9>Uk|zowx0k2=jGXz2nCQl48hYP3M@hf7!R@ zMb#E(jU%--`W)AfP0h|8l>`_Dqf)4!c~RJjLe}9vYv^pcaWZbmEZg?{8Yw#TSj`Hp zpvieCg%-+DirT$%`&(rWrrC~G0>%06X5=N*o)R|GZ+KbILbRiM5LF?&+tl$#VEay$ zXR)~0(xmC+*v!;aj*WqM{K5o$tDaobMS<;y4VK}P1!e=jlvlwq<7L!-5@33gA_cX*vywHmJp z^I~*`VO!z3D)dO^tBjJBa{HGO|BZk-!y28&r+U0c{# zo-gJ?KjQH!u|*cJ%dszM7zjs0>Z}kl$8~T5wP0KUgOcMa7B{%U-fCsu<+rZ3y9!9>)rlnGd9ih&k8HF`()9rJI3Y+^r%jX$~9_g9cpd-Yj z4n90ne{F2Vb1|3X(Rjaru9>lXz)46(I?j2cZ&z9aaD8v|Q5JA9HINqQ-q zy?&^~5i$}LqHT^5zd4qniODJMXpW21%yQf;{Xyg9nBBB|HZeg>G4AEQ6)J}*^)|uU z9pXH8z+KO&u`|9^6*fiNAIIeI;jg^O7@4oK3DJf6R=J<;vtycSA~DanDd)?|x1Gyc z2aeORO)SE0Sx{VZ7{tQ?IYpOr*|BhN!x=)LY%TDx1pKCd-Ut^P``E{{di?{TTDFJWHzZg0R% zzK)24TkS-N)oOS?ejA)Db3nUnrfKjq6WE*{6t#(Fv@<5fsf-<5rxQE}6N0XqVc0Pd zW^Hcj4L8y~zhOZ0II8et<2fB;T<{_mG`*-*V@<1zLz>&%>B41Ma6_FQ^Xl06oSj+S zU}w0?eXGURh4rvyIgU1gQTDH9uoKn>f2_ts%W1(qOt6*3;=MAiBA`o!I}E3%l!aFv`ivy$%oh_uO31Iym^rei$#5o zap-N(o*hwC$Gqmi3dVo{=z?}M7{+f0s@(ycTLBYW91%^8ReO6^Tql`=SSFJe!feZh z>IYQNym0h}P+rZcV+@S4q;%=HP-|N(WMF3j3{C!?UAMiq;agFw9r@y?>B2PUFv0?C z%BP>gv+Tk+0-Nc(C-KiLD_So+*J3M?KRNG*u(5pHn}O&EaSr@y&4XTpIhNgUVW1#c zXp`LsKhf~Et#Kh?xe1L@&`g?z3kiv>zPfddj$X?DuSd>!89628|LXhuhmh+Z0S*CI zkl#NGJOfk#1NanhBk()q`Og8516#n|z{h~=fJ49^kk8kFnK-w93;Fr4f$sx0U;xh{ z2d@I31a1I+fSmg%paJh7zkU<=8}KLKkHF7>XMsn6OMnGj1nvV)04DHp;K#_pe*`=Q zJPCXk=mE!p*N}(53VaXv4)ASY3$TGIumUUtb3h(A0sI{~`J2EmfG2@(0*?V-0@i^E zAOpMsE?)tD54;3?4R{E65Fnnv0;Hdmfs_HwFfIcW2FtK)a<1&-E{Ot5n}XOZB8g7u zPtG}%W8~CZ6VW#Jb;PLcjvjhQSCC>OE|Wv4A!%3{Z=Z;3sD$$S#bR--n9cOV6vnq~ zE>16#eKyG8JkmFD;)y}!m^<+ZwiVhSKj)`UWjU z#M#M67k3ud7=AiYoKVSh2^ke- z4=Vex5yj;5lm@APGaa9DEDAI(aglbZ88fm;ER38Mq1+dFZ;28zSl0vM%Bi<%bxR?F zl>QY%*zPRhtXxMPbyr5USFHy9^l&Rw7eR(`zy#%um27{U{ra;K#hErF^DP$kg@b31 zCL!7EAh8;lwuYOI8iy-An_giMXIMy74iF}K^{y+bf+ekH@N?#V)|fJEzUd2CS;+? zoYjZ)8s-dTWOvE%dEhmIRv(ri{)#S1?H}yK`;k1zkY-TfpwM^gbh*u1f>>xV{ zJAjg<>pEB1EIk_{DjZMyta$;<<-(b&0Omfbl*&K0`+>ZWtQw*z13E2lK1Ps5<3P`X%l&!CBhyC2TE0srzVqYe zhlHohOj9k?b7!a|HU+Fgo^gZYTDOsti%qF|t7q&u`x|D|S$`E;-rY{Y!sx_wC~2(G z%_gNlf^T&=4!eNSrXL@3`!RT}QH5ecJxB0*l}JPNRL{|39~^xs%NwyxhXtarL`|li ziI`@;07W%bPoa``6{={ei+YY;qG1sEh1I1sa@Y~0uWAs+=|}3BT>P!XMd$v}`+LN~ zX)@|YzI6AuXSl4e_UWJ`R8MOwXZ3Q{HasGID{@VfC$27W14v@PV#VDaoTeNN|6e{u X$CWO58sz@^R)3 (University of Zaragoza) +* For more information see +* +* ORB-SLAM2 is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* ORB-SLAM2 is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with ORB-SLAM2. If not, see . +*/ + + +#include +#include +#include +#include + +#include +#include + +#include + +#include"../../../include/System.h" + +#include "MsgSync/MsgSynchronizer.h" + +#include "../../../src/IMU/imudata.h" +#include "../../../src/IMU/configparam.h" +#include +#include + +#include + +using namespace std; + +class ImageGrabber +{ +public: + ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} + + void GrabImage(const sensor_msgs::ImageConstPtr& msg); + + ORB_SLAM2::System* mpSLAM; +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "Mono"); + ros::start(); + + if(argc != 3) + { + cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl; + ros::shutdown(); + return 1; + } + + // Create SLAM system. It initializes all system threads and gets ready to process frames. + ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); + + ORB_SLAM2::ConfigParam config(argv[2]); + + /** + * @brief added data sync + */ + double imageMsgDelaySec = config.GetImageDelayToIMU(); + ORBVIO::MsgSynchronizer msgsync(imageMsgDelaySec); + ros::NodeHandle nh; + ros::Subscriber imagesub; + ros::Subscriber imusub; + if(ORB_SLAM2::ConfigParam::GetRealTimeFlag()) + { + imagesub = nh.subscribe(config._imageTopic, /*200*/ 2, &ORBVIO::MsgSynchronizer::imageCallback, &msgsync); + imusub = nh.subscribe(config._imuTopic, 200, &ORBVIO::MsgSynchronizer::imuCallback, &msgsync); + } + sensor_msgs::ImageConstPtr imageMsg; + std::vector vimuMsg; + + // 3dm imu output per g. 1g=9.80665 according to datasheet + const double g3dm = 9.80665; + const bool bAccMultiply98 = config.GetAccMultiply9p8(); + + ros::Rate r(1000); + + if(!ORB_SLAM2::ConfigParam::GetRealTimeFlag()) + { + ROS_WARN("Run not-realtime"); + + std::string bagfile = config._bagfile; + rosbag::Bag bag; + bag.open(bagfile,rosbag::bagmode::Read); + + std::vector topics; + std::string imutopic = config._imuTopic; + std::string imagetopic = config._imageTopic; + topics.push_back(imagetopic); + topics.push_back(imutopic); + + rosbag::View view(bag, rosbag::TopicQuery(topics)); + //while(ros::ok()) + BOOST_FOREACH(rosbag::MessageInstance const m, view) + { + sensor_msgs::ImuConstPtr simu = m.instantiate(); + if(simu!=NULL) + msgsync.imuCallback(simu); + sensor_msgs::ImageConstPtr simage = m.instantiate(); + if(simage!=NULL) + msgsync.imageCallback(simage); + bool bdata = msgsync.getRecentMsgs(imageMsg,vimuMsg); + + if(bdata) + { + // std::vector vimuData; + ORB_SLAM2::IMUData::vector_t vimuData; + //ROS_INFO("image time: %.3f",imageMsg->header.stamp.toSec()); + for(unsigned int i=0;ilinear_acceleration.x; + double ay = imuMsg->linear_acceleration.y; + double az = imuMsg->linear_acceleration.z; + if(bAccMultiply98) + { + ax *= g3dm; + ay *= g3dm; + az *= g3dm; + } + ORB_SLAM2::IMUData imudata(imuMsg->angular_velocity.x,imuMsg->angular_velocity.y,imuMsg->angular_velocity.z, + ax,ay,az,imuMsg->header.stamp.toSec()); + vimuData.push_back(imudata); + //ROS_INFO("imu time: %.3f",vimuMsg[i]->header.stamp.toSec()); + } + + // Copy the ros image message to cv::Mat. + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(imageMsg); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return -1; + } + + // Consider delay of image message + //SLAM.TrackMonocular(cv_ptr->image, imageMsg->header.stamp.toSec() - imageMsgDelaySec); + cv::Mat im = cv_ptr->image.clone(); + { + // To test relocalization + static double startT=-1; + if(startT<0) + startT = imageMsg->header.stamp.toSec(); + // Below to test relocalizaiton + //if(imageMsg->header.stamp.toSec() > startT+25 && imageMsg->header.stamp.toSec() < startT+25.3) + if(imageMsg->header.stamp.toSec() < startT+config._testDiscardTime) + im = cv::Mat::zeros(im.rows,im.cols,im.type()); + } + SLAM.TrackMonoVI(im, vimuData, imageMsg->header.stamp.toSec() - imageMsgDelaySec); + //SLAM.TrackMonoVI(cv_ptr->image, vimuData, imageMsg->header.stamp.toSec() - imageMsgDelaySec); + //cv::imshow("image",cv_ptr->image); + + // Wait local mapping end. + bool bstop = false; + while(!SLAM.bLocalMapAcceptKF()) + { + if(!ros::ok()) + { + bstop=true; + } + }; + if(bstop) + break; + + } + + //cv::waitKey(1); + + ros::spinOnce(); + r.sleep(); + if(!ros::ok()) + break; + } + + } + else + { + ROS_WARN("Run realtime"); + while(ros::ok()) + { + bool bdata = msgsync.getRecentMsgs(imageMsg,vimuMsg); + + if(bdata) + { + // std::vector vimuData; + ORB_SLAM2::IMUData::vector_t vimuData; + //ROS_INFO("image time: %.3f",imageMsg->header.stamp.toSec()); + for(unsigned int i=0;ilinear_acceleration.x; + double ay = imuMsg->linear_acceleration.y; + double az = imuMsg->linear_acceleration.z; + if(bAccMultiply98) + { + ax *= g3dm; + ay *= g3dm; + az *= g3dm; + } + ORB_SLAM2::IMUData imudata(imuMsg->angular_velocity.x,imuMsg->angular_velocity.y,imuMsg->angular_velocity.z, + ax,ay,az,imuMsg->header.stamp.toSec()); + vimuData.push_back(imudata); + //ROS_INFO("imu time: %.3f",vimuMsg[i]->header.stamp.toSec()); + } + + // Copy the ros image message to cv::Mat. + cv_bridge::CvImageConstPtr cv_ptr; + try + { + cv_ptr = cv_bridge::toCvShare(imageMsg); + } + catch (cv_bridge::Exception& e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return -1; + } + + // Consider delay of image message + //SLAM.TrackMonocular(cv_ptr->image, imageMsg->header.stamp.toSec() - imageMsgDelaySec); + cv::Mat im = cv_ptr->image.clone(); + { + // To test relocalization + static double startT=-1; + if(startT<0) + startT = imageMsg->header.stamp.toSec(); + // Below to test relocalizaiton + //if(imageMsg->header.stamp.toSec() > startT+25 && imageMsg->header.stamp.toSec() < startT+25.3) + if(imageMsg->header.stamp.toSec() < startT+config._testDiscardTime) + im = cv::Mat::zeros(im.rows,im.cols,im.type()); + } + SLAM.TrackMonoVI(im, vimuData, imageMsg->header.stamp.toSec() - imageMsgDelaySec); + //SLAM.TrackMonoVI(cv_ptr->image, vimuData, imageMsg->header.stamp.toSec() - imageMsgDelaySec); + //cv::imshow("image",cv_ptr->image); + + } + + //cv::waitKey(1); + + ros::spinOnce(); + r.sleep(); + if(!ros::ok()) + break; + } + } + +// ImageGrabber igb(&SLAM); + +// ros::NodeHandle nodeHandler; +// ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); + +// ros::spin(); + + + // Save camera trajectory + //SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt"); + SLAM.SaveKeyFrameTrajectoryNavState(config._tmpFilePath+"KeyFrameNavStateTrajectory.txt"); + + cout<TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); +//} + + diff --git a/Examples/ROS/ORB_VIO/src/ros_vio.cc b/Examples/ROS/ORB_VIO/src/ros_vio.cc index 350bfd1..999addb 100644 --- a/Examples/ROS/ORB_VIO/src/ros_vio.cc +++ b/Examples/ROS/ORB_VIO/src/ros_vio.cc @@ -119,7 +119,8 @@ int main(int argc, char **argv) if(bdata) { - std::vector vimuData; + //std::vector vimuData; + ORB_SLAM2::IMUData::vector_t vimuData; //ROS_INFO("image time: %.3f",imageMsg->header.stamp.toSec()); for(unsigned int i=0;i vimuData; + // std::vector vimuData; + ORB_SLAM2::IMUData::vector_t vimuData; //ROS_INFO("image time: %.3f",imageMsg->header.stamp.toSec()); for(unsigned int i=0;i+TlF|jws_IhKY#ng1y%#^pg ztLg5sclH3%5+peA5AYS`gIwT}<;GreNZm>{->R;z>YlN=v_f6YCOAGp$V*?Id*Rh*KS>_HPROkxiwBv?xc5=oH5RY6 zob`KIeyudrvrr6%IroR#f%K=&1|lDhdfOR`l8|S5G>-EIAM?iM(sOGU8|y`~zOr>? zqj72L(&1EJ#(go+`6!jXHfB+HuX@yZwF38@0;9a&Xg^GvmT9QA&(1tSA3JyTzLTMD zaIHYCK&?QnK&?QnK&?QnK&`<2tU#WeAzz08XDShf)$a$6{C==Hf2{fyNAj!F`lnW) zR-jg(R-jg(R-jg(R-jg(R-jg(R-jg(R^b0o0TvK)1NAzu767pSKY9QE`^O3S8~7b~ z8+;FZ1Kb9ez-PdpA0gyV;AQX~z<~~a`7j}SU>AG^oCObppFBj!7r{S2M##@W3?2t> zew2_MAiyKwryn8Y4tN868+;4wg6F|CunEqCbKp_%?pZ?q2>t-x06zlP!Kc8lA0*^1 z_!)Q|d=tC~J_jBEuYVZ!AOu^W3z~p}PlL1IFCQZ0_uy^t7Ptdm0zU+|!B;^7x}XId z@XiB-{1*HM{22TId>_0B5+K2&fPh!9xbiZ{z$Ul~&VhGvPyPz-f>*&S;5JbA>R*!u zn*S!IX5aromm)UcRAe*|yCUEL6)|OWkcvQ~7jnv(KcqenL)yKtzCl^aDZiPg%+Hl+ zl*mlP16&Y^SVW_UraVhzoN1ix6C0B}0j*NP{vqR74cgW^~Tz(2JMX7O2^wFxTi(t|D9vVwtM2?N1CQ?{^fRVkk5wOWR2xu)ALExfjAxJ|=pd$zYwc!!PSxR&i1MrmW( zUdy%2X4A3@C#oFNaV*cXVW0|b*)6x_wehD7A1@%qwA(Gmu`ReSZQ!-tHarB0SCwp= zwt-XEvn{K1W0@@*S30)mIt!Cn+UWTmNZ}Vj)WBn;>@pOOnKds<-T zkOpcD!JMS6dOICR8Fc6-`brrg3siN<$^XjJ^#7&A3v{}K@@%Rt%55xjn#scZoX_H% z`fAcdLn#xwhYzgfC@)S_GA5<-WfdmjR952POcghEA5;N7R#kgy^qAXAWjvTKubxgv zA^H3vaf|rGfTgrJXnfh}j%$U%dX{!ErBZ5PJ}+{Cxx=kupr>=Oq>2B2oNGd5*uLNRrp?l1B`?lQL@U^>}f`y?k>xmrE&4Gxt{YVDZeJyF)C7J zO>;>zwu^Zto&jq7&2)^W7&6pGpA;koz>YvJms-!Q|6f;Gdz-?8t&7~{r zs&-X~IK_NdofPfZ71@Y|4a{;eOJd;>A#%wF_HTko+4O*LLmz diff --git a/config/mynteye.yaml b/config/mynteye.yaml new file mode 100644 index 0000000..314a1e6 --- /dev/null +++ b/config/mynteye.yaml @@ -0,0 +1,116 @@ +%YAML:1.0 + +# 1: realtime, 0: non-realtime +test.RealTime: 1 +# Time for visual-inertial initialization +test.VINSInitTime: 15.0 + +# Modify test.InitVIOTmpPath and bagfile to the correct path +# Path to save tmp files/results +test.InitVIOTmpPath: "/home/zhangs/LearnVIORB/temp/mynt/" + +## For good initialization (no movement at the beginning for some bag) +test.DiscardTime: 0 +#test.DiscardTime: 18 + +bagfile: "/home/zhangs/files_bags/30_500.bag" + +####################################### + +imutopic: "/mynteye/imu" +imagetopic: "/mynteye/left" + + +# Timestamp shift. Timage = Timu + image_delay +Camera.delaytoimu: 0 + +# acc=acc*9.8, if below is 1 +IMU.multiplyG: 0 + +# camera-imu frame transformation, Pi = Tic * Pc +Camera.Tbc: + [-0.0064662, -0.99994994, -0.00763565, 0.00533646, + 0.99997909, -0.00646566, -0.00009558, -0.04302922, + 0.0000462, -0.00763611, 0.99997084, 0.02303124, + 0.0, 0.0, 0.0, 1.0] + +# Local Window size +LocalMapping.LocalWindowSize: 20 + +#-------------------------------------------------------------------------------------------- +# Camera Parameters. Adjust them! +#-------------------------------------------------------------------------------------------- + +# Camera calibration and distortion parameters (OpenCV) +#Camera.fx: 3.6232343319626932e+02 +#Camera.fy: 3.6325085505539170e+02 +#Camera.cx: 3.9202314757024919e+02 +#Camera.cy: 2.5415988511673606e+02 +# +#Camera.k1: -1.8888759173124259e-02 +#Camera.k2: -6.1136131637366748e-03 +#Camera.p1: 0. +#Camera.p2: 0. +# +Camera.fx: 4.6701951615472183e+02 +Camera.fy: 4.6328645451624016e+02 +Camera.cx: 3.8299718661353495e+02 +Camera.cy: 2.4206459336127767e+02 + +Camera.k1: -3.9656901530934580e-01 +Camera.k2: 2.2323877914046403e-01 +Camera.p1: 0. +Camera.p2: 0. + +Camera.width: 752 +Camera.height: 480 + +# Camera frames per second +Camera.fps: 30.0 + +# IR projector baseline times fx (aprox.) +Camera.bf: 40.0 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +# Close/Far threshold. Baseline times. +ThDepth: 40 + +# Deptmap values factor +DepthMapFactor: 1.0 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 1000 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.2 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 8 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 7 + +#-------------------------------------------------------------------------------------------- +# Viewer Parameters +#-------------------------------------------------------------------------------------------- +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize:2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500 + diff --git a/include/Frame.h b/include/Frame.h index e172bf2..29a3613 100644 --- a/include/Frame.h +++ b/include/Frame.h @@ -36,6 +36,9 @@ #include #include + +#include + namespace ORB_SLAM2 { #define FRAME_GRID_ROWS 48 @@ -50,7 +53,7 @@ class Frame EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Constructor for Monocular VI - Frame(const cv::Mat &imGray, const double &timeStamp, const std::vector &vimu, ORBextractor* extractor,ORBVocabulary* voc, + Frame(const cv::Mat &imGray, const double &timeStamp, const IMUData::vector_t &vimu, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, KeyFrame* pLastKF=NULL); void ComputeIMUPreIntSinceLastFrame(const Frame* pLastF, IMUPreintegrator& imupreint) const; @@ -63,7 +66,8 @@ class Frame void SetNavStateBiasAcc(const Vector3d &ba); // IMU Data from last Frame to this Frame - std::vector mvIMUDataSinceLastFrame; + // std::vector mvIMUDataSinceLastFrame; + IMUData::vector_t mvIMUDataSinceLastFrame; // For pose optimization, use as prior and prior information(inverse covariance) Matrix mMargCovInv; diff --git a/include/KeyFrame.h b/include/KeyFrame.h index 5b5085e..d580e74 100644 --- a/include/KeyFrame.h +++ b/include/KeyFrame.h @@ -34,6 +34,8 @@ #include "IMU/NavState.h" #include "IMU/IMUPreintegrator.h" +#include + namespace ORB_SLAM2 { @@ -47,13 +49,13 @@ class KeyFrame public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB, std::vector vIMUData, KeyFrame* pLastKF=NULL); + KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB, IMUData::vector_t vIMUData, KeyFrame* pLastKF=NULL); KeyFrame* GetPrevKeyFrame(void); KeyFrame* GetNextKeyFrame(void); void SetPrevKeyFrame(KeyFrame* pKF); void SetNextKeyFrame(KeyFrame* pKF); - std::vector GetVectorIMUData(void); + IMUData::vector_t GetVectorIMUData(void); void AppendIMUDataToFront(KeyFrame* pPrevKF); void ComputePreInt(void); @@ -92,7 +94,7 @@ class KeyFrame // IMU Data from lask KeyFrame to this KeyFrame std::mutex mMutexIMUData; - std::vector mvIMUData; + IMUData::vector_t mvIMUData; IMUPreintegrator mIMUPreInt; diff --git a/include/System.h b/include/System.h index efefd98..8a8d876 100644 --- a/include/System.h +++ b/include/System.h @@ -82,7 +82,7 @@ class System // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp); - cv::Mat TrackMonoVI(const cv::Mat &im, const std::vector &vimu, const double ×tamp); + cv::Mat TrackMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double ×tamp); // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); diff --git a/include/Tracking.h b/include/Tracking.h index d95117e..7646377 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -75,11 +75,11 @@ class Tracking bool TrackLocalMapWithIMU(bool bMapUpdated=false); ConfigParam* mpParams; - cv::Mat GrabImageMonoVI(const cv::Mat &im, const std::vector &vimu, const double ×tamp); + cv::Mat GrabImageMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double ×tamp); // IMU Data since last KF. Append when new data is provided // Should be cleared in 1. initialization beginning, 2. new keyframe created. - std::vector mvIMUSinceLastKF; - IMUPreintegrator GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const std::vector& vIMUSInceLastKF); + IMUData::vector_t mvIMUSinceLastKF; + IMUPreintegrator GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const IMUData::vector_t& vIMUSInceLastKF); IMUPreintegrator GetIMUPreIntSinceLastFrame(Frame* pCurF, Frame* pLastF); diff --git a/src/Frame.cc b/src/Frame.cc index 15e8ffa..a2d013f 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -43,7 +43,7 @@ void Frame::ComputeIMUPreIntSinceLastFrame(const Frame* pLastF, IMUPreintegrator // Reset pre-integrator first IMUPreInt.reset(); - const std::vector& vIMUSInceLastFrame = mvIMUDataSinceLastFrame; + const IMUData::vector_t& vIMUSInceLastFrame = mvIMUDataSinceLastFrame; Vector3d bg = pLastF->GetNavState().Get_BiasGyr(); Vector3d ba = pLastF->GetNavState().Get_BiasAcc(); @@ -140,7 +140,7 @@ void Frame::SetNavState(const NavState& ns) mNavState = ns; } -Frame::Frame(const cv::Mat &imGray, const double &timeStamp, const std::vector &vimu, ORBextractor* extractor,ORBVocabulary* voc, +Frame::Frame(const cv::Mat &imGray, const double &timeStamp, const IMUData::vector_t &vimu, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, KeyFrame* pLastKF) :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast(NULL)), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) diff --git a/src/IMU/imudata.h b/src/IMU/imudata.h index 2aba5ab..179e69d 100644 --- a/src/IMU/imudata.h +++ b/src/IMU/imudata.h @@ -2,6 +2,8 @@ #define IMUDATA_H #include +#include +#include namespace ORB_SLAM2 { @@ -13,6 +15,8 @@ class IMUData public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef std::vector > vector_t; + // covariance of measurement static Matrix3d _gyrMeasCov; static Matrix3d _accMeasCov; @@ -40,6 +44,8 @@ class IMUData Vector3d _g; //gyr data Vector3d _a; //acc data double _t; //timestamp + + }; } diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc index c56a9b5..76c4225 100644 --- a/src/KeyFrame.cc +++ b/src/KeyFrame.cc @@ -83,7 +83,7 @@ void KeyFrame::SetNextKeyFrame(KeyFrame* pKF) mpNextKeyFrame = pKF; } -std::vector KeyFrame::GetVectorIMUData(void) +IMUData::vector_t KeyFrame::GetVectorIMUData(void) { unique_lock lock(mMutexIMUData); return mvIMUData; @@ -91,7 +91,7 @@ std::vector KeyFrame::GetVectorIMUData(void) void KeyFrame::AppendIMUDataToFront(KeyFrame* pPrevKF) { - std::vector vimunew = pPrevKF->GetVectorIMUData(); + IMUData::vector_t vimunew = pPrevKF->GetVectorIMUData(); { unique_lock lock(mMutexIMUData); vimunew.insert(vimunew.end(), mvIMUData.begin(), mvIMUData.end()); @@ -268,7 +268,7 @@ void KeyFrame::ComputePreInt(void) //------------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------------- -KeyFrame::KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB, std::vector vIMUData, KeyFrame* pPrevKF): +KeyFrame::KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB, IMUData::vector_t vIMUData, KeyFrame* pPrevKF): mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS), mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv), mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 895c576..7e8f3fe 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -25,6 +25,7 @@ #include #include "Converter.h" +#include namespace ORB_SLAM2 { @@ -42,7 +43,7 @@ class KeyFrameInit KeyFrameInit* mpPrevKeyFrame; cv::Mat Twc; IMUPreintegrator mIMUPreInt; - std::vector mvIMUData; + IMUData::vector_t mvIMUData; Vector3d bg; @@ -864,8 +865,9 @@ void LocalMapping::AddToLocalWindow(KeyFrame* pKF) KeyFrame* pKF0 = mlLocalKeyFrames.front(); while(mlLocalKeyFrames.size() < mnLocalWindowSize && pKF0->GetPrevKeyFrame()!=NULL) { - pKF0 = pKF0->GetPrevKeyFrame(); + // exchange following two line mlLocalKeyFrames.push_front(pKF0); + pKF0 = pKF0->GetPrevKeyFrame(); } } } diff --git a/src/System.cc b/src/System.cc index 5347fd4..f4bf6dd 100644 --- a/src/System.cc +++ b/src/System.cc @@ -98,7 +98,7 @@ void System::SaveKeyFrameTrajectoryNavState(const string &filename) cout << endl << "NavState trajectory saved!" << endl; } -cv::Mat System::TrackMonoVI(const cv::Mat &im, const std::vector &vimu, const double ×tamp) +cv::Mat System::TrackMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double ×tamp) { if(mSensor!=MONOCULAR) { diff --git a/src/Tracking.cc b/src/Tracking.cc index daa028e..343d223 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -400,7 +400,7 @@ bool Tracking::TrackWithIMU(bool bMapUpdated) } -IMUPreintegrator Tracking::GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const std::vector& vIMUSInceLastKF) +IMUPreintegrator Tracking::GetIMUPreIntSinceLastKF(Frame* pCurF, KeyFrame* pLastKF, const IMUData::vector_t& vIMUSInceLastKF) { // Reset pre-integrator first IMUPreintegrator IMUPreInt; @@ -461,7 +461,7 @@ IMUPreintegrator Tracking::GetIMUPreIntSinceLastFrame(Frame* pCurF, Frame* pLast } -cv::Mat Tracking::GrabImageMonoVI(const cv::Mat &im, const std::vector &vimu, const double ×tamp) +cv::Mat Tracking::GrabImageMonoVI(const cv::Mat &im, const IMUData::vector_t &vimu, const double ×tamp) { mvIMUSinceLastKF.insert(mvIMUSinceLastKF.end(), vimu.begin(),vimu.end()); mImGray = im; @@ -1138,7 +1138,7 @@ void Tracking::MonocularInitialization() void Tracking::CreateInitialMapMonocular() { // The first imu package include 2 parts for KF1 and KF2 - vector vimu1,vimu2; + IMUData::vector_t vimu1,vimu2; for(size_t i=0; i