From 2f42c990f648fa4f7352e6e4f19811aee4bd6e52 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Tue, 25 Oct 2016 15:37:47 +0200 Subject: [PATCH] Adds a fixed sampling points processor. (#96) --- cartographer/io/CMakeLists.txt | 15 ++++ .../fixed_ratio_sampling_points_processor.cc | 69 +++++++++++++++++++ .../fixed_ratio_sampling_points_processor.h | 61 ++++++++++++++++ .../io/points_processor_pipeline_builder.cc | 2 + 4 files changed, 147 insertions(+) create mode 100644 cartographer/io/fixed_ratio_sampling_points_processor.cc create mode 100644 cartographer/io/fixed_ratio_sampling_points_processor.h diff --git a/cartographer/io/CMakeLists.txt b/cartographer/io/CMakeLists.txt index f5ca6d1..4269814 100644 --- a/cartographer/io/CMakeLists.txt +++ b/cartographer/io/CMakeLists.txt @@ -4,6 +4,20 @@ google_library(io_cairo_types cairo_types.h ) +google_library(io_fixed_ratio_sampling_points_processor + USES_EIGEN + USES_GLOG + SRCS + fixed_ratio_sampling_points_processor.cc + HDRS + fixed_ratio_sampling_points_processor.h + DEPENDS + common_fixed_ratio_sampler + common_lua_parameter_dictionary + common_make_unique + io_points_processor +) + google_library(io_min_max_range_filtering_points_processor SRCS min_max_range_filtering_points_processor.cc @@ -74,6 +88,7 @@ google_library(io_points_processor_pipeline_builder DEPENDS common_lua_parameter_dictionary common_make_unique + io_fixed_ratio_sampling_points_processor io_min_max_range_filtering_points_processor io_null_points_processor io_pcd_writing_points_processor diff --git a/cartographer/io/fixed_ratio_sampling_points_processor.cc b/cartographer/io/fixed_ratio_sampling_points_processor.cc new file mode 100644 index 0000000..858e789 --- /dev/null +++ b/cartographer/io/fixed_ratio_sampling_points_processor.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/fixed_ratio_sampling_points_processor.h" + +#include "Eigen/Core" +#include "cartographer/common/make_unique.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +FixedRatioSamplingPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + const double sampling_ratio(dictionary->GetDouble("sampling_ratio")); + CHECK_LT(0., sampling_ratio) << "Sampling ratio <= 0 makes no sense."; + CHECK_LT(sampling_ratio, 1.) << "Sampling ratio >= 1 makes no sense."; + return common::make_unique(sampling_ratio, + next); +} + +FixedRatioSamplingPointsProcessor::FixedRatioSamplingPointsProcessor( + const double sampling_ratio, PointsProcessor* next) + : sampling_ratio_(sampling_ratio), + next_(next), + sampler_(new common::FixedRatioSampler(sampling_ratio_)) {} + +void FixedRatioSamplingPointsProcessor::Process( + std::unique_ptr batch) { + std::vector to_remove; + for (size_t i = 0; i < batch->points.size(); ++i) { + if (!sampler_->Pulse()) { + to_remove.push_back(i); + } + } + RemovePoints(to_remove, batch.get()); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult FixedRatioSamplingPointsProcessor::Flush() { + switch (next_->Flush()) { + case PointsProcessor::FlushResult::kFinished: + return PointsProcessor::FlushResult::kFinished; + + case PointsProcessor::FlushResult::kRestartStream: + sampler_ = + common::make_unique(sampling_ratio_); + return PointsProcessor::FlushResult::kRestartStream; + } + LOG(FATAL); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/fixed_ratio_sampling_points_processor.h b/cartographer/io/fixed_ratio_sampling_points_processor.h new file mode 100644 index 0000000..0c8763f --- /dev/null +++ b/cartographer/io/fixed_ratio_sampling_points_processor.h @@ -0,0 +1,61 @@ +/* + * 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_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/fixed_ratio_sampler.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Only let a fixed 'sampling_ratio' of points through. A 'sampling_ratio' of 1. +// makes this filter a no-op. +class FixedRatioSamplingPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "fixed_ratio_sampler"; + + FixedRatioSamplingPointsProcessor(double sampling_ratio, + PointsProcessor* next); + + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~FixedRatioSamplingPointsProcessor() override{}; + + FixedRatioSamplingPointsProcessor(const FixedRatioSamplingPointsProcessor&) = + delete; + FixedRatioSamplingPointsProcessor& operator=( + const FixedRatioSamplingPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const double sampling_ratio_; + PointsProcessor* const next_; + std::unique_ptr sampler_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_ diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index 1af355d..b21757e 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -17,6 +17,7 @@ #include "cartographer/io/points_processor_pipeline_builder.h" #include "cartographer/common/make_unique.h" +#include "cartographer/io/fixed_ratio_sampling_points_processor.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" @@ -28,6 +29,7 @@ namespace cartographer { namespace io { PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() { + RegisterNonStatic(); RegisterNonStatic(); RegisterNonStatic(); RegisterNonStatic();