precomputation_grid.cc
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
18
19 #include <algorithm>
20
21 #include "Eigen/Core"
24 #include "glog/logging.h"
25
26 namespace cartographer {
27 namespace mapping_3d {
28 namespace scan_matching {
29
30 namespace {
31
32 // C++11 defines that integer division rounds towards zero. For index math, we
33 // actually need it to round towards negative infinity. Luckily bit shifts have
34 // that property.
35 inline int DivideByTwoRoundingTowardsNegativeInfinity(const int value) {
36  return value >> 1;
37 }
38
39 // Computes the half resolution index corresponding to the full resolution
40 // 'cell_index'.
41 Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) {
42  return Eigen::Array3i(
43  DivideByTwoRoundingTowardsNegativeInfinity(cell_index[0]),
44  DivideByTwoRoundingTowardsNegativeInfinity(cell_index[1]),
45  DivideByTwoRoundingTowardsNegativeInfinity(cell_index[2]));
46 }
47
48 } // namespace
49
51  PrecomputationGrid result(hybrid_grid.resolution());
52  for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
53  const int cell_value = common::RoundToInt(
54  (mapping::ValueToProbability(it.GetValue()) -
57  CHECK_GE(cell_value, 0);
58  CHECK_LE(cell_value, 255);
59  *result.mutable_value(it.GetCellIndex()) = cell_value;
60  }
61  return result;
62 }
63
65  const bool half_resolution,
66  const Eigen::Array3i& shift) {
67  PrecomputationGrid result(grid.resolution());
68  for (auto it = PrecomputationGrid::Iterator(grid); !it.Done(); it.Next()) {
69  for (int i = 0; i != 8; ++i) {
70  // We use this value to update 8 values in the resulting grid, at
71  // position (x - {0, 'shift'}, y - {0, 'shift'}, z - {0, 'shift'}).
72  // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid,
73  // this results in precomputation grids analogous to the 2D case.
74  const Eigen::Array3i cell_index =
76  auto* const cell_value = result.mutable_value(
77  half_resolution ? CellIndexAtHalfResolution(cell_index) : cell_index);
78  *cell_value = std::max(it.GetValue(), *cell_value);
79  }
80  }
81  return result;
82 }
83
84 } // namespace scan_matching
85 } // namespace mapping_3d
86 } // namespace cartographer
Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
Definition: hybrid_grid.h:428
int RoundToInt(const float x)
Definition: port.h:42
constexpr float kMinProbability
float ValueToProbability(const uint16 value)
constexpr float kMaxProbability
PrecomputationGrid PrecomputeGrid(const PrecomputationGrid &grid, const bool half_resolution, const Eigen::Array3i &shift)
static Eigen::Array3i GetOctant(const int i)
Definition: hybrid_grid.h:436
float value
PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid &hybrid_grid)

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