From ac92dd8cbf7042da13cc4b3ba92645799464de92 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Thu, 17 Nov 2016 01:49:17 -0800 Subject: [PATCH] Support by-floor XRay generation. (#126) --- cartographer/io/CMakeLists.txt | 3 + .../io/points_processor_pipeline_builder.cc | 46 ++++-- .../io/points_processor_pipeline_builder.h | 49 +++---- cartographer/io/xray_points_processor.cc | 138 ++++++++++++------ cartographer/io/xray_points_processor.h | 21 +-- 5 files changed, 161 insertions(+), 96 deletions(-) diff --git a/cartographer/io/CMakeLists.txt b/cartographer/io/CMakeLists.txt index 7689bde..ef64115 100644 --- a/cartographer/io/CMakeLists.txt +++ b/cartographer/io/CMakeLists.txt @@ -123,6 +123,7 @@ google_library(io_points_processor_pipeline_builder io_points_processor io_xray_points_processor io_xyz_writing_points_processor + mapping_proto_trajectory ) google_library(io_xray_points_processor @@ -139,6 +140,8 @@ google_library(io_xray_points_processor io_cairo_types io_points_processor mapping_3d_hybrid_grid + mapping_detect_floors + mapping_proto_trajectory transform_rigid_transform ) diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index 726cc73..e24bb80 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -26,34 +26,52 @@ #include "cartographer/io/ply_writing_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" namespace cartographer { namespace io { -PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() { - RegisterNonStatic(); - RegisterNonStatic(); - RegisterNonStatic(); - RegisterNonStatic(); - RegisterNonStatic(); - RegisterNonStatic(); - RegisterNonStatic(); - RegisterNonStatic(); +template +void RegisterPlainPointsProcessor(PointsProcessorPipelineBuilder* const builder) { + builder->Register( + PointsProcessorType::kConfigurationFileActionName, + [](common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) -> std::unique_ptr { + return PointsProcessorType::FromDictionary(dictionary, next); + }); } -PointsProcessorPipelineBuilder* PointsProcessorPipelineBuilder::instance() { - static PointsProcessorPipelineBuilder instance; - return &instance; +void RegisterBuiltInPointsProcessors( + const mapping::proto::Trajectory& trajectory, + PointsProcessorPipelineBuilder* builder) { + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + + builder->Register( + XRayPointsProcessor::kConfigurationFileActionName, + [&trajectory]( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) -> std::unique_ptr { + return XRayPointsProcessor::FromDictionary(trajectory, dictionary, + next); + }); } -void PointsProcessorPipelineBuilder::RegisterType(const std::string& name, - FactoryFunction factory) { +void PointsProcessorPipelineBuilder::Register(const std::string& name, + FactoryFunction factory) { CHECK(factories_.count(name) == 0) << "A points processor with named '" << name << "' has already been registered."; factories_[name] = factory; } +PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {} + std::vector> PointsProcessorPipelineBuilder::CreatePipeline( common::LuaParameterDictionary* const dictionary) const { diff --git a/cartographer/io/points_processor_pipeline_builder.h b/cartographer/io/points_processor_pipeline_builder.h index ecec463..8045884 100644 --- a/cartographer/io/points_processor_pipeline_builder.h +++ b/cartographer/io/points_processor_pipeline_builder.h @@ -23,53 +23,46 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_processor.h" +#include "cartographer/mapping/proto/trajectory.pb.h" namespace cartographer { namespace io { -// Singleton that knows how to build a points processor pipeline out of a Lua -// configuration. All the PointsProcessor shipping with Cartographer are already -// registered with 'instance', but can register new classes with it that must -// define its name and a way to build itself out of a LuaParameterDictionary. -// See the various 'PointsProcessor's for examples. +// Builder to create a points processor pipeline out of a Lua configuration. +// You can register all built-in PointsProcessors using +// 'RegisterBuiltInPointsProcessors'. Non-built-in PointsProcessors must define +// a name and a factory method for building itself from a +// LuaParameterDictionary. See the various built-in PointsProcessors for +// examples. class PointsProcessorPipelineBuilder { public: + using FactoryFunction = std::function( + common::LuaParameterDictionary*, PointsProcessor* next)>; + + PointsProcessorPipelineBuilder(); + PointsProcessorPipelineBuilder(const PointsProcessorPipelineBuilder&) = delete; PointsProcessorPipelineBuilder& operator=( const PointsProcessorPipelineBuilder&) = delete; - static PointsProcessorPipelineBuilder* instance(); - - template - void Register() { - instance()->RegisterNonStatic(); - } + // Register a new PointsProcessor type uniquly identified by 'name' which will + // be created using 'factory'. + void Register(const std::string& name, FactoryFunction factory); std::vector> CreatePipeline( common::LuaParameterDictionary* dictionary) const; private: - using FactoryFunction = std::function( - common::LuaParameterDictionary*, PointsProcessor* next)>; - - template - void RegisterNonStatic() { - RegisterType( - PointsProcessorType::kConfigurationFileActionName, - [](common::LuaParameterDictionary* const dictionary, - PointsProcessor* const next) -> std::unique_ptr { - return PointsProcessorType::FromDictionary(dictionary, next); - }); - } - - PointsProcessorPipelineBuilder(); - - void RegisterType(const std::string& name, FactoryFunction factory); - std::unordered_map factories_; }; +// Register all 'PointsProcessor' that ship with Cartographer with this +// 'builder'. +void RegisterBuiltInPointsProcessors( + const mapping::proto::Trajectory& trajectory, + PointsProcessorPipelineBuilder* builder); + } // namespace io } // namespace cartographer diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index 005b4e1..d05c895 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -25,12 +25,15 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/io/cairo_types.h" +#include "cartographer/mapping/detect_floors.h" #include "cartographer/mapping_3d/hybrid_grid.h" namespace cartographer { namespace io { namespace { +using Voxels = mapping_3d::HybridGridBase; + // Takes the logarithm of each value in 'mat', clamping to 0 as smallest value. void TakeLogarithm(Eigen::MatrixXf* mat) { for (int y = 0; y < mat->rows(); ++y) { @@ -77,48 +80,7 @@ void WritePng(const string& filename, const Eigen::MatrixXf& mat) { CAIRO_STATUS_SUCCESS); } -} // namespace - -XRayPointsProcessor::XRayPointsProcessor(const double voxel_size, - const transform::Rigid3f& transform, - const string& output_filename, - PointsProcessor* next) - : next_(next), - output_filename_(output_filename), - transform_(transform), - voxels_(voxel_size, Eigen::Vector3f::Zero()) {} - -std::unique_ptr XRayPointsProcessor::FromDictionary( - common::LuaParameterDictionary* dictionary, PointsProcessor* next) { - return common::make_unique( - dictionary->GetDouble("voxel_size"), - transform::FromDictionary(dictionary->GetDictionary("transform").get()) - .cast(), - dictionary->GetString("filename"), next); -} - -void XRayPointsProcessor::Process(std::unique_ptr batch) { - for (const auto& point : batch->points) { - const Eigen::Vector3f camera_point = transform_ * point; - *voxels_.mutable_value(voxels_.GetCellIndex(camera_point)) = true; - } - next_->Process(std::move(batch)); -} - -PointsProcessor::FlushResult XRayPointsProcessor::Flush() { - WriteImage(); - switch (next_->Flush()) { - case FlushResult::kRestartStream: - LOG(FATAL) << "X-Ray generation must be configured to occur after any " - "stages that require multiple passes."; - - case FlushResult::kFinished: - return FlushResult::kFinished; - } - LOG(FATAL); -} - -void XRayPointsProcessor::WriteImage() { +void WriteVoxels(const string& filename, const Voxels& voxels) { Eigen::Array3i min(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max()); @@ -127,7 +89,7 @@ void XRayPointsProcessor::WriteImage() { std::numeric_limits::min()); // Find the maximum and minimum cells. - for (Voxels::Iterator it(voxels_); !it.Done(); it.Next()) { + for (Voxels::Iterator it(voxels); !it.Done(); it.Next()) { const Eigen::Array3i idx = it.GetCellIndex(); min = min.min(idx); max = max.max(idx); @@ -144,12 +106,98 @@ void XRayPointsProcessor::WriteImage() { const int xsize = max[1] - min[1] + 1; const int ysize = max[2] - min[2] + 1; Eigen::MatrixXf image = Eigen::MatrixXf::Zero(ysize, xsize); - for (Voxels::Iterator it(voxels_); !it.Done(); it.Next()) { + for (Voxels::Iterator it(voxels); !it.Done(); it.Next()) { const Eigen::Array2i pixel = voxel_index_to_pixel(it.GetCellIndex()); ++image(pixel.y(), pixel.x()); } TakeLogarithm(&image); - WritePng(output_filename_, image); + WritePng(filename, image); +} + +bool ContainedIn( + const common::Time& time, + const std::vector>& time_intervals) { + for (const auto& interval : time_intervals) { + if (interval.start <= time && time <= interval.end) { + return true; + } + } + return false; +} + +void Insert(const PointsBatch& batch, const transform::Rigid3f& transform, + Voxels* voxels) { + for (const auto& point : batch.points) { + const Eigen::Vector3f camera_point = transform * point; + *voxels->mutable_value(voxels->GetCellIndex(camera_point)) = true; + } +} + +} // namespace + +XRayPointsProcessor::XRayPointsProcessor( + const double voxel_size, const transform::Rigid3f& transform, + const std::vector& floors, const string& output_filename, + PointsProcessor* next) + : next_(next), + floors_(floors), + output_filename_(output_filename), + transform_(transform) { + for (int i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) { + voxels_.emplace_back(voxel_size, Eigen::Vector3f::Zero()); + } +} + +std::unique_ptr XRayPointsProcessor::FromDictionary( + const mapping::proto::Trajectory& trajectory, + common::LuaParameterDictionary* dictionary, PointsProcessor* next) { + std::vector floors; + if (dictionary->HasKey("separate_floors") && + dictionary->GetBool("separate_floors")) { + floors = mapping::DetectFloors(trajectory); + } + + return common::make_unique( + dictionary->GetDouble("voxel_size"), + transform::FromDictionary(dictionary->GetDictionary("transform").get()) + .cast(), + floors, dictionary->GetString("filename"), next); +} + +void XRayPointsProcessor::Process(std::unique_ptr batch) { + if (floors_.empty()) { + CHECK_EQ(voxels_.size(), 1); + Insert(*batch, transform_, &voxels_[0]); + } else { + for (int i = 0; i < floors_.size(); ++i) { + if (!ContainedIn(batch->time, floors_[i].timespans)) { + continue; + } + Insert(*batch, transform_, &voxels_[i]); + } + } + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult XRayPointsProcessor::Flush() { + if (floors_.empty()) { + CHECK_EQ(voxels_.size(), 1); + WriteVoxels(output_filename_ + ".png", voxels_[0]); + } else { + for (size_t i = 0; i < floors_.size(); ++i) { + WriteVoxels(output_filename_ + std::to_string(i) + ".png", voxels_[i]); + } + } + + switch (next_->Flush()) { + case FlushResult::kRestartStream: + LOG(FATAL) << "X-Ray generation must be configured to occur after any " + "stages that require multiple passes."; + + case FlushResult::kFinished: + return FlushResult::kFinished; + } + LOG(FATAL); } } // namespace io diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h index 0464e1f..bee8a41 100644 --- a/cartographer/io/xray_points_processor.h +++ b/cartographer/io/xray_points_processor.h @@ -19,24 +19,25 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_processor.h" +#include "cartographer/mapping/detect_floors.h" +#include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace io { -// Creates X-ray cuts through the points with pixels being 'voxel_size' big. All -// images created from a single XRayPointsProcessor have the same dimensions and -// are centered on the center of the bounding box, so that they can easily be -// combined into a movie. +// Creates X-ray cuts through the points with pixels being 'voxel_size' big. 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, PointsProcessor* next); static std::unique_ptr FromDictionary( + const mapping::proto::Trajectory& trajectory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~XRayPointsProcessor() override {} @@ -45,14 +46,16 @@ class XRayPointsProcessor : public PointsProcessor { FlushResult Flush() override; private: - using Voxels = mapping_3d::HybridGridBase; - - void WriteImage(); - PointsProcessor* const next_; + + // If empty, we do not separate into floors. + std::vector floors_; + const string output_filename_; const transform::Rigid3f transform_; - Voxels voxels_; + + // Only has one entry if we do not separate into floors. + std::vector> voxels_; }; } // namespace io