12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Change Color to Uint8Color and FloatColor (#423) 

As almost all computations for color are done in float, use FloatColor
format by default and convert to Uint8Color only when needed.
master
damienrg 2017-07-27 11:03:55 +02:00 committed by Wolfgang Hess
parent 6035f63860
commit 46c7ce886d
14 changed files with 81 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
cartographer/io/color.cc
View File

@ -31,13 +31,7 @@ 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<uint8_t>(common::RoundToInt(r * 255.)),
static_cast<uint8_t>(common::RoundToInt(g * 255.)),
static_cast<uint8_t>(common::RoundToInt(b * 255.))}};
}
Color HsvToRgb(const float h, const float s, const float v) {
FloatColor 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;
@ -47,25 +41,25 @@ Color HsvToRgb(const float h, const float s, const float v) {
const float t = v * (1.f - (1.f - f) * s);
if (h_i == 0) {
return CreateRgba(v, t, p);
return {{v, t, p}};
} else if (h_i == 1) {
return CreateRgba(q, v, p);
return {{q, v, p}};
} else if (h_i == 2) {
return CreateRgba(p, v, t);
return {{p, v, t}};
} else if (h_i == 3) {
return CreateRgba(p, q, v);
return {{p, q, v}};
} else if (h_i == 4) {
return CreateRgba(t, p, v);
return {{t, p, v}};
} else if (h_i == 5) {
return CreateRgba(v, p, q);
return {{v, p, q}};
} else {
return CreateRgba(0.f, 0.f, 0.f);
return {{0.f, 0.f, 0.f}};
}
}
} // namespace
Color GetColor(int id) {
FloatColor 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/

26
cartographer/io/color.h
View File

@ -19,15 +19,35 @@
#include <array>
#include "cartographer/common/math.h"
#include "cartographer/common/port.h"
namespace cartographer {
namespace io {
// TODO(hrapp): Should probably be represented as floats.
using Color = std::array<uint8_t, 3>;
using Uint8Color = std::array<uint8, 3>;
using FloatColor = std::array<float, 3>;
// A function for on-demand generation of a color palette, with every two
// direct successors having large contrast.
Color GetColor(int id);
FloatColor GetColor(int id);
inline uint8 FloatComponentToUint8(float c) {
return static_cast<uint8>(cartographer::common::RoundToInt(
cartographer::common::Clamp(c, 0.f, 1.f) * 255));
}
inline float Uint8ComponentToFloat(uint8 c) { return c / 255.f; }
inline Uint8Color ToUint8Color(const FloatColor& color) {
return {{FloatComponentToUint8(color[0]), FloatComponentToUint8(color[1]),
FloatComponentToUint8(color[2])}};
}
inline FloatColor ToFloatColor(const Uint8Color& color) {
return {{Uint8ComponentToFloat(color[0]), Uint8ComponentToFloat(color[1]),
Uint8ComponentToFloat(color[2])}};
}
} // namespace io
} // namespace cartographer

11
cartographer/io/coloring_points_processor.cc
View File

@ -30,13 +30,14 @@ ColoringPointsProcessor::FromDictionary(
const string frame_id = dictionary->GetString("frame_id");
const std::vector<double> color_values =
dictionary->GetDictionary("color")->GetArrayValuesAsDoubles();
const Color color = {{static_cast<uint8_t>(color_values[0]),
static_cast<uint8_t>(color_values[1]),
static_cast<uint8_t>(color_values[2])}};
return common::make_unique<ColoringPointsProcessor>(color, frame_id, next);
const Uint8Color color = {{static_cast<uint8>(color_values[0]),
static_cast<uint8>(color_values[1]),
static_cast<uint8>(color_values[2])}};
return common::make_unique<ColoringPointsProcessor>(ToFloatColor(color),
frame_id, next);
}
ColoringPointsProcessor::ColoringPointsProcessor(const Color& color,
ColoringPointsProcessor::ColoringPointsProcessor(const FloatColor& color,
const string& frame_id,
PointsProcessor* const next)
: color_(color), frame_id_(frame_id), next_(next) {}

4
cartographer/io/coloring_points_processor.h
View File

@ -31,7 +31,7 @@ class ColoringPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName = "color_points";
ColoringPointsProcessor(const Color& color, const string& frame_id,
ColoringPointsProcessor(const FloatColor& color, const string& frame_id,
PointsProcessor* next);
static std::unique_ptr<ColoringPointsProcessor> FromDictionary(
@ -46,7 +46,7 @@ class ColoringPointsProcessor : public PointsProcessor {
FlushResult Flush() override;
private:
const Color color_;
const FloatColor color_;
const string frame_id_;
PointsProcessor* const next_;
};

5
cartographer/io/draw_trajectories.cc
View File

@ -23,7 +23,7 @@ namespace cartographer {
namespace io {
void DrawTrajectory(const mapping::proto::Trajectory& trajectory,
const Color& color, PoseToPixelFunction pose_to_pixel,
const FloatColor& color, PoseToPixelFunction pose_to_pixel,
cairo_surface_t* surface) {
if (trajectory.node_size() == 0) {
return;
@ -34,8 +34,7 @@ void DrawTrajectory(const mapping::proto::Trajectory& trajectory,
auto cr = ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface));
cairo_set_source_rgba(cr.get(), color[0] / 255., color[1] / 255.,
color[2] / 255., kAlpha);
cairo_set_source_rgba(cr.get(), color[0], color[1], color[2], kAlpha);
cairo_set_line_width(cr.get(), kTrajectoryWidth);
for (const auto& node : trajectory.node()) {

2
cartographer/io/draw_trajectories.h
View File

@ -32,7 +32,7 @@ using PoseToPixelFunction =
// 'pose_to_pixel' function must translate a trajectory node's position into the
// pixel on 'surface'.
void DrawTrajectory(const mapping::proto::Trajectory& trajectory,
const Color& color, PoseToPixelFunction pose_to_pixel,
const FloatColor& color, PoseToPixelFunction pose_to_pixel,
cairo_surface_t* surface);
} // namespace io

23
cartographer/io/image.cc
View File

@ -9,6 +9,18 @@ namespace cartographer {
namespace io {
namespace {
uint32 Uint8ColorToCairo(const Uint8Color& color) {
return static_cast<uint32>(255) << 24 | static_cast<uint32>(color[0]) << 16 |
static_cast<uint32>(color[1]) << 8 | color[2];
}
Uint8Color CairoToUint8Color(uint32 color) {
uint8 r = color >> 16;
uint8 g = color >> 8;
uint8 b = color;
return {{r, g, b}};
}
cairo_status_t CairoWriteCallback(void* const closure,
const unsigned char* data,
const unsigned int length) {
@ -56,15 +68,12 @@ void Image::WritePng(FileWriter* const file_writer) {
CAIRO_STATUS_SUCCESS);
}
const Color Image::GetPixel(int x, int y) const {
const uint32_t value = pixels_[y * stride_ / 4 + x];
return {{static_cast<uint8_t>(value >> 16), static_cast<uint8_t>(value >> 8),
static_cast<uint8_t>(value)}};
const Uint8Color Image::GetPixel(int x, int y) const {
return CairoToUint8Color(pixels_[y * stride_ / 4 + x]);
}
void Image::SetPixel(int x, int y, const Color& color) {
pixels_[y * stride_ / 4 + x] =
(255 << 24) | (color[0] << 16) | (color[1] << 8) | color[2];
void Image::SetPixel(int x, int y, const Uint8Color& color) {
pixels_[y * stride_ / 4 + x] = Uint8ColorToCairo(color);
}
UniqueCairoSurfacePtr Image::GetCairoSurface() {

8
cartographer/io/image.h
View File

@ -5,6 +5,8 @@
#include <vector>
#include "cairo/cairo.h"
#include "cartographer/common/port.h"
#include "cartographer/io/color.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_batch.h"
@ -29,8 +31,8 @@ class Image {
public:
Image(int width, int height);
const Color GetPixel(int x, int y) const;
void SetPixel(int x, int y, const Color& color);
const Uint8Color GetPixel(int x, int y) const;
void SetPixel(int x, int y, const Uint8Color& color);
void WritePng(FileWriter* const file_writer);
// Returns a pointer to a cairo surface that contains the current pixel data.
@ -43,7 +45,7 @@ class Image {
int width_;
int height_;
int stride_;
std::vector<uint32_t> pixels_;
std::vector<uint32> pixels_;
};
} // namespace io

10
cartographer/io/intensity_to_color_points_processor.cc
View File

@ -50,12 +50,10 @@ void IntensityToColorPointsProcessor::Process(
(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}});
const float gray = cartographer::common::Clamp(
(intensity - min_intensity_) / (max_intensity_ - min_intensity_), 0.f,
1.f);
batch->colors.push_back({{gray, gray, gray}});
}
}
next_->Process(std::move(batch));

5
cartographer/io/pcd_writing_points_processor.cc
View File

@ -65,7 +65,7 @@ void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point,
CHECK(file_writer->Write(buffer, 12));
}
void WriteBinaryPcdPointColor(const Color& color,
void WriteBinaryPcdPointColor(const Uint8Color& color,
FileWriter* const file_writer) {
char buffer[4];
buffer[0] = color[2];
@ -121,7 +121,8 @@ void PcdWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
for (size_t i = 0; i < batch->points.size(); ++i) {
WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get());
if (!batch->colors.empty()) {
WriteBinaryPcdPointColor(batch->colors[i], file_writer_.get());
WriteBinaryPcdPointColor(ToUint8Color(batch->colors[i]),
file_writer_.get());
}
++num_points_;
}

4
cartographer/io/ply_writing_points_processor.cc
View File

@ -61,7 +61,7 @@ void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point,
CHECK(file_writer->Write(buffer, 12));
}
void WriteBinaryPlyPointColor(const Color& color,
void WriteBinaryPlyPointColor(const Uint8Color& color,
FileWriter* const file_writer) {
CHECK(file_writer->Write(reinterpret_cast<const char*>(color.data()),
color.size()));
@ -120,7 +120,7 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
for (size_t i = 0; i < batch->points.size(); ++i) {
WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
if (has_colors_) {
WriteBinaryPlyPointColor(batch->colors[i], file_.get());
WriteBinaryPlyPointColor(ToUint8Color(batch->colors[i]), file_.get());
}
++num_points_;
}

2
cartographer/io/points_batch.h
View File

@ -61,7 +61,7 @@ struct PointsBatch {
std::vector<float> intensities;
// Colors are optional. If set, they are RGB values.
std::vector<Color> colors;
std::vector<FloatColor> colors;
};
// Removes the indices in 'to_remove' from 'batch'.

4
cartographer/io/probability_grid_points_processor.cc
View File

@ -28,11 +28,11 @@ void WriteGrid(
const Eigen::Array2i& index) {
if (probability_grid.IsKnown(index)) {
const float probability = 1.f - probability_grid.GetProbability(index);
return static_cast<uint8_t>(
return static_cast<uint8>(
255 * ((probability - mapping::kMinProbability) /
(mapping::kMaxProbability - mapping::kMinProbability)));
} else {
constexpr uint8_t kUnknownValue = 128;
constexpr uint8 kUnknownValue = 128;
return kUnknownValue;
}
};

10
cartographer/io/xray_points_processor.cc
View File

@ -82,10 +82,10 @@ Image IntoImage(const PixelDataMatrix& mat) {
double mix_g = Mix(1., mean_g_in_column, saturation);
double mix_b = Mix(1., mean_b_in_column, saturation);
const uint8_t r = common::RoundToInt(mix_r * 255.);
const uint8_t g = common::RoundToInt(mix_g * 255.);
const uint8_t b = common::RoundToInt(mix_b * 255.);
image.SetPixel(x, y, {{r, g, b}});
image.SetPixel(
x, y,
{{FloatComponentToUint8(mix_r), FloatComponentToUint8(mix_g),
FloatComponentToUint8(mix_b)}});
}
}
return image;
@ -200,7 +200,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
void XRayPointsProcessor::Insert(const PointsBatch& batch,
Aggregation* const aggregation) {
constexpr Color kDefaultColor = {{0, 0, 0}};
constexpr FloatColor kDefaultColor = {{0.f, 0.f, 0.f}};
for (size_t i = 0; i < batch.points.size(); ++i) {
const Eigen::Vector3f camera_point = transform_ * batch.points[i];
const Eigen::Array3i cell_index =