From 965caf747044bbf75b01ed93fac2e595c799fbb5 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Tue, 24 Jan 2017 15:08:41 +0100 Subject: [PATCH] Make intensities a separate channel in PointsBatch. (#187) And add a PointsProcessor that translates from intensities to RGB. Fixes #171. --- cartographer/io/coloring_points_processor.cc | 6 +- .../io/intensity_to_color_points_processor.cc | 69 +++++++++++++++++++ .../io/intensity_to_color_points_processor.h | 64 +++++++++++++++++ cartographer/io/points_batch.cc | 3 + cartographer/io/points_batch.h | 10 +++ .../io/points_processor_pipeline_builder.cc | 2 + 6 files changed, 152 insertions(+), 2 deletions(-) create mode 100644 cartographer/io/intensity_to_color_points_processor.cc create mode 100644 cartographer/io/intensity_to_color_points_processor.h diff --git a/cartographer/io/coloring_points_processor.cc b/cartographer/io/coloring_points_processor.cc index 5dbfb35..9c55000 100644 --- a/cartographer/io/coloring_points_processor.cc +++ b/cartographer/io/coloring_points_processor.cc @@ -30,7 +30,9 @@ ColoringPointsProcessor::FromDictionary( const string frame_id = dictionary->GetString("frame_id"); const std::vector color_values = dictionary->GetDictionary("color")->GetArrayValuesAsDoubles(); - Color color = {{color_values[0], color_values[1], color_values[2]}}; + const Color color = {{static_cast(color_values[0]), + static_cast(color_values[1]), + static_cast(color_values[2])}}; return common::make_unique(color, frame_id, next); } @@ -42,7 +44,7 @@ ColoringPointsProcessor::ColoringPointsProcessor(const Color& color, void ColoringPointsProcessor::Process(std::unique_ptr batch) { if (batch->frame_id == frame_id_) { batch->colors.clear(); - for (int i = 0; i < batch->points.size(); ++i) { + for (size_t i = 0; i < batch->points.size(); ++i) { batch->colors.push_back(color_); } } diff --git a/cartographer/io/intensity_to_color_points_processor.cc b/cartographer/io/intensity_to_color_points_processor.cc new file mode 100644 index 0000000..08b504b --- /dev/null +++ b/cartographer/io/intensity_to_color_points_processor.cc @@ -0,0 +1,69 @@ +/* + * Copyright 2016 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/intensity_to_color_points_processor.h" + +#include "Eigen/Core" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/math.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +IntensityToColorPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + const string frame_id = + dictionary->HasKey("frame_id") ? dictionary->GetString("frame_id") : ""; + const float min_intensity = dictionary->GetDouble("min_intensity"); + const float max_intensity = dictionary->GetDouble("max_intensity"); + return common::make_unique( + min_intensity, max_intensity, frame_id, next); +} + +IntensityToColorPointsProcessor::IntensityToColorPointsProcessor( + const float min_intensity, const float max_intensity, + const string& frame_id, PointsProcessor* const next) + : min_intensity_(min_intensity), + max_intensity_(max_intensity), + frame_id_(frame_id), + next_(next) {} + +void IntensityToColorPointsProcessor::Process( + std::unique_ptr batch) { + if (!batch->intensities.empty() && + (frame_id_.empty() || batch->frame_id == frame_id_)) { + batch->colors.clear(); + for (const float intensity : batch->intensities) { + const uint8_t gray = + cartographer::common::Clamp( + (intensity - min_intensity_) / (max_intensity_ - min_intensity_), + 0.f, 1.f) * + 255; + batch->colors.push_back(Color{{gray, gray, gray}}); + } + } + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult IntensityToColorPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/intensity_to_color_points_processor.h b/cartographer/io/intensity_to_color_points_processor.h new file mode 100644 index 0000000..f3d3346 --- /dev/null +++ b/cartographer/io/intensity_to_color_points_processor.h @@ -0,0 +1,64 @@ +/* + * Copyright 2016 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_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +class IntensityToColorPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "intensity_to_color"; + + // Applies ('intensity' - min ) / (max - min) * 255 and color the point grey + // with this value for each point that comes from the sensor with 'frame_id'. + // If 'frame_id' is empty, this applies to all points. + IntensityToColorPointsProcessor(float min_intensity, float max_intensity, + const string& frame_id, + PointsProcessor* next); + + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~IntensityToColorPointsProcessor() override{}; + + IntensityToColorPointsProcessor(const IntensityToColorPointsProcessor&) = + delete; + IntensityToColorPointsProcessor& operator=( + const IntensityToColorPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const float min_intensity_; + const float max_intensity_; + const string frame_id_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_ diff --git a/cartographer/io/points_batch.cc b/cartographer/io/points_batch.cc index 4776958..19f1ec4 100644 --- a/cartographer/io/points_batch.cc +++ b/cartographer/io/points_batch.cc @@ -26,6 +26,9 @@ void RemovePoints(std::vector to_remove, PointsBatch* batch) { if (!batch->colors.empty()) { batch->colors.erase(batch->colors.begin() + index); } + if (!batch->intensities.empty()) { + batch->intensities.erase(batch->intensities.begin() + index); + } } } diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h index 89b0429..c3eea17 100644 --- a/cartographer/io/points_batch.h +++ b/cartographer/io/points_batch.h @@ -51,7 +51,17 @@ struct PointsBatch { // Trajectory index that produced this point. int trajectory_index; + // Geometry of the points in a metric frame. std::vector points; + + // Intensities are optional and may be unspecified. The meaning of these + // intensity values varies by device. For example, the VLP16 provides values + // in the range [0, 100] for non-specular return values and values up to 255 + // for specular returns. On the other hand, Hokuyo lasers provide a 16-bit + // value that rarely peaks above 4096. + std::vector intensities; + + // Colors are optional. If set, they are RGB values. std::vector colors; }; diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index b1f1e93..53eb7cc 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -20,6 +20,7 @@ #include "cartographer/io/coloring_points_processor.h" #include "cartographer/io/counting_points_processor.h" #include "cartographer/io/fixed_ratio_sampling_points_processor.h" +#include "cartographer/io/intensity_to_color_points_processor.h" #include "cartographer/io/min_max_range_filtering_points_processor.h" #include "cartographer/io/null_points_processor.h" #include "cartographer/io/outlier_removing_points_processor.h" @@ -66,6 +67,7 @@ void RegisterBuiltInPointsProcessors( RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); RegisterFileWritingPointsProcessor( file_writer_factory, builder); RegisterFileWritingPointsProcessor(