probability_grid.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
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_2D_PROBABILITY_GRID_H_
18 #define CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
19 
20 #include <algorithm>
21 #include <cmath>
22 #include <iostream>
23 #include <limits>
24 #include <memory>
25 #include <string>
26 #include <vector>
27 
32 #include "cartographer/mapping_2d/proto/probability_grid.pb.h"
34 #include "glog/logging.h"
35 
36 namespace cartographer {
37 namespace mapping_2d {
38 
39 // Represents a 2D grid of probabilities.
41  public:
42  explicit ProbabilityGrid(const MapLimits& limits)
43  : limits_(limits),
44  cells_(limits_.cell_limits().num_x_cells *
45  limits_.cell_limits().num_y_cells,
46  mapping::kUnknownProbabilityValue),
47  max_x_(0),
48  max_y_(0),
49  min_x_(limits_.cell_limits().num_x_cells - 1),
50  min_y_(limits_.cell_limits().num_y_cells - 1) {}
51 
52  explicit ProbabilityGrid(const proto::ProbabilityGrid& proto)
53  : limits_(proto.limits()),
54  cells_(),
55  update_indices_(proto.update_indices().begin(),
56  proto.update_indices().end()),
57  max_x_(proto.max_x()),
58  max_y_(proto.max_y()),
59  min_x_(proto.min_x()),
60  min_y_(proto.min_y()) {
61  cells_.reserve(proto.cells_size());
62  for (const auto cell : proto.cells()) {
63  CHECK_LE(cell, std::numeric_limits<uint16>::max());
64  cells_.push_back(cell);
65  }
66  }
67 
68  // Returns the limits of this ProbabilityGrid.
69  const MapLimits& limits() const { return limits_; }
70 
71  // Starts the next update sequence.
72  void StartUpdate() {
73  while (!update_indices_.empty()) {
74  DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
76  update_indices_.pop_back();
77  }
78  }
79 
80  // Sets the probability of the cell at 'xy_index' to the given 'probability'.
81  // Only allowed if the cell was unknown before.
82  void SetProbability(const Eigen::Array2i& xy_index, const float probability) {
83  uint16& cell = cells_[GetIndexOfCell(xy_index)];
84  CHECK_EQ(cell, mapping::kUnknownProbabilityValue);
85  cell = mapping::ProbabilityToValue(probability);
86  UpdateBounds(xy_index);
87  }
88 
89  // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
90  // to the probability of the cell at 'xy_index' if the cell has not already
91  // been updated. Multiple updates of the same cell will be ignored until
92  // StartUpdate() is called. Returns true if the cell was updated.
93  //
94  // If this is the first call to ApplyOdds() for the specified cell, its value
95  // will be set to probability corresponding to 'odds'.
96  bool ApplyLookupTable(const Eigen::Array2i& xy_index,
97  const std::vector<uint16>& table) {
98  DCHECK_EQ(table.size(), mapping::kUpdateMarker);
99  const int cell_index = GetIndexOfCell(xy_index);
100  uint16& cell = cells_[cell_index];
101  if (cell >= mapping::kUpdateMarker) {
102  return false;
103  }
104  update_indices_.push_back(cell_index);
105  cell = table[cell];
106  DCHECK_GE(cell, mapping::kUpdateMarker);
107  UpdateBounds(xy_index);
108  return true;
109  }
110 
111  // Returns the probability of the cell with 'xy_index'.
112  float GetProbability(const Eigen::Array2i& xy_index) const {
113  if (limits_.Contains(xy_index)) {
115  }
117  }
118 
119  // Returns the probability of the cell containing the point ('x', 'y').
120  float GetProbability(const double x, const double y) const {
122  }
123 
124  // Returns true if the probability at the specified index is known.
125  bool IsKnown(const Eigen::Array2i& xy_index) const {
126  return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] !=
128  }
129 
130  // Fills in 'offset' and 'limits' to define a subregion of that contains all
131  // known cells.
132  void ComputeCroppedLimits(Eigen::Array2i* const offset,
133  CellLimits* const limits) const {
134  *offset = Eigen::Array2i(min_x_, min_y_);
135  *limits = CellLimits(std::max(max_x_, min_x_) - min_x_ + 1,
136  std::max(max_y_, min_y_) - min_y_ + 1);
137  }
138 
139  // Grows the map as necessary to include 'x' and 'y'. This changes the meaning
140  // of these coordinates going forward. This method must be called immediately
141  // after 'StartUpdate', before any calls to 'ApplyLookupTable'.
142  void GrowLimits(const double x, const double y) {
143  CHECK(update_indices_.empty());
145  const int x_offset = limits_.cell_limits().num_x_cells / 2;
146  const int y_offset = limits_.cell_limits().num_y_cells / 2;
147  const MapLimits new_limits(
149  limits_.max() +
150  limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
153  const int stride = new_limits.cell_limits().num_x_cells;
154  const int offset = x_offset + stride * y_offset;
155  const int new_size = new_limits.cell_limits().num_x_cells *
156  new_limits.cell_limits().num_y_cells;
157  std::vector<uint16> new_cells(new_size,
159  for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
160  for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
161  new_cells[offset + j + i * stride] =
163  }
164  }
165  cells_ = new_cells;
166  limits_ = new_limits;
167  min_x_ += x_offset;
168  min_y_ += y_offset;
169  max_x_ += x_offset;
170  max_y_ += y_offset;
171  }
172  }
173 
174  proto::ProbabilityGrid ToProto() const {
175  proto::ProbabilityGrid result;
176  *result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_);
177  result.mutable_cells()->Reserve(cells_.size());
178  for (const auto cell : cells_) {
179  result.mutable_cells()->Add(cell);
180  }
181  result.mutable_update_indices()->Reserve(update_indices_.size());
182  for (const auto update : update_indices_) {
183  result.mutable_update_indices()->Add(update);
184  }
185  result.set_max_x(max_x_);
186  result.set_max_y(max_y_);
187  result.set_min_x(min_x_);
188  result.set_min_y(min_y_);
189  return result;
190  }
191 
192  private:
193  // Returns the index of the cell at 'xy_index'.
194  int GetIndexOfCell(const Eigen::Array2i& xy_index) const {
195  CHECK(limits_.Contains(xy_index)) << xy_index;
196  return limits_.cell_limits().num_x_cells * xy_index.y() + xy_index.x();
197  }
198 
199  void UpdateBounds(const Eigen::Array2i& xy_index) {
200  min_x_ = std::min(min_x_, xy_index.x());
201  min_y_ = std::min(min_y_, xy_index.y());
202  max_x_ = std::max(max_x_, xy_index.x());
203  max_y_ = std::max(max_y_, xy_index.y());
204  }
205 
207  std::vector<uint16> cells_; // Highest bit is update marker.
208  std::vector<int> update_indices_;
209 
210  // Minimum and maximum cell coordinates of known cells to efficiently compute
211  // cropping limits.
212  int max_x_;
213  int max_y_;
214  int min_x_;
215  int min_y_;
216 };
217 
218 } // namespace mapping_2d
219 } // namespace cartographer
220 
221 #endif // CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
void UpdateBounds(const Eigen::Array2i &xy_index)
const Eigen::Vector2d & max() const
Definition: map_limits.h:61
constexpr float kMinProbability
bool ApplyLookupTable(const Eigen::Array2i &xy_index, const std::vector< uint16 > &table)
proto::ProbabilityGrid ToProto() const
Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x, const double y) const
Definition: map_limits.h:69
int GetIndexOfCell(const Eigen::Array2i &xy_index) const
float ValueToProbability(const uint16 value)
void ComputeCroppedLimits(Eigen::Array2i *const offset, CellLimits *const limits) const
float GetProbability(const double x, const double y) const
float GetProbability(const Eigen::Array2i &xy_index) const
const CellLimits & cell_limits() const
Definition: map_limits.h:64
void GrowLimits(const double x, const double y)
void SetProbability(const Eigen::Array2i &xy_index, const float probability)
bool IsKnown(const Eigen::Array2i &xy_index) const
constexpr uint16 kUnknownProbabilityValue
ProbabilityGrid(const proto::ProbabilityGrid &proto)
bool Contains(const Eigen::Array2i &xy_index) const
Definition: map_limits.h:80
proto::MapLimits ToProto(const MapLimits &map_limits)
Definition: map_limits.h:93
uint16 ProbabilityToValue(const float probability)
constexpr uint16 kUpdateMarker
uint16_t uint16
Definition: port.h:33


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