Color X-Rays using the color of points. (#185)
Colorless points are considered to be black. Creating assets with intensities will give now brighter X-Rays. I plan to fix this by adding intensities besides colors into PointBatch and having a PointsProcessor that converts intensities into colors.master
Holger Rapp
GitHub
parent
92b89d12c8
commit
0fe51185be
|
|
@ -32,20 +32,18 @@ namespace cartographer {
|
|||
namespace io {
|
||||
namespace {
|
||||
|
||||
using Voxels = mapping_3d::HybridGridBase<bool>;
|
||||
struct PixelData {
|
||||
size_t num_occupied_cells_in_column = 0;
|
||||
double mean_r = 0.;
|
||||
double mean_g = 0.;
|
||||
double mean_b = 0.;
|
||||
};
|
||||
|
||||
// Takes the logarithm of each value in 'mat', clamping to 0 as smallest value.
|
||||
void TakeLogarithm(Eigen::MatrixXf* mat) {
|
||||
for (int y = 0; y < mat->rows(); ++y) {
|
||||
for (int x = 0; x < mat->cols(); ++x) {
|
||||
const float value = (*mat)(y, x);
|
||||
if (value == 0.f) {
|
||||
continue;
|
||||
}
|
||||
const float new_value = std::log(value);
|
||||
(*mat)(y, x) = new_value;
|
||||
}
|
||||
}
|
||||
using PixelDataMatrix =
|
||||
Eigen::Matrix<PixelData, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
double Mix(const double a, const double b, const double t) {
|
||||
return a * (1. - t) + t * b;
|
||||
}
|
||||
|
||||
cairo_status_t CairoWriteCallback(void* const closure,
|
||||
|
|
@ -59,19 +57,49 @@ cairo_status_t CairoWriteCallback(void* const closure,
|
|||
}
|
||||
|
||||
// Write 'mat' as a pleasing-to-look-at PNG into 'filename'
|
||||
void WritePng(const Eigen::MatrixXf& mat, FileWriter* const file_writer) {
|
||||
void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) {
|
||||
const int stride =
|
||||
cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols());
|
||||
CHECK_EQ(stride % 4, 0);
|
||||
std::vector<uint32_t> pixels(stride / 4 * mat.rows(), 0.);
|
||||
|
||||
const float max = mat.maxCoeff();
|
||||
float max = std::numeric_limits<float>::min();
|
||||
for (int y = 0; y < mat.rows(); ++y) {
|
||||
for (int x = 0; x < mat.cols(); ++x) {
|
||||
const float value = mat(y, x);
|
||||
uint8_t shade = common::RoundToInt(255.f * (1.f - value / max));
|
||||
const PixelData& cell = mat(y, x);
|
||||
if (cell.num_occupied_cells_in_column == 0.) {
|
||||
continue;
|
||||
}
|
||||
max = std::max<float>(max, std::log(cell.num_occupied_cells_in_column));
|
||||
}
|
||||
}
|
||||
|
||||
for (int y = 0; y < mat.rows(); ++y) {
|
||||
for (int x = 0; x < mat.cols(); ++x) {
|
||||
const PixelData& cell = mat(y, x);
|
||||
if (cell.num_occupied_cells_in_column == 0.) {
|
||||
pixels[y * stride / 4 + x] =
|
||||
(255 << 24) | (shade << 16) | (shade << 8) | shade;
|
||||
(255 << 24) | (255 << 16) | (255 << 8) | 255;
|
||||
continue;
|
||||
}
|
||||
|
||||
// We use a logarithmic weighting for how saturated a pixel will be. The
|
||||
// basic idea here was that walls (full height) are fully saturated, but
|
||||
// details like chairs and tables are still well visible.
|
||||
const float saturation =
|
||||
std::log(cell.num_occupied_cells_in_column) / max;
|
||||
double mean_r_in_column = (cell.mean_r / 255.);
|
||||
double mean_g_in_column = (cell.mean_g / 255.);
|
||||
double mean_b_in_column = (cell.mean_b / 255.);
|
||||
|
||||
double mix_r = Mix(1., mean_r_in_column, saturation);
|
||||
double mix_g = Mix(1., mean_g_in_column, saturation);
|
||||
double mix_b = Mix(1., mean_b_in_column, saturation);
|
||||
|
||||
const int r = common::RoundToInt(mix_r * 255.);
|
||||
const int g = common::RoundToInt(mix_g * 255.);
|
||||
const int b = common::RoundToInt(mix_b * 255.);
|
||||
pixels[y * stride / 4 + x] = (255 << 24) | (r << 16) | (g << 8) | b;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -92,40 +120,6 @@ void WritePng(const Eigen::MatrixXf& mat, FileWriter* const file_writer) {
|
|||
CHECK(file_writer->Close());
|
||||
}
|
||||
|
||||
void WriteVoxels(const Voxels& voxels, FileWriter* const file_writer) {
|
||||
Eigen::Array3i min(std::numeric_limits<int>::max(),
|
||||
std::numeric_limits<int>::max(),
|
||||
std::numeric_limits<int>::max());
|
||||
Eigen::Array3i max(std::numeric_limits<int>::min(),
|
||||
std::numeric_limits<int>::min(),
|
||||
std::numeric_limits<int>::min());
|
||||
|
||||
// Find the maximum and minimum cells.
|
||||
for (Voxels::Iterator it(voxels); !it.Done(); it.Next()) {
|
||||
const Eigen::Array3i idx = it.GetCellIndex();
|
||||
min = min.min(idx);
|
||||
max = max.max(idx);
|
||||
}
|
||||
|
||||
// Returns the (x, y) pixel of the given 'index'.
|
||||
const auto voxel_index_to_pixel = [&max, &min](const Eigen::Array3i& index) {
|
||||
// We flip the y axis, since matrices rows are counted from the top.
|
||||
return Eigen::Array2i(max[1] - index[1], max[2] - index[2]);
|
||||
};
|
||||
|
||||
// Hybrid grid uses X: forward, Y: left, Z: up.
|
||||
// For the screen we are using. X: right, Y: up
|
||||
const int xsize = max[1] - min[1] + 1;
|
||||
const int ysize = max[2] - min[2] + 1;
|
||||
Eigen::MatrixXf image = Eigen::MatrixXf::Zero(ysize, xsize);
|
||||
for (Voxels::Iterator it(voxels); !it.Done(); it.Next()) {
|
||||
const Eigen::Array2i pixel = voxel_index_to_pixel(it.GetCellIndex());
|
||||
++image(pixel.y(), pixel.x());
|
||||
}
|
||||
TakeLogarithm(&image);
|
||||
WritePng(image, file_writer);
|
||||
}
|
||||
|
||||
bool ContainedIn(
|
||||
const common::Time& time,
|
||||
const std::vector<common::Interval<common::Time>>& time_intervals) {
|
||||
|
|
@ -137,14 +131,6 @@ bool ContainedIn(
|
|||
return false;
|
||||
}
|
||||
|
||||
void Insert(const PointsBatch& batch, const transform::Rigid3f& transform,
|
||||
Voxels* voxels) {
|
||||
for (const auto& point : batch.points) {
|
||||
const Eigen::Vector3f camera_point = transform * point;
|
||||
*voxels->mutable_value(voxels->GetCellIndex(camera_point)) = true;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
XRayPointsProcessor::XRayPointsProcessor(
|
||||
|
|
@ -157,7 +143,9 @@ XRayPointsProcessor::XRayPointsProcessor(
|
|||
output_filename_(output_filename),
|
||||
transform_(transform) {
|
||||
for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
|
||||
voxels_.emplace_back(voxel_size, Eigen::Vector3f::Zero());
|
||||
aggregations_.emplace_back(Aggregation{
|
||||
mapping_3d::HybridGridBase<bool>(voxel_size, Eigen::Vector3f::Zero()),
|
||||
{}});
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -179,16 +167,79 @@ std::unique_ptr<XRayPointsProcessor> XRayPointsProcessor::FromDictionary(
|
|||
floors, dictionary->GetString("filename"), file_writer_factory, next);
|
||||
}
|
||||
|
||||
void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
|
||||
FileWriter* const file_writer) {
|
||||
Eigen::Array3i min(std::numeric_limits<int>::max(),
|
||||
std::numeric_limits<int>::max(),
|
||||
std::numeric_limits<int>::max());
|
||||
Eigen::Array3i max(std::numeric_limits<int>::min(),
|
||||
std::numeric_limits<int>::min(),
|
||||
std::numeric_limits<int>::min());
|
||||
|
||||
// Find the maximum and minimum cells.
|
||||
for (mapping_3d::HybridGridBase<bool>::Iterator it(aggregation.voxels);
|
||||
!it.Done(); it.Next()) {
|
||||
const Eigen::Array3i idx = it.GetCellIndex();
|
||||
min = min.min(idx);
|
||||
max = max.max(idx);
|
||||
}
|
||||
|
||||
// Returns the (x, y) pixel of the given 'index'.
|
||||
const auto voxel_index_to_pixel = [&max, &min](const Eigen::Array3i& index) {
|
||||
// We flip the y axis, since matrices rows are counted from the top.
|
||||
return Eigen::Array2i(max[1] - index[1], max[2] - index[2]);
|
||||
};
|
||||
|
||||
// Hybrid grid uses X: forward, Y: left, Z: up.
|
||||
// For the screen we are using. X: right, Y: up
|
||||
const int xsize = max[1] - min[1] + 1;
|
||||
const int ysize = max[2] - min[2] + 1;
|
||||
PixelDataMatrix image = PixelDataMatrix(ysize, xsize);
|
||||
for (mapping_3d::HybridGridBase<bool>::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());
|
||||
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;
|
||||
pixel_data.mean_g = column_data.sum_g / column_data.count;
|
||||
pixel_data.mean_b = column_data.sum_b / column_data.count;
|
||||
++pixel_data.num_occupied_cells_in_column;
|
||||
}
|
||||
WritePng(image, file_writer);
|
||||
}
|
||||
|
||||
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::Array3i cell_index =
|
||||
aggregation->voxels.GetCellIndex(camera_point);
|
||||
*aggregation->voxels.mutable_value(cell_index) = true;
|
||||
ColumnData& column_data =
|
||||
aggregation->column_data[std::make_pair(cell_index[1], cell_index[2])];
|
||||
const auto& color =
|
||||
batch.colors.empty() ? kDefaultColor : batch.colors.at(i);
|
||||
column_data.sum_r += color[0];
|
||||
column_data.sum_g += color[1];
|
||||
column_data.sum_b += color[2];
|
||||
++column_data.count;
|
||||
}
|
||||
}
|
||||
|
||||
void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
||||
if (floors_.empty()) {
|
||||
CHECK_EQ(voxels_.size(), 1);
|
||||
Insert(*batch, transform_, &voxels_[0]);
|
||||
CHECK_EQ(aggregations_.size(), 1);
|
||||
Insert(*batch, transform_, &aggregations_[0]);
|
||||
} else {
|
||||
for (size_t i = 0; i < floors_.size(); ++i) {
|
||||
if (!ContainedIn(batch->time, floors_[i].timespans)) {
|
||||
continue;
|
||||
}
|
||||
Insert(*batch, transform_, &voxels_[i]);
|
||||
Insert(*batch, transform_, &aggregations_[i]);
|
||||
}
|
||||
}
|
||||
next_->Process(std::move(batch));
|
||||
|
|
@ -196,13 +247,13 @@ void XRayPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
|
|||
|
||||
PointsProcessor::FlushResult XRayPointsProcessor::Flush() {
|
||||
if (floors_.empty()) {
|
||||
CHECK_EQ(voxels_.size(), 1);
|
||||
WriteVoxels(voxels_[0],
|
||||
CHECK_EQ(aggregations_.size(), 1);
|
||||
WriteVoxels(aggregations_[0],
|
||||
file_writer_factory_(output_filename_ + ".png").get());
|
||||
} else {
|
||||
for (size_t i = 0; i < floors_.size(); ++i) {
|
||||
WriteVoxels(
|
||||
voxels_[i],
|
||||
aggregations_[i],
|
||||
file_writer_factory_(output_filename_ + std::to_string(i) + ".png")
|
||||
.get());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,6 +17,9 @@
|
|||
#ifndef CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
|
||||
#define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_
|
||||
|
||||
#include <map>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/io/file_writer.h"
|
||||
#include "cartographer/io/points_processor.h"
|
||||
|
|
@ -50,6 +53,22 @@ class XRayPointsProcessor : public PointsProcessor {
|
|||
FlushResult Flush() override;
|
||||
|
||||
private:
|
||||
struct ColumnData {
|
||||
double sum_r = 0.;
|
||||
double sum_g = 0.;
|
||||
double sum_b = 0.;
|
||||
uint32_t count = 0;
|
||||
};
|
||||
|
||||
struct Aggregation {
|
||||
mapping_3d::HybridGridBase<bool> voxels;
|
||||
std::map<std::pair<int, int>, ColumnData> column_data;
|
||||
};
|
||||
|
||||
void WriteVoxels(const Aggregation& aggregation, FileWriter* const file_writer);
|
||||
void Insert(const PointsBatch& batch, const transform::Rigid3f& transform,
|
||||
Aggregation* aggregation);
|
||||
|
||||
PointsProcessor* const next_;
|
||||
const FileWriterFactory& file_writer_factory_;
|
||||
|
||||
|
|
@ -60,7 +79,7 @@ class XRayPointsProcessor : public PointsProcessor {
|
|||
const transform::Rigid3f transform_;
|
||||
|
||||
// Only has one entry if we do not separate into floors.
|
||||
std::vector<mapping_3d::HybridGridBase<bool>> voxels_;
|
||||
std::vector<Aggregation> aggregations_;
|
||||
};
|
||||
|
||||
} // namespace io
|
||||
|
|
|
|||
Loading…
Reference in New Issue