Skip to content

Commit d14c636

Browse files
zihaomuzihaomu
committed
add better keyframe strategy.
1 parent 1446a10 commit d14c636

File tree

5 files changed

+305
-161
lines changed

5 files changed

+305
-161
lines changed

‎modules/rgbd/src/keyframe.cpp

Lines changed: 180 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,180 @@
1+
// This file is part of OpenCV project.
2+
// It is subject to the license terms in the LICENSE file found in the top-level directory
3+
// of this distribution and at http://opencv.org/license.html.
4+
5+
#include "precomp.hpp"
6+
#include "keyframe.hpp"
7+
8+
namespace cv
9+
{
10+
namespace large_kinfu
11+
{
12+
13+
KeyFrame::KeyFrame():submapID(-1), preKeyFrameID(-1)
14+
{
15+
nextKeyFrameID = -1;
16+
}
17+
18+
KeyFrame::KeyFrame(Mat _DNNfeature, int _submapID) : DNNFeature(_DNNfeature), submapID(_submapID), preKeyFrameID(-1)
19+
{
20+
nextKeyFrameID = -1;
21+
};
22+
23+
KeyFrame::KeyFrame(Mat _DNNfeature, int _submapID, int _preKeyFrameID) : DNNFeature(_DNNfeature), submapID(_submapID), preKeyFrameID(_preKeyFrameID)
24+
{
25+
nextKeyFrameID = -1;
26+
};
27+
28+
// Using INT_MAX by default.
29+
KeyFrameDatabase::KeyFrameDatabase():maxSizeDB(INT_MAX), lastKeyFrameID(-1)
30+
{
31+
};
32+
33+
KeyFrameDatabase::KeyFrameDatabase(int _maxSizeDB):maxSizeDB(_maxSizeDB),lastKeyFrameID(-1)
34+
{
35+
};
36+
37+
void KeyFrameDatabase::addKeyFrame(const Mat& DNNFeature, int frameID, int submapID)
38+
{
39+
Ptr<KeyFrame> kf, preKF;
40+
preKF = getKeyFrameByID(lastKeyFrameID);
41+
42+
// new start for KeyFrame in different submaps.
43+
if(preKF)
44+
{
45+
kf = makePtr<KeyFrame>(DNNFeature, submapID, lastKeyFrameID);
46+
preKF->nextKeyFrameID = frameID;
47+
}
48+
else
49+
{
50+
kf = makePtr<KeyFrame>(DNNFeature, submapID, -1);
51+
}
52+
53+
// Adding new KF to DB
54+
DataBase[frameID] = kf;
55+
56+
if(int(DataBase.size()) > maxSizeDB)
57+
shrinkDB();
58+
59+
// Change the last
60+
lastKeyFrameID = frameID;
61+
}
62+
63+
bool KeyFrameDatabase::deleteKeyFrameByID(int keyFrameID)
64+
{
65+
auto keyFrame = DataBase.find(keyFrameID);
66+
67+
if(keyFrame == DataBase.end())
68+
{
69+
return false;
70+
} else
71+
{
72+
// Remove nowKF, and link the perKF to nextKF.
73+
Ptr<KeyFrame> preKF, nextKF, nowKF;
74+
nowKF = keyFrame->second;
75+
preKF = getKeyFrameByID(nowKF->preKeyFrameID);
76+
nextKF = getKeyFrameByID(nowKF->nextKeyFrameID);
77+
78+
if(preKF)
79+
{
80+
preKF->nextKeyFrameID = nowKF->nextKeyFrameID;
81+
}
82+
83+
if(nextKF)
84+
{
85+
nextKF->preKeyFrameID = nowKF->preKeyFrameID;
86+
}
87+
88+
DataBase.erase(keyFrame);
89+
return true;
90+
}
91+
}
92+
93+
Ptr<KeyFrame> KeyFrameDatabase::getKeyFrameByID(int keyFrameID)
94+
{
95+
if(keyFrameID < 0)
96+
return {};
97+
98+
auto keyFrame = DataBase.find(keyFrameID);
99+
if(keyFrame == DataBase.end())
100+
{
101+
return {};
102+
} else
103+
{
104+
return keyFrame->second;
105+
}
106+
}
107+
108+
int KeyFrameDatabase::getSize()
109+
{
110+
return DataBase.size();
111+
}
112+
113+
bool KeyFrameDatabase::empty()
114+
{
115+
return DataBase.empty();
116+
}
117+
118+
int KeyFrameDatabase::getLastKeyFrameID()
119+
{
120+
return lastKeyFrameID;
121+
}
122+
123+
double KeyFrameDatabase::score(InputArray feature1, InputArray feature2)
124+
{
125+
Mat mat1, mat2;
126+
mat1 = feature1.getMat();
127+
mat2 = feature2.getMat();
128+
double out = mat2.dot(mat1);
129+
return out;
130+
}
131+
132+
std::vector<int> KeyFrameDatabase::getCandidateKF(const Mat& currentFeature, const double& similarityLow, double& bestSimilarity, int& bestId)
133+
{
134+
std::vector<int> cadidateKFs;
135+
float similarity;
136+
137+
bestSimilarity = 0;
138+
139+
for(std::map<int, Ptr<KeyFrame> >::const_iterator iter = DataBase.begin(); iter != DataBase.end(); iter++)
140+
{
141+
similarity = score(currentFeature, iter->second->DNNFeature);
142+
143+
if(similarity > similarityLow)
144+
{
145+
cadidateKFs.push_back(iter->first);
146+
}
147+
148+
if(similarity > bestSimilarity)
149+
{
150+
bestSimilarity = similarity;
151+
bestId = iter->first;
152+
}
153+
}
154+
155+
return cadidateKFs;
156+
}
157+
158+
// If size of DB is large than the maxSizeDB, then remove some KFs in DB.
159+
void KeyFrameDatabase::shrinkDB()
160+
{
161+
for(std::map<int, Ptr<KeyFrame> >::const_iterator iter = DataBase.begin(); iter != DataBase.end(); iter++)
162+
{
163+
deleteKeyFrameByID(iter->first);
164+
165+
iter++;
166+
167+
if(iter == DataBase.end())
168+
{
169+
break;
170+
}
171+
}
172+
}
173+
174+
void KeyFrameDatabase::reset()
175+
{
176+
DataBase.clear();
177+
}
178+
179+
}// namespace large_kinfu
180+
}// namespace cv

‎modules/rgbd/src/keyframe.hpp

Lines changed: 24 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -11,18 +11,21 @@ namespace cv
1111
{
1212
namespace large_kinfu
1313
{
14-
using namespace kinfu;
1514

15+
// It works like doubly linked list.
1616
struct KeyFrame
1717
{
18-
const Mat DNNFeature;
18+
19+
Mat DNNFeature;
1920
int submapID;
2021

21-
KeyFrame():DNNFeature(), submapID(-1)
22-
{};
22+
int preKeyFrameID;
23+
int nextKeyFrameID;
24+
25+
KeyFrame();
26+
KeyFrame(Mat _DNNfeature, int _submapID);
27+
KeyFrame(Mat _DNNfeature, int _submapID, int preKeyFrameID);
2328

24-
KeyFrame(Mat _DNNfeature, int _submapID) : DNNFeature(_DNNfeature), submapID(_submapID)
25-
{};
2629
};
2730

2831
class KeyFrameDatabase
@@ -31,80 +34,40 @@ class KeyFrameDatabase
3134

3235
KeyFrameDatabase();
3336

37+
KeyFrameDatabase(int maxSizeDB);
38+
3439
~KeyFrameDatabase() = default;
3540

3641
void addKeyFrame( const Mat& DNNFeature, int frameID, int submapID);
3742

3843
Ptr<KeyFrame> getKeyFrameByID(int keyFrameID);
3944

40-
bool deleteKeyFrame(int keyFrameID);
41-
42-
Ptr<KeyFrame> getKeyFrameByIndex(int index);
45+
bool deleteKeyFrameByID(int keyFrameID);
4346

4447
int getSize();
4548

4649
bool empty();
4750

4851
void reset();
4952

53+
void shrinkDB();
54+
55+
int getLastKeyFrameID();
56+
57+
std::vector<int> getCandidateKF(const Mat& currentFeature, const double& similarityLow, double& bestSimilarity, int& bestId);
58+
59+
double score(InputArray feature1, InputArray feature2);
60+
5061
private:
5162
// < keyFrameID, KeyFrame>
52-
std::map<int, Ptr<KeyFrame>> DataBase;
53-
};
63+
std::map<int, Ptr<KeyFrame> > DataBase;
5464

55-
void KeyFrameDatabase::addKeyFrame(const Mat& DNNFeature, int frameID, int submapID)
56-
{
57-
DataBase[frameID] = makePtr<KeyFrame>(DNNFeature, submapID);
58-
}
65+
int maxSizeDB;
5966

60-
bool KeyFrameDatabase::deleteKeyFrame(int keyFrameID)
61-
{
62-
auto keyFrame = DataBase.find(keyFrameID);
63-
if(keyFrame == DataBase.end())
64-
{
65-
return false;
66-
} else{
67-
DataBase.erase(keyFrame);
68-
return true;
69-
}
70-
}
71-
72-
Ptr<KeyFrame> KeyFrameDatabase::getKeyFrameByID(int keyFrameID)
73-
{
74-
auto keyFrame = DataBase.find(keyFrameID);
75-
if(keyFrame == DataBase.end())
76-
{
77-
return {};
78-
} else
79-
{
80-
return keyFrame->second;
81-
}
82-
}
83-
84-
Ptr<KeyFrame> KeyFrameDatabase::getKeyFrameByIndex(int index)
85-
{
86-
if(index > this->getSize())
87-
{
88-
return {};
89-
} else{
90-
return DataBase[index];
91-
}
92-
}
93-
94-
int KeyFrameDatabase::getSize()
95-
{
96-
return DataBase.size();
97-
}
67+
int lastKeyFrameID;
9868

99-
bool KeyFrameDatabase::empty()
100-
{
101-
return DataBase.empty();
102-
}
10369

104-
void KeyFrameDatabase::reset()
105-
{
106-
DataBase.clear();
107-
}
70+
};
10871

10972
}// namespace large_kinfu
11073
}// namespace cv

0 commit comments

Comments
 (0)