-
Notifications
You must be signed in to change notification settings - Fork 0
/
submap.h
46 lines (31 loc) · 1.01 KB
/
submap.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
#pragma once
#include "2dmapping/frame.h"
#include "2dmapping/likelihood_field.h"
#include "2dmapping/occupancy_map.h"
namespace zqx {
class Submap {
public:
Submap(const SE2& pose) : pose_(pose) {
field_.SetPose(pose);
occu_map_.SetPose(pose);
}
bool MatchScan(std::shared_ptr<Frame> frame);
void AddKeyFrame(std::shared_ptr<Frame> frame) { frames_.emplace_back(frame); }
void AddScanInOccupancyMap(std::shared_ptr<Frame> frame);
void SetOccuFromOtherSubmap(std::shared_ptr<Submap> other);
OccupancyMap& GetOccuMap() { return occu_map_; }
bool HasOutsidePoints() const;
size_t NumFrames() const;
std::vector<std::shared_ptr<Frame>>& GetFrames() { return frames_; }
void SetId(size_t id) { id_ = id; }
size_t GetId() const { return id_; }
void SetPose(const SE2& pose);
SE2 GetPose() const { return pose_; }
private:
SE2 pose_; // T_W_S
size_t id_ = 0;
std::vector<std::shared_ptr<Frame>> frames_;
LikelihoodField field_;
OccupancyMap occu_map_;
};
} // namespace zqx