mapping_2d/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 <cstdlib>
20 
21 #include "Eigen/Core"
22 #include "Eigen/Geometry"
25 #include "glog/logging.h"
26 
27 namespace cartographer {
28 namespace mapping_2d {
29 
30 proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
31  common::LuaParameterDictionary* const parameter_dictionary) {
32  proto::RangeDataInserterOptions options;
33  options.set_hit_probability(
34  parameter_dictionary->GetDouble("hit_probability"));
35  options.set_miss_probability(
36  parameter_dictionary->GetDouble("miss_probability"));
37  options.set_insert_free_space(
38  parameter_dictionary->HasKey("insert_free_space")
39  ? parameter_dictionary->GetBool("insert_free_space")
40  : true);
41  CHECK_GT(options.hit_probability(), 0.5);
42  CHECK_LT(options.miss_probability(), 0.5);
43  return options;
44 }
45 
47  const proto::RangeDataInserterOptions& options)
48  : options_(options),
49  hit_table_(mapping::ComputeLookupTableToApplyOdds(
50  mapping::Odds(options.hit_probability()))),
51  miss_table_(mapping::ComputeLookupTableToApplyOdds(
52  mapping::Odds(options.miss_probability()))) {}
53 
55  ProbabilityGrid* const probability_grid) const {
56  CHECK_NOTNULL(probability_grid)->StartUpdate();
57 
58  // By not starting a new update after hits are inserted, we give hits priority
59  // (i.e. no hits will be ignored because of a miss in the same cell).
60  CastRays(range_data, probability_grid->limits(),
61  [this, &probability_grid](const Eigen::Array2i& hit) {
62  probability_grid->ApplyLookupTable(hit, hit_table_);
63  },
64  [this, &probability_grid](const Eigen::Array2i& miss) {
65  if (options_.insert_free_space()) {
66  probability_grid->ApplyLookupTable(miss, miss_table_);
67  }
68  });
69 }
70 
71 } // namespace mapping_2d
72 } // namespace cartographer
proto::RangeDataInserterOptions options_
std::vector< uint16 > ComputeLookupTableToApplyOdds(const float odds)
bool ApplyLookupTable(const Eigen::Array2i &xy_index, const std::vector< uint16 > &table)
void Insert(const sensor::RangeData &range_data, ProbabilityGrid *probability_grid) const
float Odds(float probability)
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(common::LuaParameterDictionary *const parameter_dictionary)
RangeDataInserter(const proto::RangeDataInserterOptions &options)
bool hit
Definition: 3d/submaps.cc:36
void CastRays(const sensor::RangeData &range_data, const MapLimits &limits, const std::function< void(const Eigen::Array2i &)> &hit_visitor, const std::function< void(const Eigen::Array2i &)> &miss_visitor)
Definition: ray_casting.cc:150


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