diff --git a/cartographer/io/cairo_types.h b/cartographer/io/cairo_types.h deleted file mode 100644 index 568df4b..0000000 --- a/cartographer/io/cairo_types.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef CARTOGRAPHER_IO_CAIRO_TYPES_H_ -#define CARTOGRAPHER_IO_CAIRO_TYPES_H_ - -#include - -#include "cairo/cairo.h" - -namespace cartographer { -namespace io { -namespace cairo { - -// std::unique_ptr for Cairo surfaces. The surface is destroyed when the -// std::unique_ptr is reset or destroyed. -using UniqueSurfacePtr = - std::unique_ptr; - -// std::unique_ptr for Cairo contexts. The context is destroyed when the -// std::unique_ptr is reset or destroyed. -using UniqueContextPtr = std::unique_ptr; - -// std::unique_ptr for Cairo paths. The path is destroyed when the -// std::unique_ptr is reset or destroyed. -using UniquePathPtr = std::unique_ptr; - -} // namespace cairo -} // namespace io -} // namespace cartographer - -#endif // CARTOGRAPHER_IO_CAIRO_TYPES_H_ diff --git a/cartographer/io/image.cc b/cartographer/io/image.cc new file mode 100644 index 0000000..24b9aa7 --- /dev/null +++ b/cartographer/io/image.cc @@ -0,0 +1,72 @@ +#include "cartographer/io/image.h" + +#include + +#include "cairo/cairo.h" +#include "cartographer/io/file_writer.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { +namespace { + +// std::unique_ptr for Cairo surfaces. The surface is destroyed when the +// std::unique_ptr is reset or destroyed. +using UniqueSurfacePtr = + std::unique_ptr; + +cairo_status_t CairoWriteCallback(void* const closure, + const unsigned char* data, + const unsigned int length) { + if (static_cast(closure)->Write( + reinterpret_cast(data), length)) { + return CAIRO_STATUS_SUCCESS; + } + return CAIRO_STATUS_WRITE_ERROR; +} + +constexpr cairo_format_t kCairoFormat = CAIRO_FORMAT_ARGB32; + +int StrideForWidth(int width) { + const int stride = cairo_format_stride_for_width(kCairoFormat, width); + CHECK_EQ(stride % 4, 0); + return stride; +} + +} // namespace + +Image::Image(int width, int height) + : width_(width), + height_(height), + stride_(StrideForWidth(width)), + pixels_(stride_ / 4 * height, 0) {} + +void Image::WritePng(FileWriter* const file_writer) { + // TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of + // the data until the surface is finalized. Once it is finalized though, + // cairo_surface_write_to_png fails, complaining that the surface is already + // finalized. This makes it pretty hard to pass back ownership of the image to + // the caller. + UniqueSurfacePtr surface(cairo_image_surface_create_for_data( + reinterpret_cast(pixels_.data()), + kCairoFormat, width_, height_, stride_), + cairo_surface_destroy); + CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS); + CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback, + file_writer), + CAIRO_STATUS_SUCCESS); +} + +const Color Image::GetPixel(int x, int y) const { + const uint32_t value = pixels_[y * stride_ / 4 + x]; + return {{static_cast(value >> 16), static_cast(value >> 8), + static_cast(value)}}; +} + +void Image::SetPixel(int x, int y, const Color& color) { + pixels_[y * stride_ / 4 + x] = + (255 << 24) | (color[0] << 16) | (color[1] << 8) | color[2]; +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/image.h b/cartographer/io/image.h new file mode 100644 index 0000000..b000c9c --- /dev/null +++ b/cartographer/io/image.h @@ -0,0 +1,31 @@ +#ifndef CARTOGRAPHER_IO_IMAGE_H_ +#define CARTOGRAPHER_IO_IMAGE_H_ + +#include +#include + +#include "cartographer/io/file_writer.h" +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +class Image { + public: + Image(int width, int height); + + const Color GetPixel(int x, int y) const; + void SetPixel(int x, int y, const Color& color); + void WritePng(FileWriter* const file_writer); + + private: + int width_; + int height_; + int stride_; + std::vector pixels_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_IMAGE_H_ diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index 53204ec..db3e6b2 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -27,6 +27,7 @@ #include "cartographer/io/outlier_removing_points_processor.h" #include "cartographer/io/pcd_writing_points_processor.h" #include "cartographer/io/ply_writing_points_processor.h" +#include "cartographer/io/probability_grid_points_processor.h" #include "cartographer/io/xray_points_processor.h" #include "cartographer/io/xyz_writing_points_processor.h" #include "cartographer/mapping/proto/trajectory.pb.h" @@ -77,6 +78,8 @@ void RegisterBuiltInPointsProcessors( file_writer_factory, builder); RegisterFileWritingPointsProcessor( file_writer_factory, builder); + RegisterFileWritingPointsProcessor( + file_writer_factory, builder); // X-Ray is an odd ball since it requires the trajectory to figure out the // different building levels we walked on to separate the images. diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc new file mode 100644 index 0000000..270269a --- /dev/null +++ b/cartographer/io/probability_grid_points_processor.cc @@ -0,0 +1,108 @@ +#include "cartographer/io/probability_grid_points_processor.h" + +#include "Eigen/Core" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "cartographer/io/image.h" +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { +namespace { + +void WriteGrid(const mapping_2d::ProbabilityGrid& probability_grid, + 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; + } + const auto grid_index_to_pixel = [cell_limits](const Eigen::Array2i& index) { + return Eigen::Array2i(cell_limits.num_y_cells - index(1) - 1, + cell_limits.num_x_cells - index(0) - 1); + }; + 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_t kUnknownValue = 128; + return kUnknownValue; + } + }; + int width = cell_limits.num_y_cells; + int height = cell_limits.num_x_cells; + Image image(width, height); + for (auto xy_index : + cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) { + auto index = xy_index + offset; + uint8 value = compute_color_value(index); + const Eigen::Array2i pixel = grid_index_to_pixel(xy_index); + image.SetPixel(pixel.x(), pixel.y(), {{value, value, value}}); + } + 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))); +} + +} // namespace + +ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor( + const double resolution, + const mapping_2d::proto::RangeDataInserterOptions& + range_data_inserter_options, + std::unique_ptr file_writer, PointsProcessor* const next) + : next_(next), + file_writer_(std::move(file_writer)), + range_data_inserter_(range_data_inserter_options), + probability_grid_(CreateProbabilityGrid(resolution)) {} + +std::unique_ptr +ProbabilityGridPointsProcessor::FromDictionary( + FileWriterFactory file_writer_factory, + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return common::make_unique( + dictionary->GetDouble("resolution"), + mapping_2d::CreateRangeDataInserterOptions( + dictionary->GetDictionary("range_data_inserter").get()), + file_writer_factory(dictionary->GetString("filename") + ".png"), next); +} + +void ProbabilityGridPointsProcessor::Process( + std::unique_ptr batch) { + range_data_inserter_.Insert({batch->origin, batch->points, {}}, + &probability_grid_); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() { + WriteGrid(probability_grid_, file_writer_.get()); + switch (next_->Flush()) { + case FlushResult::kRestartStream: + LOG(FATAL) << "ProbabilityGrid generation must be configured to occur " + "after any stages that require multiple passes."; + + case FlushResult::kFinished: + return FlushResult::kFinished; + } + LOG(FATAL); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h new file mode 100644 index 0000000..4d3e12b --- /dev/null +++ b/cartographer/io/probability_grid_points_processor.h @@ -0,0 +1,54 @@ +#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/points_batch.h" +#include "cartographer/io/points_processor.h" +#include "cartographer/mapping_2d/probability_grid.h" +#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h" +#include "cartographer/mapping_2d/range_data_inserter.h" + +namespace cartographer { +namespace io { + +// Creates a probability grid with the specified 'resolution'. As all points are +// projected into the x-y plane the z component of the data is ignored. +// 'range_data_inserter' options are used to configure the range data ray +// tracing through the probability grid. +class ProbabilityGridPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "write_probability_grid"; + ProbabilityGridPointsProcessor( + double resolution, + const mapping_2d::proto::RangeDataInserterOptions& + range_data_inserter_options, + std::unique_ptr file_writer, PointsProcessor* next); + ProbabilityGridPointsProcessor(const ProbabilityGridPointsProcessor&) = + delete; + ProbabilityGridPointsProcessor& operator=( + const ProbabilityGridPointsProcessor&) = delete; + + static std::unique_ptr FromDictionary( + FileWriterFactory file_writer_factory, + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~ProbabilityGridPointsProcessor() override {} + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + PointsProcessor* const next_; + std::unique_ptr file_writer_; + mapping_2d::RangeDataInserter range_data_inserter_; + mapping_2d::ProbabilityGrid probability_grid_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_ diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index eeebda8..2bd089c 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -20,11 +20,10 @@ #include #include "Eigen/Core" -#include "cairo/cairo.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" -#include "cartographer/io/cairo_types.h" +#include "cartographer/io/image.h" #include "cartographer/mapping/detect_floors.h" #include "cartographer/mapping_3d/hybrid_grid.h" @@ -46,22 +45,9 @@ double Mix(const double a, const double b, const double t) { return a * (1. - t) + t * b; } -cairo_status_t CairoWriteCallback(void* const closure, - const unsigned char* data, - const unsigned int length) { - if (static_cast(closure)->Write( - reinterpret_cast(data), length)) { - return CAIRO_STATUS_SUCCESS; - } - return CAIRO_STATUS_WRITE_ERROR; -} - // Write 'mat' as a pleasing-to-look-at PNG into 'filename' -void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) { - const int stride = - cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols()); - CHECK_EQ(stride % 4, 0); - std::vector pixels(stride / 4 * mat.rows(), 0.); +void WriteImage(const PixelDataMatrix& mat, FileWriter* const file_writer) { + Image image(mat.cols(), mat.rows()); float max = std::numeric_limits::min(); for (int y = 0; y < mat.rows(); ++y) { @@ -78,8 +64,7 @@ void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) { for (int x = 0; x < mat.cols(); ++x) { const PixelData& cell = mat(y, x); if (cell.num_occupied_cells_in_column == 0.) { - pixels[y * stride / 4 + x] = - (255 << 24) | (255 << 16) | (255 << 8) | 255; + image.SetPixel(x, y, {{255, 255, 255}}); continue; } @@ -96,27 +81,14 @@ void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) { double mix_g = Mix(1., mean_g_in_column, saturation); double mix_b = Mix(1., mean_b_in_column, saturation); - const int r = common::RoundToInt(mix_r * 255.); - const int g = common::RoundToInt(mix_g * 255.); - const int b = common::RoundToInt(mix_b * 255.); - pixels[y * stride / 4 + x] = (255 << 24) | (r << 16) | (g << 8) | b; + const uint8_t r = common::RoundToInt(mix_r * 255.); + const uint8_t g = common::RoundToInt(mix_g * 255.); + const uint8_t b = common::RoundToInt(mix_b * 255.); + image.SetPixel(x, y, {{r, g, b}}); } } - // TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of - // the data until the surface is finalized. Once it is finalized though, - // cairo_surface_write_to_png fails, complaining that the surface is already - // finalized. This makes it pretty hard to pass back ownership of the image to - // the caller. - cairo::UniqueSurfacePtr surface( - cairo_image_surface_create_for_data( - reinterpret_cast(pixels.data()), CAIRO_FORMAT_ARGB32, - mat.cols(), mat.rows(), stride), - cairo_surface_destroy); - CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS); - CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback, - file_writer), - CAIRO_STATUS_SUCCESS); + image.WritePng(file_writer); CHECK(file_writer->Close()); } @@ -196,7 +168,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, pixel_data.mean_b = column_data.sum_b / column_data.count; ++pixel_data.num_occupied_cells_in_column; } - WritePng(image, file_writer); + WriteImage(image, file_writer); } void XRayPointsProcessor::Insert(const PointsBatch& batch,