2d/submaps.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 <cinttypes>
20 #include <cmath>
21 #include <cstdlib>
22 #include <fstream>
23 #include <limits>
24 
25 #include "Eigen/Geometry"
28 #include "glog/logging.h"
29 #include "webp/encode.h"
30 
31 namespace cartographer {
32 namespace mapping_2d {
33 
34 namespace {
35 
36 void WriteDebugImage(const string& filename,
37  const ProbabilityGrid& probability_grid) {
38  constexpr int kUnknown = 128;
39  const CellLimits& cell_limits = probability_grid.limits().cell_limits();
40  const int width = cell_limits.num_x_cells;
41  const int height = cell_limits.num_y_cells;
42  std::vector<uint8_t> rgb;
43  for (const Eigen::Array2i& xy_index :
44  XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
45  CHECK(probability_grid.limits().Contains(xy_index));
46  const uint8_t value =
47  probability_grid.IsKnown(xy_index)
49  (1. - probability_grid.GetProbability(xy_index)) * 255 + 0)
50  : kUnknown;
51  rgb.push_back(value);
52  rgb.push_back(value);
53  rgb.push_back(value);
54  }
55  uint8_t* output = nullptr;
56  size_t output_size =
57  WebPEncodeLosslessRGB(rgb.data(), width, height, 3 * width, &output);
58  std::unique_ptr<uint8_t, void (*)(void*)> output_deleter(output, std::free);
59  std::ofstream output_file(filename, std::ios::out | std::ios::binary);
60  output_file.write(reinterpret_cast<char*>(output), output_size);
61  output_file.close();
62  CHECK(output_file) << "Writing " << filename << " failed.";
63 }
64 
65 } // namespace
66 
68  const ProbabilityGrid& probability_grid) {
69  Eigen::Array2i offset;
70  CellLimits limits;
71  probability_grid.ComputeCroppedLimits(&offset, &limits);
72  const double resolution = probability_grid.limits().resolution();
73  const Eigen::Vector2d max =
74  probability_grid.limits().max() -
75  resolution * Eigen::Vector2d(offset.y(), offset.x());
76  ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits));
77  cropped_grid.StartUpdate();
78  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
79  if (probability_grid.IsKnown(xy_index + offset)) {
80  cropped_grid.SetProbability(
81  xy_index, probability_grid.GetProbability(xy_index + offset));
82  }
83  }
84  return cropped_grid;
85 }
86 
87 proto::SubmapsOptions CreateSubmapsOptions(
88  common::LuaParameterDictionary* const parameter_dictionary) {
89  proto::SubmapsOptions options;
90  options.set_resolution(parameter_dictionary->GetDouble("resolution"));
91  options.set_half_length(parameter_dictionary->GetDouble("half_length"));
92  options.set_num_range_data(
93  parameter_dictionary->GetNonNegativeInt("num_range_data"));
94  options.set_output_debug_images(
95  parameter_dictionary->GetBool("output_debug_images"));
96  *options.mutable_range_data_inserter_options() =
98  parameter_dictionary->GetDictionary("range_data_inserter").get());
99  CHECK_GT(options.num_range_data(), 0);
100  return options;
101 }
102 
103 Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
104  : mapping::Submap(transform::Rigid3d::Translation(
105  Eigen::Vector3d(origin.x(), origin.y(), 0.))),
106  probability_grid_(limits) {}
107 
109  const transform::Rigid3d&,
110  mapping::proto::SubmapQuery::Response* const response) const {
111  response->set_submap_version(num_range_data());
112 
113  Eigen::Array2i offset;
114  CellLimits limits;
115  probability_grid_.ComputeCroppedLimits(&offset, &limits);
116 
117  string cells;
118  for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
119  if (probability_grid_.IsKnown(xy_index + offset)) {
120  // We would like to add 'delta' but this is not possible using a value and
121  // alpha. We use premultiplied alpha, so when 'delta' is positive we can
122  // add it by setting 'alpha' to zero. If it is negative, we set 'value' to
123  // zero, and use 'alpha' to subtract. This is only correct when the pixel
124  // is currently white, so walls will look too gray. This should be hard to
125  // detect visually for the user, though.
126  const int delta =
128  probability_grid_.GetProbability(xy_index + offset));
129  const uint8 alpha = delta > 0 ? 0 : -delta;
130  const uint8 value = delta > 0 ? delta : 0;
131  cells.push_back(value);
132  cells.push_back((value || alpha) ? alpha : 1);
133  } else {
134  constexpr uint8 kUnknownLogOdds = 0;
135  cells.push_back(static_cast<uint8>(kUnknownLogOdds)); // value
136  cells.push_back(0); // alpha
137  }
138  }
139  common::FastGzipString(cells, response->mutable_cells());
140 
141  response->set_width(limits.num_x_cells);
142  response->set_height(limits.num_y_cells);
143  const double resolution = probability_grid_.limits().resolution();
144  response->set_resolution(resolution);
145  const double max_x =
146  probability_grid_.limits().max().x() - resolution * offset.y();
147  const double max_y =
148  probability_grid_.limits().max().y() - resolution * offset.x();
149  *response->mutable_slice_pose() = transform::ToProto(
150  local_pose().inverse() *
151  transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
152 }
153 
154 Submaps::Submaps(const proto::SubmapsOptions& options)
155  : options_(options),
156  range_data_inserter_(options.range_data_inserter_options()) {
157  // We always want to have at least one likelihood field which we can return,
158  // and will create it at the origin in absence of a better choice.
159  AddSubmap(Eigen::Vector2f::Zero());
160 }
161 
163  for (const int index : insertion_indices()) {
164  Submap* submap = submaps_[index].get();
165  CHECK(submap->finished_probability_grid_ == nullptr);
166  range_data_inserter_.Insert(range_data, &submap->probability_grid_);
167  ++submap->num_range_data_;
168  }
169  const Submap* const last_submap = Get(size() - 1);
170  if (last_submap->num_range_data_ == options_.num_range_data()) {
171  AddSubmap(range_data.origin.head<2>());
172  }
173 }
174 
175 const Submap* Submaps::Get(int index) const {
176  CHECK_GE(index, 0);
177  CHECK_LT(index, size());
178  return submaps_[index].get();
179 }
180 
181 int Submaps::size() const { return submaps_.size(); }
182 
183 void Submaps::FinishSubmap(int index) {
184  // Crop the finished Submap before inserting a new Submap to reduce peak
185  // memory usage a bit.
186  Submap* submap = submaps_[index].get();
187  CHECK(submap->finished_probability_grid_ == nullptr);
188  submap->probability_grid_ =
191  if (options_.output_debug_images()) {
192  // Output the Submap that won't be changed from now on.
193  WriteDebugImage("submap" + std::to_string(index) + ".webp",
194  submap->probability_grid_);
195  }
196 }
197 
198 void Submaps::AddSubmap(const Eigen::Vector2f& origin) {
199  if (size() > 1) {
200  FinishSubmap(size() - 2);
201  }
202  const int num_cells_per_dimension =
203  common::RoundToInt(2. * options_.half_length() / options_.resolution()) +
204  1;
205  submaps_.push_back(common::make_unique<Submap>(
206  MapLimits(options_.resolution(),
207  origin.cast<double>() +
208  options_.half_length() * Eigen::Vector2d::Ones(),
209  CellLimits(num_cells_per_dimension, num_cells_per_dimension)),
210  origin));
211  LOG(INFO) << "Added submap " << size();
212 }
213 
214 } // namespace mapping_2d
215 } // namespace cartographer
ProbabilityGrid probability_grid_
Definition: 2d/submaps.h:58
size_t num_range_data() const
Definition: submaps.h:68
proto::RangeDataInserterOptions options_
std::vector< std::unique_ptr< Submap > > submaps_
Definition: 2d/submaps.h:81
void FastGzipString(const string &uncompressed, string *compressed)
Definition: port.h:50
Rigid3< double > Rigid3d
const Eigen::Vector2d & max() const
Definition: map_limits.h:61
int RoundToInt(const float x)
Definition: port.h:42
uint8_t uint8
Definition: port.h:32
const proto::SubmapsOptions options_
Definition: 2d/submaps.h:79
transform::Rigid3d local_pose() const
Definition: submaps.h:65
void Insert(const sensor::RangeData &range_data, ProbabilityGrid *probability_grid) const
ProbabilityGrid ComputeCroppedProbabilityGrid(const ProbabilityGrid &probability_grid)
Definition: 2d/submaps.cc:67
std::vector< int > insertion_indices() const
Definition: submaps.cc:38
const Submap * Get(int index) const override
Definition: 2d/submaps.cc:175
void ComputeCroppedLimits(Eigen::Array2i *const offset, CellLimits *const limits) const
void ToResponseProto(const transform::Rigid3d &global_submap_pose, mapping::proto::SubmapQuery::Response *response) const override
Definition: 2d/submaps.cc:108
proto::Rigid2d ToProto(const transform::Rigid2d &transform)
Definition: transform.cc:44
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(common::LuaParameterDictionary *const parameter_dictionary)
float GetProbability(const Eigen::Array2i &xy_index) const
proto::SubmapsOptions CreateSubmapsOptions(common::LuaParameterDictionary *const parameter_dictionary)
Definition: 2d/submaps.cc:87
uint8 ProbabilityToLogOddsInteger(const float probability)
Definition: submaps.h:46
Submap(const MapLimits &limits, const Eigen::Vector2f &origin)
Definition: 2d/submaps.cc:103
ProbabilityGrid probability_grid_
bool IsKnown(const Eigen::Array2i &xy_index) const
RangeDataInserter range_data_inserter_
Definition: 2d/submaps.h:82
void AddSubmap(const Eigen::Vector2f &origin)
Definition: 2d/submaps.cc:198
void InsertRangeData(const sensor::RangeData &range_data)
Definition: 2d/submaps.cc:162
float value
static Rigid3 Translation(const Vector &vector)
std::unique_ptr< RangeDataInserter > range_data_inserter_
std::unique_ptr< LuaParameterDictionary > GetDictionary(const string &key)
const mapping_2d::ProbabilityGrid * finished_probability_grid_
Definition: submaps.h:88


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:59