00001 /* 00002 * Copyright 2016 The Cartographer Authors 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 * 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 #include "cartographer/io/min_max_range_filtering_points_processor.h" 00018 00019 #include "absl/memory/memory.h" 00020 #include "cartographer/common/lua_parameter_dictionary.h" 00021 #include "cartographer/io/points_batch.h" 00022 00023 namespace cartographer { 00024 namespace io { 00025 00026 std::unique_ptr<MinMaxRangeFiteringPointsProcessor> 00027 MinMaxRangeFiteringPointsProcessor::FromDictionary( 00028 common::LuaParameterDictionary* const dictionary, 00029 PointsProcessor* const next) { 00030 return absl::make_unique<MinMaxRangeFiteringPointsProcessor>( 00031 dictionary->GetDouble("min_range"), dictionary->GetDouble("max_range"), 00032 next); 00033 } 00034 00035 MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor( 00036 const double min_range, const double max_range, PointsProcessor* next) 00037 : min_range_(min_range), max_range_(max_range), next_(next) {} 00038 00039 void MinMaxRangeFiteringPointsProcessor::Process( 00040 std::unique_ptr<PointsBatch> batch) { 00041 absl::flat_hash_set<int> to_remove; 00042 for (size_t i = 0; i < batch->points.size(); ++i) { 00043 const float range = (batch->points[i].position - batch->origin).norm(); 00044 if (!(min_range_ <= range && range <= max_range_)) { 00045 to_remove.insert(i); 00046 } 00047 } 00048 RemovePoints(to_remove, batch.get()); 00049 next_->Process(std::move(batch)); 00050 } 00051 00052 PointsProcessor::FlushResult MinMaxRangeFiteringPointsProcessor::Flush() { 00053 return next_->Flush(); 00054 } 00055 00056 } // namespace io 00057 } // namespace cartographer