输入文件
./Examples/Monocular-Inertial/mono_inertial_euroc
./Vocabulary/ORBvoc.txt
./Examples/Monocular-Inertial/EuRoC.yaml 存储相机/imu等初始化参数
./Datasets/EuRoC/MH01
./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH01.txt 存相机时间戳
dataset-MH01_monoi
imu0/data.csv
中存储imu数据,时间戳,陀螺仪x y z,加速度x y z
1403636579758555392,-0.099134701513277898,0.14730578886832138,0.02722713633111154,8.1476917083333333,-0.37592158333333331,-2.4026292499999999
cam0/data.csv
中存储相机数据,时间戳,图片名字
1403636579763555584,1403636579763555584.png
两个相机的时间戳之间大概会有10个imu数据
入口函数/框架结构
【1】Examples/Monocular-Inertial/mono_inertial_euroc.cc
主要功能:
(1)读入数据,得到时间戳、所有帧、帧间imu数据
(2)新建System函数,执行TrackMonocular()
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);
SLAM.TrackMonocular(im,tframe,vImuMeas);
【2】system.cc --- TrackMonocular()
主要功能:
(1)执行GrabImageMonocular()
(2)返回相机位姿
if (mSensor == System::IMU_MONOCULAR)
for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
【3】tracking.cc --- GrabImageMonocular()
主要功能:
(1)构建Frame表征当前帧,用特这点描述子来表示当前帧
(2)调用了Track()函数
Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename)
{
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);
mCurrentFrame.mNameFile = filename;
mCurrentFrame.mnDataset = mnNumDataset;
lastID = mCurrentFrame.mnId;
Track();
return mCurrentFrame.GetPose();
}
【4】tracking.cc --- Track()
函数主体部分
下面为我认为的主体部分(还有纯定位部分没有考虑进去):
(1)imu预积分PreintegrateIMU()
(2)单目imu初始化MonocularInitialization();
(3)初始化完成进行跟踪,跟踪同步建图CheckReplacedInLastFrame(),两种跟踪模式【纯用关键帧TrackReferenceKeyFrame(),融合imu的跟踪TrackWithMotionModel()】;
(4)跟踪临时丢失用imu复原;
(5)跟踪完全丢失重置地图或新建地图;
(6)跟踪成功,TrackLocalMap()进行优化位姿,增加地图点
();
if(mState==NOT_INITIALIZED){
MonocularInitialization();
}else{
if(mState==OK){
CheckReplacedInLastFrame();
if((!mbVelocity && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2)
bOK = TrackReferenceKeyFrame();
else
bOK = TrackWithMotionModel();
if(!bOK)
bOK = TrackReferenceKeyFrame();
if (!bOK)
mState = LOST;
mState = RECENTLY_LOST;
}
else if(mState == RECENTLY_LOST){
PredictStateIMU();
}
else if(mState == LOST){
mpSystem->ResetActiveMap();
CreateMapInAtlas();
}
}
TrackLocalMap();
【5】tracking.cc --- TrackLocalMap()
主体部分
这个函数主要是利用局部窗口的关键帧和地图点,为当前帧找到更多的匹配地图点,再进行位姿优化,使得位姿更加准确。
以下三个优化函数都在optimizer.cc中
UpdateLocalMap();
SearchLocalPoints();
PoseOptimization(&mCurrentFrame);
PoseInertialOptimizationLastFrame(&mCurrentFrame);
PoseInertialOptimizationLastKeyFrame(&mCurrentFrame);
【6】tracking.cc --- PreintegrateIMU()
IMU预积分,主要实现在IMUType.cc中,IntegrateNewMeasurement(acc,angVel,tstep)
??目前不明白IntegrateNewMeasurement的具体操作,好像只积分了一个imu数据,怎么就能表示到上一关键帧的呢??
IMU::Preintegrated* pImuPreintegratedFromLastFrame = new IMU::Preintegrated(mLastFrame.mImuBias,mCurrentFrame.mImuCalib);
for(int i=0; i<n; i++){
mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep);
pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep);
}
mCurrentFrame.mpImuPreintegratedFrame = pImuPreintegratedFromLastFrame;
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame;
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)