From 49ead6055c496f2b1eb67aaeea6ca3506f677e6b Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Thu, 19 Oct 2017 12:30:19 +0200 Subject: [PATCH] Towards a ROS map writing PointsProcessor. (#598) - Refactor ProbabilityGridPointsProcessor for code reuse. - Adds Image::width and height. - Adds GetFilename for FileWriter. Related to googlecartographer/cartographer_ros#475. --- cartographer/io/file_writer.cc | 4 +- cartographer/io/file_writer.h | 3 + cartographer/io/image.h | 19 +++ .../io/probability_grid_points_processor.cc | 135 +++++++++++------- .../io/probability_grid_points_processor.h | 28 +++- 5 files changed, 133 insertions(+), 56 deletions(-) diff --git a/cartographer/io/file_writer.cc b/cartographer/io/file_writer.cc index 769763d..4b68c40 100644 --- a/cartographer/io/file_writer.cc +++ b/cartographer/io/file_writer.cc @@ -20,7 +20,7 @@ namespace cartographer { namespace io { StreamFileWriter::StreamFileWriter(const string& filename) - : out_(filename, std::ios::out | std::ios::binary) {} + : filename_(filename), out_(filename, std::ios::out | std::ios::binary) {} StreamFileWriter::~StreamFileWriter() {} @@ -49,5 +49,7 @@ bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) { return Write(data, len); } +string StreamFileWriter::GetFilename() { return filename_; } + } // namespace io } // namespace cartographer diff --git a/cartographer/io/file_writer.h b/cartographer/io/file_writer.h index 1effdf1..96b7dc2 100644 --- a/cartographer/io/file_writer.h +++ b/cartographer/io/file_writer.h @@ -42,6 +42,7 @@ class FileWriter { virtual bool Write(const char* data, size_t len) = 0; virtual bool Close() = 0; + virtual string GetFilename() = 0; }; // An Implementation of file using std::ofstream. @@ -54,8 +55,10 @@ class StreamFileWriter : public FileWriter { bool Write(const char* data, size_t len) override; bool WriteHeader(const char* data, size_t len) override; bool Close() override; + string GetFilename() override; private: + const string filename_; std::ofstream out_; }; diff --git a/cartographer/io/image.h b/cartographer/io/image.h index 81d702a..db27189 100644 --- a/cartographer/io/image.h +++ b/cartographer/io/image.h @@ -1,3 +1,19 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #ifndef CARTOGRAPHER_IO_IMAGE_H_ #define CARTOGRAPHER_IO_IMAGE_H_ @@ -41,6 +57,9 @@ class Image { // to this surface is alive. UniqueCairoSurfacePtr GetCairoSurface(); + int width() const { return width_; } + int height() const { return height_; } + private: int width_; int height_; diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index 230f153..20f3bfb 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -1,3 +1,19 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #include "cartographer/io/probability_grid_points_processor.h" #include "Eigen/Core" @@ -12,63 +28,28 @@ namespace cartographer { namespace io { namespace { -void WriteGrid( +void DrawTrajectoriesIntoImage( const mapping_2d::ProbabilityGrid& probability_grid, - const ProbabilityGridPointsProcessor::DrawTrajectories& draw_trajectories, + const Eigen::Array2i& offset, const std::vector& trajectories, - FileWriter* const file_writer) { - Eigen::Array2i offset; - mapping_2d::CellLimits cell_limits; - probability_grid.ComputeCroppedLimits(&offset, &cell_limits); - if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) { - LOG(WARNING) << "Not writing output: empty probability grid"; - return; + cairo_surface_t* cairo_surface) { + for (size_t i = 0; i < trajectories.size(); ++i) { + DrawTrajectory(trajectories[i], GetColor(i), + [&probability_grid, + &offset](const transform::Rigid3d& pose) -> Eigen::Array2i { + return probability_grid.limits().GetCellIndex( + pose.cast().translation().head<2>()) - + offset; + }, + cairo_surface); } - const auto compute_color_value = [&probability_grid]( - const Eigen::Array2i& index) { - if (probability_grid.IsKnown(index)) { - const float probability = 1.f - probability_grid.GetProbability(index); - return static_cast( - 255 * ((probability - mapping::kMinProbability) / - (mapping::kMaxProbability - mapping::kMinProbability))); - } else { - constexpr uint8 kUnknownValue = 128; - return kUnknownValue; - } - }; - Image image(cell_limits.num_x_cells, cell_limits.num_y_cells); - for (const auto& xy_index : - cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) { - const auto index = xy_index + offset; - uint8 value = compute_color_value(index); - image.SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}}); - } - - if (draw_trajectories == - ProbabilityGridPointsProcessor::DrawTrajectories::kYes) { - for (size_t i = 0; i < trajectories.size(); ++i) { - DrawTrajectory(trajectories[i], GetColor(i), - [&probability_grid, &offset]( - const transform::Rigid3d& pose) -> Eigen::Array2i { - return probability_grid.limits().GetCellIndex( - pose.cast().translation().head<2>()) - - offset; - }, - image.GetCairoSurface().get()); - } - } - image.WritePng(file_writer); - CHECK(file_writer->Close()); } -mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution) { - constexpr int kInitialProbabilityGridSize = 100; - Eigen::Vector2d max = - 0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones(); - return mapping_2d::ProbabilityGrid(cartographer::mapping_2d::MapLimits( - resolution, max, - mapping_2d::CellLimits(kInitialProbabilityGridSize, - kInitialProbabilityGridSize))); +uint8 ProbabilityToColor(float probability_from_grid) { + const float probability = 1.f - probability_from_grid; + return ::cartographer::common::RoundToInt( + 255 * ((probability - mapping::kMinProbability) / + (mapping::kMaxProbability - mapping::kMinProbability))); } } // namespace @@ -115,8 +96,19 @@ void ProbabilityGridPointsProcessor::Process( } PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() { - WriteGrid(probability_grid_, draw_trajectories_, trajectories_, - file_writer_.get()); + Eigen::Array2i offset; + std::unique_ptr image = + DrawProbabilityGrid(probability_grid_, &offset); + if (image != nullptr) { + if (draw_trajectories_ == + ProbabilityGridPointsProcessor::DrawTrajectories::kYes) { + DrawTrajectoriesIntoImage(probability_grid_, offset, trajectories_, + image->GetCairoSurface().get()); + } + image->WritePng(file_writer_.get()); + CHECK(file_writer_->Close()); + } + switch (next_->Flush()) { case FlushResult::kRestartStream: LOG(FATAL) << "ProbabilityGrid generation must be configured to occur " @@ -128,5 +120,40 @@ PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() { LOG(FATAL); } +std::unique_ptr DrawProbabilityGrid( + const mapping_2d::ProbabilityGrid& probability_grid, + Eigen::Array2i* offset) { + mapping_2d::CellLimits cell_limits; + probability_grid.ComputeCroppedLimits(offset, &cell_limits); + if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) { + LOG(WARNING) << "Not writing output: empty probability grid"; + return nullptr; + } + auto image = common::make_unique(cell_limits.num_x_cells, + cell_limits.num_y_cells); + for (const auto& xy_index : + cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) { + const auto index = xy_index + *offset; + constexpr uint8 kUnknownValue = 128; + const uint8 value = + probability_grid.IsKnown(index) + ? ProbabilityToColor(probability_grid.GetProbability(index)) + : kUnknownValue; + ; + image->SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}}); + } + return image; +} + +mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution) { + constexpr int kInitialProbabilityGridSize = 100; + Eigen::Vector2d max = + 0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones(); + return mapping_2d::ProbabilityGrid(cartographer::mapping_2d::MapLimits( + resolution, max, + mapping_2d::CellLimits(kInitialProbabilityGridSize, + kInitialProbabilityGridSize))); +} + } // namespace io } // namespace cartographer diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h index 5e5dec8..091e4ef 100644 --- a/cartographer/io/probability_grid_points_processor.h +++ b/cartographer/io/probability_grid_points_processor.h @@ -1,10 +1,26 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + #ifndef CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_ #include -#include #include "cartographer/io/file_writer.h" +#include "cartographer/io/image.h" #include "cartographer/io/points_batch.h" #include "cartographer/io/points_processor.h" #include "cartographer/mapping/proto/trajectory.pb.h" @@ -56,6 +72,16 @@ class ProbabilityGridPointsProcessor : public PointsProcessor { mapping_2d::ProbabilityGrid probability_grid_; }; +// Draws 'probability_grid' into an image and fills in 'offset' with the cropped +// map limits. Returns 'nullptr' if probability_grid was empty. +std::unique_ptr DrawProbabilityGrid( + const mapping_2d::ProbabilityGrid& probability_grid, + Eigen::Array2i* offset); + +// Create an initially empty probability grid with 'resolution' and a small +// size, suitable for a PointsBatchProcessor. +mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution); + } // namespace io } // namespace cartographer