xray_points_processor.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 <cmath>
20 #include <string>
21 
22 #include "Eigen/Core"
23 #include "cairo/cairo.h"
30 
31 namespace cartographer {
32 namespace io {
33 namespace {
34 
35 struct PixelData {
37  double mean_r = 0.;
38  double mean_g = 0.;
39  double mean_b = 0.;
40 };
41 
42 using PixelDataMatrix =
43  Eigen::Matrix<PixelData, Eigen::Dynamic, Eigen::Dynamic>;
44 
45 double Mix(const double a, const double b, const double t) {
46  return a * (1. - t) + t * b;
47 }
48 
49 cairo_status_t CairoWriteCallback(void* const closure,
50  const unsigned char* data,
51  const unsigned int length) {
52  if (static_cast<FileWriter*>(closure)->Write(
53  reinterpret_cast<const char*>(data), length)) {
54  return CAIRO_STATUS_SUCCESS;
55  }
56  return CAIRO_STATUS_WRITE_ERROR;
57 }
58 
59 // Write 'mat' as a pleasing-to-look-at PNG into 'filename'
60 void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) {
61  const int stride =
62  cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols());
63  CHECK_EQ(stride % 4, 0);
64  std::vector<uint32_t> pixels(stride / 4 * mat.rows(), 0.);
65 
66  float max = std::numeric_limits<float>::min();
67  for (int y = 0; y < mat.rows(); ++y) {
68  for (int x = 0; x < mat.cols(); ++x) {
69  const PixelData& cell = mat(y, x);
70  if (cell.num_occupied_cells_in_column == 0.) {
71  continue;
72  }
73  max = std::max<float>(max, std::log(cell.num_occupied_cells_in_column));
74  }
75  }
76 
77  for (int y = 0; y < mat.rows(); ++y) {
78  for (int x = 0; x < mat.cols(); ++x) {
79  const PixelData& cell = mat(y, x);
80  if (cell.num_occupied_cells_in_column == 0.) {
81  pixels[y * stride / 4 + x] =
82  (255 << 24) | (255 << 16) | (255 << 8) | 255;
83  continue;
84  }
85 
86  // We use a logarithmic weighting for how saturated a pixel will be. The
87  // basic idea here was that walls (full height) are fully saturated, but
88  // details like chairs and tables are still well visible.
89  const float saturation =
90  std::log(cell.num_occupied_cells_in_column) / max;
91  double mean_r_in_column = (cell.mean_r / 255.);
92  double mean_g_in_column = (cell.mean_g / 255.);
93  double mean_b_in_column = (cell.mean_b / 255.);
94 
95  double mix_r = Mix(1., mean_r_in_column, saturation);
96  double mix_g = Mix(1., mean_g_in_column, saturation);
97  double mix_b = Mix(1., mean_b_in_column, saturation);
98 
99  const int r = common::RoundToInt(mix_r * 255.);
100  const int g = common::RoundToInt(mix_g * 255.);
101  const int b = common::RoundToInt(mix_b * 255.);
102  pixels[y * stride / 4 + x] = (255 << 24) | (r << 16) | (g << 8) | b;
103  }
104  }
105 
106  // TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of
107  // the data until the surface is finalized. Once it is finalized though,
108  // cairo_surface_write_to_png fails, complaining that the surface is already
109  // finalized. This makes it pretty hard to pass back ownership of the image to
110  // the caller.
111  cairo::UniqueSurfacePtr surface(
112  cairo_image_surface_create_for_data(
113  reinterpret_cast<unsigned char*>(pixels.data()), CAIRO_FORMAT_ARGB32,
114  mat.cols(), mat.rows(), stride),
115  cairo_surface_destroy);
116  CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS);
117  CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback,
118  file_writer),
119  CAIRO_STATUS_SUCCESS);
120  CHECK(file_writer->Close());
121 }
122 
123 bool ContainedIn(const common::Time& time,
124  const std::vector<mapping::Timespan>& timespans) {
125  for (const mapping::Timespan& timespan : timespans) {
126  if (timespan.start <= time && time <= timespan.end) {
127  return true;
128  }
129  }
130  return false;
131 }
132 
133 } // namespace
134 
136  const double voxel_size, const transform::Rigid3f& transform,
137  const std::vector<mapping::Floor>& floors, const string& output_filename,
138  FileWriterFactory file_writer_factory, PointsProcessor* const next)
139  : next_(next),
140  file_writer_factory_(file_writer_factory),
141  floors_(floors),
142  output_filename_(output_filename),
143  transform_(transform) {
144  for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
145  aggregations_.emplace_back(
147  }
148 }
149 
150 std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
151  const mapping::proto::Trajectory& trajectory,
152  FileWriterFactory file_writer_factory,
153  common::LuaParameterDictionary* const dictionary,
154  PointsProcessor* const next) {
155  std::vector<mapping::Floor> floors;
156  if (dictionary->HasKey("separate_floors") &&
157  dictionary->GetBool("separate_floors")) {
158  floors = mapping::DetectFloors(trajectory);
159  }
160 
161  return common::make_unique<XRayPointsProcessor>(
162  dictionary->GetDouble("voxel_size"),
163  transform::FromDictionary(dictionary->GetDictionary("transform").get())
164  .cast<float>(),
165  floors, dictionary->GetString("filename"), file_writer_factory, next);
166 }
167 
169  FileWriter* const file_writer) {
170  if (bounding_box_.isEmpty()) {
171  LOG(WARNING) << "Not writing output: bounding box is empty.";
172  return;
173  }
174 
175  // Returns the (x, y) pixel of the given 'index'.
176  const auto voxel_index_to_pixel = [this](const Eigen::Array3i& index) {
177  // We flip the y axis, since matrices rows are counted from the top.
178  return Eigen::Array2i(bounding_box_.max()[1] - index[1],
179  bounding_box_.max()[2] - index[2]);
180  };
181 
182  // Hybrid grid uses X: forward, Y: left, Z: up.
183  // For the screen we are using. X: right, Y: up
184  const int xsize = bounding_box_.sizes()[1] + 1;
185  const int ysize = bounding_box_.sizes()[2] + 1;
186  PixelDataMatrix image = PixelDataMatrix(ysize, xsize);
188  !it.Done(); it.Next()) {
189  const Eigen::Array3i cell_index = it.GetCellIndex();
190  const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index);
191  PixelData& pixel_data = image(pixel.y(), pixel.x());
192  const auto& column_data = aggregation.column_data.at(
193  std::make_pair(cell_index[1], cell_index[2]));
194  pixel_data.mean_r = column_data.sum_r / column_data.count;
195  pixel_data.mean_g = column_data.sum_g / column_data.count;
196  pixel_data.mean_b = column_data.sum_b / column_data.count;
197  ++pixel_data.num_occupied_cells_in_column;
198  }
199  WritePng(image, file_writer);
200 }
201 
203  const transform::Rigid3f& transform,
204  Aggregation* const aggregation) {
205  constexpr Color kDefaultColor = {{0, 0, 0}};
206  for (size_t i = 0; i < batch.points.size(); ++i) {
207  const Eigen::Vector3f camera_point = transform * batch.points[i];
208  const Eigen::Array3i cell_index =
209  aggregation->voxels.GetCellIndex(camera_point);
210  *aggregation->voxels.mutable_value(cell_index) = true;
211  bounding_box_.extend(cell_index.matrix());
212  ColumnData& column_data =
213  aggregation->column_data[std::make_pair(cell_index[1], cell_index[2])];
214  const auto& color =
215  batch.colors.empty() ? kDefaultColor : batch.colors.at(i);
216  column_data.sum_r += color[0];
217  column_data.sum_g += color[1];
218  column_data.sum_b += color[2];
219  ++column_data.count;
220  }
221 }
222 
223 void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
224  if (floors_.empty()) {
225  CHECK_EQ(aggregations_.size(), 1);
226  Insert(*batch, transform_, &aggregations_[0]);
227  } else {
228  for (size_t i = 0; i < floors_.size(); ++i) {
229  if (!ContainedIn(batch->time, floors_[i].timespans)) {
230  continue;
231  }
232  Insert(*batch, transform_, &aggregations_[i]);
233  }
234  }
235  next_->Process(std::move(batch));
236 }
237 
239  if (floors_.empty()) {
240  CHECK_EQ(aggregations_.size(), 1);
242  file_writer_factory_(output_filename_ + ".png").get());
243  } else {
244  for (size_t i = 0; i < floors_.size(); ++i) {
245  WriteVoxels(
246  aggregations_[i],
247  file_writer_factory_(output_filename_ + std::to_string(i) + ".png")
248  .get());
249  }
250  }
251 
252  switch (next_->Flush()) {
254  LOG(FATAL) << "X-Ray generation must be configured to occur after any "
255  "stages that require multiple passes.";
256 
258  return FlushResult::kFinished;
259  }
260  LOG(FATAL);
261 }
262 
263 } // namespace io
264 } // namespace cartographer
virtual void Process(std::unique_ptr< PointsBatch > points_batch)=0
double mean_g
Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
Definition: hybrid_grid.h:428
std::function< std::unique_ptr< FileWriter >(const string &filename)> FileWriterFactory
Definition: file_writer.h:63
int RoundToInt(const float x)
Definition: port.h:42
double mean_r
std::map< std::pair< int, int >, ColumnData > column_data
UniversalTimeScaleClock::time_point Time
Definition: time.h:44
double mean_b
ValueType * mutable_value(const Eigen::Array3i &index)
Definition: hybrid_grid.h:283
std::unique_ptr< cairo_surface_t, void(*)(cairo_surface_t *)> UniqueSurfacePtr
Definition: cairo_types.h:15
void Insert(const PointsBatch &batch, const transform::Rigid3f &transform, Aggregation *aggregation)
static std::unique_ptr< XRayPointsProcessor > FromDictionary(const mapping::proto::Trajectory &trajectory, FileWriterFactory file_writer_factory, common::LuaParameterDictionary *dictionary, PointsProcessor *next)
std::array< uint8_t, 3 > Color
Definition: points_batch.h:31
std::vector< mapping::Floor > floors_
XRayPointsProcessor(double voxel_size, const transform::Rigid3f &transform, const std::vector< mapping::Floor > &floors, const string &output_filename, FileWriterFactory file_writer_factory, PointsProcessor *next)
std::vector< Color > colors
Definition: points_batch.h:66
std::vector< Aggregation > aggregations_
typename Grid< ValueType >::Iterator Iterator
Definition: hybrid_grid.h:416
virtual FlushResult Flush()=0
transform::Rigid3d FromDictionary(common::LuaParameterDictionary *dictionary)
size_t num_occupied_cells_in_column
void WriteVoxels(const Aggregation &aggregation, FileWriter *const file_writer)
std::vector< Floor > DetectFloors(const proto::Trajectory &trajectory)
std::unique_ptr< LuaParameterDictionary > GetDictionary(const string &key)
std::vector< Eigen::Vector3f > points
Definition: points_batch.h:56
void Process(std::unique_ptr< PointsBatch > batch) override


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