From 2f60378140a1b8229a68311f1c5383f30a379ecc Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Tue, 9 Jun 2020 12:35:47 +0200 Subject: [PATCH] Add VerticalRangeFilteringPointsProcessor. (#1636) Add an extra filter for 3D data. Solve the problem with 2D maps created from 3D sensors. --- .../io/points_processor_pipeline_builder.cc | 2 + ...rtical_range_filtering_points_processor.cc | 59 +++++++++++++++++++ ...ertical_range_filtering_points_processor.h | 58 ++++++++++++++++++ 3 files changed, 119 insertions(+) create mode 100644 cartographer/io/vertical_range_filtering_points_processor.cc create mode 100644 cartographer/io/vertical_range_filtering_points_processor.h diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index e50cc77..6592245 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -29,6 +29,7 @@ #include "cartographer/io/pcd_writing_points_processor.h" #include "cartographer/io/ply_writing_points_processor.h" #include "cartographer/io/probability_grid_points_processor.h" +#include "cartographer/io/vertical_range_filtering_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" @@ -84,6 +85,7 @@ void RegisterBuiltInPointsProcessors( RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); diff --git a/cartographer/io/vertical_range_filtering_points_processor.cc b/cartographer/io/vertical_range_filtering_points_processor.cc new file mode 100644 index 0000000..325d359 --- /dev/null +++ b/cartographer/io/vertical_range_filtering_points_processor.cc @@ -0,0 +1,59 @@ +/* + * 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/vertical_range_filtering_points_processor.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +VerticalRangeFilteringPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique( + dictionary->GetDouble("min_z"), dictionary->GetDouble("max_z"), + next); +} + +VerticalRangeFilteringPointsProcessor::VerticalRangeFilteringPointsProcessor( + const double min_z, const double max_z, + PointsProcessor* next) + : min_z_(min_z), max_z_(max_z), + next_(next) {} + +void VerticalRangeFilteringPointsProcessor::Process( + std::unique_ptr batch) { + absl::flat_hash_set to_remove; + for (size_t i = 0; i < batch->points.size(); ++i) { + const float distance_z = batch->points[i].position.z() - batch->origin.z(); + if (!(min_z_ <= distance_z && distance_z <= max_z_) ) { + to_remove.insert(i); + } + } + RemovePoints(to_remove, batch.get()); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult VerticalRangeFilteringPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/vertical_range_filtering_points_processor.h b/cartographer/io/vertical_range_filtering_points_processor.h new file mode 100644 index 0000000..d23eb20 --- /dev/null +++ b/cartographer/io/vertical_range_filtering_points_processor.h @@ -0,0 +1,58 @@ +/* + * 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_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Filters all points which distance in the Z direction from their 'origin' +// exceeds 'max_z' or 'min_z'. +class VerticalRangeFilteringPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "vertical_range_filter"; + VerticalRangeFilteringPointsProcessor(double min_z, double max_z, + PointsProcessor* next); + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~VerticalRangeFilteringPointsProcessor() override {} + + VerticalRangeFilteringPointsProcessor( + const VerticalRangeFilteringPointsProcessor&) = delete; + VerticalRangeFilteringPointsProcessor& operator=( + const VerticalRangeFilteringPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const double min_z_; + const double max_z_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_