ORB-SLAM2 소스 코드 읽기(2)
로컬 매핑 스레드
우리는 mono_tim.cc 샘플 프로그램을 예로 들어, 이 예에서 먼저 ORB-SLAM 시스템을 구축해야 합니다.
// Create SLAM system. It initializes all system threads and gets ready to process frames.
//构建SLAM系统，调用有参构造函数，传入参数为：ORB字典，参数配置文件，相机类型
ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);
System의 함수 정의에서는 Local Mapping, Loop Closing 등의 쓰레드가 초기화되는데, 자세한 사항은 이전 블로그를 참조하시기 바랍니다.
이전 블로그에서 ORB-SLAM의 메인 쓰레드를 소개했는데, 트래킹 쓰레드, 오늘은 나머지 2개의 쓰레드를 위주로 소개하겠습니다.
//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
Local Mapping 쓰레드의 초기화는 주로 ORB_SLAM2::LocalMapping::Run() 함수를 통해 구현되는 것을 볼 수 있다. 이 함수의 구현을 살펴보자.
void LocalMapping::Run()
{
mbFinished = false;
while(1)
{
// Tracking will see that Local Mapping is busy
SetAcceptKeyFrames(false);
// Check if there are keyframes in the queue
if(CheckNewKeyFrames()) //若队列中有新的关键帧
{
// BoW conversion and insertion in Map
ProcessNewKeyFrame();
// Check recent MapPoints
MapPointCulling();
// Triangulate new MapPoints
CreateNewMapPoints();
if(!CheckNewKeyFrames()) //队列中的关键帧处理完毕
{
//检查Current KF的Covisible KFS,对重复构建的MapPoints进行融合
// Find more matches in neighbor keyframes and fuse point duplications
SearchInNeighbors();
}
mbAbortBA = false;
if(!CheckNewKeyFrames() && !stopRequested()) //没有新的KF且无停止LoopClosing线程的请求
{
// Local BA
if(mpMap->KeyFramesInMap()>2) //地图中的关键帧要大于两帧
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
//剔除冗余关键帧
// Check redundant local Keyframes
KeyFrameCulling();
}
//筛选后的关键帧插入LoopClosing线程
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
else if(Stop())
{
//确保KF筛选完成
// Safe area to stop
while(isStopped() && !CheckFinish())
{
usleep(3000);
}
if(CheckFinish())
break;
}
//在收到请求后可以删除新添加的KF和MapPoints
ResetIfRequested();
// Tracking will see that Local Mapping is busy
SetAcceptKeyFrames(true);
//等待插入完成
if(CheckFinish())
break;
usleep(3000);
}
SetFinish();
}
우리는 이 사진에 대해 프로그램을 다시 볼 수 있습니다
주요 단계는 다음과 같습니다.
- Tracking 쓰레드로부터 삽입된 KF를 받아 전처리를 하고
, Tracking 쓰레드가 삽입할 KF를 결정하면, 실제로 KF를 Map에 삽입하는 동작을 완료하지 않는다. 많은 업데이트 작업
업데이트 Covisibility Graph,
업데이트 스패닝 트리:
새 KF의 BoW 계산 - 저품질 MapPoint 제거
Map에 저장된 MapPoint는 고품질이어야 하므로(좋은 추적, 올바른 삼각 측량) 품질이 좋지 않은 MapPoint를 제거하려면 여기에서 몇 가지 조치를 취해야 합니다. - 삼각 측량을 통해 새 MapPoint 생성
ORB-SLAM은 기존 MapPoint와 일치하지 않는 현재 KF의 FeaturePoint와 기존 MapPoint와 일치하지 않는 Covisible KF의 FeaturePoint와 일치합니다. 일치하는
경우 삼각 측량을 통해 다음을 수행할 수 있습니다. (위치, 시차, 재투영 오차, 축척 일관성은 생성 후 확인해야 함)
이 FeaturePoint와 FeaturePoint 간의 일치는 BoW 검색을 통해 이루어집니다.
두 개의 KF를 통해 새 MapPoint를 생성한 후, 또한 확인 다른 KF에서 나타납니다. 따라서 MapPoint를 다른 Covisible KF에 투영하고 해당 FeaturePoint를 일치시키려면 일치하는
경우 MapPoint를 해당 KF의 FeaturePoint와 연결합니다. - 로컬 맵 BA 최적화
Current KF 및 Covisible KF 및 관찰되는 모든 MapPoint의 BA 최적화 - 중복 로컬 키프레임 제거
추적 스레드에서 ORB-SLAM은 매우 느슨한 조건에서 많은 KF를 맵에 삽입하지만 분명히 맵은 너무 많은 KF를 영구적으로 유지할 수 없으므로 맵이 너무 커지고 계산 부하가 크게 증가합니다. 현재 KF 및 Covisible
KF에 KF가 있는 경우 관찰되는 MapPoint의 90%를 3개 이상의 다른(동일한 규모)에서 사용할 수 있습니다. KF가 관찰되면 이 KF의 정보가 중복된 것으로 간주되어 제거됩니다. 맵에 있는 KF의 개수가 너무 많지 않도록 하는 것이 목적이며, 일정 규모의 시나리오에서는 맵에 있는 KF의 개수가 상한선 없이 늘어나지 않아야 하므로 부담을 줄이는 데도 도움이 됩니다. BA 최적화.
해당 프로그램 순서도는 다음과 같습니다.
루프 닫기 스레드
루프 닫기 스레드는 주로 ORB_SLAM2::LoopClosing::Run() 함수를 통해 구현됩니다.
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
구체적인 기능은 다음과 같습니다.
void LoopClosing::Run()
{
mbFinished =false;
while(1)
{
// Check if there are keyframes in the queue
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
if(DetectLoop())
{
// 计算SIM3或SE3，确定最终的Loop KF
// Compute similarity transformation [sR|t]
// In the stereo/RGBD case s=1
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
CorrectLoop();
}
}
}
ResetIfRequested();
if(CheckFinish())
break;
usleep(5000);
}
SetFinish();
}
다음은 다른 사진입니다.
루프 닫기 스레드는 두 단계로 나눌 수 있습니다.
- 루프 감지: 루프 감지
- 루프 수정: 루프 수정
- DetectLoop() 함수는 후보 KF의 배치를 감지합니다.
- ComputeSim3() 함수는 현재 KF와 후보 KF 간의 유사성 변환을 계산하고 그에 따라 최종 루프 KF를 결정합니다.
- CorrectLoop() 함수는 루프백 수정을 수행합니다.
위의 모든 단계가 완료된 후에도 정확도는 이미 매우 높지만 ORB-SLAM2는 여전히 마지막에 전역 BA를 수행하여 케이크에 아이싱을 추가하도록 선택합니다(이 단계는 ORB-SLAM1에는 없는 것 같습니다).
메인 3개 스레드의 작업에 영향을 미치지 않기 위해 글로벌 BA에 대해 4번째 스레드가 생성되지만 글로벌 BA는 언제든지 중단될 수 있으며 시스템이 특히 유휴 상태일 때만 실행됩니다.
해당 프로그램 순서도는 다음과 같습니다.
주요 참조
ORB-SLAM2 종이 및 코드 학습 - LocalMapping 스레드 - MingruiYu - Blog Park (cnblogs.com)
ORB-SLAM2 종이 및 코드 학습 - LoopClosing 스레드 - MingruiYu - Blog Park (cnblogs.com)
위반사항이 있는 경우 삭제를 위해 연락주세요.