12345qiupeng
/
orb_slam3_details
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/electech6/ORB_SLAM3_detailed_comments

master
laobai 2022-04-19 22:51:20 +08:00
parent 4cf21e5ed5 81c07bfe77
commit b9b28931ec
9 changed files with 37 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -6,7 +6,7 @@
![课程大纲](https://github.com/electech6/ORB_SLAM3_detailed_comments/blob/master/outline.png) ![课程大纲](https://github.com/electech6/ORB_SLAM3_detailed_comments/blob/master/outline.png)
[ORB-SLAM3 逐行源码讲解](https://appafc4omci9700.h5.xiaoeknow.com/content_page/eyJ0eXBlIjoiMyIsInJlc291cmNlX3R5cGUiOjI1LCJyZXNvdXJjZV9pZCI6IiIsImFwcF9pZCI6ImFwcGFmYzRvbWNpOTcwMCIsInByb2R1Y3RfaWQiOiJ0ZXJtXzYxMzcxNTEwODY4MzNfQXRXa2YzIiwiZXhwYW5kX2RhdGEiOltdfQ) [ORB-SLAM3 逐行源码讲解](https://cvlife.net/detail/term_6255372036a53_o5VfgR/25)
---- ----
# ORB-SLAM3 # ORB-SLAM3

2
include/ImuTypes.h
View File

@ -223,7 +223,7 @@ public:
private: private:
// Updated bias // Updated bias
Bias bu; Bias bu; //更新后的零偏
// Dif between original and updated bias // Dif between original and updated bias
// This is used to compute the updated values of the preintegration // This is used to compute the updated values of the preintegration
Eigen::Matrix<float,6,1> db; Eigen::Matrix<float,6,1> db;

BIN
outline.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 112 KiB

After

Width:  |  Height:  |  Size: 103 KiB

12
src/G2oTypes.cc
View File

@ -579,12 +579,12 @@ EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast<double>(
void EdgeInertial::computeError() void EdgeInertial::computeError()
{ {
// TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]); const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]); //位姿Ti
const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]); const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]); //速度vi
const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]); const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]); //零偏Bgi
const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]); const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]); //零偏Bai
const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]); const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]); //位姿Tj
const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]); const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]); //速度vj
const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]); const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]);
const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>(); const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast<double>();
const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast<double>(); const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast<double>();

2
src/ImuTypes.cc
View File

@ -242,7 +242,7 @@ void Preintegrated::Reintegrate()
* *
* @param[in] acceleration 加速度计数据 * @param[in] acceleration 加速度计数据
* @param[in] angVel 陀螺仪数据 * @param[in] angVel 陀螺仪数据
* @param[in] dt 两帧之间时间差 * @param[in] dt 两图像 帧之间时间差
*/ */
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt) void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt)
{ {

4
src/LocalMapping.cc
View File

@ -1547,10 +1547,8 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
pKF = pKF->mPrevKF; pKF = pKF->mPrevKF;
} }
lpKF.push_front(pKF); lpKF.push_front(pKF);
// 以相同内容再构建一个vector // 同样内容再构建一个和lpKF一样的容器vpKF
vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end()); vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());
// TODO 跟上面重复？
if(vpKF.size()<nMinKF) if(vpKF.size()<nMinKF)
return; return;

39
src/LoopClosing.cc
View File

@ -178,9 +178,9 @@ void LoopClosing::Run()
} }
// If inertial, force only yaw // If inertial, force only yaw
// 如果是imu模式并且完成了初始化,强制将焊接变换的 roll 和 pitch 设为0 // 如果是imu模式并且完成了初始化,强制将焊接变换的 roll 和 pitch 设为0
// 可以理解成两个坐标轴都经过了imu初始化，肯定都是水平的 // 通过物理约束来保证两个坐标轴都是水平的
if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) && if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO) &&
mpCurrentKF->GetMap()->GetIniertialBA1()) mpCurrentKF->GetMap()->GetIniertialBA1()) // TODO, maybe with GetIniertialBA1
{ {
Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix()); Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix());
phi(0)=0; phi(0)=0;
@ -465,6 +465,7 @@ bool LoopClosing::NewDetectCommonRegions()
mnLoopNumCoincidences++; mnLoopNumCoincidences++;
// 不再参与新的回环检测 // 不再参与新的回环检测
mpLoopLastCurrentKF->SetErase(); mpLoopLastCurrentKF->SetErase();
// 将当前关键帧作为上次关键帧
mpLoopLastCurrentKF = mpCurrentKF; mpLoopLastCurrentKF = mpCurrentKF;
mg2oLoopSlw = gScw; // 记录当前优化的结果为{last T_cw}即为 T_lw mg2oLoopSlw = gScw; // 记录当前优化的结果为{last T_cw}即为 T_lw
// 记录匹配到的点 // 记录匹配到的点
@ -505,7 +506,8 @@ bool LoopClosing::NewDetectCommonRegions()
//Merge candidates //Merge candidates
bool bMergeDetectedInKF = false; bool bMergeDetectedInKF = false;
// Step 3.2 融合的时序几何校验: 注意初始化时mnMergeNumCoincidences=0, 所以可以先跳过看后面 // Step 3.2 融合的时序几何校验: 注意初始化时mnMergeNumCoincidences=0, 所以可以先跳过看后面
// 如果成功验证总次数大于0 // mnMergeNumCoincidences表示成功校验总次数，初始化为0
// 会先经过后面共视几何校验，如果小于3，会进到如下判断开始时序几何校验
if(mnMergeNumCoincidences > 0) if(mnMergeNumCoincidences > 0)
{ {
// Find from the last KF candidates // Find from the last KF candidates
@ -534,7 +536,7 @@ bool LoopClosing::NewDetectCommonRegions()
mpMergeLastCurrentKF = mpCurrentKF; mpMergeLastCurrentKF = mpCurrentKF;
mg2oMergeSlw = gScw; mg2oMergeSlw = gScw;
mvpMergeMatchedMPs = vpMatchedMPs; mvpMergeMatchedMPs = vpMatchedMPs;
// 如果验证数大于等于3则为成功回环 // 如果验证数大于等于3则为成功
mbMergeDetected = mnMergeNumCoincidences >= 3; mbMergeDetected = mnMergeNumCoincidences >= 3;
} }
// 如果没找到共同区域(时序验证失败一次) // 如果没找到共同区域(时序验证失败一次)
@ -690,8 +692,9 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame*
// 若匹配的数量大于一定的数目 // 若匹配的数量大于一定的数目
if(numOptMatches > nProjOptMatches) if(numOptMatches > nProjOptMatches)
{ {
//!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm.t //!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm^-1
g2o::Sim3 gScw_estimation(gScw.rotation(), gScw.translation(),1.0); g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)),
Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0);
vector<MapPoint*> vpMatchedMP; vector<MapPoint*> vpMatchedMP;
vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL)); vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
@ -1548,8 +1551,8 @@ void LoopClosing::CorrectLoop()
mpAtlas->InformNewBigChange(); mpAtlas->InformNewBigChange();
// Add loop edge // Add loop edge
// Step 8. 添加当前帧与闭环匹配帧之间的边（这个连接关系不优化） // Step 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
// !感觉这两句话应该放在OptimizeEssentialGraph之前，因为OptimizeEssentialGraph的步骤4.2中有优化 // 它在下一次的Essential Graph里面使用
mpLoopMatchedKF->AddLoopEdge(mpCurrentKF); mpLoopMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpLoopMatchedKF); mpCurrentKF->AddLoopEdge(mpLoopMatchedKF);
@ -2110,8 +2113,8 @@ void LoopClosing::MergeLocal()
bool bStop = false; bool bStop = false;
// 为Local BA的接口, 把set转为vector // 为Local BA的接口, 把set转为vector
// Step 7 在缝合(Welding)区域进行local BA // Step 7 在缝合(Welding)区域进行local BA
vpLocalCurrentWindowKFs.clear(); vpLocalCurrentWindowKFs.clear(); //当前关键帧的窗口
vpMergeConnectedKFs.clear(); vpMergeConnectedKFs.clear(); //融合关键帧的窗口
std::copy(spLocalWindowKFs.begin(), spLocalWindowKFs.end(), std::back_inserter(vpLocalCurrentWindowKFs)); std::copy(spLocalWindowKFs.begin(), spLocalWindowKFs.end(), std::back_inserter(vpLocalCurrentWindowKFs));
std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(), std::back_inserter(vpMergeConnectedKFs)); std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(), std::back_inserter(vpMergeConnectedKFs));
if (mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) if (mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD)
@ -2272,8 +2275,8 @@ void LoopClosing::MergeLocal()
// Essential graph 优化后可以重新开始局部建图了 // Essential graph 优化后可以重新开始局部建图了
mpLocalMapper->Release(); mpLocalMapper->Release();
// 如果之前停掉了全局的BA,就开启全局BA // 全局的BA（永远不会执行）
// 这里没有imu, 所以isImuInitialized一定是false, 所以第二个条件（当前地图关键帧数量小于200且地图只有一个）一定是true // 这里没有imu, 所以isImuInitialized一定是false, 此时地图融合Atlas至少2个地图，所以第二个条件也一定是false
// Step 9 全局BA // Step 9 全局BA
if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1))) if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1)))
{ {
@ -2404,7 +2407,7 @@ void LoopClosing::MergeLocal2()
Optimizer::InertialOptimization(pCurrentMap,bg,ba); Optimizer::InertialOptimization(pCurrentMap,bg,ba);
IMU::Bias b (ba[0],ba[1],ba[2],bg[0],bg[1],bg[2]); IMU::Bias b (ba[0],ba[1],ba[2],bg[0],bg[1],bg[2]);
unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
// 用优化得到的 bias 更新地图 // 用优化得到的 bias 更新普通帧位姿
mpTracker->UpdateFrameIMU(1.0f,b,mpTracker->GetLastKeyFrame()); mpTracker->UpdateFrameIMU(1.0f,b,mpTracker->GetLastKeyFrame());
// Set map initialized // Set map initialized
@ -2457,9 +2460,9 @@ void LoopClosing::MergeLocal2()
pMergeMap->EraseMapPoint(pMPi); pMergeMap->EraseMapPoint(pMPi);
} }
// ? BUG! pMergeMap没有设置为BAD // ? BUG! pMergeMap没有设置为BAD
// ? 感觉应该加入如下代码吧？ // ? 应该加入如下代码吧？
// ? mpAtlas->SetMapBad(pCurrentMap); // ? mpAtlas->SetMapBad(pMergeMap);
// Save non corrected poses (already merged maps) // Save non corrected poses (already merged maps)
// 存下所有关键帧在融合矫正之前的位姿 // 存下所有关键帧在融合矫正之前的位姿
vector<KeyFrame*> vpKFs = pCurrentMap->GetAllKeyFrames(); vector<KeyFrame*> vpKFs = pCurrentMap->GetAllKeyFrames();
@ -2510,7 +2513,7 @@ void LoopClosing::MergeLocal2()
vector<MapPoint*> vpCheckFuseMapPoint; // MapPoint vector from current map to allow to fuse duplicated points with the old map (merge) vector<MapPoint*> vpCheckFuseMapPoint; // MapPoint vector from current map to allow to fuse duplicated points with the old map (merge)
vector<KeyFrame*> vpCurrentConnectedKFs; vector<KeyFrame*> vpCurrentConnectedKFs;
// 为后续SearchAndFuse及welding BA准备数据 // 为后续SearchAndFuse准备数据
// 拿出融合帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧 // 拿出融合帧的局部窗口, 确保最后是(1+5), 1: 融合帧自己 2: 5个共视关键帧
mvpMergeConnectedKFs.push_back(mpMergeMatchedKF); mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);
vector<KeyFrame*> aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames(); vector<KeyFrame*> aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();

4
src/Optimizer.cc
View File

@ -4551,8 +4551,10 @@ int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame *pFrame, bool bRecInit
cv::KeyPoint kpUn; cv::KeyPoint kpUn;
// Left monocular observation // Left monocular observation
// 这里说的Left monocular包含两种情况：1.单目情况 2.两个相机情况下的相机1
if((!bRight && pFrame->mvuRight[i]<0) || i < Nleft) if((!bRight && pFrame->mvuRight[i]<0) || i < Nleft)
{ {
//如果是两个相机情况下的相机1
if(i < Nleft) // pair left-right if(i < Nleft) // pair left-right
kpUn = pFrame->mvKeys[i]; kpUn = pFrame->mvKeys[i];
else else
@ -4934,8 +4936,10 @@ int Optimizer::PoseInertialOptimizationLastFrame(Frame *pFrame, bool bRecInit)
{ {
cv::KeyPoint kpUn; cv::KeyPoint kpUn;
// Left monocular observation // Left monocular observation
// 这里说的Left monocular包含两种情况：1.单目情况 2.两个相机情况下的相机1
if((!bRight && pFrame->mvuRight[i]<0) || i < Nleft) if((!bRight && pFrame->mvuRight[i]<0) || i < Nleft)
{ {
//如果是两个相机情况下的相机1
if(i < Nleft) // pair left-right if(i < Nleft) // pair left-right
kpUn = pFrame->mvKeys[i]; kpUn = pFrame->mvKeys[i];
else else

2
src/Tracking.cc
View File

@ -3515,11 +3515,13 @@ bool Tracking::TrackLocalMap()
if(!mbMapUpdated) // && (mnMatchesInliers>30)) if(!mbMapUpdated) // && (mnMatchesInliers>30))
{ {
Verbose::PrintMess("TLM: PoseInertialOptimizationLastFrame ", Verbose::VERBOSITY_DEBUG); Verbose::PrintMess("TLM: PoseInertialOptimizationLastFrame ", Verbose::VERBOSITY_DEBUG);
// 使用上一普通帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏
inliers = Optimizer::PoseInertialOptimizationLastFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); inliers = Optimizer::PoseInertialOptimizationLastFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1());
} }
else else
{ {
Verbose::PrintMess("TLM: PoseInertialOptimizationLastKeyFrame ", Verbose::VERBOSITY_DEBUG); Verbose::PrintMess("TLM: PoseInertialOptimizationLastKeyFrame ", Verbose::VERBOSITY_DEBUG);
// 使用上一关键帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏
inliers = Optimizer::PoseInertialOptimizationLastKeyFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); inliers = Optimizer::PoseInertialOptimizationLastKeyFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1());
} }
} }