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/fixed_ratio_sampling_points_processor.h" 00018 00019 #include "Eigen/Core" 00020 #include "absl/memory/memory.h" 00021 #include "glog/logging.h" 00022 00023 namespace cartographer { 00024 namespace io { 00025 00026 std::unique_ptr<FixedRatioSamplingPointsProcessor> 00027 FixedRatioSamplingPointsProcessor::FromDictionary( 00028 common::LuaParameterDictionary* const dictionary, 00029 PointsProcessor* const next) { 00030 const double sampling_ratio(dictionary->GetDouble("sampling_ratio")); 00031 CHECK_LT(0., sampling_ratio) << "Sampling ratio <= 0 makes no sense."; 00032 CHECK_LT(sampling_ratio, 1.) << "Sampling ratio >= 1 makes no sense."; 00033 return absl::make_unique<FixedRatioSamplingPointsProcessor>(sampling_ratio, 00034 next); 00035 } 00036 00037 FixedRatioSamplingPointsProcessor::FixedRatioSamplingPointsProcessor( 00038 const double sampling_ratio, PointsProcessor* next) 00039 : sampling_ratio_(sampling_ratio), 00040 next_(next), 00041 sampler_(new common::FixedRatioSampler(sampling_ratio_)) {} 00042 00043 void FixedRatioSamplingPointsProcessor::Process( 00044 std::unique_ptr<PointsBatch> batch) { 00045 absl::flat_hash_set<int> to_remove; 00046 for (size_t i = 0; i < batch->points.size(); ++i) { 00047 if (!sampler_->Pulse()) { 00048 to_remove.insert(i); 00049 } 00050 } 00051 RemovePoints(to_remove, batch.get()); 00052 next_->Process(std::move(batch)); 00053 } 00054 00055 PointsProcessor::FlushResult FixedRatioSamplingPointsProcessor::Flush() { 00056 switch (next_->Flush()) { 00057 case PointsProcessor::FlushResult::kFinished: 00058 return PointsProcessor::FlushResult::kFinished; 00059 00060 case PointsProcessor::FlushResult::kRestartStream: 00061 sampler_ = absl::make_unique<common::FixedRatioSampler>(sampling_ratio_); 00062 return PointsProcessor::FlushResult::kRestartStream; 00063 } 00064 LOG(FATAL); 00065 } 00066 00067 } // namespace io 00068 } // namespace cartographer