From ea7c39b6f078c693b92fed06d86ca501021147d9 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Tue, 25 Jul 2017 13:36:50 +0200 Subject: [PATCH] Draw Trajectories onto X-Rays and ProbabilityGrids. (#421) This behavior can be turned off with the 'draw_trajectories' setting in Lua. Fixes #174. --- cartographer/io/draw_trajectories.cc | 69 +++++++++++++++++++ cartographer/io/draw_trajectories.h | 41 +++++++++++ cartographer/io/image.h | 2 +- .../io/points_processor_pipeline_builder.cc | 35 ++++++---- .../io/points_processor_pipeline_builder.h | 2 +- .../io/probability_grid_points_processor.cc | 53 ++++++++++---- .../io/probability_grid_points_processor.h | 12 +++- cartographer/io/xray_points_processor.cc | 44 +++++++++--- cartographer/io/xray_points_processor.h | 18 +++-- 9 files changed, 227 insertions(+), 49 deletions(-) create mode 100644 cartographer/io/draw_trajectories.cc create mode 100644 cartographer/io/draw_trajectories.h diff --git a/cartographer/io/draw_trajectories.cc b/cartographer/io/draw_trajectories.cc new file mode 100644 index 0000000..65090da --- /dev/null +++ b/cartographer/io/draw_trajectories.cc @@ -0,0 +1,69 @@ +/* + * 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. + */ + +#include "cartographer/io/draw_trajectories.h" + +#include "cartographer/io/image.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace io { + +void DrawTrajectory(const mapping::proto::Trajectory& trajectory, + const Color& color, PoseToPixelFunction pose_to_pixel, + cairo_surface_t* surface) { + if (trajectory.node_size() == 0) { + return; + } + constexpr double kTrajectoryWidth = 4.; + constexpr double kTrajectoryEndMarkers = 6.; + constexpr double kAlpha = 0.7; + + auto cr = ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface)); + + cairo_set_source_rgba(cr.get(), color[0] / 255., color[1] / 255., + color[2] / 255., kAlpha); + cairo_set_line_width(cr.get(), kTrajectoryWidth); + + for (const auto& node : trajectory.node()) { + const Eigen::Array2i pixel = + pose_to_pixel(transform::ToRigid3(node.pose())); + cairo_line_to(cr.get(), pixel.x(), pixel.y()); + } + cairo_stroke(cr.get()); + + // Draw beginning and end markers. + { + const Eigen::Array2i pixel = + pose_to_pixel(transform::ToRigid3(trajectory.node(0).pose())); + cairo_set_source_rgba(cr.get(), 0., 1., 0., kAlpha); + cairo_arc(cr.get(), pixel.x(), pixel.y(), kTrajectoryEndMarkers, 0, + 2 * M_PI); + cairo_fill(cr.get()); + } + { + const Eigen::Array2i pixel = pose_to_pixel(transform::ToRigid3( + trajectory.node(trajectory.node_size() - 1).pose())); + cairo_set_source_rgba(cr.get(), 1., 0., 0., kAlpha); + cairo_arc(cr.get(), pixel.x(), pixel.y(), kTrajectoryEndMarkers, 0, + 2 * M_PI); + cairo_fill(cr.get()); + } + cairo_surface_flush(surface); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/draw_trajectories.h b/cartographer/io/draw_trajectories.h new file mode 100644 index 0000000..ffed28c --- /dev/null +++ b/cartographer/io/draw_trajectories.h @@ -0,0 +1,41 @@ +/* + * 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_DRAW_TRAJECTORIES_H_ +#define CARTOGRAPHER_IO_DRAW_TRAJECTORIES_H_ + +#include "cairo/cairo.h" +#include "cartographer/io/color.h" +#include "cartographer/mapping/proto/trajectory.pb.h" +#include "cartographer/transform/rigid_transform.h" + +namespace cartographer { +namespace io { + +using PoseToPixelFunction = + std::function; + +// Draws the 'trajectory' with the given 'color' onto 'surface'. The +// 'pose_to_pixel' function must translate a trajectory node's position into the +// pixel on 'surface'. +void DrawTrajectory(const mapping::proto::Trajectory& trajectory, + const Color& color, PoseToPixelFunction pose_to_pixel, + cairo_surface_t* surface); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_DRAW_TRAJECTORIES_H_ diff --git a/cartographer/io/image.h b/cartographer/io/image.h index a3287a5..6dcd0fb 100644 --- a/cartographer/io/image.h +++ b/cartographer/io/image.h @@ -33,13 +33,13 @@ class Image { void SetPixel(int x, int y, const Color& color); void WritePng(FileWriter* const file_writer); - private: // Returns a pointer to a cairo surface that contains the current pixel data. // The 'Image' object must therefore outlive the returned surface object. It // is undefined behavior to call any of the mutating functions while a pointer // to this surface is alive. UniqueCairoSurfacePtr GetCairoSurface(); + private: int width_; int height_; int stride_; diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index db3e6b2..5f7f1d4 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -60,8 +60,23 @@ void RegisterFileWritingPointsProcessor( }); } +template +void RegisterFileWritingPointsProcessorWithTrajectories( + const std::vector& trajectories, + FileWriterFactory file_writer_factory, + PointsProcessorPipelineBuilder* const builder) { + builder->Register( + PointsProcessorType::kConfigurationFileActionName, + [&trajectories, file_writer_factory]( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) -> std::unique_ptr { + return PointsProcessorType::FromDictionary( + trajectories, file_writer_factory, dictionary, next); + }); +} + void RegisterBuiltInPointsProcessors( - const mapping::proto::Trajectory& trajectory, + const std::vector& trajectories, FileWriterFactory file_writer_factory, PointsProcessorPipelineBuilder* builder) { RegisterPlainPointsProcessor(builder); @@ -78,19 +93,11 @@ 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. - builder->Register( - XRayPointsProcessor::kConfigurationFileActionName, - [&trajectory, file_writer_factory]( - common::LuaParameterDictionary* const dictionary, - PointsProcessor* const next) -> std::unique_ptr { - return XRayPointsProcessor::FromDictionary( - trajectory, file_writer_factory, dictionary, next); - }); + RegisterFileWritingPointsProcessorWithTrajectories( + trajectories, file_writer_factory, builder); + RegisterFileWritingPointsProcessorWithTrajectories< + ProbabilityGridPointsProcessor>(trajectories, file_writer_factory, + builder); } void PointsProcessorPipelineBuilder::Register(const std::string& name, diff --git a/cartographer/io/points_processor_pipeline_builder.h b/cartographer/io/points_processor_pipeline_builder.h index 70060ee..0cdae3c 100644 --- a/cartographer/io/points_processor_pipeline_builder.h +++ b/cartographer/io/points_processor_pipeline_builder.h @@ -61,7 +61,7 @@ class PointsProcessorPipelineBuilder { // Register all 'PointsProcessor' that ship with Cartographer with this // 'builder'. void RegisterBuiltInPointsProcessors( - const mapping::proto::Trajectory& trajectory, + const std::vector& trajectories, FileWriterFactory file_writer_factory, PointsProcessorPipelineBuilder* builder); diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index 6641b8e..727c358 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -4,6 +4,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" +#include "cartographer/io/draw_trajectories.h" #include "cartographer/io/image.h" #include "cartographer/io/points_batch.h" @@ -11,8 +12,11 @@ namespace cartographer { namespace io { namespace { -void WriteGrid(const mapping_2d::ProbabilityGrid& probability_grid, - FileWriter* const file_writer) { +void WriteGrid( + const mapping_2d::ProbabilityGrid& probability_grid, + const ProbabilityGridPointsProcessor::DrawTrajectories& draw_trajectories, + const std::vector& trajectories, + FileWriter* const file_writer) { Eigen::Array2i offset; mapping_2d::CellLimits cell_limits; probability_grid.ComputeCroppedLimits(&offset, &cell_limits); @@ -20,9 +24,6 @@ void WriteGrid(const mapping_2d::ProbabilityGrid& probability_grid, LOG(WARNING) << "Not writing output: empty probability grid"; return; } - const auto grid_index_to_pixel = [cell_limits](const Eigen::Array2i& index) { - return Eigen::Array2i(index(0), index(1)); - }; const auto compute_color_value = [&probability_grid]( const Eigen::Array2i& index) { if (probability_grid.IsKnown(index)) { @@ -35,15 +36,26 @@ void WriteGrid(const mapping_2d::ProbabilityGrid& probability_grid, return kUnknownValue; } }; - int width = cell_limits.num_x_cells; - int height = cell_limits.num_y_cells; - Image image(width, height); + Image image(cell_limits.num_x_cells, cell_limits.num_y_cells); 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.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()); @@ -65,22 +77,34 @@ ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor( const double resolution, const mapping_2d::proto::RangeDataInserterOptions& range_data_inserter_options, - std::unique_ptr file_writer, PointsProcessor* const next) - : next_(next), + const DrawTrajectories& draw_trajectories, + std::unique_ptr file_writer, + const std::vector& trajectories, + PointsProcessor* const next) + : draw_trajectories_(draw_trajectories), + trajectories_(trajectories), file_writer_(std::move(file_writer)), + next_(next), range_data_inserter_(range_data_inserter_options), probability_grid_(CreateProbabilityGrid(resolution)) {} std::unique_ptr ProbabilityGridPointsProcessor::FromDictionary( + const std::vector& trajectories, FileWriterFactory file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { + const auto draw_trajectories = (!dictionary->HasKey("draw_trajectories") || + dictionary->GetBool("draw_trajectories")) + ? DrawTrajectories::kYes + : DrawTrajectories::kNo; return common::make_unique( dictionary->GetDouble("resolution"), mapping_2d::CreateRangeDataInserterOptions( dictionary->GetDictionary("range_data_inserter").get()), - file_writer_factory(dictionary->GetString("filename") + ".png"), next); + draw_trajectories, + file_writer_factory(dictionary->GetString("filename") + ".png"), + trajectories, next); } void ProbabilityGridPointsProcessor::Process( @@ -91,7 +115,8 @@ void ProbabilityGridPointsProcessor::Process( } PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() { - WriteGrid(probability_grid_, file_writer_.get()); + WriteGrid(probability_grid_, draw_trajectories_, trajectories_, + file_writer_.get()); switch (next_->Flush()) { case FlushResult::kRestartStream: LOG(FATAL) << "ProbabilityGrid generation must be configured to occur " diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h index 4d3e12b..3d42f80 100644 --- a/cartographer/io/probability_grid_points_processor.h +++ b/cartographer/io/probability_grid_points_processor.h @@ -7,6 +7,7 @@ #include "cartographer/io/file_writer.h" #include "cartographer/io/points_batch.h" #include "cartographer/io/points_processor.h" +#include "cartographer/mapping/proto/trajectory.pb.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" @@ -22,17 +23,22 @@ class ProbabilityGridPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_probability_grid"; + enum class DrawTrajectories { kNo, kYes }; ProbabilityGridPointsProcessor( double resolution, const mapping_2d::proto::RangeDataInserterOptions& range_data_inserter_options, - std::unique_ptr file_writer, PointsProcessor* next); + const DrawTrajectories& draw_trajectories, + std::unique_ptr file_writer, + const std::vector& trajectorios, + PointsProcessor* next); ProbabilityGridPointsProcessor(const ProbabilityGridPointsProcessor&) = delete; ProbabilityGridPointsProcessor& operator=( const ProbabilityGridPointsProcessor&) = delete; static std::unique_ptr FromDictionary( + const std::vector& trajectories, FileWriterFactory file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); @@ -42,8 +48,10 @@ class ProbabilityGridPointsProcessor : public PointsProcessor { FlushResult Flush() override; private: - PointsProcessor* const next_; + const DrawTrajectories draw_trajectories_; + const std::vector trajectories_; std::unique_ptr file_writer_; + PointsProcessor* const next_; mapping_2d::RangeDataInserter range_data_inserter_; mapping_2d::ProbabilityGrid probability_grid_; }; diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index 0d7d417..1bccae8 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -23,6 +23,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" +#include "cartographer/io/draw_trajectories.h" #include "cartographer/io/image.h" #include "cartographer/mapping/detect_floors.h" #include "cartographer/mapping_3d/hybrid_grid.h" @@ -46,7 +47,7 @@ double Mix(const double a, const double b, const double t) { return a * (1. - t) + t * b; } -// Write 'mat' as a pleasing-to-look-at PNG into 'filename' +// Convert 'mat' into a pleasing-to-look-at image. Image IntoImage(const PixelDataMatrix& mat) { Image image(mat.cols(), mat.rows()); float max = std::numeric_limits::min(); @@ -104,10 +105,14 @@ bool ContainedIn(const common::Time& time, XRayPointsProcessor::XRayPointsProcessor( const double voxel_size, const transform::Rigid3f& transform, - const std::vector& floors, const string& output_filename, + const std::vector& floors, + const DrawTrajectories& draw_trajectories, const string& output_filename, + const std::vector& trajectories, FileWriterFactory file_writer_factory, PointsProcessor* const next) - : next_(next), + : draw_trajectories_(draw_trajectories), + trajectories_(trajectories), file_writer_factory_(file_writer_factory), + next_(next), floors_(floors), output_filename_(output_filename), transform_(transform) { @@ -118,21 +123,29 @@ XRayPointsProcessor::XRayPointsProcessor( } std::unique_ptr XRayPointsProcessor::FromDictionary( - const mapping::proto::Trajectory& trajectory, + const std::vector& trajectories, FileWriterFactory file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { std::vector floors; - if (dictionary->HasKey("separate_floors") && - dictionary->GetBool("separate_floors")) { - floors = mapping::DetectFloors(trajectory); + const bool separate_floor = dictionary->HasKey("separate_floors") && + dictionary->GetBool("separate_floors"); + const auto draw_trajectories = (!dictionary->HasKey("draw_trajectories") || + dictionary->GetBool("draw_trajectories")) + ? DrawTrajectories::kYes + : DrawTrajectories::kNo; + if (separate_floor) { + CHECK_EQ(trajectories.size(), 0) + << "Can only detect floors with a single trajectory."; + floors = mapping::DetectFloors(trajectories.at(0)); } return common::make_unique( dictionary->GetDouble("voxel_size"), transform::FromDictionary(dictionary->GetDictionary("transform").get()) .cast(), - floors, dictionary->GetString("filename"), file_writer_factory, next); + floors, draw_trajectories, dictionary->GetString("filename"), + trajectories, file_writer_factory, next); } void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, @@ -166,9 +179,20 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, pixel_data.mean_b = column_data.sum_b / column_data.count; ++pixel_data.num_occupied_cells_in_column; } - Image image = IntoImage(pixel_data_matrix); - // TODO(hrapp): Draw trajectories here. + Image image = IntoImage(pixel_data_matrix); + if (draw_trajectories_ == DrawTrajectories::kYes) { + for (size_t i = 0; i < trajectories_.size(); ++i) { + DrawTrajectory( + trajectories_[i], GetColor(i), + [&voxel_index_to_pixel, &aggregation, + this](const transform::Rigid3d& pose) -> Eigen::Array2i { + return voxel_index_to_pixel(aggregation.voxels.GetCellIndex( + (transform_ * pose.cast()).translation())); + }, + image.GetCairoSurface().get()); + } + } image.WritePng(file_writer); CHECK(file_writer->Close()); diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h index 4517ef3..d84aef8 100644 --- a/cartographer/io/xray_points_processor.h +++ b/cartographer/io/xray_points_processor.h @@ -36,14 +36,16 @@ class XRayPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_xray_image"; - XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform, - const std::vector& floors, - const string& output_filename, - FileWriterFactory file_writer_factory, - PointsProcessor* next); + enum class DrawTrajectories { kNo, kYes }; + XRayPointsProcessor( + double voxel_size, const transform::Rigid3f& transform, + const std::vector& floors, + const DrawTrajectories& draw_trajectories, const string& output_filename, + const std::vector& trajectories, + FileWriterFactory file_writer_factory, PointsProcessor* next); static std::unique_ptr FromDictionary( - const mapping::proto::Trajectory& trajectory, + const std::vector& trajectories, FileWriterFactory file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); @@ -71,8 +73,10 @@ class XRayPointsProcessor : public PointsProcessor { FileWriter* const file_writer); void Insert(const PointsBatch& batch, Aggregation* aggregation); - PointsProcessor* const next_; + const DrawTrajectories draw_trajectories_; + const std::vector trajectories_; FileWriterFactory file_writer_factory_; + PointsProcessor* const next_; // If empty, we do not separate into floors. std::vector floors_;