hybrid_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_3D_HYBRID_GRID_H_
18 #define CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_
19 
20 #include <array>
21 #include <cmath>
22 #include <limits>
23 #include <utility>
24 #include <vector>
25 
26 #include "Eigen/Core"
31 #include "cartographer/mapping/proto/3d/hybrid_grid.pb.h"
33 #include "glog/logging.h"
34 
35 namespace cartographer {
36 namespace mapping {
37 
38 // Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat
39 // z-major index.
40 inline int ToFlatIndex(const Eigen::Array3i& index, const int bits) {
41  DCHECK((index >= 0).all() && (index < (1 << bits)).all()) << index;
42  return (((index.z() << bits) + index.y()) << bits) + index.x();
43 }
44 
45 // Converts a flat z-major 'index' to a 3-dimensional index with each dimension
46 // from 0 to 2^'bits' - 1.
47 inline Eigen::Array3i To3DIndex(const int index, const int bits) {
48  DCHECK_LT(index, 1 << (3 * bits));
49  const int mask = (1 << bits) - 1;
50  return Eigen::Array3i(index & mask, (index >> bits) & mask,
51  (index >> bits) >> bits);
52 }
53 
54 // A function to compare value to the default value. (Allows specializations).
55 template <typename TValueType>
56 bool IsDefaultValue(const TValueType& v) {
57  return v == TValueType();
58 }
59 
60 // Specialization to compare a std::vector to the default value.
61 template <typename TElementType>
62 bool IsDefaultValue(const std::vector<TElementType>& v) {
63  return v.empty();
64 }
65 
66 // A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of
67 // type 'ValueType' in contiguous memory. Indices in each dimension are 0-based.
68 template <typename TValueType, int kBits>
69 class FlatGrid {
70  public:
71  using ValueType = TValueType;
72 
73  // Creates a new flat grid with all values being default constructed.
75  for (ValueType& value : cells_) {
76  value = ValueType();
77  }
78  }
79 
80  FlatGrid(const FlatGrid&) = delete;
81  FlatGrid& operator=(const FlatGrid&) = delete;
82 
83  // Returns the number of voxels per dimension.
84  static int grid_size() { return 1 << kBits; }
85 
86  // Returns the value stored at 'index', each dimension of 'index' being
87  // between 0 and grid_size() - 1.
88  ValueType value(const Eigen::Array3i& index) const {
89  return cells_[ToFlatIndex(index, kBits)];
90  }
91 
92  // Returns a pointer to a value to allow changing it.
93  ValueType* mutable_value(const Eigen::Array3i& index) {
94  return &cells_[ToFlatIndex(index, kBits)];
95  }
96 
97  // An iterator for iterating over all values not comparing equal to the
98  // default constructed value.
99  class Iterator {
100  public:
101  Iterator() : current_(nullptr), end_(nullptr) {}
102 
103  explicit Iterator(const FlatGrid& flat_grid)
104  : current_(flat_grid.cells_.data()),
105  end_(flat_grid.cells_.data() + flat_grid.cells_.size()) {
106  while (!Done() && IsDefaultValue(*current_)) {
107  ++current_;
108  }
109  }
110 
111  void Next() {
112  DCHECK(!Done());
113  do {
114  ++current_;
115  } while (!Done() && IsDefaultValue(*current_));
116  }
117 
118  bool Done() const { return current_ == end_; }
119 
120  Eigen::Array3i GetCellIndex() const {
121  DCHECK(!Done());
122  const int index = (1 << (3 * kBits)) - (end_ - current_);
123  return To3DIndex(index, kBits);
124  }
125 
126  const ValueType& GetValue() const {
127  DCHECK(!Done());
128  return *current_;
129  }
130 
131  private:
133  const ValueType* end_;
134  };
135 
136  private:
137  std::array<ValueType, 1 << (3 * kBits)> cells_;
138 };
139 
140 // A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type
141 // 'WrappedGrid'. Wrapped grids are constructed on first access via
142 // 'mutable_value()'.
143 template <typename WrappedGrid, int kBits>
144 class NestedGrid {
145  public:
146  using ValueType = typename WrappedGrid::ValueType;
147 
148  // Returns the number of voxels per dimension.
149  static int grid_size() { return WrappedGrid::grid_size() << kBits; }
150 
151  // Returns the value stored at 'index', each dimension of 'index' being
152  // between 0 and grid_size() - 1.
153  ValueType value(const Eigen::Array3i& index) const {
154  const Eigen::Array3i meta_index = GetMetaIndex(index);
155  const WrappedGrid* const meta_cell =
156  meta_cells_[ToFlatIndex(meta_index, kBits)].get();
157  if (meta_cell == nullptr) {
158  return ValueType();
159  }
160  const Eigen::Array3i inner_index =
161  index - meta_index * WrappedGrid::grid_size();
162  return meta_cell->value(inner_index);
163  }
164 
165  // Returns a pointer to the value at 'index' to allow changing it. If
166  // necessary a new wrapped grid is constructed to contain that value.
167  ValueType* mutable_value(const Eigen::Array3i& index) {
168  const Eigen::Array3i meta_index = GetMetaIndex(index);
169  std::unique_ptr<WrappedGrid>& meta_cell =
170  meta_cells_[ToFlatIndex(meta_index, kBits)];
171  if (meta_cell == nullptr) {
172  meta_cell = common::make_unique<WrappedGrid>();
173  }
174  const Eigen::Array3i inner_index =
175  index - meta_index * WrappedGrid::grid_size();
176  return meta_cell->mutable_value(inner_index);
177  }
178 
179  // An iterator for iterating over all values not comparing equal to the
180  // default constructed value.
181  class Iterator {
182  public:
183  Iterator() : current_(nullptr), end_(nullptr), nested_iterator_() {}
184 
185  explicit Iterator(const NestedGrid& nested_grid)
186  : current_(nested_grid.meta_cells_.data()),
187  end_(nested_grid.meta_cells_.data() + nested_grid.meta_cells_.size()),
188  nested_iterator_() {
189  AdvanceToValidNestedIterator();
190  }
191 
192  void Next() {
193  DCHECK(!Done());
194  nested_iterator_.Next();
195  if (!nested_iterator_.Done()) {
196  return;
197  }
198  ++current_;
199  AdvanceToValidNestedIterator();
200  }
201 
202  bool Done() const { return current_ == end_; }
203 
204  Eigen::Array3i GetCellIndex() const {
205  DCHECK(!Done());
206  const int index = (1 << (3 * kBits)) - (end_ - current_);
207  return To3DIndex(index, kBits) * WrappedGrid::grid_size() +
208  nested_iterator_.GetCellIndex();
209  }
210 
211  const ValueType& GetValue() const {
212  DCHECK(!Done());
213  return nested_iterator_.GetValue();
214  }
215 
216  private:
218  for (; !Done(); ++current_) {
219  if (*current_ != nullptr) {
220  nested_iterator_ = typename WrappedGrid::Iterator(**current_);
221  if (!nested_iterator_.Done()) {
222  break;
223  }
224  }
225  }
226  }
227 
228  const std::unique_ptr<WrappedGrid>* current_;
229  const std::unique_ptr<WrappedGrid>* end_;
230  typename WrappedGrid::Iterator nested_iterator_;
231  };
232 
233  private:
234  // Returns the Eigen::Array3i (meta) index of the meta cell containing
235  // 'index'.
236  Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const {
237  DCHECK((index >= 0).all()) << index;
238  const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
239  DCHECK((meta_index < (1 << kBits)).all()) << index;
240  return meta_index;
241  }
242 
243  std::array<std::unique_ptr<WrappedGrid>, 1 << (3 * kBits)> meta_cells_;
244 };
245 
246 // A grid consisting of 2x2x2 grids of type 'WrappedGrid' initially. Wrapped
247 // grids are constructed on first access via 'mutable_value()'. If necessary,
248 // the grid grows to twice the size in each dimension. The range of indices is
249 // (almost) symmetric around the origin, i.e. negative indices are allowed.
250 template <typename WrappedGrid>
251 class DynamicGrid {
252  public:
253  using ValueType = typename WrappedGrid::ValueType;
254 
255  DynamicGrid() : bits_(1), meta_cells_(8) {}
256  DynamicGrid(DynamicGrid&&) = default;
257  DynamicGrid& operator=(DynamicGrid&&) = default;
258 
259  // Returns the current number of voxels per dimension.
260  int grid_size() const { return WrappedGrid::grid_size() << bits_; }
261 
262  // Returns the value stored at 'index'.
263  ValueType value(const Eigen::Array3i& index) const {
264  const Eigen::Array3i shifted_index = index + (grid_size() >> 1);
265  // The cast to unsigned is for performance to check with 3 comparisons
266  // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
267  if ((shifted_index.cast<unsigned int>() >= grid_size()).any()) {
268  return ValueType();
269  }
270  const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
271  const WrappedGrid* const meta_cell =
272  meta_cells_[ToFlatIndex(meta_index, bits_)].get();
273  if (meta_cell == nullptr) {
274  return ValueType();
275  }
276  const Eigen::Array3i inner_index =
277  shifted_index - meta_index * WrappedGrid::grid_size();
278  return meta_cell->value(inner_index);
279  }
280 
281  // Returns a pointer to the value at 'index' to allow changing it, dynamically
282  // growing the DynamicGrid and constructing new WrappedGrids as needed.
283  ValueType* mutable_value(const Eigen::Array3i& index) {
284  const Eigen::Array3i shifted_index = index + (grid_size() >> 1);
285  // The cast to unsigned is for performance to check with 3 comparisons
286  // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
287  if ((shifted_index.cast<unsigned int>() >= grid_size()).any()) {
288  Grow();
289  return mutable_value(index);
290  }
291  const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
292  std::unique_ptr<WrappedGrid>& meta_cell =
293  meta_cells_[ToFlatIndex(meta_index, bits_)];
294  if (meta_cell == nullptr) {
295  meta_cell = common::make_unique<WrappedGrid>();
296  }
297  const Eigen::Array3i inner_index =
298  shifted_index - meta_index * WrappedGrid::grid_size();
299  return meta_cell->mutable_value(inner_index);
300  }
301 
302  // An iterator for iterating over all values not comparing equal to the
303  // default constructed value.
304  class Iterator {
305  public:
306  explicit Iterator(const DynamicGrid& dynamic_grid)
307  : bits_(dynamic_grid.bits_),
308  current_(dynamic_grid.meta_cells_.data()),
309  end_(dynamic_grid.meta_cells_.data() +
310  dynamic_grid.meta_cells_.size()),
311  nested_iterator_() {
312  AdvanceToValidNestedIterator();
313  }
314 
315  void Next() {
316  DCHECK(!Done());
317  nested_iterator_.Next();
318  if (!nested_iterator_.Done()) {
319  return;
320  }
321  ++current_;
322  AdvanceToValidNestedIterator();
323  }
324 
325  bool Done() const { return current_ == end_; }
326 
327  Eigen::Array3i GetCellIndex() const {
328  DCHECK(!Done());
329  const int outer_index = (1 << (3 * bits_)) - (end_ - current_);
330  const Eigen::Array3i shifted_index =
331  To3DIndex(outer_index, bits_) * WrappedGrid::grid_size() +
332  nested_iterator_.GetCellIndex();
333  return shifted_index - ((1 << (bits_ - 1)) * WrappedGrid::grid_size());
334  }
335 
336  const ValueType& GetValue() const {
337  DCHECK(!Done());
338  return nested_iterator_.GetValue();
339  }
340 
341  void AdvanceToEnd() { current_ = end_; }
342 
343  const std::pair<Eigen::Array3i, ValueType> operator*() const {
344  return std::pair<Eigen::Array3i, ValueType>(GetCellIndex(), GetValue());
345  }
346 
348  Next();
349  return *this;
350  }
351 
352  bool operator!=(const Iterator& it) const {
353  return it.current_ != current_;
354  }
355 
356  private:
358  for (; !Done(); ++current_) {
359  if (*current_ != nullptr) {
360  nested_iterator_ = typename WrappedGrid::Iterator(**current_);
361  if (!nested_iterator_.Done()) {
362  break;
363  }
364  }
365  }
366  }
367 
368  int bits_;
369  const std::unique_ptr<WrappedGrid>* current_;
370  const std::unique_ptr<WrappedGrid>* const end_;
371  typename WrappedGrid::Iterator nested_iterator_;
372  };
373 
374  private:
375  // Returns the Eigen::Array3i (meta) index of the meta cell containing
376  // 'index'.
377  Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const {
378  DCHECK((index >= 0).all()) << index;
379  const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
380  DCHECK((meta_index < (1 << bits_)).all()) << index;
381  return meta_index;
382  }
383 
384  // Grows this grid by a factor of 2 in each of the 3 dimensions.
385  void Grow() {
386  const int new_bits = bits_ + 1;
387  CHECK_LE(new_bits, 8);
388  std::vector<std::unique_ptr<WrappedGrid>> new_meta_cells_(
389  8 * meta_cells_.size());
390  for (int z = 0; z != (1 << bits_); ++z) {
391  for (int y = 0; y != (1 << bits_); ++y) {
392  for (int x = 0; x != (1 << bits_); ++x) {
393  const Eigen::Array3i original_meta_index(x, y, z);
394  const Eigen::Array3i new_meta_index =
395  original_meta_index + (1 << (bits_ - 1));
396  new_meta_cells_[ToFlatIndex(new_meta_index, new_bits)] =
397  std::move(meta_cells_[ToFlatIndex(original_meta_index, bits_)]);
398  }
399  }
400  }
401  meta_cells_ = std::move(new_meta_cells_);
402  bits_ = new_bits;
403  }
404 
405  int bits_;
406  std::vector<std::unique_ptr<WrappedGrid>> meta_cells_;
407 };
408 
409 template <typename ValueType>
411 
412 // Represents a 3D grid as a wide, shallow tree.
413 template <typename ValueType>
414 class HybridGridBase : public GridBase<ValueType> {
415  public:
417 
418  // Creates a new tree-based probability grid with voxels having edge length
419  // 'resolution' around the origin which becomes the center of the cell at
420  // index (0, 0, 0).
421  explicit HybridGridBase(const float resolution) : resolution_(resolution) {}
422 
423  float resolution() const { return resolution_; }
424 
425  // Returns the index of the cell containing the 'point'. Indices are integer
426  // vectors identifying cells, for this the coordinates are rounded to the next
427  // multiple of the resolution.
428  Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const {
429  Eigen::Array3f index = point.array() / resolution_;
430  return Eigen::Array3i(common::RoundToInt(index.x()),
431  common::RoundToInt(index.y()),
432  common::RoundToInt(index.z()));
433  }
434 
435  // Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1).
436  static Eigen::Array3i GetOctant(const int i) {
437  DCHECK_GE(i, 0);
438  DCHECK_LT(i, 8);
439  return Eigen::Array3i(static_cast<bool>(i & 1), static_cast<bool>(i & 2),
440  static_cast<bool>(i & 4));
441  }
442 
443  // Returns the center of the cell at 'index'.
444  Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i& index) const {
445  return index.matrix().cast<float>() * resolution_;
446  }
447 
448  // Iterator functions for range-based for loops.
449  Iterator begin() const { return Iterator(*this); }
450 
451  Iterator end() const {
452  Iterator it(*this);
453  it.AdvanceToEnd();
454  return it;
455  }
456 
457  private:
458  // Edge length of each voxel.
459  const float resolution_;
460 };
461 
462 // A grid containing probability values stored using 15 bits, and an update
463 // marker per voxel.
464 // Points are expected to be close to the origin. Points far from the origin
465 // require the grid to grow dynamically. For centimeter resolution, points
466 // can only be tens of meters from the origin.
467 // The hard limit of cell indexes is +/- 8192 around the origin.
468 class HybridGrid : public HybridGridBase<uint16> {
469  public:
470  explicit HybridGrid(const float resolution)
471  : HybridGridBase<uint16>(resolution) {}
472 
473  explicit HybridGrid(const proto::HybridGrid& proto)
474  : HybridGrid(proto.resolution()) {
475  CHECK_EQ(proto.values_size(), proto.x_indices_size());
476  CHECK_EQ(proto.values_size(), proto.y_indices_size());
477  CHECK_EQ(proto.values_size(), proto.z_indices_size());
478  for (int i = 0; i < proto.values_size(); ++i) {
479  // SetProbability does some error checking for us.
480  SetProbability(Eigen::Vector3i(proto.x_indices(i), proto.y_indices(i),
481  proto.z_indices(i)),
482  ValueToProbability(proto.values(i)));
483  }
484  }
485 
486  // Sets the probability of the cell at 'index' to the given 'probability'.
487  void SetProbability(const Eigen::Array3i& index, const float probability) {
488  *mutable_value(index) = ProbabilityToValue(probability);
489  }
490 
491  // Finishes the update sequence.
492  void FinishUpdate() {
493  while (!update_indices_.empty()) {
494  DCHECK_GE(*update_indices_.back(), kUpdateMarker);
495  *update_indices_.back() -= kUpdateMarker;
496  update_indices_.pop_back();
497  }
498  }
499 
500  // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
501  // to the probability of the cell at 'index' if the cell has not already been
502  // updated. Multiple updates of the same cell will be ignored until
503  // FinishUpdate() is called. Returns true if the cell was updated.
504  //
505  // If this is the first call to ApplyOdds() for the specified cell, its value
506  // will be set to probability corresponding to 'odds'.
507  bool ApplyLookupTable(const Eigen::Array3i& index,
508  const std::vector<uint16>& table) {
509  DCHECK_EQ(table.size(), kUpdateMarker);
510  uint16* const cell = mutable_value(index);
511  if (*cell >= kUpdateMarker) {
512  return false;
513  }
514  update_indices_.push_back(cell);
515  *cell = table[*cell];
516  DCHECK_GE(*cell, kUpdateMarker);
517  return true;
518  }
519 
520  // Returns the probability of the cell with 'index'.
521  float GetProbability(const Eigen::Array3i& index) const {
522  return ValueToProbability(value(index));
523  }
524 
525  // Returns true if the probability at the specified 'index' is known.
526  bool IsKnown(const Eigen::Array3i& index) const { return value(index) != 0; }
527 
528  proto::HybridGrid ToProto() const {
529  CHECK(update_indices_.empty()) << "Serializing a grid during an update is "
530  "not supported. Finish the update first.";
531  proto::HybridGrid result;
532  result.set_resolution(resolution());
533  for (const auto it : *this) {
534  result.add_x_indices(it.first.x());
535  result.add_y_indices(it.first.y());
536  result.add_z_indices(it.first.z());
537  result.add_values(it.second);
538  }
539  return result;
540  }
541 
542  private:
543  // Markers at changed cells.
544  std::vector<ValueType*> update_indices_;
545 };
546 
547 } // namespace mapping
548 } // namespace cartographer
549 
550 #endif // CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_
Iterator(const DynamicGrid &dynamic_grid)
Definition: hybrid_grid.h:306
const std::unique_ptr< WrappedGrid > * end_
Definition: hybrid_grid.h:229
proto::HybridGrid ToProto() const
Definition: hybrid_grid.h:528
ValueType * mutable_value(const Eigen::Array3i &index)
Definition: hybrid_grid.h:283
typename GridBase< cartographer::io::OutlierRemovingPointsProcessor::VoxelData >::Iterator Iterator
Definition: hybrid_grid.h:416
ValueType value(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:263
int RoundToInt(const float x)
Definition: port.h:41
HybridGrid(const proto::HybridGrid &proto)
Definition: hybrid_grid.h:473
bool IsDefaultValue(const TValueType &v)
Definition: hybrid_grid.h:56
const std::unique_ptr< WrappedGrid > * current_
Definition: hybrid_grid.h:369
const ValueType & GetValue() const
Definition: hybrid_grid.h:126
const std::pair< Eigen::Array3i, ValueType > operator*() const
Definition: hybrid_grid.h:343
std::vector< std::unique_ptr< WrappedGrid > > meta_cells_
Definition: hybrid_grid.h:406
float ValueToProbability(const uint16 value)
HybridGridBase(const float resolution)
Definition: hybrid_grid.h:421
Eigen::Array3i GetMetaIndex(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:236
HybridGrid(const float resolution)
Definition: hybrid_grid.h:470
uint16_t uint16
Definition: port.h:35
float GetProbability(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:521
typename WrappedGrid::ValueType ValueType
Definition: hybrid_grid.h:146
bool IsKnown(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:526
Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:444
bool ApplyLookupTable(const Eigen::Array3i &index, const std::vector< uint16 > &table)
Definition: hybrid_grid.h:507
typename WrappedGrid::ValueType ValueType
Definition: hybrid_grid.h:253
std::vector< ValueType * > update_indices_
Definition: hybrid_grid.h:544
const std::unique_ptr< WrappedGrid > * current_
Definition: hybrid_grid.h:228
Iterator(const FlatGrid &flat_grid)
Definition: hybrid_grid.h:103
FlatGrid & operator=(const FlatGrid &)=delete
ValueType * mutable_value(const Eigen::Array3i &index)
Definition: hybrid_grid.h:93
Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
Definition: hybrid_grid.h:428
const std::unique_ptr< WrappedGrid > *const end_
Definition: hybrid_grid.h:370
void SetProbability(const Eigen::Array3i &index, const float probability)
Definition: hybrid_grid.h:487
ValueType value(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:88
ValueType * mutable_value(const Eigen::Array3i &index)
Definition: hybrid_grid.h:167
Eigen::Array3i To3DIndex(const int index, const int bits)
Definition: hybrid_grid.h:47
uint16 ProbabilityToValue(const float probability)
Iterator(const NestedGrid &nested_grid)
Definition: hybrid_grid.h:185
bool operator!=(const Iterator &it) const
Definition: hybrid_grid.h:352
constexpr uint16 kUpdateMarker
int ToFlatIndex(const Eigen::Array3i &index, const int bits)
Definition: hybrid_grid.h:40
Eigen::Array3i GetMetaIndex(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:377
ValueType value(const Eigen::Array3i &index) const
Definition: hybrid_grid.h:153
std::array< ValueType, 1<<(3 *kBits)> cells_
Definition: hybrid_grid.h:137
static Eigen::Array3i GetOctant(const int i)
Definition: hybrid_grid.h:436


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58