Fix bugs in loop detection

[fishmarch]

The iterator is not updated when the pKFi is bad, thus it will get stuck in the loop. In our modified SLAM system, this would happen. But I am not sure in the origin version whether pKFi could be bad thus causing this problem. Please check.

[jeremysalwen]

Hello @fishmarch, I am creating a community version of ORB_SLAM3 at https://github.com/jeremysalwen/ORB_SLAM_COMMUNITY since this repository seems to be inactive. These bugfixes seem very interesting but I am having a hard time understanding some of them. I have already merged most of your patches, but the Sim3Solver and DetectNBestCandidates changes are not self-evident to me. Could you explain a bit more the reasoning behind those changes?

[fishmarch]

Hi @jeremysalwen , I am glad to help.

You mean those bugs fixed in my repo https://github.com/fishmarch/ORB_SLAM3_Fixed ?

Sim3Solver

Before Sim3Solver, the current keyframe mpCurrentKF is matched with the candidate keyframe pMostBoWMatchesKF and its neighbors vpCovKFi. The result vpMatchedPoints contains all matched mappoints. Note that not all matched map points are from the candidate keyframe pMostBoWMatchesKF, and thus vpKeyFrameMatchedMP contains the corresponding keyframes.

Therefore in Sim3Solver, the flag bDifferentKFs should be ture by default, and then the related keyframe pKFm can be retrieved correctly. By the way, it seems vpKeyFrameMatchedMP will never be empty, if there are matched mappoints...

DetectNBestCandidates

The first bug is to update the iteration for bad keyframe for

ORB_SLAM3/src/KeyFrameDatabase.cc

Lines 712 to 713 in 4452a3c

	if(pKFi->isBad())
	continue;

The similarity score is accumulated in neighbors in

ORB_SLAM3/src/KeyFrameDatabase.cc

Line 689 in 4452a3c

accScore+=pKF2->mPlaceRecognitionScore;

Thus it needs to ensure the similarity score is calculated with the current candidate keyframe. But it was calculated only under the following condition:

ORB_SLAM3/src/KeyFrameDatabase.cc

Lines 659 to 665 in 4452a3c

	if(pKFi->mnPlaceRecognitionWords>minCommonWords)
	{
	nscores++;
	float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
	pKFi->mPlaceRecognitionScore=si;
	lScoreAndMatch.push_back(make_pair(si,pKFi));
	}

[jeremysalwen]

Hmm, ok the Sim3Solver code makes sense to me and I merged it, but for the DetectNBestCandidates, Are you sure that it is calculating the right score?

float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);

Shouldn't it be scoring pKF2?

[fishmarch]

Yes, it is pKF2 in my repo. pKFi is the original codes.

[jeremysalwen]

Oh I see you fixed it in later commits. Okay that makes sense.

I'm still not clear if the original intention was to ignore covisibility keyframes that don't have enough common words, or to dynamically score then like you have. I assume it works fine to ignore them, since the original code was using invalid scores?

[fishmarch]

Yes, agree with you. These similarity score can also be set to zero directly. It is hard to say which one is better, but setting to zeros can be faster than my codes. Anyway the original codes need to be modified, since using wrong scores computed from previous keyframes.

[jeremysalwen]

Hmm, something else I noticed, DetectNBestCandidates (where this code change is) only is called inside NewDetectCommonRegions, which itself isn't called anywhere... is all of this code dead?

[fishmarch]

NewDetectCommonRegions is called in LoopClosing::Run() at:

ORB_SLAM3/src/LoopClosing.cc

Line 112 in 4452a3c

bool bFindedRegion = NewDetectCommonRegions();

[jeremysalwen]

Ah, just a bug in github search:

It is indeed used. Thanks again, I will incorporate everything into the community fork :)