probability_grid_points_processor.cc
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer/io/probability_grid_points_processor.h"
00018 
00019 #include "Eigen/Core"
00020 #include "absl/memory/memory.h"
00021 #include "cartographer/common/lua_parameter_dictionary.h"
00022 #include "cartographer/common/math.h"
00023 #include "cartographer/io/draw_trajectories.h"
00024 #include "cartographer/io/image.h"
00025 #include "cartographer/io/points_batch.h"
00026 #include "cartographer/mapping/probability_values.h"
00027 #include "glog/logging.h"
00028 
00029 namespace cartographer {
00030 namespace io {
00031 namespace {
00032 
00033 void DrawTrajectoriesIntoImage(
00034     const mapping::ProbabilityGrid& probability_grid,
00035     const Eigen::Array2i& offset,
00036     const std::vector<mapping::proto::Trajectory>& trajectories,
00037     cairo_surface_t* cairo_surface) {
00038   for (size_t i = 0; i < trajectories.size(); ++i) {
00039     DrawTrajectory(
00040         trajectories[i], GetColor(i),
00041         [&probability_grid,
00042          &offset](const transform::Rigid3d& pose) -> Eigen::Array2i {
00043           return probability_grid.limits().GetCellIndex(
00044                      pose.cast<float>().translation().head<2>()) -
00045                  offset;
00046         },
00047         cairo_surface);
00048   }
00049 }
00050 
00051 uint8 ProbabilityToColor(float probability_from_grid) {
00052   const float probability = 1.f - probability_from_grid;
00053   return ::cartographer::common::RoundToInt(
00054       255 * ((probability - mapping::kMinProbability) /
00055              (mapping::kMaxProbability - mapping::kMinProbability)));
00056 }
00057 
00058 std::string FileExtensionFromOutputType(
00059     const ProbabilityGridPointsProcessor::OutputType& output_type) {
00060   if (output_type == ProbabilityGridPointsProcessor::OutputType::kPng) {
00061     return ".png";
00062   } else if (output_type == ProbabilityGridPointsProcessor::OutputType::kPb) {
00063     return ".pb";
00064   }
00065   LOG(FATAL) << "OutputType does not exist!";
00066 }
00067 
00068 ProbabilityGridPointsProcessor::OutputType OutputTypeFromString(
00069     const std::string& output_type) {
00070   if (output_type == "png") {
00071     return ProbabilityGridPointsProcessor::OutputType::kPng;
00072   } else if (output_type == "pb") {
00073     return ProbabilityGridPointsProcessor::OutputType::kPb;
00074   } else {
00075     LOG(FATAL) << "OutputType " << output_type << " does not exist!";
00076   }
00077 }
00078 
00079 }  // namespace
00080 
00081 ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
00082     const double resolution,
00083     const mapping::proto::ProbabilityGridRangeDataInserterOptions2D&
00084         probability_grid_range_data_inserter_options,
00085     const DrawTrajectories& draw_trajectories, const OutputType& output_type,
00086     std::unique_ptr<FileWriter> file_writer,
00087     const std::vector<mapping::proto::Trajectory>& trajectories,
00088     PointsProcessor* const next)
00089     : draw_trajectories_(draw_trajectories),
00090       output_type_(output_type),
00091       trajectories_(trajectories),
00092       file_writer_(std::move(file_writer)),
00093       next_(next),
00094       range_data_inserter_(probability_grid_range_data_inserter_options),
00095       probability_grid_(
00096           CreateProbabilityGrid(resolution, &conversion_tables_)) {
00097   LOG_IF(WARNING, output_type == OutputType::kPb &&
00098                       draw_trajectories_ == DrawTrajectories::kYes)
00099       << "Drawing the trajectories is not supported when writing the "
00100          "probability grid as protobuf.";
00101 }
00102 
00103 std::unique_ptr<ProbabilityGridPointsProcessor>
00104 ProbabilityGridPointsProcessor::FromDictionary(
00105     const std::vector<mapping::proto::Trajectory>& trajectories,
00106     const FileWriterFactory& file_writer_factory,
00107     common::LuaParameterDictionary* const dictionary,
00108     PointsProcessor* const next) {
00109   const auto draw_trajectories = (!dictionary->HasKey("draw_trajectories") ||
00110                                   dictionary->GetBool("draw_trajectories"))
00111                                      ? DrawTrajectories::kYes
00112                                      : DrawTrajectories::kNo;
00113   const auto output_type =
00114       dictionary->HasKey("output_type")
00115           ? OutputTypeFromString(dictionary->GetString("output_type"))
00116           : OutputType::kPng;
00117   return absl::make_unique<ProbabilityGridPointsProcessor>(
00118       dictionary->GetDouble("resolution"),
00119       mapping::CreateProbabilityGridRangeDataInserterOptions2D(
00120           dictionary->GetDictionary("range_data_inserter").get()),
00121       draw_trajectories, output_type,
00122       file_writer_factory(dictionary->GetString("filename") +
00123                           FileExtensionFromOutputType(output_type)),
00124       trajectories, next);
00125 }
00126 
00127 void ProbabilityGridPointsProcessor::Process(
00128     std::unique_ptr<PointsBatch> batch) {
00129   range_data_inserter_.Insert({batch->origin, batch->points, {}},
00130                               &probability_grid_);
00131   next_->Process(std::move(batch));
00132 }
00133 
00134 PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
00135   if (output_type_ == OutputType::kPng) {
00136     Eigen::Array2i offset;
00137     std::unique_ptr<Image> image =
00138         DrawProbabilityGrid(probability_grid_, &offset);
00139     if (image != nullptr) {
00140       if (draw_trajectories_ ==
00141           ProbabilityGridPointsProcessor::DrawTrajectories::kYes) {
00142         DrawTrajectoriesIntoImage(probability_grid_, offset, trajectories_,
00143                                   image->GetCairoSurface().get());
00144       }
00145       image->WritePng(file_writer_.get());
00146       CHECK(file_writer_->Close());
00147     }
00148   } else if (output_type_ == OutputType::kPb) {
00149     const auto probability_grid_proto = probability_grid_.ToProto();
00150     std::string probability_grid_serialized;
00151     probability_grid_proto.SerializeToString(&probability_grid_serialized);
00152     file_writer_->Write(probability_grid_serialized.data(),
00153                         probability_grid_serialized.size());
00154     CHECK(file_writer_->Close());
00155   } else {
00156     LOG(FATAL) << "Output Type " << FileExtensionFromOutputType(output_type_)
00157                << " is not supported.";
00158   }
00159 
00160   switch (next_->Flush()) {
00161     case FlushResult::kRestartStream:
00162       LOG(FATAL) << "ProbabilityGrid generation must be configured to occur "
00163                     "after any stages that require multiple passes.";
00164 
00165     case FlushResult::kFinished:
00166       return FlushResult::kFinished;
00167   }
00168   LOG(FATAL);
00169   // The following unreachable return statement is needed to avoid a GCC bug
00170   // described at https://gcc.gnu.org/bugzilla/show_bug.cgi?id=81508
00171   return FlushResult::kFinished;
00172 }
00173 
00174 std::unique_ptr<Image> DrawProbabilityGrid(
00175     const mapping::ProbabilityGrid& probability_grid, Eigen::Array2i* offset) {
00176   mapping::CellLimits cell_limits;
00177   probability_grid.ComputeCroppedLimits(offset, &cell_limits);
00178   if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
00179     LOG(WARNING) << "Not writing output: empty probability grid";
00180     return nullptr;
00181   }
00182   auto image = absl::make_unique<Image>(cell_limits.num_x_cells,
00183                                         cell_limits.num_y_cells);
00184   for (const Eigen::Array2i& xy_index :
00185        mapping::XYIndexRangeIterator(cell_limits)) {
00186     const Eigen::Array2i index = xy_index + *offset;
00187     constexpr uint8 kUnknownValue = 128;
00188     const uint8 value =
00189         probability_grid.IsKnown(index)
00190             ? ProbabilityToColor(probability_grid.GetProbability(index))
00191             : kUnknownValue;
00192     image->SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}});
00193   }
00194   return image;
00195 }
00196 
00197 mapping::ProbabilityGrid CreateProbabilityGrid(
00198     const double resolution,
00199     mapping::ValueConversionTables* conversion_tables) {
00200   constexpr int kInitialProbabilityGridSize = 100;
00201   Eigen::Vector2d max =
00202       0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones();
00203   return mapping::ProbabilityGrid(
00204       mapping::MapLimits(resolution, max,
00205                          mapping::CellLimits(kInitialProbabilityGridSize,
00206                                              kInitialProbabilityGridSize)),
00207       conversion_tables);
00208 }
00209 
00210 }  // namespace io
00211 }  // namespace cartographer


cartographer
Author(s): The Cartographer Authors
autogenerated on Thu May 9 2019 02:27:35