Skip to content

Commit 4bf8cbf

Browse files
zihaomuzihaomu
committed
combining ORB and DeepLCD.
1 parent c7a7c16 commit 4bf8cbf

File tree

7 files changed

+149
-38
lines changed

7 files changed

+149
-38
lines changed

‎.vscode/settings.json

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
{
2+
"files.associations": {
3+
"cmath": "cpp",
4+
"map": "cpp",
5+
"vector": "cpp",
6+
"limits": "cpp",
7+
"iterator": "cpp"
8+
}
9+
}

‎modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -165,8 +165,8 @@ class CV_EXPORTS_W LoopClosureDetection {
165165
virtual ~LoopClosureDetection() = default;
166166

167167
// Adding Frame.
168-
// If there is loop, function will set ifLoop to TRUE and assign target SubmapID to tarSubmapID.
169-
CV_WRAP virtual void addFrame(InputArray img, const int frameID, const int submapID, int& tarSubmapID, bool& ifLoop) = 0;
168+
// If there is loop, function will return TURE, and the tarSubmapID will be set as the target submap ID, otherwise return False.
169+
CV_WRAP virtual bool addFrame(InputArray img, const int frameID, const int submapID, CV_OUT int& tarSubmapID) = 0;
170170

171171
// Stop run loop closing detection.
172172
CV_WRAP virtual void reset() = 0;

‎modules/rgbd/src/keyframe.cpp

Lines changed: 34 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -20,34 +20,50 @@ KeyFrame::KeyFrame(Mat _DNNfeature, int _submapID) : DNNFeature(_DNNfeature), su
2020
nextKeyFrameID = -1;
2121
};
2222

23-
KeyFrame::KeyFrame(Mat _DNNfeature, int _submapID, int _preKeyFrameID) : DNNFeature(_DNNfeature), submapID(_submapID), preKeyFrameID(_preKeyFrameID)
23+
KeyFrame::KeyFrame(Mat _DNNfeature, int _submapID, int _preKeyFrameID):DNNFeature(_DNNfeature), submapID(_submapID), preKeyFrameID(_preKeyFrameID)
24+
{}
25+
26+
KeyFrame::KeyFrame(Mat _DNNfeature, int _submapID, int _preKeyFrameID, std::vector<KeyPoint> _keypoints, Mat _ORBFeatures):DNNFeature(_DNNfeature), submapID(_submapID), preKeyFrameID(_preKeyFrameID)
2427
{
25-
nextKeyFrameID = -1;
26-
};
28+
keypoints = _keypoints;
29+
ORBFeatures = _ORBFeatures;
30+
}
2731

2832
// Using INT_MAX by default.
2933
KeyFrameDatabase::KeyFrameDatabase():maxSizeDB(INT_MAX), lastKeyFrameID(-1)
3034
{
31-
};
35+
}
3236

3337
KeyFrameDatabase::KeyFrameDatabase(int _maxSizeDB):maxSizeDB(_maxSizeDB),lastKeyFrameID(-1)
3438
{
35-
};
39+
}
40+
41+
void KeyFrameDatabase::addKeyFrame( const Mat& _DNNFeature, int _frameID, int _submapID)
42+
{
43+
std::vector<KeyPoint> _keypoints;
44+
Mat _ORBFeatures;
45+
addKeyFrameT(_DNNFeature, _frameID, _submapID, _keypoints, _ORBFeatures);
46+
}
3647

37-
void KeyFrameDatabase::addKeyFrame(const Mat& DNNFeature, int frameID, int submapID)
48+
void KeyFrameDatabase::addKeyFrame( const Mat& _DNNFeature, int _frameID, int _submapID, std::vector<KeyPoint>& _keypoints, const Mat& _ORBFeatures)
49+
{
50+
addKeyFrameT(_DNNFeature, _frameID, _submapID, _keypoints, _ORBFeatures);
51+
}
52+
53+
void KeyFrameDatabase::addKeyFrameT(const Mat& DNNFeature, int frameID, int submapID, std::vector<KeyPoint>& keypoints, const Mat& ORBFeatures)
3854
{
3955
Ptr<KeyFrame> kf, preKF;
4056
preKF = getKeyFrameByID(lastKeyFrameID);
4157

4258
// new start for KeyFrame in different submaps.
4359
if(preKF)
4460
{
45-
kf = makePtr<KeyFrame>(DNNFeature, submapID, lastKeyFrameID);
61+
kf = makePtr<KeyFrame>(DNNFeature, submapID, lastKeyFrameID, keypoints, ORBFeatures);
4662
preKF->nextKeyFrameID = frameID;
4763
}
4864
else
4965
{
50-
kf = makePtr<KeyFrame>(DNNFeature, submapID, -1);
66+
kf = makePtr<KeyFrame>(DNNFeature, submapID, -1, keypoints, ORBFeatures);
5167
}
5268

5369
// Adding new KF to DB
@@ -171,6 +187,16 @@ void KeyFrameDatabase::shrinkDB()
171187
}
172188
}
173189

190+
// Debug Only.
191+
void KeyFrameDatabase::printDB()
192+
{
193+
for(std::map<int, Ptr<KeyFrame> >::const_iterator iter = DataBase.begin(); iter != DataBase.end(); iter++)
194+
{
195+
std::cout<<"frame Id= "<<iter->first<<", feature = "<<iter->second->DNNFeature<<std::endl;
196+
std::cout<<"ORB Feature"<<iter->second->ORBFeatures<<std::endl;
197+
}
198+
}
199+
174200
void KeyFrameDatabase::reset()
175201
{
176202
DataBase.clear();

‎modules/rgbd/src/keyframe.hpp

Lines changed: 17 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -16,16 +16,21 @@ namespace large_kinfu
1616
struct KeyFrame
1717
{
1818

19+
// DNN Feature.
1920
Mat DNNFeature;
2021
int submapID;
2122

2223
int preKeyFrameID;
2324
int nextKeyFrameID;
2425

25-
KeyFrame();
26-
KeyFrame(Mat _DNNfeature, int _submapID);
27-
KeyFrame(Mat _DNNfeature, int _submapID, int preKeyFrameID);
26+
// ORB Feature
27+
std::vector<KeyPoint> keypoints;
28+
Mat ORBFeatures;
2829

30+
KeyFrame();
31+
KeyFrame(Mat DNNfeature, int submapID);
32+
KeyFrame(Mat DNNfeature, int submapID, int preKeyFrameID);
33+
KeyFrame(Mat DNNfeature, int submapID, int preKeyFrameID, std::vector<KeyPoint> keypoints, Mat ORBdescriptors);
2934
};
3035

3136
class KeyFrameDatabase
@@ -40,6 +45,8 @@ class KeyFrameDatabase
4045

4146
void addKeyFrame( const Mat& DNNFeature, int frameID, int submapID);
4247

48+
void addKeyFrame( const Mat& DNNFeature, int frameID, int submapID, std::vector<KeyPoint>& keypoints, const Mat& ORBFeatures);
49+
4350
Ptr<KeyFrame> getKeyFrameByID(int keyFrameID);
4451

4552
bool deleteKeyFrameByID(int keyFrameID);
@@ -50,20 +57,24 @@ class KeyFrameDatabase
5057

5158
void reset();
5259

53-
void shrinkDB();
54-
5560
int getLastKeyFrameID();
5661

5762
std::vector<int> getCandidateKF(const Mat& currentFeature, const double& similarityLow, double& bestSimilarity, int& bestId);
5863

5964
double score(InputArray feature1, InputArray feature2);
6065

66+
// Debug only
67+
void printDB();
68+
6169
private:
70+
void addKeyFrameT(const Mat& DNNFeature, int frameID, int submapID, std::vector<KeyPoint>& keypoints, const Mat& ORBFeatures);
71+
72+
void shrinkDB();
73+
6274
// < keyFrameID, KeyFrame>
6375
std::map<int, Ptr<KeyFrame> > DataBase;
6476

6577
int maxSizeDB;
66-
6778
int lastKeyFrameID;
6879

6980

‎modules/rgbd/src/large_kinfu.cpp

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -316,9 +316,8 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth, const Mat& _img)
316316
if(currentSubmapId != -1)
317317
{
318318
int tarSubmapID = -1;
319-
bool ifLoop = false;
320319

321-
lcd->addFrame(grayImg, frameCounter, currentSubmapId, tarSubmapID, ifLoop);
320+
bool ifLoop = lcd->addFrame(grayImg, frameCounter, currentSubmapId, tarSubmapID);
322321

323322
if(ifLoop && tarSubmapID != -1 && currentSubmapId != tarSubmapID)
324323
{

‎modules/rgbd/src/loop_closure_detection.cpp

Lines changed: 64 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44

55
#include "precomp.hpp"
66
#include "loop_closure_detection.hpp"
7-
#include "keyframe.cpp"
7+
88
namespace cv{
99
namespace large_kinfu{
1010

@@ -39,7 +39,7 @@ bool LoopClosureDetectionImpl::loopCheck(int& tarSubmapID)
3939
std::vector<int> candidateKFs;
4040

4141
// Find candidate key frames which similarity are greater than the similarityLow.
42-
candidateKFs = KFDataBase->getCandidateKF(currentFeature, similarityLow, maxScore, bestId);
42+
candidateKFs = KFDataBase->getCandidateKF(currentDNNFeature, similarityLow, maxScore, bestId);
4343

4444
if( candidateKFs.empty() || maxScore < similarityHigh)
4545
return false;
@@ -122,11 +122,49 @@ bool LoopClosureDetectionImpl::loopCheck(int& tarSubmapID)
122122
preLoopedKFID = currentFrameID;
123123
currentFrameID = -1;
124124

125+
#ifdef HAVE_OPENCV_FEATURES2D
126+
// ORB Feature Matcher.
127+
return ORBMather(bestLoopFrame->ORBFeatures, currentORBFeature);
128+
#else
129+
return true;
130+
#endif
131+
132+
}
133+
}
134+
135+
bool LoopClosureDetectionImpl::ORBMather(InputArray feature1, InputArray feature2)
136+
{
137+
#ifdef HAVE_OPENCV_FEATURES2D
138+
std::vector<DMatch> matches;
139+
ORBmatcher->match(feature1,feature2, matches);
140+
auto min_max = minmax_element(matches.begin(), matches.end(),
141+
[](const DMatch &m1, const DMatch &m2) { return m1.distance < m2.distance; });
142+
double minDist = min_max.first->distance;
143+
144+
std::vector<DMatch> goodMatches;
145+
for (auto &match: matches)
146+
{
147+
if (match.distance <= std::max(2 * minDist, 30.0))
148+
{
149+
goodMatches.push_back(match);
150+
}
151+
}
152+
if(goodMatches.size() < ORBminMathing)
153+
{
154+
CV_LOG_INFO(NULL, "Currently there are too few ORB matching pairs.");
155+
return false;
156+
}
157+
else
158+
{
125159
return true;
126160
}
161+
162+
#else
163+
return true;
164+
#endif
127165
}
128166

129-
void LoopClosureDetectionImpl::addFrame(InputArray _img, const int frameID, const int submapID, int& tarSubmapID, bool& ifLoop)
167+
bool LoopClosureDetectionImpl::addFrame(InputArray _img, const int frameID, const int submapID, int& tarSubmapID)
130168
{
131169

132170
CV_Assert(!_img.empty());
@@ -144,37 +182,48 @@ void LoopClosureDetectionImpl::addFrame(InputArray _img, const int frameID, cons
144182
}
145183

146184
// feature Extract.
147-
processFrame(img, currentFeature);
185+
processFrame(img, currentDNNFeature, currentKeypoints, currentORBFeature);
186+
148187

149188
// Key frame filtering.
150-
ifLoop = loopCheck(tarSubmapID);
189+
bool ifLoop = loopCheck(tarSubmapID);
151190

152191
// add Frame to KeyFrameDataset.
153-
if(ifLoop)
154-
return;
155-
else
192+
if(!ifLoop)
156193
{
157-
KFDataBase->addKeyFrame(currentFeature, frameID, submapID);
194+
#ifdef HAVE_OPENCV_FEATURES2D
195+
KFDataBase->addKeyFrame(currentDNNFeature, frameID, submapID, currentKeypoints, currentORBFeature);
196+
#else
197+
KFDataBase->addKeyFrame(currentDNNFeature, frameID, submapID);
198+
#endif
158199
}
200+
return ifLoop;
159201
}
160202

161203
void LoopClosureDetectionImpl::reset()
162204
{
163205
KFDataBase->reset();
164206
}
165207

166-
void LoopClosureDetectionImpl::processFrame(InputArray img, Mat& output)
208+
void LoopClosureDetectionImpl::processFrame(InputArray img, Mat& DNNfeature, std::vector<KeyPoint>& currentKeypoints, Mat& ORBFeature)
167209
{
168-
Mat imgBlur, outMat;
210+
// DNN processing.
211+
Mat imgBlur, outDNN, outORB;
169212
cv::GaussianBlur(img, imgBlur, cv::Size(7, 7), 0);
170213
Mat blob = dnn::blobFromImage(imgBlur, 1.0/255.0, inputSize);
171214
net->setInput(blob);
172-
net->forward(outMat);
215+
net->forward(outDNN);
216+
217+
outDNN /= norm(outDNN);
218+
DNNfeature = outDNN.clone();
173219

174-
outMat /= norm(outMat);
175-
output = outMat.clone();
220+
// ORB process
221+
#ifdef HAVE_OPENCV_FEATURES2D
222+
ORBdetector->detect(img, currentKeypoints);
223+
ORBdescriptor->compute(img,currentKeypoints,outORB);
224+
ORBFeature = outORB.clone();
225+
#endif
176226

177-
//! Add ORB feature.
178227
}
179228

180229
Ptr<LoopClosureDetection> LoopClosureDetection::create(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId, int targetId)

‎modules/rgbd/src/loop_closure_detection.hpp

Lines changed: 22 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,10 @@
88
#include "opencv2/dnn.hpp"
99
#include "keyframe.hpp"
1010

11+
#ifdef HAVE_OPENCV_FEATURES2D
12+
#include <opencv2/features2d.hpp>
13+
#endif
14+
1115
namespace cv{
1216
namespace large_kinfu{
1317

@@ -16,34 +20,47 @@ class LoopClosureDetectionImpl : public LoopClosureDetection
1620
public:
1721
LoopClosureDetectionImpl(const String& modelBin, const String& modelTxt, const Size& input_size, int backendId = 0, int targetId = 0);
1822

19-
void addFrame(InputArray img, const int frameID, const int submapID, int& tarSubmapID, bool& ifLoop) CV_OVERRIDE;
23+
bool addFrame(InputArray img, const int frameID, const int submapID, int& tarSubmapID) CV_OVERRIDE;
2024

2125
bool loopCheck(int& tarSubmapID);
2226

2327
void reset() CV_OVERRIDE;
2428

25-
void processFrame(InputArray img, Mat& DNNfeature);
29+
void processFrame(InputArray img, Mat& DNNfeature,std::vector<KeyPoint>& currentKeypoints, Mat& ORBFeature);
30+
31+
bool ORBMather(InputArray feature1, InputArray feature2);
2632

2733
bool newFrameCheck();
2834

35+
void ORBExtract();
36+
2937
private:
3038
Ptr<KeyFrameDatabase> KFDataBase;
3139
Ptr<dnn::Net> net;
3240
Size inputSize;
3341
int currentFrameID;
34-
Mat currentFeature;
42+
std::vector<KeyPoint> currentKeypoints;
43+
Mat currentDNNFeature;
44+
Mat currentORBFeature;
3545
Ptr<KeyFrame> bestLoopFrame;
3646

3747
int currentSubmapID = -1;
3848

49+
#ifdef HAVE_OPENCV_FEATURES2D
50+
Ptr<FeatureDetector> ORBdetector = ORB::create();
51+
Ptr<DescriptorExtractor> ORBdescriptor = ORB::create();
52+
Ptr<DescriptorMatcher> ORBmatcher = DescriptorMatcher::create("BruteForce-Hamming");
53+
#endif
54+
size_t ORBminMathing = 10;
55+
3956
// Param: Only for DeepLCD
40-
int minDatabaseSize = 50;
57+
int minDatabaseSize = 5;
4158
int maxDatabaseSize = 2000;
4259

4360
int preLoopedKFID = -1;
4461

4562
double similarityHigh = 0.94;
46-
double similarityLow = 0.91;
63+
double similarityLow = 0.92;
4764

4865
};
4966

0 commit comments

Comments
 (0)