From 46f8883d6a93353d3450a8063573f12163f46f8a Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Tue, 25 Oct 2016 15:18:32 +0200 Subject: [PATCH] Make all 'PointProcessor's registerable with the PipelineBuilder. (#95) Also the default instance has now knowledge of all of Cartographer's 'PointsProcessor's by default. --- cartographer/io/CMakeLists.txt | 36 ++++++++++++------- ...in_max_range_filtering_points_processor.cc | 11 ++++++ ...min_max_range_filtering_points_processor.h | 6 ++++ .../io/pcd_writing_points_processor.cc | 29 ++++++++++----- .../io/pcd_writing_points_processor.h | 6 ++++ .../io/ply_writing_points_processor.cc | 21 ++++++++--- .../io/ply_writing_points_processor.h | 5 +++ .../io/points_processor_pipeline_builder.cc | 13 ++++++- .../io/points_processor_pipeline_builder.h | 20 +++++++---- 9 files changed, 112 insertions(+), 35 deletions(-) diff --git a/cartographer/io/CMakeLists.txt b/cartographer/io/CMakeLists.txt index 84dbab1..f5ca6d1 100644 --- a/cartographer/io/CMakeLists.txt +++ b/cartographer/io/CMakeLists.txt @@ -10,6 +10,8 @@ google_library(io_min_max_range_filtering_points_processor HDRS min_max_range_filtering_points_processor.h DEPENDS + common_lua_parameter_dictionary + common_make_unique io_points_batch io_points_processor ) @@ -21,6 +23,19 @@ google_library(io_null_points_processor io_points_processor ) +google_library(io_pcd_writing_points_processor + USES_GLOG + SRCS + pcd_writing_points_processor.cc + HDRS + pcd_writing_points_processor.h + DEPENDS + common_lua_parameter_dictionary + common_make_unique + io_points_batch + io_points_processor +) + google_library(io_ply_writing_points_processor USES_GLOG SRCS @@ -28,6 +43,9 @@ google_library(io_ply_writing_points_processor HDRS ply_writing_points_processor.h DEPENDS + common_lua_parameter_dictionary + common_make_unique + io_points_batch io_points_processor ) @@ -56,8 +74,13 @@ google_library(io_points_processor_pipeline_builder DEPENDS common_lua_parameter_dictionary common_make_unique + io_min_max_range_filtering_points_processor io_null_points_processor + io_pcd_writing_points_processor + io_ply_writing_points_processor io_points_processor + io_xray_points_processor + io_xyz_writing_points_processor ) google_library(io_xray_points_processor @@ -88,16 +111,3 @@ google_library(io_xyz_writing_points_processor common_make_unique io_points_processor ) - -google_library(io_pcd_points_processor - USES_EIGEN - SRCS - pcd_writing_points_processor.cc - HDRS - pcd_writing_points_processor.h - DEPENDS - common_math - io_points_processor - mapping_3d_hybrid_grid - transform_rigid_transform -) diff --git a/cartographer/io/min_max_range_filtering_points_processor.cc b/cartographer/io/min_max_range_filtering_points_processor.cc index d535c4f..e2846a8 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.cc +++ b/cartographer/io/min_max_range_filtering_points_processor.cc @@ -16,11 +16,22 @@ #include "cartographer/io/min_max_range_filtering_points_processor.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/make_unique.h" #include "cartographer/io/points_batch.h" namespace cartographer { namespace io { +std::unique_ptr +MinMaxRangeFiteringPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return common::make_unique( + dictionary->GetDouble("min_range"), dictionary->GetDouble("max_range"), + next); +} + MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor( const double min_range, const double max_range, PointsProcessor* next) : min_range_(min_range), max_range_(max_range), next_(next) {} diff --git a/cartographer/io/min_max_range_filtering_points_processor.h b/cartographer/io/min_max_range_filtering_points_processor.h index 07d5b5b..2219cb6 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.h +++ b/cartographer/io/min_max_range_filtering_points_processor.h @@ -19,6 +19,7 @@ #include +#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_processor.h" namespace cartographer { @@ -28,8 +29,13 @@ namespace io { // or closer than 'min_range'. class MinMaxRangeFiteringPointsProcessor : public PointsProcessor { public: + constexpr static const char* kConfigurationFileActionName = + "min_max_range_filter"; MinMaxRangeFiteringPointsProcessor(double min_range, double max_range, PointsProcessor* next); + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + ~MinMaxRangeFiteringPointsProcessor() override {} MinMaxRangeFiteringPointsProcessor( diff --git a/cartographer/io/pcd_writing_points_processor.cc b/cartographer/io/pcd_writing_points_processor.cc index 7765f4c..a56f7c2 100644 --- a/cartographer/io/pcd_writing_points_processor.cc +++ b/cartographer/io/pcd_writing_points_processor.cc @@ -18,6 +18,9 @@ #include +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/io/points_batch.h" #include "glog/logging.h" namespace cartographer { @@ -28,7 +31,7 @@ namespace { // Writes the PCD header claiming 'num_points' will follow it into // 'output_file'. void WriteBinaryPcdHeader(const bool has_color, const int64 num_points, - std::ofstream* stream) { + std::ofstream* const stream) { string color_header_field = !has_color ? "" : " rgb"; string color_header_type = !has_color ? "" : " U"; string color_header_size = !has_color ? "" : " 4"; @@ -40,17 +43,17 @@ void WriteBinaryPcdHeader(const bool has_color, const int64 num_points, << "SIZE 4 4 4" << color_header_size << "\n" << "TYPE F F F" << color_header_type << "\n" << "COUNT 1 1 1" << color_header_count << "\n" - << "WIDTH " << std::setw(15) << std::setfill('0') - << num_points << "\n" + << "WIDTH " << std::setw(15) << std::setfill('0') << num_points + << "\n" << "HEIGHT 1\n" << "VIEWPOINT 0 0 0 1 0 0 0\n" - << "POINTS " << std::setw(15) << std::setfill('0') - << num_points << "\n" + << "POINTS " << std::setw(15) << std::setfill('0') << num_points + << "\n" << "DATA binary\n"; } void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point, - std::ofstream* stream) { + std::ofstream* const stream) { char buffer[12]; memcpy(buffer, &point[0], sizeof(float)); memcpy(buffer + 4, &point[1], sizeof(float)); @@ -60,7 +63,7 @@ void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point, } } -void WriteBinaryPcdPointColor(const Color& color, std::ostream* stream) { +void WriteBinaryPcdPointColor(const Color& color, std::ostream* const stream) { // Pack the color as uint32 little-endian stream->put(color[2]); stream->put(color[1]); @@ -70,8 +73,16 @@ void WriteBinaryPcdPointColor(const Color& color, std::ostream* stream) { } // namespace -PcdWritingPointsProcessor::PcdWritingPointsProcessor(const string& filename, - PointsProcessor* next) +std::unique_ptr +PcdWritingPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return common::make_unique( + dictionary->GetString("filename"), next); +} + +PcdWritingPointsProcessor::PcdWritingPointsProcessor( + const string& filename, PointsProcessor* const next) : next_(next), num_points_(0), has_colors_(false), diff --git a/cartographer/io/pcd_writing_points_processor.h b/cartographer/io/pcd_writing_points_processor.h index 8fb1be7..5a6debf 100644 --- a/cartographer/io/pcd_writing_points_processor.h +++ b/cartographer/io/pcd_writing_points_processor.h @@ -16,6 +16,7 @@ #include +#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_processor.h" namespace cartographer { @@ -24,7 +25,12 @@ namespace io { // Streams a PCD file to disk. The header is written in 'Flush'. class PcdWritingPointsProcessor : public PointsProcessor { public: + constexpr static const char* kConfigurationFileActionName = "write_pcd"; PcdWritingPointsProcessor(const string& filename, PointsProcessor* next); + + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + ~PcdWritingPointsProcessor() override {} PcdWritingPointsProcessor(const PcdWritingPointsProcessor&) = delete; diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc index 18731c0..17053eb 100644 --- a/cartographer/io/ply_writing_points_processor.cc +++ b/cartographer/io/ply_writing_points_processor.cc @@ -18,6 +18,9 @@ #include +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/io/points_batch.h" #include "glog/logging.h" namespace cartographer { @@ -28,7 +31,7 @@ namespace { // Writes the PLY header claiming 'num_points' will follow it into // 'output_file'. void WriteBinaryPlyHeader(const bool has_color, const int64 num_points, - std::ofstream* stream) { + std::ofstream* const stream) { string color_header = !has_color ? "" : "property uchar red\n" "property uchar green\n" "property uchar blue\n"; @@ -45,7 +48,7 @@ void WriteBinaryPlyHeader(const bool has_color, const int64 num_points, } void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point, - std::ofstream* stream) { + std::ofstream* const stream) { char buffer[12]; memcpy(buffer, &point[0], sizeof(float)); memcpy(buffer + 4, &point[1], sizeof(float)); @@ -55,7 +58,7 @@ void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point, } } -void WriteBinaryPlyPointColor(const Color& color, std::ostream* stream) { +void WriteBinaryPlyPointColor(const Color& color, std::ostream* const stream) { stream->put(color[0]); stream->put(color[1]); stream->put(color[2]); @@ -63,8 +66,16 @@ void WriteBinaryPlyPointColor(const Color& color, std::ostream* stream) { } // namespace -PlyWritingPointsProcessor::PlyWritingPointsProcessor(const string& filename, - PointsProcessor* next) +std::unique_ptr +PlyWritingPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return common::make_unique( + dictionary->GetString("filename"), next); +} + +PlyWritingPointsProcessor::PlyWritingPointsProcessor( + const string& filename, PointsProcessor* const next) : next_(next), num_points_(0), has_colors_(false), diff --git a/cartographer/io/ply_writing_points_processor.h b/cartographer/io/ply_writing_points_processor.h index c9bca86..d977ff3 100644 --- a/cartographer/io/ply_writing_points_processor.h +++ b/cartographer/io/ply_writing_points_processor.h @@ -16,6 +16,7 @@ #include +#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_processor.h" namespace cartographer { @@ -24,9 +25,13 @@ namespace io { // Streams a PLY file to disk. The header is written in 'Flush'. class PlyWritingPointsProcessor : public PointsProcessor { public: + constexpr static const char* kConfigurationFileActionName = "write_ply"; PlyWritingPointsProcessor(const string& filename, PointsProcessor* next); ~PlyWritingPointsProcessor() override {} + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + PlyWritingPointsProcessor(const PlyWritingPointsProcessor&) = delete; PlyWritingPointsProcessor& operator=(const PlyWritingPointsProcessor&) = delete; diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index afebc1f..1af355d 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -17,12 +17,23 @@ #include "cartographer/io/points_processor_pipeline_builder.h" #include "cartographer/common/make_unique.h" +#include "cartographer/io/min_max_range_filtering_points_processor.h" #include "cartographer/io/null_points_processor.h" +#include "cartographer/io/pcd_writing_points_processor.h" +#include "cartographer/io/ply_writing_points_processor.h" +#include "cartographer/io/xray_points_processor.h" +#include "cartographer/io/xyz_writing_points_processor.h" namespace cartographer { namespace io { -PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {} +PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() { + RegisterNonStatic(); + RegisterNonStatic(); + RegisterNonStatic(); + RegisterNonStatic(); + RegisterNonStatic(); +} PointsProcessorPipelineBuilder* PointsProcessorPipelineBuilder::instance() { static PointsProcessorPipelineBuilder instance; diff --git a/cartographer/io/points_processor_pipeline_builder.h b/cartographer/io/points_processor_pipeline_builder.h index 0b3d7d1..ecec463 100644 --- a/cartographer/io/points_processor_pipeline_builder.h +++ b/cartographer/io/points_processor_pipeline_builder.h @@ -28,7 +28,8 @@ namespace cartographer { namespace io { // Singleton that knows how to build a points processor pipeline out of a Lua -// configuration. You can register new classes with this instance that must +// 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. class PointsProcessorPipelineBuilder { @@ -42,12 +43,7 @@ class PointsProcessorPipelineBuilder { template void Register() { - instance()->RegisterType( - PointsProcessorType::kConfigurationFileActionName, - [](common::LuaParameterDictionary* const dictionary, - PointsProcessor* const next) -> std::unique_ptr { - return PointsProcessorType::FromDictionary(dictionary, next); - }); + instance()->RegisterNonStatic(); } std::vector> CreatePipeline( @@ -57,6 +53,16 @@ class PointsProcessorPipelineBuilder { 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);