-
Notifications
You must be signed in to change notification settings - Fork 374
/
keyframe_inserter.cc
194 lines (164 loc) · 8.43 KB
/
keyframe_inserter.cc
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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
#include "stella_vslam/mapping_module.h"
#include "stella_vslam/data/landmark.h"
#include "stella_vslam/data/marker.h"
#include "stella_vslam/data/map_database.h"
#include "stella_vslam/marker_model/base.h"
#include "stella_vslam/module/keyframe_inserter.h"
#include <spdlog/spdlog.h>
namespace stella_vslam {
namespace module {
keyframe_inserter::keyframe_inserter(const double max_interval,
const double min_interval,
const double max_distance,
const double lms_ratio_thr_almost_all_lms_are_tracked,
const double lms_ratio_thr_view_changed,
const unsigned int enough_lms_thr)
: max_interval_(max_interval),
min_interval_(min_interval),
max_distance_(max_distance),
lms_ratio_thr_almost_all_lms_are_tracked_(lms_ratio_thr_almost_all_lms_are_tracked),
lms_ratio_thr_view_changed_(lms_ratio_thr_view_changed),
enough_lms_thr_(enough_lms_thr) {}
keyframe_inserter::keyframe_inserter(const YAML::Node& yaml_node)
: keyframe_inserter(yaml_node["max_interval"].as<double>(1.0),
yaml_node["min_interval"].as<double>(0.1),
yaml_node["max_distance"].as<double>(-1.0),
yaml_node["lms_ratio_thr_almost_all_lms_are_tracked"].as<double>(0.9),
yaml_node["lms_ratio_thr_view_changed"].as<double>(0.8),
yaml_node["enough_lms_thr"].as<unsigned int>(100)) {}
void keyframe_inserter::set_mapping_module(mapping_module* mapper) {
mapper_ = mapper;
}
void keyframe_inserter::reset() {
}
bool keyframe_inserter::new_keyframe_is_needed(data::map_database* map_db,
const data::frame& curr_frm,
const unsigned int num_tracked_lms,
const unsigned int num_reliable_lms,
const data::keyframe& ref_keyfrm,
const unsigned int min_num_obs_thr) const {
assert(mapper_);
// Any keyframes are not able to be added when the mapping module stops
if (mapper_->is_paused() || mapper_->pause_is_requested()) {
return false;
}
auto last_inserted_keyfrm = map_db->get_last_inserted_keyframe();
// Count the number of the 3D points that are observed from more than two keyframes
const auto num_reliable_lms_ref = ref_keyfrm.get_num_tracked_landmarks(min_num_obs_thr);
// When the mapping module skips localBA, it does not insert keyframes
const auto mapper_is_skipping_localBA = mapper_->is_skipping_localBA();
constexpr unsigned int num_enough_keyfrms_thr = 5;
const bool enough_keyfrms = map_db->get_num_keyframes() > num_enough_keyfrms_thr;
// New keyframe is needed if the time elapsed since the last keyframe insertion reaches the threshold
bool max_interval_elapsed = false;
if (max_interval_ > 0.0) {
max_interval_elapsed = last_inserted_keyfrm && last_inserted_keyfrm->timestamp_ + max_interval_ <= curr_frm.timestamp_;
}
bool min_interval_elapsed = false;
if (min_interval_ > 0.0) {
min_interval_elapsed = last_inserted_keyfrm && last_inserted_keyfrm->timestamp_ + min_interval_ <= curr_frm.timestamp_;
}
bool max_distance_traveled = false;
if (max_distance_ > 0.0) {
max_distance_traveled = last_inserted_keyfrm && (last_inserted_keyfrm->get_cam_center() - curr_frm.get_cam_center()).norm() > max_distance_;
}
// New keyframe is needed if the field-of-view of the current frame is changed a lot
const bool view_changed = num_reliable_lms < num_reliable_lms_ref * lms_ratio_thr_view_changed_;
// const bool view_changed = num_tracked_lms < num_tracked_lms_on_ref_keyfrm * lms_ratio_thr_view_changed_;
const bool not_enough_lms = num_reliable_lms < enough_lms_thr_;
// (Mandatory for keyframe insertion)
// New keyframe is needed if the number of 3D points exceeds the threshold,
// and concurrently the ratio of the reliable 3D points larger than the threshold ratio
constexpr unsigned int num_tracked_lms_thr_unstable = 15;
bool tracking_is_unstable = num_tracked_lms < num_tracked_lms_thr_unstable;
bool almost_all_lms_are_tracked = num_reliable_lms > num_reliable_lms_ref * lms_ratio_thr_almost_all_lms_are_tracked_;
return (max_interval_elapsed || max_distance_traveled || view_changed || not_enough_lms)
&& (!enough_keyfrms || min_interval_elapsed)
&& !tracking_is_unstable
&& !almost_all_lms_are_tracked
&& !mapper_is_skipping_localBA;
}
std::shared_ptr<data::keyframe> keyframe_inserter::insert_new_keyframe(data::map_database* map_db,
data::frame& curr_frm) {
// Do not pause mapping_module to let this keyframe process
if (!mapper_->prevent_pause_if_not_paused()) {
// If it is already paused, exit
return nullptr;
}
curr_frm.update_pose_params();
auto keyfrm = data::keyframe::make_keyframe(curr_frm);
for (const auto& id_mkr2d : keyfrm->markers_2d_) {
auto marker = map_db->get_marker(id_mkr2d.first);
if (!marker) {
// Create new marker
auto mkr2d = id_mkr2d.second;
eigen_alloc_vector<Vec3_t> corners_pos_w = mkr2d.compute_corners_pos_w(keyfrm->get_cam_pose_inv(), mkr2d.marker_model_->corners_pos_);
marker = std::make_shared<data::marker>(corners_pos_w, id_mkr2d.first, mkr2d.marker_model_);
// add the marker to the map DB
map_db->add_marker(marker);
}
// Set the association to the new marker
keyfrm->add_marker(marker);
marker->observations_.push_back(keyfrm);
}
// Queue up the keyframe to the mapping module
if (!keyfrm->depth_is_available()) {
queue_keyframe(keyfrm);
return keyfrm;
}
// Save the valid depth and index pairs
std::vector<std::pair<float, unsigned int>> depth_idx_pairs;
depth_idx_pairs.reserve(curr_frm.frm_obs_.num_keypts_);
for (unsigned int idx = 0; idx < curr_frm.frm_obs_.num_keypts_; ++idx) {
assert(!curr_frm.frm_obs_.depths_.empty());
const auto depth = curr_frm.frm_obs_.depths_.at(idx);
// Add if the depth is valid
if (0 < depth) {
depth_idx_pairs.emplace_back(std::make_pair(depth, idx));
}
}
// Queue up the keyframe to the mapping module if any valid depth values don't exist
if (depth_idx_pairs.empty()) {
queue_keyframe(keyfrm);
return keyfrm;
}
// Sort in order of distance to the camera
std::sort(depth_idx_pairs.begin(), depth_idx_pairs.end());
// Create 3D points by using a depth parameter
constexpr unsigned int min_num_to_create = 100;
for (unsigned int count = 0; count < depth_idx_pairs.size(); ++count) {
const auto depth = depth_idx_pairs.at(count).first;
const auto idx = depth_idx_pairs.at(count).second;
// Stop adding a keyframe if the number of 3D points exceeds the minimal threshold,
// and concurrently the depth value exceeds the threshold
if (min_num_to_create < count && keyfrm->camera_->depth_thr_ < depth) {
break;
}
// Stereo-triangulation cannot be performed if the 3D point has been already associated to the keypoint index
{
const auto& lm = curr_frm.landmarks_.at(idx);
if (lm) {
assert(lm->has_observation());
continue;
}
}
// Stereo-triangulation can be performed if the 3D point is not yet associated to the keypoint index
const Vec3_t pos_w = curr_frm.triangulate_stereo(idx);
auto lm = std::make_shared<data::landmark>(pos_w, keyfrm);
lm->add_observation(keyfrm, idx);
keyfrm->add_landmark(lm, idx);
curr_frm.landmarks_.at(idx) = lm;
lm->compute_descriptor();
lm->update_mean_normal_and_obs_scale_variance();
map_db->add_landmark(lm);
}
// Queue up the keyframe to the mapping module
queue_keyframe(keyfrm);
return keyfrm;
}
void keyframe_inserter::queue_keyframe(const std::shared_ptr<data::keyframe>& keyfrm) {
mapper_->queue_keyframe(keyfrm);
mapper_->stop_prevent_pause();
}
} // namespace module
} // namespace stella_vslam