Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/io/intensity_to_color_points_processor.h"
00018
00019 #include "Eigen/Core"
00020 #include "absl/memory/memory.h"
00021 #include "cartographer/common/math.h"
00022 #include "glog/logging.h"
00023
00024 namespace cartographer {
00025 namespace io {
00026
00027 std::unique_ptr<IntensityToColorPointsProcessor>
00028 IntensityToColorPointsProcessor::FromDictionary(
00029 common::LuaParameterDictionary* const dictionary,
00030 PointsProcessor* const next) {
00031 const std::string frame_id =
00032 dictionary->HasKey("frame_id") ? dictionary->GetString("frame_id") : "";
00033 const float min_intensity = dictionary->GetDouble("min_intensity");
00034 const float max_intensity = dictionary->GetDouble("max_intensity");
00035 return absl::make_unique<IntensityToColorPointsProcessor>(
00036 min_intensity, max_intensity, frame_id, next);
00037 }
00038
00039 IntensityToColorPointsProcessor::IntensityToColorPointsProcessor(
00040 const float min_intensity, const float max_intensity,
00041 const std::string& frame_id, PointsProcessor* const next)
00042 : min_intensity_(min_intensity),
00043 max_intensity_(max_intensity),
00044 frame_id_(frame_id),
00045 next_(next) {}
00046
00047 void IntensityToColorPointsProcessor::Process(
00048 std::unique_ptr<PointsBatch> batch) {
00049 if (!batch->intensities.empty() &&
00050 (frame_id_.empty() || batch->frame_id == frame_id_)) {
00051 batch->colors.clear();
00052 for (const float intensity : batch->intensities) {
00053 const float gray = cartographer::common::Clamp(
00054 (intensity - min_intensity_) / (max_intensity_ - min_intensity_), 0.f,
00055 1.f);
00056 batch->colors.push_back({{gray, gray, gray}});
00057 }
00058 }
00059 next_->Process(std::move(batch));
00060 }
00061
00062 PointsProcessor::FlushResult IntensityToColorPointsProcessor::Flush() {
00063 return next_->Flush();
00064 }
00065
00066 }
00067 }