submaps.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
9  *
10  * Unless required by applicable law or agreed to in writing, software
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16
17 #ifndef CARTOGRAPHER_MAPPING_SUBMAPS_H_
18 #define CARTOGRAPHER_MAPPING_SUBMAPS_H_
19
20 #include <memory>
21 #include <vector>
22
23 #include "Eigen/Geometry"
28 #include "cartographer/mapping/proto/submap_visualization.pb.h"
31 #include "glog/logging.h"
32
33 namespace cartographer {
34 namespace mapping {
35
36 // Converts the given probability to log odds.
37 inline float Logit(float probability) {
38  return std::log(probability / (1.f - probability));
39 }
40
43
44 // Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds,
45 // kMaxLogOdds] is mapped to [1, 255].
46 inline uint8 ProbabilityToLogOddsInteger(const float probability) {
47  const int value = common::RoundToInt((Logit(probability) - kMinLogOdds) *
48  254.f / (kMaxLogOdds - kMinLogOdds)) +
49  1;
50  CHECK_LE(1, value);
51  CHECK_GE(255, value);
52  return value;
53 }
54
55 // An individual submap, which has a 'local_pose' in the local SLAM frame, keeps
56 // track of how many range data were inserted into it, and sets the
57 // 'finished_probability_grid' to be used for loop closing once the map no
58 // longer changes.
59 class Submap {
60  public:
62  virtual ~Submap() {}
63
64  // Local SLAM pose of this submap.
66
67  // Number of RangeData inserted.
68  size_t num_range_data() const { return num_range_data_; }
69
70  // The 'finished_probability_grid' when this submap is finished and will not
71  // change anymore. Otherwise, this is nullptr and the next call to
72  // InsertRangeData() will change the submap.
75  }
76
77  // Fills data into the 'response'.
78  virtual void ToResponseProto(
79  const transform::Rigid3d& global_submap_pose,
80  proto::SubmapQuery::Response* response) const = 0;
81
82  private:
84
85  protected:
86  // TODO(hrapp): All of this should be private.
87  int num_range_data_ = 0;
89 };
90
91 // Submaps is a sequence of maps to which scans are matched and into which scans
92 // are inserted.
93 //
94 // Except during initialization when only a single submap exists, there are
95 // always two submaps into which scans are inserted: an old submap that is used
96 // for matching, and a new one, which will be used for matching next, that is
97 // being initialized.
98 //
99 // Once a certain number of scans have been inserted, the new submap is
100 // considered initialized: the old submap is no longer changed, the "new" submap
101 // is now the "old" submap and is used for scan-to-map matching. Moreover,
102 // a "new" submap gets inserted.
103 class Submaps {
104  public:
105  Submaps();
106  virtual ~Submaps();
107
108  Submaps(const Submaps&) = delete;
109  Submaps& operator=(const Submaps&) = delete;
110
111  // Returns the index of the newest initialized Submap which can be
112  // used for scan-to-map matching.
113  int matching_index() const;
114
115  // Returns the indices of the Submap into which point clouds will
116  // be inserted.
117  std::vector<int> insertion_indices() const;
118
119  // Returns the Submap with the given 'index'. The same 'index' will always
120  // return the same pointer, so that Submaps can be identified by it.
121  virtual const Submap* Get(int index) const = 0;
122
123  // Returns the number of Submaps.
124  virtual int size() const = 0;
125 };
126
127 } // namespace mapping
128 } // namespace cartographer
129
130 #endif // CARTOGRAPHER_MAPPING_SUBMAPS_H_
size_t num_range_data() const
Definition: submaps.h:68
const float kMinLogOdds
Definition: submaps.h:42
int RoundToInt(const float x)
Definition: port.h:42
const mapping_2d::ProbabilityGrid * finished_probability_grid() const
Definition: submaps.h:73
constexpr float kMinProbability
uint8_t uint8
Definition: port.h:32
transform::Rigid3d local_pose() const
Definition: submaps.h:65
const float kMaxLogOdds
Definition: submaps.h:41
constexpr float kMaxProbability
float Logit(float probability)
Definition: submaps.h:37
const transform::Rigid3d local_pose_
Definition: submaps.h:83
uint8 ProbabilityToLogOddsInteger(const float probability)
Definition: submaps.h:46
virtual void ToResponseProto(const transform::Rigid3d &global_submap_pose, proto::SubmapQuery::Response *response) const =0
float value
const mapping_2d::ProbabilityGrid * finished_probability_grid_
Definition: submaps.h:88
Submap(const transform::Rigid3d &local_pose)
Definition: submaps.h:61

cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:39