Skip to content

Commit dfe7753

Browse files
gitounigitouni
authored
Add files via upload
1 parent dcfdefd commit dfe7753

File tree

5 files changed

+177
-171
lines changed

5 files changed

+177
-171
lines changed

‎src/orb_slam/src/KeyFrame.cc‎

Lines changed: 57 additions & 49 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,52 @@ namespace ORB_SLAM2
2828

2929
long unsigned int KeyFrame::nNextId=0;
3030

31+
KeyFrameConstInfo::KeyFrameConstInfo(cv::FileStorage &fs, KeyFrameDatabase *pKFDB, ORBVocabulary *pVoc):mpORBvocabulary(pVoc),mpKeyFrameDB(pKFDB){
32+
fs["mnId"] >> mnId;
33+
fs["mnFrameId"] >> mnFrameId;
34+
fs["mTImeStamp"] >> mTImeStamp;
35+
fs["mnGridCols"] >> mnGridCols;
36+
fs["mnGridRows"] >> mnGridRows;
37+
fs["mfGridElementWidthInv"] >> mfGridElementWidthInv;
38+
fs["mfGridElementHeightInv"] >> mfGridElementHeightInv;
39+
fs["fx"] >> fx;
40+
fs["fy"] >> fy;
41+
fs["cx"] >> cx;
42+
fs["cy"] >> cy;
43+
fs["invfx"] >> invfx;
44+
fs["invfy"] >> invfy;
45+
fs["mbf"] >> mbf;
46+
fs["mb"] >> mb;
47+
fs["mThDepth"] >> mThDepth;
48+
fs["N"] >> N;
49+
fs["mnScaleLevels"] >> mnScaleLevels;
50+
fs["mfScaleFactor"] >> mfScaleFactor;
51+
fs["mfLogScaleFactor"] >> mfLogScaleFactor;
52+
fs["mvScaleFactors"] >> mvScaleFactors;
53+
fs["mvLevelSigma2"] >> mvLevelSigma2;
54+
fs["mvInvLevelSigma2"] >> mvInvLevelSigma2;
55+
fs["mnMinX"] >> mnMinX;
56+
fs["mnMinY"] >> mnMinY;
57+
fs["mnMaxX"] >> mnMaxX;
58+
fs["mnMaxY"] >> mnMaxY;
59+
fs["mK"] >> mK;
60+
fs["mvKeys"] >> mvKeys;
61+
fs["mvKeysUn"] >> mvKeysUn;
62+
fs["mvuRight"] >> mvuRight;
63+
fs["mvDepth"] >> mvDepth;
64+
fs["mDescriptors"] >> mDescriptors;
65+
fs["mbFirstConnection"] >> mbFirstConnection;
66+
fs["mHalfBaseline"] >> mHalfBaseline;
67+
fs["mvpMapPointsId"] >> mvpMapPointsId;
68+
fs["mvpOrderedConnectedKeyFramesId"] >> mvpOrderedConnectedKeyFramesId;
69+
fs["mvOrderedWeights"] >> mvOrderedWeights;
70+
fs["mpParentId"] >> mpParentId;
71+
fs["mspChildrensId"] >> mspChildrensId;
72+
fs["mspLoopEdgesId"] >> mspLoopEdgesId;
73+
fs["Pose"] >> Tcw;
74+
fs.release();
75+
}
76+
3177
KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
3278
mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
3379
mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv),
@@ -62,8 +108,10 @@ KeyFrame::KeyFrame(boost::archive::binary_iarchive &ira, const KeyFrameConstInfo
62108
mnMinX(info->mnMinX),mnMinY(info->mnMinY),mnMaxX(info->mnMaxX),mnMaxY(info->mnMaxY),mHalfBaseline(info->mHalfBaseline),
63109
mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
64110
mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
65-
mpORBvocabulary(info->mpORBvocabulary), mpKeyFrameDB(info->mpKeyFrameDB),mvKeys(info->mvKeys),mvKeysUn(info->mvKeysUn),mDescriptors(info->mDescriptors),
66-
mbFirstConnection(info->mbFirstConnection), mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false){
111+
mpORBvocabulary(info->mpORBvocabulary), mpKeyFrameDB(info->mpKeyFrameDB),mvKeys(info->mvKeys),mvKeysUn(info->mvKeysUn),
112+
mvuRight(info->mvuRight),mvDepth(info->mvDepth),mDescriptors(info->mDescriptors),
113+
mbFirstConnection(info->mbFirstConnection), mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false)
114+
{
67115
ira >> *this;
68116
for(int mapId:info->mvpMapPointsId){
69117
auto it = mapMptsId.find(mapId);
@@ -74,50 +122,9 @@ KeyFrame::KeyFrame(boost::archive::binary_iarchive &ira, const KeyFrameConstInfo
74122
mvpMapPoints.push_back(it->second);
75123
}
76124
SetPose(info->Tcw);
125+
77126
}
78-
KeyFrameConstInfo::KeyFrameConstInfo(cv::FileStorage &fs, KeyFrameDatabase *pKFDB, ORBVocabulary *pVoc):mpORBvocabulary(pVoc),mpKeyFrameDB(pKFDB){
79-
fs["mnId"] >> mnId;
80-
fs["mnFrameId"] >> mnFrameId;
81-
fs["mTImeStamp"] >> mTImeStamp;
82-
fs["mnGridCols"] >> mnGridCols;
83-
fs["mnGridRows"] >> mnGridRows;
84-
fs["mfGridElementWidthInv"] >> mfGridElementWidthInv;
85-
fs["mfGridElementHeightInv"] >> mfGridElementHeightInv;
86-
fs["fx"] >> fx;
87-
fs["fy"] >> fy;
88-
fs["cx"] >> cx;
89-
fs["cy"] >> cy;
90-
fs["invfx"] >> invfx;
91-
fs["invfy"] >> invfy;
92-
fs["mbf"] >> mbf;
93-
fs["mb"] >> mb;
94-
fs["mThDepth"] >> mThDepth;
95-
fs["N"] >> N;
96-
fs["mnScaleLevels"] >> mnScaleLevels;
97-
fs["mfScaleFactor"] >> mfScaleFactor;
98-
fs["mfLogScaleFactor"] >> mfLogScaleFactor;
99-
fs["mvScaleFactors"] >> mvScaleFactors;
100-
fs["mvLevelSigma2"] >> mvLevelSigma2;
101-
fs["mvInvLevelSigma2"] >> mvInvLevelSigma2;
102-
fs["mnMinX"] >> mnMinX;
103-
fs["mnMinY"] >> mnMinY;
104-
fs["mnMaxX"] >> mnMaxX;
105-
fs["mnMaxY"] >> mnMaxY;
106-
fs["mK"] >> mK;
107-
fs["mvKeys"] >> mvKeys;
108-
fs["mvKeysUn"] >> mvKeysUn;
109-
fs["mDescriptors"] >> mDescriptors;
110-
fs["mbFirstConnection"] >> mbFirstConnection;
111-
fs["mHalfBaseline"] >> mHalfBaseline;
112-
fs["mvpMapPointsId"] >> mvpMapPointsId;
113-
fs["mvpOrderedConnectedKeyFramesId"] >> mvpOrderedConnectedKeyFramesId;
114-
fs["mvOrderedWeights"] >> mvOrderedWeights;
115-
fs["mpParentId"] >> mpParentId;
116-
fs["mspChildrensId"] >> mspChildrensId;
117-
fs["mspLoopEdgesId"] >> mspLoopEdgesId;
118-
fs["Pose"] >> Tcw;
119-
fs.release();
120-
}
127+
121128

122129
void KeyFrame::GlobalConnection(const KeyFrameConstInfo *info, const unordered_map<int, KeyFrame*> &mapKFId){
123130
for(int id_:info->mspChildrensId){
@@ -144,8 +151,10 @@ void KeyFrame::GlobalConnection(const KeyFrameConstInfo *info, const unordered_m
144151
}
145152
mvpOrderedConnectedKeyFrames.push_back(it->second);
146153
}
154+
if(info->mnId == 0) // skip Frame 0 (do not have Parent)
155+
return;
147156
auto it = mapKFId.find(info->mpParentId);
148-
if(mnId!=0 && it==mapKFId.end()) // do not render warning for the first KeyFrame
157+
if(it==mapKFId.end())
149158
std::cout << "[Warning]" << __FILE__ << " Line " << __LINE__ <<": \033[33;1mUnconnected KeyFrame for mpParentId\033[0m" << std::endl;
150159
else
151160
mpParent = it->second;
@@ -202,9 +211,8 @@ void KeyFrame::saveData(const string baseName, const unordered_map<KeyFrame*, in
202211
of << "N" << N;
203212
of << "mvKeys" << mvKeys;
204213
of << "mvKeysUn" << mvKeysUn;
205-
// Stereo:
206-
// of << "mvuRight" << mvuRight;
207-
// of << "mvDepth" << mvDepth;
214+
of << "mvuRight" << mvuRight;
215+
of << "mvDepth" << mvDepth;
208216
of << "mDescriptors" << mDescriptors;
209217
of << "mnScaleLevels" << mnScaleLevels;
210218
of << "mfScaleFactor" << mfScaleFactor;

‎src/orb_slam/src/Map.cc‎

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -166,32 +166,34 @@ void Map::RestoreMap(cv::FileStorage &fs, vector<int> &vkFId)
166166
void Map::RestoreMapPointsConnection(cv::FileStorage &fs, unordered_map<int, KeyFrame*> &mapKFId, unordered_map<int, MapPoint*> &mapMptId)
167167
{
168168
cv::FileNode mptsfn = fs["mspMapPoints"];
169-
for(cv::FileNodeIterator NodeIt = mptsfn.begin(); NodeIt != mptsfn.end(); ++NodeIt){
169+
for(cv::FileNodeIterator NodeIt = mptsfn.begin(); NodeIt != mptsfn.end(); ++NodeIt) // MapPoint FileNode
170+
{
170171
vector<int> mobsMapKFId, mMapKFInId;
171172
int MptId;
172173
(*NodeIt)["mobsMapKFId"] >> mobsMapKFId;
173174
(*NodeIt)["mMapKFInId"] >> mMapKFInId;
174175
(*NodeIt)["mnId"] >> MptId;
175176
assert(mobsMapKFId.size() == mMapKFInId.size());
176-
auto MptIt = mapMptId.find(MptId);
177+
auto MptIt = mapMptId.find(MptId); // verify the MapPoint
177178
if(MptIt == mapMptId.end()){
178179
std::cout << "[Warning] " <<__FILE__ << " Line " << __LINE__ <<": \033[33;1mLost MapPoint in Hash Table\033[0m" << std::endl;
179180
continue;
180181
}
181-
MapPoint* pMpt = mapMptId[MptId];
182+
MapPoint* pMpt = mapMptId[MptId]; // retrive the valid MapPoint
182183
for(int i = 0; i < (int) mobsMapKFId.size(); ++i){
183184
int KFId = mobsMapKFId[i], KFInId = mMapKFInId[i];
184-
auto KFIt = mapKFId.find(KFId);
185+
auto KFIt = mapKFId.find(KFId); // verify the KeyFrame
185186
if(KFIt == mapKFId.end()){
186187
std::cout << "[Warning] " <<__FILE__ << " Line " << __LINE__ <<": \033[33;1mUnconnected MapPoint for KeyFrame\033[0m" << std::endl;
187188
continue;
188189
}
189-
if(KFInId > (int)KFIt->second->mvKeysUn.size() - 1){
190+
if(KFInId > (int)(KFIt->second->mvKeysUn.size()-1) || KFInId < 0){
190191
std::cout << "[Warning] " <<__FILE__ << " Line " << __LINE__ <<": \033[33;1mIndex of Connected MapPoint Overflow the size of Keypoints\033[0m" << std::endl;
191192
continue;
192193
}
193194
KFIt->second->mmapMpt2Kpt[pMpt] = KFInId;
194-
// pMpt->AddObservation(KFIt->second, KFInId); // why Segment Fault??
195+
if(!pMpt->IsInKeyFrame(KFIt->second))
196+
pMpt->AddObservationStatic(KFIt->second, (size_t)KFInId); // why Segment Fault??
195197
}
196198
}
197199
fs.release();

‎src/orb_slam/src/MapPoint.cc‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,14 @@ void MapPoint::AddObservation(KeyFrame* pKF, size_t idx)
113113
nObs++;
114114
}
115115

116+
void MapPoint::AddObservationStatic(KeyFrame* pKF, size_t idx)
117+
{
118+
unique_lock<mutex> lock(mMutexFeatures);
119+
if(mObservations.count(pKF))
120+
return;
121+
mObservations[pKF]=idx;
122+
}
123+
116124
void MapPoint::EraseObservation(KeyFrame* pKF)
117125
{
118126
bool bBad=false;

0 commit comments

Comments
 (0)