mapping_3d/range_data_inserter.cc
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 
18 
19 #include "Eigen/Core"
21 #include "glog/logging.h"
22 
23 namespace cartographer {
24 namespace mapping_3d {
25 
26 namespace {
27 
28 void InsertMissesIntoGrid(const std::vector<uint16>& miss_table,
29  const Eigen::Vector3f& origin,
30  const sensor::PointCloud& returns,
31  HybridGrid* hybrid_grid,
32  const int num_free_space_voxels) {
33  const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin);
34  for (const Eigen::Vector3f& hit : returns) {
35  const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
36 
37  const Eigen::Array3i delta = hit_cell - origin_cell;
38  const int num_samples = delta.cwiseAbs().maxCoeff();
39  CHECK_LT(num_samples, 1 << 15);
40  // 'num_samples' is the number of samples we equi-distantly place on the
41  // line between 'origin' and 'hit'. (including a fractional part for sub-
42  // voxels) It is chosen so that between two samples we change from one voxel
43  // to the next on the fastest changing dimension.
44  //
45  // Only the last 'num_free_space_voxels' are updated for performance.
46  for (int position = std::max(0, num_samples - num_free_space_voxels);
47  position < num_samples; ++position) {
48  const Eigen::Array3i miss_cell =
49  origin_cell + delta * position / num_samples;
50  hybrid_grid->ApplyLookupTable(miss_cell, miss_table);
51  }
52  }
53 }
54 
55 } // namespace
56 
57 proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
58  common::LuaParameterDictionary* parameter_dictionary) {
59  proto::RangeDataInserterOptions options;
60  options.set_hit_probability(
61  parameter_dictionary->GetDouble("hit_probability"));
62  options.set_miss_probability(
63  parameter_dictionary->GetDouble("miss_probability"));
64  options.set_num_free_space_voxels(
65  parameter_dictionary->GetInt("num_free_space_voxels"));
66  CHECK_GT(options.hit_probability(), 0.5);
67  CHECK_LT(options.miss_probability(), 0.5);
68  return options;
69 }
70 
72  const proto::RangeDataInserterOptions& options)
73  : options_(options),
74  hit_table_(mapping::ComputeLookupTableToApplyOdds(
75  mapping::Odds(options_.hit_probability()))),
76  miss_table_(mapping::ComputeLookupTableToApplyOdds(
77  mapping::Odds(options_.miss_probability()))) {}
78 
80  HybridGrid* hybrid_grid) const {
81  CHECK_NOTNULL(hybrid_grid)->StartUpdate();
82 
83  for (const Eigen::Vector3f& hit : range_data.returns) {
84  const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit);
85  hybrid_grid->ApplyLookupTable(hit_cell, hit_table_);
86  }
87 
88  // By not starting a new update after hits are inserted, we give hits priority
89  // (i.e. no hits will be ignored because of a miss in the same cell).
90  InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns,
91  hybrid_grid, options_.num_free_space_voxels());
92 }
93 
94 } // namespace mapping_3d
95 } // namespace cartographer
proto::RangeDataInserterOptions options_
std::vector< uint16 > ComputeLookupTableToApplyOdds(const float odds)
Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
Definition: hybrid_grid.h:428
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(common::LuaParameterDictionary *parameter_dictionary)
float Odds(float probability)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
bool hit
Definition: 3d/submaps.cc:36
RangeDataInserter(const proto::RangeDataInserterOptions &options)
void Insert(const sensor::RangeData &range_data, HybridGrid *hybrid_grid) const
bool ApplyLookupTable(const Eigen::Array3i &index, const std::vector< uint16 > &table)
Definition: hybrid_grid.h:504


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