更新track流程注释
parent
2f44474715
commit
23394ee656
|
@ -193,7 +193,7 @@ protected:
|
||||||
// Fix scale in the stereo/RGB-D case
|
// Fix scale in the stereo/RGB-D case
|
||||||
bool mbFixScale;
|
bool mbFixScale;
|
||||||
|
|
||||||
|
// ? 这里的bool值为什么用mn,疑似bug,后面会用到这个变量自加,但IDE提示bool类型不能自加
|
||||||
bool mnFullBAIdx;
|
bool mnFullBAIdx;
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -1277,7 +1277,10 @@ void Tracking::PreintegrateIMU()
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 跟踪不成功的时候,用初始化好的imu数据做跟踪处理
|
* @brief 跟踪不成功的时候,用初始化好的imu数据做跟踪处理,通过IMU预测状态
|
||||||
|
* 两个地方用到:
|
||||||
|
* 1. 匀速模型计算速度,但并没有给当前帧位姿赋值;
|
||||||
|
* 2. 跟踪丢失时不直接判定丢失,通过这个函数预测当前帧位姿看看能不能拽回来,代替纯视觉中的重定位
|
||||||
*
|
*
|
||||||
* @return true
|
* @return true
|
||||||
* @return false
|
* @return false
|
||||||
|
@ -1289,6 +1292,13 @@ bool Tracking::PredictStateIMU()
|
||||||
Verbose::PrintMess("No last frame", Verbose::VERBOSITY_NORMAL);
|
Verbose::PrintMess("No last frame", Verbose::VERBOSITY_NORMAL);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
// 总结下都在什么时候地图更新,也就是mbMapUpdated为true
|
||||||
|
// 1. 回环或融合
|
||||||
|
// 2. 局部地图LocalBundleAdjustment
|
||||||
|
// 3. IMU三阶段的初始化
|
||||||
|
// 下面的代码流程一模一样,只不过计算时相对的帧不同,地图有更新后相对于上一关键帧做的,反之相对于上一帧
|
||||||
|
// 地图更新后会更新关键帧与MP,所以相对于关键帧更准
|
||||||
|
// 而没更新的话,距离上一帧更近,计算起来误差更小
|
||||||
// 地图更新时,并且上一个图像关键帧存在
|
// 地图更新时,并且上一个图像关键帧存在
|
||||||
if(mbMapUpdated && mpLastKeyFrame)
|
if(mbMapUpdated && mpLastKeyFrame)
|
||||||
{
|
{
|
||||||
|
@ -1297,14 +1307,14 @@ bool Tracking::PredictStateIMU()
|
||||||
const cv::Mat Vwb1 = mpLastKeyFrame->GetVelocity();
|
const cv::Mat Vwb1 = mpLastKeyFrame->GetVelocity();
|
||||||
|
|
||||||
const cv::Mat Gz = (cv::Mat_<float>(3,1) << 0,0,-IMU::GRAVITY_VALUE);
|
const cv::Mat Gz = (cv::Mat_<float>(3,1) << 0,0,-IMU::GRAVITY_VALUE);
|
||||||
// ? mpImuPreintegratedFromLastKF是将图像帧转成imu格式的?
|
// ? mpImuPreintegratedFromLastKF是上一帧关键帧?
|
||||||
const float t12 = mpImuPreintegratedFromLastKF->dT;
|
const float t12 = mpImuPreintegratedFromLastKF->dT;
|
||||||
// 计算当前帧在世界坐标系的位姿,原理都是用预积分的位姿(预积分的值不会变化)与上一帧的位姿(会迭代变化)进行更新
|
// 计算当前帧在世界坐标系的位姿,原理都是用预积分的位姿(预积分的值不会变化)与上一帧的位姿(会迭代变化)进行更新
|
||||||
// 旋转 R_wb2 = R_wb1 * R_b1b2
|
// 旋转 R_wb2 = R_wb1 * R_b1b2
|
||||||
cv::Mat Rwb2 = IMU::NormalizeRotation(Rwb1*mpImuPreintegratedFromLastKF->GetDeltaRotation(mpLastKeyFrame->GetImuBias()));
|
cv::Mat Rwb2 = IMU::NormalizeRotation(Rwb1*mpImuPreintegratedFromLastKF->GetDeltaRotation(mpLastKeyFrame->GetImuBias()));
|
||||||
// 位移
|
// 位移 公式可见Forster论文公式32,只是要变化一下,邱笑晨的那篇文档有详细推导
|
||||||
cv::Mat twb2 = twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mpImuPreintegratedFromLastKF->GetDeltaPosition(mpLastKeyFrame->GetImuBias());
|
cv::Mat twb2 = twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mpImuPreintegratedFromLastKF->GetDeltaPosition(mpLastKeyFrame->GetImuBias());
|
||||||
// 速度
|
// 速度 公式可见Forster论文公式32,只是要变化一下,邱笑晨的那篇文档有详细推导
|
||||||
cv::Mat Vwb2 = Vwb1 + t12*Gz + Rwb1*mpImuPreintegratedFromLastKF->GetDeltaVelocity(mpLastKeyFrame->GetImuBias());
|
cv::Mat Vwb2 = Vwb1 + t12*Gz + Rwb1*mpImuPreintegratedFromLastKF->GetDeltaVelocity(mpLastKeyFrame->GetImuBias());
|
||||||
// 设置当前帧的世界坐标系的相机位姿
|
// 设置当前帧的世界坐标系的相机位姿
|
||||||
mCurrentFrame.SetImuPoseVelocity(Rwb2,twb2,Vwb2);
|
mCurrentFrame.SetImuPoseVelocity(Rwb2,twb2,Vwb2);
|
||||||
|
@ -1323,7 +1333,7 @@ bool Tracking::PredictStateIMU()
|
||||||
const cv::Mat Rwb1 = mLastFrame.GetImuRotation();
|
const cv::Mat Rwb1 = mLastFrame.GetImuRotation();
|
||||||
const cv::Mat Vwb1 = mLastFrame.mVw;
|
const cv::Mat Vwb1 = mLastFrame.mVw;
|
||||||
const cv::Mat Gz = (cv::Mat_<float>(3,1) << 0,0,-IMU::GRAVITY_VALUE);
|
const cv::Mat Gz = (cv::Mat_<float>(3,1) << 0,0,-IMU::GRAVITY_VALUE);
|
||||||
// ? mpImuPreintegratedFrame是当前帧上一帧,未必是关键帧的图像帧?
|
// mpImuPreintegratedFrame是当前帧上一帧,不一定是关键帧
|
||||||
const float t12 = mCurrentFrame.mpImuPreintegratedFrame->dT;
|
const float t12 = mCurrentFrame.mpImuPreintegratedFrame->dT;
|
||||||
|
|
||||||
cv::Mat Rwb2 = IMU::NormalizeRotation(Rwb1*mCurrentFrame.mpImuPreintegratedFrame->GetDeltaRotation(mLastFrame.mImuBias));
|
cv::Mat Rwb2 = IMU::NormalizeRotation(Rwb1*mCurrentFrame.mpImuPreintegratedFrame->GetDeltaRotation(mLastFrame.mImuBias));
|
||||||
|
@ -1727,9 +1737,10 @@ void Tracking::Track()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if (mState == LOST)
|
else if (mState == LOST) // 上一帧为最近丢失且重定位失败时
|
||||||
{
|
{
|
||||||
// Step 6.6 如果是LOST状态
|
// Step 6.6 如果是LOST状态
|
||||||
|
// 开启一个新地图
|
||||||
Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);
|
Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);
|
||||||
|
|
||||||
if (pCurrentMap->KeyFramesInMap()<10)
|
if (pCurrentMap->KeyFramesInMap()<10)
|
||||||
|
@ -1740,7 +1751,7 @@ void Tracking::Track()
|
||||||
}else
|
}else
|
||||||
// 当前地图中关键帧数目超过10,创建新地图
|
// 当前地图中关键帧数目超过10,创建新地图
|
||||||
CreateMapInAtlas();
|
CreateMapInAtlas();
|
||||||
|
// 干掉上一个关键帧
|
||||||
if(mpLastKeyFrame)
|
if(mpLastKeyFrame)
|
||||||
mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
|
mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
|
||||||
|
|
||||||
|
@ -1758,7 +1769,7 @@ void Tracking::Track()
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
}
|
}
|
||||||
else
|
else // 纯定位模式,时间关系暂时不看 TOSEE
|
||||||
{
|
{
|
||||||
// Localization Mode: Local Mapping is deactivated (TODO Not available in inertial mode)
|
// Localization Mode: Local Mapping is deactivated (TODO Not available in inertial mode)
|
||||||
// 只进行跟踪tracking,局部地图不工作
|
// 只进行跟踪tracking,局部地图不工作
|
||||||
|
@ -1856,12 +1867,18 @@ void Tracking::Track()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
// 将最新的关键帧作为当前帧的参考关键帧
|
// 将最新的关键帧作为当前帧的参考关键帧
|
||||||
|
// mpReferenceKF先是上一时刻的参考关键帧,如果当前为新关键帧则变成当前关键帧,如果不是新的关键帧则先为上一帧的参考关键帧,而后经过更新局部关键帧重新确定
|
||||||
if(!mCurrentFrame.mpReferenceKF)
|
if(!mCurrentFrame.mpReferenceKF)
|
||||||
mCurrentFrame.mpReferenceKF = mpReferenceKF;
|
mCurrentFrame.mpReferenceKF = mpReferenceKF;
|
||||||
|
|
||||||
// If we have an initial estimation of the camera pose and matching. Track the local map.
|
// If we have an initial estimation of the camera pose and matching. Track the local map.
|
||||||
// Step 7 在跟踪得到当前帧初始姿态后,现在对local map进行跟踪得到更多的匹配,并优化当前位姿
|
// Step 7 在跟踪得到当前帧初始姿态后,现在对local map进行跟踪得到更多的匹配,并优化当前位姿
|
||||||
// 前面只是跟踪一帧得到初始位姿,这里搜索局部关键帧、局部地图点,和当前帧进行投影匹配,得到更多匹配的MapPoints后进行Pose优化
|
// 前面只是跟踪一帧得到初始位姿,这里搜索局部关键帧、局部地图点,和当前帧进行投影匹配,得到更多匹配的MapPoints后进行Pose优化
|
||||||
|
// 在帧间匹配得到初始的姿态后,现在对local map进行跟踪得到更多的匹配,并优化当前位姿
|
||||||
|
// local map:当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系
|
||||||
|
// 前面主要是两两跟踪(恒速模型跟踪上一帧、跟踪参考帧),这里搜索局部关键帧后搜集所有局部MapPoints,
|
||||||
|
// 然后将局部MapPoints和当前帧进行投影匹配,得到更多匹配的MapPoints后进行Pose优化
|
||||||
|
// If we have an initial estimation of the camera pose and matching. Track the local map.
|
||||||
if(!mbOnlyTracking)
|
if(!mbOnlyTracking)
|
||||||
{
|
{
|
||||||
if(bOK)
|
if(bOK)
|
||||||
|
@ -1890,13 +1907,31 @@ void Tracking::Track()
|
||||||
if(bOK && !mbVO)
|
if(bOK && !mbVO)
|
||||||
bOK = TrackLocalMap();
|
bOK = TrackLocalMap();
|
||||||
}
|
}
|
||||||
|
// 到此为止跟踪确定位姿阶段结束,下面开始做收尾工作和为下一帧做准备
|
||||||
|
|
||||||
|
// 查看到此为止时的两个状态变化
|
||||||
|
// bOK的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---true -->OK 1 跟踪局部地图成功
|
||||||
|
// \ \ \---局部地图跟踪失败---false
|
||||||
|
// \ \---当前帧跟踪失败---false
|
||||||
|
// \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---true -->OK 2 重定位
|
||||||
|
// \ \---局部地图跟踪失败---false
|
||||||
|
// \---重定位失败---false
|
||||||
|
|
||||||
|
//
|
||||||
|
// mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK -->OK 1 跟踪局部地图成功
|
||||||
|
// \ \ \---局部地图跟踪失败---OK -->OK 3 正常跟踪
|
||||||
|
// \ \---当前帧跟踪失败---非OK
|
||||||
|
// \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---非OK
|
||||||
|
// \ \---局部地图跟踪失败---非OK
|
||||||
|
// \---重定位失败---非OK(传不到这里,因为直接return了)
|
||||||
|
// 由上图可知当前帧的状态OK的条件是跟踪局部地图成功,重定位或正常跟踪都可
|
||||||
// Step 8 根据上面的操作来判断是否追踪成功
|
// Step 8 根据上面的操作来判断是否追踪成功
|
||||||
if(bOK)
|
if(bOK)
|
||||||
// 此时还OK才说明跟踪成功了
|
// 此时还OK才说明跟踪成功了
|
||||||
mState = OK;
|
mState = OK;
|
||||||
else if (mState == OK)
|
else if (mState == OK) // 由上图可知只有当正常跟踪成功,但局部地图跟踪失败时执行
|
||||||
{
|
{
|
||||||
|
// 带有IMU时状态变为最近丢失,否则直接丢失
|
||||||
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
|
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
|
||||||
{
|
{
|
||||||
Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL);
|
Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL);
|
||||||
|
@ -1931,6 +1966,7 @@ void Tracking::Track()
|
||||||
pF->mpPrevFrame = new Frame(mLastFrame);
|
pF->mpPrevFrame = new Frame(mLastFrame);
|
||||||
|
|
||||||
// Load preintegration
|
// Load preintegration
|
||||||
|
// 构造预积分处理器
|
||||||
pF->mpImuPreintegratedFrame = new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame);
|
pF->mpImuPreintegratedFrame = new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1938,14 +1974,14 @@ void Tracking::Track()
|
||||||
{
|
{
|
||||||
if(bOK) // 跟踪成功
|
if(bOK) // 跟踪成功
|
||||||
{
|
{
|
||||||
// 当前帧距离上次重定位帧刚好等于1s,重置IMU
|
// 当前帧距离上次重定位帧刚好等于1s,重置IMU(还未实现 TODO)
|
||||||
if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU))
|
if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU))
|
||||||
{
|
{
|
||||||
cout << "RESETING FRAME!!!" << endl;
|
cout << "RESETING FRAME!!!" << endl;
|
||||||
ResetFrameIMU();
|
ResetFrameIMU();
|
||||||
}
|
}
|
||||||
else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30))
|
else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30))
|
||||||
mLastBias = mCurrentFrame.mImuBias;
|
mLastBias = mCurrentFrame.mImuBias; // 没啥用,后面会重新赋值后传给普通帧
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1954,7 +1990,21 @@ void Tracking::Track()
|
||||||
mpFrameDrawer->Update(this);
|
mpFrameDrawer->Update(this);
|
||||||
if(!mCurrentFrame.mTcw.empty())
|
if(!mCurrentFrame.mTcw.empty())
|
||||||
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
|
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
|
||||||
|
// 查看到此为止时的两个状态变化
|
||||||
|
// bOK的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---true
|
||||||
|
// \ \ \---局部地图跟踪失败---false
|
||||||
|
// \ \---当前帧跟踪失败---false
|
||||||
|
// \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---true
|
||||||
|
// \ \---局部地图跟踪失败---false
|
||||||
|
// \---重定位失败---false
|
||||||
|
|
||||||
|
// mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK
|
||||||
|
// \ \ \---局部地图跟踪失败---非OK(IMU时为RECENTLY_LOST)
|
||||||
|
// \ \---当前帧跟踪失败---非OK(地图超过10个关键帧时 RECENTLY_LOST)
|
||||||
|
// \---上一帧跟踪失败(RECENTLY_LOST)---重定位成功---局部地图跟踪成功---OK
|
||||||
|
// \ \ \---局部地图跟踪失败---LOST
|
||||||
|
// \ \---重定位失败---LOST(传不到这里,因为直接return了)
|
||||||
|
// \--上一帧跟踪失败(LOST)--LOST(传不到这里,因为直接return了)
|
||||||
// Step 9 如果跟踪成功 或 最近刚刚跟丢,更新速度,清除无效地图点,按需创建关键帧
|
// Step 9 如果跟踪成功 或 最近刚刚跟丢,更新速度,清除无效地图点,按需创建关键帧
|
||||||
if(bOK || mState==RECENTLY_LOST)
|
if(bOK || mState==RECENTLY_LOST)
|
||||||
{
|
{
|
||||||
|
@ -1971,7 +2021,7 @@ void Tracking::Track()
|
||||||
else
|
else
|
||||||
// 否则速度为空
|
// 否则速度为空
|
||||||
mVelocity = cv::Mat();
|
mVelocity = cv::Mat();
|
||||||
|
// 使用IMU积分的位姿显示
|
||||||
if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
|
if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
|
||||||
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
|
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
|
||||||
|
|
||||||
|
@ -2066,7 +2116,15 @@ void Tracking::Track()
|
||||||
// 保存上一帧的数据,当前帧变上一帧
|
// 保存上一帧的数据,当前帧变上一帧
|
||||||
mLastFrame = Frame(mCurrentFrame);
|
mLastFrame = Frame(mCurrentFrame);
|
||||||
}
|
}
|
||||||
|
// 查看到此为止
|
||||||
|
// mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK
|
||||||
|
// \ \ \---局部地图跟踪失败---非OK(IMU时为RECENTLY_LOST)
|
||||||
|
// \ \---当前帧跟踪失败---非OK(地图超过10个关键帧时 RECENTLY_LOST)
|
||||||
|
// \---上一帧跟踪失败(RECENTLY_LOST)---重定位成功---局部地图跟踪成功---OK
|
||||||
|
// \ \ \---局部地图跟踪失败---LOST
|
||||||
|
// \ \---重定位失败---LOST(传不到这里,因为直接return了)
|
||||||
|
// \--上一帧跟踪失败(LOST)--LOST(传不到这里,因为直接return了)
|
||||||
|
// last.记录位姿信息,用于轨迹复现
|
||||||
// Step 11 记录位姿信息,用于最后保存所有的轨迹
|
// Step 11 记录位姿信息,用于最后保存所有的轨迹
|
||||||
if(mState==OK || mState==RECENTLY_LOST)
|
if(mState==OK || mState==RECENTLY_LOST)
|
||||||
{
|
{
|
||||||
|
|
Loading…
Reference in New Issue