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/coloring_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<ColoringPointsProcessor> 00027 ColoringPointsProcessor::FromDictionary( 00028 common::LuaParameterDictionary* const dictionary, 00029 PointsProcessor* const next) { 00030 const std::string frame_id = dictionary->GetString("frame_id"); 00031 const std::vector<double> color_values = 00032 dictionary->GetDictionary("color")->GetArrayValuesAsDoubles(); 00033 const Uint8Color color = {{static_cast<uint8>(color_values[0]), 00034 static_cast<uint8>(color_values[1]), 00035 static_cast<uint8>(color_values[2])}}; 00036 return absl::make_unique<ColoringPointsProcessor>(ToFloatColor(color), 00037 frame_id, next); 00038 } 00039 00040 ColoringPointsProcessor::ColoringPointsProcessor(const FloatColor& color, 00041 const std::string& frame_id, 00042 PointsProcessor* const next) 00043 : color_(color), frame_id_(frame_id), next_(next) {} 00044 00045 void ColoringPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) { 00046 if (batch->frame_id == frame_id_) { 00047 batch->colors.clear(); 00048 for (size_t i = 0; i < batch->points.size(); ++i) { 00049 batch->colors.push_back(color_); 00050 } 00051 } 00052 next_->Process(std::move(batch)); 00053 } 00054 00055 PointsProcessor::FlushResult ColoringPointsProcessor::Flush() { 00056 return next_->Flush(); 00057 } 00058 00059 } // namespace io 00060 } // namespace cartographer