diff --git a/cartographer/io/color.cc b/cartographer/io/color.cc new file mode 100644 index 0000000..51ea40a --- /dev/null +++ b/cartographer/io/color.cc @@ -0,0 +1,77 @@ +/* + * Copyright 2017 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/color.h" + +#include + +#include "cartographer/common/port.h" +#include "glog/logging.h" + +namespace cartographer { +namespace io { + +namespace { + +constexpr float kInitialHue = 0.69f; +constexpr float kSaturation = 0.85f; +constexpr float kValue = 0.77f; +constexpr float kGoldenRatioConjugate = (std::sqrt(5.f) - 1.f) / 2.f; + +Color CreateRgba(const float r, const float g, const float b) { + return Color{{static_cast(common::RoundToInt(r * 255.)), + static_cast(common::RoundToInt(g * 255.)), + static_cast(common::RoundToInt(b * 255.))}}; +} + +Color HsvToRgb(const float h, const float s, const float v) { + const float h_6 = (h == 1.f) ? 0.f : 6 * h; + const int h_i = std::floor(h_6); + const float f = h_6 - h_i; + + const float p = v * (1.f - s); + const float q = v * (1.f - f * s); + const float t = v * (1.f - (1.f - f) * s); + + if (h_i == 0) { + return CreateRgba(v, t, p); + } else if (h_i == 1) { + return CreateRgba(q, v, p); + } else if (h_i == 2) { + return CreateRgba(p, v, t); + } else if (h_i == 3) { + return CreateRgba(p, q, v); + } else if (h_i == 4) { + return CreateRgba(t, p, v); + } else if (h_i == 5) { + return CreateRgba(v, p, q); + } else { + return CreateRgba(0.f, 0.f, 0.f); + } +} + +} // namespace + +Color GetColor(int id) { + CHECK_GE(id, 0); + // Uniform color sampling using the golden ratio from + // http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/ + const float hue = std::fmod(kInitialHue + kGoldenRatioConjugate * id, 1.f); + return HsvToRgb(hue, kSaturation, kValue); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/color.h b/cartographer/io/color.h new file mode 100644 index 0000000..a06e225 --- /dev/null +++ b/cartographer/io/color.h @@ -0,0 +1,35 @@ +/* + * Copyright 2017 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_COLOR_H_ +#define CARTOGRAPHER_IO_COLOR_H_ + +#include + +namespace cartographer { +namespace io { + +// TODO(hrapp): Should probably be represented as floats. +using Color = std::array; + +// A function for on-demand generation of a color palette, with every two +// direct successors having large contrast. +Color GetColor(int id); + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_COLOR_H_ diff --git a/cartographer/io/image.cc b/cartographer/io/image.cc index 24b9aa7..e3cfc51 100644 --- a/cartographer/io/image.cc +++ b/cartographer/io/image.cc @@ -2,7 +2,6 @@ #include -#include "cairo/cairo.h" #include "cartographer/io/file_writer.h" #include "glog/logging.h" @@ -10,11 +9,6 @@ namespace cartographer { namespace io { namespace { -// std::unique_ptr for Cairo surfaces. The surface is destroyed when the -// std::unique_ptr is reset or destroyed. -using UniqueSurfacePtr = - std::unique_ptr; - cairo_status_t CairoWriteCallback(void* const closure, const unsigned char* data, const unsigned int length) { @@ -35,6 +29,14 @@ int StrideForWidth(int width) { } // namespace +UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface) { + return UniqueCairoSurfacePtr(surface, cairo_surface_destroy); +} + +UniqueCairoPtr MakeUniqueCairoPtr(cairo_t* surface) { + return UniqueCairoPtr(surface, cairo_destroy); +} + Image::Image(int width, int height) : width_(width), height_(height), @@ -47,10 +49,7 @@ void Image::WritePng(FileWriter* const file_writer) { // cairo_surface_write_to_png fails, complaining that the surface is already // finalized. This makes it pretty hard to pass back ownership of the image to // the caller. - UniqueSurfacePtr surface(cairo_image_surface_create_for_data( - reinterpret_cast(pixels_.data()), - kCairoFormat, width_, height_, stride_), - cairo_surface_destroy); + UniqueCairoSurfacePtr surface = GetCairoSurface(); CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS); CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback, file_writer), @@ -68,5 +67,11 @@ void Image::SetPixel(int x, int y, const Color& color) { (255 << 24) | (color[0] << 16) | (color[1] << 8) | color[2]; } +UniqueCairoSurfacePtr Image::GetCairoSurface() { + return MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data( + reinterpret_cast(pixels_.data()), kCairoFormat, width_, + height_, stride_)); +} + } // namespace io } // namespace cartographer diff --git a/cartographer/io/image.h b/cartographer/io/image.h index b000c9c..a3287a5 100644 --- a/cartographer/io/image.h +++ b/cartographer/io/image.h @@ -4,12 +4,27 @@ #include #include +#include "cairo/cairo.h" #include "cartographer/io/file_writer.h" #include "cartographer/io/points_batch.h" namespace cartographer { namespace io { +// std::unique_ptr for Cairo surfaces. The surface is destroyed when the +// std::unique_ptr is reset or destroyed. +using UniqueCairoSurfacePtr = + std::unique_ptr; + +// Takes ownership. +UniqueCairoSurfacePtr MakeUniqueCairoSurfacePtr(cairo_surface_t* surface); + +// std::unique_ptr for Cairo contexts. +using UniqueCairoPtr = std::unique_ptr; + +// Takes ownership. +UniqueCairoPtr MakeUniqueCairoPtr(cairo_t* surface); + class Image { public: Image(int width, int height); @@ -19,6 +34,12 @@ class Image { void WritePng(FileWriter* const file_writer); private: + // Returns a pointer to a cairo surface that contains the current pixel data. + // The 'Image' object must therefore outlive the returned surface object. It + // is undefined behavior to call any of the mutating functions while a pointer + // to this surface is alive. + UniqueCairoSurfacePtr GetCairoSurface(); + int width_; int height_; int stride_; diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h index f8c8c70..6303c3e 100644 --- a/cartographer/io/points_batch.h +++ b/cartographer/io/points_batch.h @@ -23,13 +23,11 @@ #include "Eigen/Core" #include "cartographer/common/time.h" +#include "cartographer/io/color.h" namespace cartographer { namespace io { -// A point's color. -using Color = std::array; - // A number of points, captured around the same 'time' and by a // sensor at the same 'origin'. struct PointsBatch { diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index 2bd089c..0d7d417 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -26,6 +26,7 @@ #include "cartographer/io/image.h" #include "cartographer/mapping/detect_floors.h" #include "cartographer/mapping_3d/hybrid_grid.h" +#include "cartographer/transform/transform.h" namespace cartographer { namespace io { @@ -46,9 +47,8 @@ double Mix(const double a, const double b, const double t) { } // Write 'mat' as a pleasing-to-look-at PNG into 'filename' -void WriteImage(const PixelDataMatrix& mat, FileWriter* const file_writer) { +Image IntoImage(const PixelDataMatrix& mat) { Image image(mat.cols(), mat.rows()); - float max = std::numeric_limits::min(); for (int y = 0; y < mat.rows(); ++y) { for (int x = 0; x < mat.cols(); ++x) { @@ -87,9 +87,7 @@ void WriteImage(const PixelDataMatrix& mat, FileWriter* const file_writer) { image.SetPixel(x, y, {{r, g, b}}); } } - - image.WritePng(file_writer); - CHECK(file_writer->Close()); + return image; } bool ContainedIn(const common::Time& time, @@ -155,12 +153,12 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, // For the screen we are using. X: right, Y: up const int xsize = bounding_box_.sizes()[1] + 1; const int ysize = bounding_box_.sizes()[2] + 1; - PixelDataMatrix image = PixelDataMatrix(ysize, xsize); + PixelDataMatrix pixel_data_matrix = PixelDataMatrix(ysize, xsize); for (mapping_3d::HybridGridBase::Iterator it(aggregation.voxels); !it.Done(); it.Next()) { const Eigen::Array3i cell_index = it.GetCellIndex(); const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index); - PixelData& pixel_data = image(pixel.y(), pixel.x()); + PixelData& pixel_data = pixel_data_matrix(pixel.y(), pixel.x()); const auto& column_data = aggregation.column_data.at( std::make_pair(cell_index[1], cell_index[2])); pixel_data.mean_r = column_data.sum_r / column_data.count; @@ -168,15 +166,19 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, pixel_data.mean_b = column_data.sum_b / column_data.count; ++pixel_data.num_occupied_cells_in_column; } - WriteImage(image, file_writer); + Image image = IntoImage(pixel_data_matrix); + + // TODO(hrapp): Draw trajectories here. + + image.WritePng(file_writer); + CHECK(file_writer->Close()); } void XRayPointsProcessor::Insert(const PointsBatch& batch, - const transform::Rigid3f& transform, Aggregation* const aggregation) { constexpr Color kDefaultColor = {{0, 0, 0}}; for (size_t i = 0; i < batch.points.size(); ++i) { - const Eigen::Vector3f camera_point = transform * batch.points[i]; + const Eigen::Vector3f camera_point = transform_ * batch.points[i]; const Eigen::Array3i cell_index = aggregation->voxels.GetCellIndex(camera_point); *aggregation->voxels.mutable_value(cell_index) = true; @@ -195,13 +197,13 @@ void XRayPointsProcessor::Insert(const PointsBatch& batch, void XRayPointsProcessor::Process(std::unique_ptr batch) { if (floors_.empty()) { CHECK_EQ(aggregations_.size(), 1); - Insert(*batch, transform_, &aggregations_[0]); + Insert(*batch, &aggregations_[0]); } else { for (size_t i = 0; i < floors_.size(); ++i) { if (!ContainedIn(batch->time, floors_[i].timespans)) { continue; } - Insert(*batch, transform_, &aggregations_[i]); + Insert(*batch, &aggregations_[i]); } } next_->Process(std::move(batch)); diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h index a53a9b6..4517ef3 100644 --- a/cartographer/io/xray_points_processor.h +++ b/cartographer/io/xray_points_processor.h @@ -69,8 +69,7 @@ class XRayPointsProcessor : public PointsProcessor { void WriteVoxels(const Aggregation& aggregation, FileWriter* const file_writer); - void Insert(const PointsBatch& batch, const transform::Rigid3f& transform, - Aggregation* aggregation); + void Insert(const PointsBatch& batch, Aggregation* aggregation); PointsProcessor* const next_; FileWriterFactory file_writer_factory_;