-
Notifications
You must be signed in to change notification settings - Fork 374
/
marker_vertex_container.h
100 lines (78 loc) · 3.71 KB
/
marker_vertex_container.h
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
#ifndef STELLA_VSLAM_OPTIMIZE_G2O_MARKER_VERTEX_CONTAINER_H
#define STELLA_VSLAM_OPTIMIZE_G2O_MARKER_VERTEX_CONTAINER_H
#include "stella_vslam/type.h"
#include "stella_vslam/data/marker.h"
#include "stella_vslam/optimize/internal/landmark_vertex.h"
#include <unordered_map>
#include <memory>
namespace stella_vslam {
namespace data {
class marker;
} // namespace data
namespace optimize {
namespace internal {
class marker_vertex_container {
public:
//! Constructor
explicit marker_vertex_container(const std::shared_ptr<unsigned int> offset, const unsigned int num_reserve = 200);
//! Destructor
virtual ~marker_vertex_container() = default;
//! Create and return the g2o vertex created from the specified marker
std::vector<landmark_vertex*> create_vertices(const std::shared_ptr<data::marker>& mkr, const bool is_constant);
//! Get vertex corresponding with the specified marker
landmark_vertex* get_vertex(const std::shared_ptr<data::marker>& mkr, const unsigned int corner_id) const;
//! Get vertex corresponding with the specified marker ID
landmark_vertex* get_vertex(const unsigned int marker_id, const unsigned int corner_id) const;
private:
landmark_vertex* create_vertex(const unsigned int marker_id, const unsigned int corner_id,
const Vec3_t& pos_w, const bool is_constant);
//! vertex ID = offset + marker ID * 4 + corner ID
const std::shared_ptr<unsigned int> offset_ = nullptr;
//! key: marker ID, value: vertexs
std::unordered_map<unsigned int, std::vector<landmark_vertex*>> vtx_container_;
//! key: marker ID, value: vertex IDs
std::unordered_map<unsigned int, std::vector<unsigned int>> vtx_id_container_;
//! key: vertex ID, value: marker ID and corner ID
std::unordered_map<unsigned int, std::pair<unsigned int, unsigned int>> id_container_;
};
inline marker_vertex_container::marker_vertex_container(const std::shared_ptr<unsigned int> offset, const unsigned int num_reserve)
: offset_(offset) {
vtx_container_.reserve(num_reserve);
vtx_id_container_.reserve(num_reserve);
id_container_.reserve(num_reserve);
}
inline std::vector<landmark_vertex*> marker_vertex_container::create_vertices(const std::shared_ptr<data::marker>& mkr, const bool is_constant) {
std::vector<landmark_vertex*> vertices;
std::vector<unsigned int> vtx_ids;
for (unsigned int i = 0; i < mkr->corners_pos_w_.size(); ++i) {
vtx_ids.push_back(*offset_);
vertices.push_back(create_vertex(mkr->id_, i, mkr->corners_pos_w_[i], is_constant));
}
vtx_id_container_[mkr->id_] = vtx_ids;
vtx_container_[mkr->id_] = vertices;
return vertices;
}
inline landmark_vertex* marker_vertex_container::create_vertex(const unsigned int marker_id, const unsigned int corner_id,
const Vec3_t& pos_w, const bool is_constant) {
// Create vertex
const auto vtx_id = *offset_;
(*offset_)++;
auto vtx = new landmark_vertex();
vtx->setId(vtx_id);
vtx->setEstimate(pos_w);
vtx->setFixed(is_constant);
vtx->setMarginalized(true);
// Set to id database
id_container_[vtx_id] = std::make_pair(marker_id, corner_id);
return vtx;
}
inline landmark_vertex* marker_vertex_container::get_vertex(const std::shared_ptr<data::marker>& mkr, const unsigned int corner_id) const {
return get_vertex(mkr->id_, corner_id);
}
inline landmark_vertex* marker_vertex_container::get_vertex(const unsigned int marker_id, const unsigned int corner_id) const {
return vtx_container_.at(marker_id)[corner_id];
}
} // namespace internal
} // namespace optimize
} // namespace stella_vslam
#endif // STELLA_VSLAM_OPTIMIZE_G2O_MARKER_VERTEX_CONTAINER_H