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

Support by-floor XRay generation. (#126)

master
Holger Rapp 2016-11-17 01:49:17 -08:00 committed by GitHub
parent c8f33ee853
commit ac92dd8cbf
5 changed files with 161 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/io/CMakeLists.txt
View File

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

46
cartographer/io/points_processor_pipeline_builder.cc
View File

@ -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<CountingPointsProcessor>();
RegisterNonStatic<FixedRatioSamplingPointsProcessor>();
RegisterNonStatic<MinMaxRangeFiteringPointsProcessor>();
RegisterNonStatic<OutlierRemovingPointsProcessor>();
RegisterNonStatic<PcdWritingPointsProcessor>();
RegisterNonStatic<PlyWritingPointsProcessor>();
RegisterNonStatic<XRayPointsProcessor>();
RegisterNonStatic<XyzWriterPointsProcessor>();
template <typename PointsProcessorType>
void RegisterPlainPointsProcessor(PointsProcessorPipelineBuilder* const builder) {
builder->Register(
PointsProcessorType::kConfigurationFileActionName,
[](common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
return PointsProcessorType::FromDictionary(dictionary, next);
});
}
PointsProcessorPipelineBuilder* PointsProcessorPipelineBuilder::instance() {
static PointsProcessorPipelineBuilder instance;
return &instance;
void RegisterBuiltInPointsProcessors(
const mapping::proto::Trajectory& trajectory,
PointsProcessorPipelineBuilder* builder) {
RegisterPlainPointsProcessor<CountingPointsProcessor>(builder);
RegisterPlainPointsProcessor<FixedRatioSamplingPointsProcessor>(builder);
RegisterPlainPointsProcessor<MinMaxRangeFiteringPointsProcessor>(builder);
RegisterPlainPointsProcessor<OutlierRemovingPointsProcessor>(builder);
RegisterPlainPointsProcessor<PcdWritingPointsProcessor>(builder);
RegisterPlainPointsProcessor<PlyWritingPointsProcessor>(builder);
RegisterPlainPointsProcessor<XyzWriterPointsProcessor>(builder);
builder->Register(
XRayPointsProcessor::kConfigurationFileActionName,
[&trajectory](
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) -> std::unique_ptr<PointsProcessor> {
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<std::unique_ptr<PointsProcessor>>
PointsProcessorPipelineBuilder::CreatePipeline(
common::LuaParameterDictionary* const dictionary) const {

49
cartographer/io/points_processor_pipeline_builder.h
View File

@ -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<std::unique_ptr<PointsProcessor>(
common::LuaParameterDictionary*, PointsProcessor* next)>;
PointsProcessorPipelineBuilder();
PointsProcessorPipelineBuilder(const PointsProcessorPipelineBuilder&) =
delete;
PointsProcessorPipelineBuilder& operator=(
const PointsProcessorPipelineBuilder&) = delete;
static PointsProcessorPipelineBuilder* instance();
template <typename PointsProcessorType>
void Register() {
instance()->RegisterNonStatic<PointsProcessorType>();
}
// 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<std::unique_ptr<PointsProcessor>> CreatePipeline(
common::LuaParameterDictionary* dictionary) const;
private:
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);
std::unordered_map<std::string, FactoryFunction> factories_;
};
// Register all 'PointsProcessor' that ship with Cartographer with this
// 'builder'.
void RegisterBuiltInPointsProcessors(
const mapping::proto::Trajectory& trajectory,
PointsProcessorPipelineBuilder* builder);
} // namespace io
} // namespace cartographer

138
cartographer/io/xray_points_processor.cc
View File

@ -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<bool>;
// 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> XRayPointsProcessor::FromDictionary(
common::LuaParameterDictionary* dictionary, PointsProcessor* next) {
return common::make_unique<XRayPointsProcessor>(
dictionary->GetDouble("voxel_size"),
transform::FromDictionary(dictionary->GetDictionary("transform").get())
.cast<float>(),
dictionary->GetString("filename"), next);
}
void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> 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<int>::max(),
std::numeric_limits<int>::max(),
std::numeric_limits<int>::max());
@ -127,7 +89,7 @@ void XRayPointsProcessor::WriteImage() {
std::numeric_limits<int>::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<common::Interval<common::Time>>& 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<mapping::Floor>& 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> XRayPointsProcessor::FromDictionary(
const mapping::proto::Trajectory& trajectory,
common::LuaParameterDictionary* dictionary, PointsProcessor* next) {
std::vector<mapping::Floor> floors;
if (dictionary->HasKey("separate_floors") &&
dictionary->GetBool("separate_floors")) {
floors = mapping::DetectFloors(trajectory);
}
return common::make_unique<XRayPointsProcessor>(
dictionary->GetDouble("voxel_size"),
transform::FromDictionary(dictionary->GetDictionary("transform").get())
.cast<float>(),
floors, dictionary->GetString("filename"), next);
}
void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> 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

21
cartographer/io/xray_points_processor.h
View File

@ -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<mapping::Floor>& floors,
const string& output_filename, PointsProcessor* next);
static std::unique_ptr<XRayPointsProcessor> 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<bool>;
void WriteImage();
PointsProcessor* const next_;
// If empty, we do not separate into floors.
std::vector<mapping::Floor> floors_;
const string output_filename_;
const transform::Rigid3f transform_;
Voxels voxels_;
// Only has one entry if we do not separate into floors.
std::vector<mapping_3d::HybridGridBase<bool>> voxels_;
};
} // namespace io