12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Holger Rapp 2016-10-25 15:18:32 +02:00 committed by GitHub
parent 188dcb57e5
commit 46f8883d6a
9 changed files with 112 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
cartographer/io/CMakeLists.txt
View File

@ -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
)

11
cartographer/io/min_max_range_filtering_points_processor.cc
View File

@ -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>
MinMaxRangeFiteringPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<MinMaxRangeFiteringPointsProcessor>(
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) {}

6
cartographer/io/min_max_range_filtering_points_processor.h
View File

@ -19,6 +19,7 @@
#include <memory>
#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<MinMaxRangeFiteringPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~MinMaxRangeFiteringPointsProcessor() override {}
MinMaxRangeFiteringPointsProcessor(

29
cartographer/io/pcd_writing_points_processor.cc
View File

@ -18,6 +18,9 @@
#include <iomanip>
#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>
PcdWritingPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<PcdWritingPointsProcessor>(
dictionary->GetString("filename"), next);
}
PcdWritingPointsProcessor::PcdWritingPointsProcessor(
const string& filename, PointsProcessor* const next)
: next_(next),
num_points_(0),
has_colors_(false),

6
cartographer/io/pcd_writing_points_processor.h
View File

@ -16,6 +16,7 @@
#include <fstream>
#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<PcdWritingPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~PcdWritingPointsProcessor() override {}
PcdWritingPointsProcessor(const PcdWritingPointsProcessor&) = delete;

21
cartographer/io/ply_writing_points_processor.cc
View File

@ -18,6 +18,9 @@
#include <iomanip>
#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>
PlyWritingPointsProcessor::FromDictionary(
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<PlyWritingPointsProcessor>(
dictionary->GetString("filename"), next);
}
PlyWritingPointsProcessor::PlyWritingPointsProcessor(
const string& filename, PointsProcessor* const next)
: next_(next),
num_points_(0),
has_colors_(false),

5
cartographer/io/ply_writing_points_processor.h
View File

@ -16,6 +16,7 @@
#include <fstream>
#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<PlyWritingPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
PlyWritingPointsProcessor(const PlyWritingPointsProcessor&) = delete;
PlyWritingPointsProcessor& operator=(const PlyWritingPointsProcessor&) =
delete;

13
cartographer/io/points_processor_pipeline_builder.cc
View File

@ -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<MinMaxRangeFiteringPointsProcessor>();
RegisterNonStatic<PcdWritingPointsProcessor>();
RegisterNonStatic<PlyWritingPointsProcessor>();
RegisterNonStatic<XRayPointsProcessor>();
RegisterNonStatic<XyzWriterPointsProcessor>();
}
PointsProcessorPipelineBuilder* PointsProcessorPipelineBuilder::instance() {
static PointsProcessorPipelineBuilder instance;

20
cartographer/io/points_processor_pipeline_builder.h
View File

@ -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 <typename PointsProcessorType>
void Register() {
instance()->RegisterType(
PointsProcessorType::kConfigurationFileActionName,
[](common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
return PointsProcessorType::FromDictionary(dictionary, next);
});
instance()->RegisterNonStatic<PointsProcessorType>();
}
std::vector<std::unique_ptr<PointsProcessor>> CreatePipeline(
@ -57,6 +53,16 @@ class PointsProcessorPipelineBuilder {
using FactoryFunction = std::function<std::unique_ptr<PointsProcessor>(
common::LuaParameterDictionary*, PointsProcessor* next)>;
template <typename PointsProcessorType>
void RegisterNonStatic() {
RegisterType(
PointsProcessorType::kConfigurationFileActionName,
[](common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
return PointsProcessorType::FromDictionary(dictionary, next);
});
}
PointsProcessorPipelineBuilder();
void RegisterType(const std::string& name, FactoryFunction factory);