diff --git a/cartographer/io/frame_id_filtering_points_processor.cc b/cartographer/io/frame_id_filtering_points_processor.cc new file mode 100644 index 0000000..089d453 --- /dev/null +++ b/cartographer/io/frame_id_filtering_points_processor.cc @@ -0,0 +1,70 @@ +/* + * Copyright 2017 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/frame_id_filtering_points_processor.h" + +#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 { +namespace io { + +std::unique_ptr +FrameIdFilteringPointsProcessor::FromDictionary( + common::LuaParameterDictionary* dictionary, + PointsProcessor* next) { + std::vector keep_frames, drop_frames; + if (dictionary->HasKey("keep_frames")) { + keep_frames = + dictionary->GetDictionary("keep_frames")->GetArrayValuesAsStrings(); + } + if (dictionary->HasKey("drop_frames")) { + drop_frames = + dictionary->GetDictionary("drop_frames")->GetArrayValuesAsStrings(); + } + return common::make_unique( + std::unordered_set(keep_frames.begin(), keep_frames.end()), + std::unordered_set(drop_frames.begin(), drop_frames.end()), next); +} + +FrameIdFilteringPointsProcessor::FrameIdFilteringPointsProcessor( + const std::unordered_set& keep_frame_ids, + const std::unordered_set& drop_frame_ids, + PointsProcessor* next) + : keep_frame_ids_(keep_frame_ids), + drop_frame_ids_(drop_frame_ids), + next_(next) { + CHECK_NE(keep_frame_ids.empty(), drop_frame_ids.empty()) + << "You have to specify exactly one of the `keep_frames` property or the " + << "`drop_frames` property, but not both at the same time."; +} + +void FrameIdFilteringPointsProcessor::Process( + std::unique_ptr batch) { + if ((!keep_frame_ids_.empty() && keep_frame_ids_.count(batch->frame_id)) || + (!drop_frame_ids_.empty() && !drop_frame_ids_.count(batch->frame_id))) { + next_->Process(std::move(batch)); + } +} + +PointsProcessor::FlushResult FrameIdFilteringPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/frame_id_filtering_points_processor.h b/cartographer/io/frame_id_filtering_points_processor.h new file mode 100644 index 0000000..5e4f272 --- /dev/null +++ b/cartographer/io/frame_id_filtering_points_processor.h @@ -0,0 +1,60 @@ +/* + * Copyright 2017 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_FRAME_ID_FILTERING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_FRAME_ID_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 with blacklisted frame id or a non-whitelisted frame id. +// Note that you can either specify the whitelist or the blacklist, but not both +// at the same time. +class FrameIdFilteringPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = "frame_id_filter"; + FrameIdFilteringPointsProcessor( + const std::unordered_set& keep_frame_ids, + const std::unordered_set& drop_frame_ids, + PointsProcessor* next); + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, + PointsProcessor* next); + ~FrameIdFilteringPointsProcessor() override {} + + FrameIdFilteringPointsProcessor(const FrameIdFilteringPointsProcessor&) = + delete; + FrameIdFilteringPointsProcessor& operator=( + const FrameIdFilteringPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const std::unordered_set keep_frame_ids_; + const std::unordered_set drop_frame_ids_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_FRAME_ID_FILTERING_POINTS_PROCESSOR_H_ diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index 09697ae..bea92da 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -19,6 +19,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/io/coloring_points_processor.h" #include "cartographer/io/counting_points_processor.h" +#include "cartographer/io/frame_id_filtering_points_processor.h" #include "cartographer/io/fixed_ratio_sampling_points_processor.h" #include "cartographer/io/hybrid_grid_points_processor.h" #include "cartographer/io/intensity_to_color_points_processor.h" @@ -81,6 +82,7 @@ void RegisterBuiltInPointsProcessors( PointsProcessorPipelineBuilder* builder) { RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder);