-
Notifications
You must be signed in to change notification settings - Fork 2k
/
map.cpp
113 lines (102 loc) · 3.64 KB
/
map.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
/*
* <one line to give the program's name and a brief idea of what it does.>
* Copyright (C) 2016 <copyright holder> <email>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
#include "myslam/map.h"
#include "myslam/feature.h"
namespace myslam {
void Map::InsertKeyFrame(Frame::Ptr frame) {
current_frame_ = frame;
if (keyframes_.find(frame->keyframe_id_) == keyframes_.end()) {
keyframes_.insert(make_pair(frame->keyframe_id_, frame));
active_keyframes_.insert(make_pair(frame->keyframe_id_, frame));
} else {
keyframes_[frame->keyframe_id_] = frame;
active_keyframes_[frame->keyframe_id_] = frame;
}
if (active_keyframes_.size() > num_active_keyframes_) {
RemoveOldKeyframe();
}
}
void Map::InsertMapPoint(MapPoint::Ptr map_point) {
if (landmarks_.find(map_point->id_) == landmarks_.end()) {
landmarks_.insert(make_pair(map_point->id_, map_point));
active_landmarks_.insert(make_pair(map_point->id_, map_point));
} else {
landmarks_[map_point->id_] = map_point;
active_landmarks_[map_point->id_] = map_point;
}
}
void Map::RemoveOldKeyframe() {
if (current_frame_ == nullptr) return;
// 寻找与当前帧最近与最远的两个关键帧
double max_dis = 0, min_dis = 9999;
double max_kf_id = 0, min_kf_id = 0;
auto Twc = current_frame_->Pose().inverse();
for (auto& kf : active_keyframes_) {
if (kf.second == current_frame_) continue;
auto dis = (kf.second->Pose() * Twc).log().norm();
if (dis > max_dis) {
max_dis = dis;
max_kf_id = kf.first;
}
if (dis < min_dis) {
min_dis = dis;
min_kf_id = kf.first;
}
}
const double min_dis_th = 0.2; // 最近阈值
Frame::Ptr frame_to_remove = nullptr;
if (min_dis < min_dis_th) {
// 如果存在很近的帧,优先删掉最近的
frame_to_remove = keyframes_.at(min_kf_id);
} else {
// 删掉最远的
frame_to_remove = keyframes_.at(max_kf_id);
}
LOG(INFO) << "remove keyframe " << frame_to_remove->keyframe_id_;
// remove keyframe and landmark observation
active_keyframes_.erase(frame_to_remove->keyframe_id_);
for (auto feat : frame_to_remove->features_left_) {
auto mp = feat->map_point_.lock();
if (mp) {
mp->RemoveObservation(feat);
}
}
for (auto feat : frame_to_remove->features_right_) {
if (feat == nullptr) continue;
auto mp = feat->map_point_.lock();
if (mp) {
mp->RemoveObservation(feat);
}
}
CleanMap();
}
void Map::CleanMap() {
int cnt_landmark_removed = 0;
for (auto iter = active_landmarks_.begin();
iter != active_landmarks_.end();) {
if (iter->second->observed_times_ == 0) {
iter = active_landmarks_.erase(iter);
cnt_landmark_removed++;
} else {
++iter;
}
}
LOG(INFO) << "Removed " << cnt_landmark_removed << " active landmarks";
}
} // namespace myslam