- assert 断言错误:Optimizer.cc:4436:
assert(mit->second>=3);
,但实际运行不满足该条件的情况还是蛮多的。 - Optimizer.cc:7305 :空指针问题
if (!pFp->mpcpi)
Verbose::PrintMess("pFp->mpcpi does not exist!!!\nPrevious Frame " + to_string(pFp->mnId), Verbose::VERBOSITY_NORMAL);
若pFp->mpcpi
为空指针,仅输出提示信息,但仍进行后续操作,导致空指针错误
此错误实际是由于该帧图像未分配IMU数据,导致IMU数据 vector为空
其他注意问题:
- ORB-SLAM3双目IMU版本默认首帧IMU时间戳先于图像时间戳,采集自己数据集时应注意相关改动
- ORB-SLAM3为每张图像分配IMU的策略是,当IMU时间戳满足:
IMU.timestamp>lastImg.timestamp&&IMU.timestamp<=currentImg.timestamp
,则将IMU数据分配给当前图像。若自己采集的数据两帧图像间无IMU数据,会导致Tracking.cc:1536行 const float t12 = mCurrentFrame.mpImuPreintegratedFrame->dT
中的mpImuPreintegratedFrame
为空指针。 - ORB-SLAM3 依赖pangolin0.5,在使用其他版本时,在地图保存阶段和系统关闭阶段可能遇到coredump问题,在ubuntu20.04 上安装pangolin-0.5 见1
- 知乎上也有人总结了相关bug,见知乎。
逻辑错误:
bool LoopClosing::DetectAndReffineSim3FromLastKF
中利用OptimizeSim3
优化匹配帧到当前帧的变换矩阵gScm
后,最终的结果并不是基于优化后的结果给出的
解决方法:
将
g2o::Sim3 gScw_estimation(gScw.rotation(), gScw.translation(),1.0);
修改为:
Sophus::SE3d mTmw = pMatchedKF->GetPose().cast<double>();
g2o::Sim3 gSmw(mTmw.unit_quaternion(),mTmw.translation(),1.0);
g2o::Sim3 gScw_estimation=gScm*gSmw;
LoopClosing::FindMatchesByProjection
中,基于重投影将相似帧的地图点和当前帧的特征点进行关联。利用相似帧的一级共视帧以组成相似帧集合vpCovKFm
,若vpCovKFm
帧数小于10,代码本意是通过搜索相似帧的二级共视帧将`vpCovKFm``帧数补充至10,但代码中直接将所有二级共视帧进行添加。
解决方法:
修改前:
// if(nInitialCov < nNumCovisibles)//此处代码逻辑为,若相似帧的一级共视帧集合的帧数小于10,则添加相似帧的所有二级帧集合;这样做使得集合数量大大增加,集合中的帧地图点很多和当前帧都无关联关系
// {
// for(int i=0; i<nInitialCov; ++i)
// {
// vector<KeyFrame*> vpKFs = vpCovKFm[i]->GetBestCovisibilityKeyFrames(nNumCovisibles);
// int nInserted = 0;
// int j = 0;
// while(j < vpKFs.size() && nInserted < nNumCovisibles)
// {
// if(spCheckKFs.find(vpKFs[j]) == spCheckKFs.end() && spCurrentCovisbles.find(vpKFs[j]) == spCurrentCovisbles.end())
// {
// spCheckKFs.insert(vpKFs[j]);
// ++nInserted;
// }
// ++j;
// }
// vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end());
// }
// }
修改后:
if(nInitialCov < nNumCovisibles)//将上述代码逻辑修改为,若相似帧的一级共视帧集合的帧数小于10,则添加相似帧的二级帧直至集合帧数超过10;
{
for(int i=0; i<nInitialCov; ++i)
{
vector<KeyFrame*> vpKFs = vpCovKFm[i]->GetBestCovisibilityKeyFrames(nNumCovisibles);
int nInserted = 0;
int j = 0;
while(j < vpKFs.size() && nInserted < nNumCovisibles)
{
if(spCheckKFs.find(vpKFs[j]) == spCheckKFs.end() && spCurrentCovisbles.find(vpKFs[j]) == spCurrentCovisbles.end())
{
spCheckKFs.insert(vpKFs[j]);
++nInserted;
vpCovKFm.push_back(vpKFs[j]);
}
++j;
}
if(nInserted+nInitialCov>nNumCovisibles)
break;
// vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end());
}
}
LoopClosing::DetectAndReffineSim3FromLastKF
阈值设置错误
查看该部分代码可知:
第一个nNumProjMatches
的数值一定是大于numOptMatches
的,但是函数的代码流程是
bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
{
set<MapPoint*> spAlreadyMatchedMPs;
nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs);
int nProjMatches = 30;
int nProjOptMatches = 50;
int nProjMatchesRep = 100;
if(nNumProjMatches >=nProjMatches)//30
{
优化代码
if(numOptMatches > nProjOptMatches)//50
{
...
}
}
return false;
}
此处的阈值nProjMatches
设置得不合理,会使得函数进行多余的优化代码计算后,才发现满足不了第二个阈值限制的要求。
LoopClosing
中MergeLocal2
的mvpMergeConnectedKFs
在使用前应该先清除
mvpMergeConnectedKFs.clear();//修改后
mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);//融合帧及融合帧的共视图