Skip to content

Commit

Permalink
Color X-Rays using the color of points. (#185)
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@SirVer
SirVer committed Jan 23, 2017
1 parent 92b89d1 commit 0fe5118
Show file tree
Hide file tree
Showing 2 changed files with 140 additions and 70 deletions.
189 changes: 120 additions & 69 deletions cartographer/io/xray_points_processor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,20 +32,18 @@ namespace cartographer {
namespace io {
namespace {

using Voxels = mapping_3d::HybridGridBase<bool>;

// 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;
}
}
struct PixelData {
size_t num_occupied_cells_in_column = 0;
double mean_r = 0.;
double mean_g = 0.;
double mean_b = 0.;
};

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,
Expand All @@ -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 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 float value = mat(y, x);
uint8_t shade = common::RoundToInt(255.f * (1.f - value / max));
pixels[y * stride / 4 + x] =
(255 << 24) | (shade << 16) | (shade << 8) | shade;
const PixelData& cell = mat(y, x);
if (cell.num_occupied_cells_in_column == 0.) {
pixels[y * stride / 4 + x] =
(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;
}
}

Expand All @@ -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) {
Expand All @@ -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(
Expand All @@ -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()),
{}});
}
}

Expand All @@ -179,30 +167,93 @@ 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));
}

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());
}
Expand Down
21 changes: 20 additions & 1 deletion cartographer/io/xray_points_processor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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_;

Expand All @@ -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
Expand Down

0 comments on commit 0fe5118

Please sign in to comment.