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

Implements a ProbabilityGridPointsProcessor (#383)

master
damienrg 2017-07-10 11:02:14 +02:00 committed by Holger Rapp
parent 78bd37ec26
commit 39cb8401a5
7 changed files with 278 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
cartographer/io/cairo_types.h
View File

@ -1,29 +0,0 @@
#ifndef CARTOGRAPHER_IO_CAIRO_TYPES_H_
#define CARTOGRAPHER_IO_CAIRO_TYPES_H_
#include <memory>
#include "cairo/cairo.h"
namespace cartographer {
namespace io {
namespace cairo {
// 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_surface_t, void (*)(cairo_surface_t*)>;
// std::unique_ptr for Cairo contexts. The context is destroyed when the
// std::unique_ptr is reset or destroyed.
using UniqueContextPtr = std::unique_ptr<cairo_t, void (*)(cairo_t*)>;
// std::unique_ptr for Cairo paths. The path is destroyed when the
// std::unique_ptr is reset or destroyed.
using UniquePathPtr = std::unique_ptr<cairo_path_t, void (*)(cairo_path_t*)>;
} // namespace cairo
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_CAIRO_TYPES_H_

72
cartographer/io/image.cc Normal file
View File

@ -0,0 +1,72 @@
#include "cartographer/io/image.h"
#include <memory>
#include "cairo/cairo.h"
#include "cartographer/io/file_writer.h"
#include "glog/logging.h"
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_surface_t, void (*)(cairo_surface_t*)>;
cairo_status_t CairoWriteCallback(void* const closure,
const unsigned char* data,
const unsigned int length) {
if (static_cast<FileWriter*>(closure)->Write(
reinterpret_cast<const char*>(data), length)) {
return CAIRO_STATUS_SUCCESS;
}
return CAIRO_STATUS_WRITE_ERROR;
}
constexpr cairo_format_t kCairoFormat = CAIRO_FORMAT_ARGB32;
int StrideForWidth(int width) {
const int stride = cairo_format_stride_for_width(kCairoFormat, width);
CHECK_EQ(stride % 4, 0);
return stride;
}
} // namespace
Image::Image(int width, int height)
: width_(width),
height_(height),
stride_(StrideForWidth(width)),
pixels_(stride_ / 4 * height, 0) {}
void Image::WritePng(FileWriter* const file_writer) {
// TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of
// the data until the surface is finalized. Once it is finalized though,
// 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<unsigned char*>(pixels_.data()),
kCairoFormat, width_, height_, stride_),
cairo_surface_destroy);
CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS);
CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback,
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)}};
}
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];
}
} // namespace io
} // namespace cartographer

31
cartographer/io/image.h Normal file
View File

@ -0,0 +1,31 @@
#ifndef CARTOGRAPHER_IO_IMAGE_H_
#define CARTOGRAPHER_IO_IMAGE_H_
#include <cstdint>
#include <vector>
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_batch.h"
namespace cartographer {
namespace io {
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);
void WritePng(FileWriter* const file_writer);
private:
int width_;
int height_;
int stride_;
std::vector<uint32_t> pixels_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_IMAGE_H_

3
cartographer/io/points_processor_pipeline_builder.cc
View File

@ -27,6 +27,7 @@
#include "cartographer/io/outlier_removing_points_processor.h"
#include "cartographer/io/pcd_writing_points_processor.h"
#include "cartographer/io/ply_writing_points_processor.h"
#include "cartographer/io/probability_grid_points_processor.h"
#include "cartographer/io/xray_points_processor.h"
#include "cartographer/io/xyz_writing_points_processor.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
@ -77,6 +78,8 @@ void RegisterBuiltInPointsProcessors(
file_writer_factory, builder);
RegisterFileWritingPointsProcessor<HybridGridPointsProcessor>(
file_writer_factory, builder);
RegisterFileWritingPointsProcessor<ProbabilityGridPointsProcessor>(
file_writer_factory, builder);
// X-Ray is an odd ball since it requires the trajectory to figure out the
// different building levels we walked on to separate the images.

108
cartographer/io/probability_grid_points_processor.cc Normal file
View File

@ -0,0 +1,108 @@
#include "cartographer/io/probability_grid_points_processor.h"
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/io/image.h"
#include "cartographer/io/points_batch.h"
namespace cartographer {
namespace io {
namespace {
void WriteGrid(const mapping_2d::ProbabilityGrid& probability_grid,
FileWriter* const file_writer) {
Eigen::Array2i offset;
mapping_2d::CellLimits cell_limits;
probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
LOG(WARNING) << "Not writing output: empty probability grid";
return;
}
const auto grid_index_to_pixel = [cell_limits](const Eigen::Array2i& index) {
return Eigen::Array2i(cell_limits.num_y_cells - index(1) - 1,
cell_limits.num_x_cells - index(0) - 1);
};
const auto compute_color_value = [&probability_grid](
const Eigen::Array2i& index) {
if (probability_grid.IsKnown(index)) {
const float probability = 1.f - probability_grid.GetProbability(index);
return static_cast<uint8_t>(
255 * ((probability - mapping::kMinProbability) /
(mapping::kMaxProbability - mapping::kMinProbability)));
} else {
constexpr uint8_t kUnknownValue = 128;
return kUnknownValue;
}
};
int width = cell_limits.num_y_cells;
int height = cell_limits.num_x_cells;
Image image(width, height);
for (auto xy_index :
cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) {
auto index = xy_index + offset;
uint8 value = compute_color_value(index);
const Eigen::Array2i pixel = grid_index_to_pixel(xy_index);
image.SetPixel(pixel.x(), pixel.y(), {{value, value, value}});
}
image.WritePng(file_writer);
CHECK(file_writer->Close());
}
mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution) {
constexpr int kInitialProbabilityGridSize = 100;
Eigen::Vector2d max =
0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones();
return mapping_2d::ProbabilityGrid(cartographer::mapping_2d::MapLimits(
resolution, max,
mapping_2d::CellLimits(kInitialProbabilityGridSize,
kInitialProbabilityGridSize)));
}
} // namespace
ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
const double resolution,
const mapping_2d::proto::RangeDataInserterOptions&
range_data_inserter_options,
std::unique_ptr<FileWriter> file_writer, PointsProcessor* const next)
: next_(next),
file_writer_(std::move(file_writer)),
range_data_inserter_(range_data_inserter_options),
probability_grid_(CreateProbabilityGrid(resolution)) {}
std::unique_ptr<ProbabilityGridPointsProcessor>
ProbabilityGridPointsProcessor::FromDictionary(
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* const dictionary,
PointsProcessor* const next) {
return common::make_unique<ProbabilityGridPointsProcessor>(
dictionary->GetDouble("resolution"),
mapping_2d::CreateRangeDataInserterOptions(
dictionary->GetDictionary("range_data_inserter").get()),
file_writer_factory(dictionary->GetString("filename") + ".png"), next);
}
void ProbabilityGridPointsProcessor::Process(
std::unique_ptr<PointsBatch> batch) {
range_data_inserter_.Insert({batch->origin, batch->points, {}},
&probability_grid_);
next_->Process(std::move(batch));
}
PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
WriteGrid(probability_grid_, file_writer_.get());
switch (next_->Flush()) {
case FlushResult::kRestartStream:
LOG(FATAL) << "ProbabilityGrid generation must be configured to occur "
"after any stages that require multiple passes.";
case FlushResult::kFinished:
return FlushResult::kFinished;
}
LOG(FATAL);
}
} // namespace io
} // namespace cartographer

54
cartographer/io/probability_grid_points_processor.h Normal file
View File

@ -0,0 +1,54 @@
#ifndef CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_
#include <memory>
#include <string>
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_batch.h"
#include "cartographer/io/points_processor.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
namespace cartographer {
namespace io {
// Creates a probability grid with the specified 'resolution'. As all points are
// projected into the x-y plane the z component of the data is ignored.
// 'range_data_inserter' options are used to configure the range data ray
// tracing through the probability grid.
class ProbabilityGridPointsProcessor : public PointsProcessor {
public:
constexpr static const char* kConfigurationFileActionName =
"write_probability_grid";
ProbabilityGridPointsProcessor(
double resolution,
const mapping_2d::proto::RangeDataInserterOptions&
range_data_inserter_options,
std::unique_ptr<FileWriter> file_writer, PointsProcessor* next);
ProbabilityGridPointsProcessor(const ProbabilityGridPointsProcessor&) =
delete;
ProbabilityGridPointsProcessor& operator=(
const ProbabilityGridPointsProcessor&) = delete;
static std::unique_ptr<ProbabilityGridPointsProcessor> FromDictionary(
FileWriterFactory file_writer_factory,
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~ProbabilityGridPointsProcessor() override {}
void Process(std::unique_ptr<PointsBatch> batch) override;
FlushResult Flush() override;
private:
PointsProcessor* const next_;
std::unique_ptr<FileWriter> file_writer_;
mapping_2d::RangeDataInserter range_data_inserter_;
mapping_2d::ProbabilityGrid probability_grid_;
};
} // namespace io
} // namespace cartographer
#endif // CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_

48
cartographer/io/xray_points_processor.cc
View File

@ -20,11 +20,10 @@
#include <string>
#include "Eigen/Core"
#include "cairo/cairo.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/io/cairo_types.h"
#include "cartographer/io/image.h"
#include "cartographer/mapping/detect_floors.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
@ -46,22 +45,9 @@ double Mix(const double a, const double b, const double t) {
return a * (1. - t) + t * b;
}
cairo_status_t CairoWriteCallback(void* const closure,
const unsigned char* data,
const unsigned int length) {
if (static_cast<FileWriter*>(closure)->Write(
reinterpret_cast<const char*>(data), length)) {
return CAIRO_STATUS_SUCCESS;
}
return CAIRO_STATUS_WRITE_ERROR;
}
// Write 'mat' as a pleasing-to-look-at PNG into 'filename'
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.);
void WriteImage(const PixelDataMatrix& mat, FileWriter* const file_writer) {
Image image(mat.cols(), mat.rows());
float max = std::numeric_limits<float>::min();
for (int y = 0; y < mat.rows(); ++y) {
@ -78,8 +64,7 @@ void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) {
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) | (255 << 16) | (255 << 8) | 255;
image.SetPixel(x, y, {{255, 255, 255}});
continue;
}
@ -96,27 +81,14 @@ void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) {
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;
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}});
}
}
// TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of
// the data until the surface is finalized. Once it is finalized though,
// 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.
cairo::UniqueSurfacePtr surface(
cairo_image_surface_create_for_data(
reinterpret_cast<unsigned char*>(pixels.data()), CAIRO_FORMAT_ARGB32,
mat.cols(), mat.rows(), stride),
cairo_surface_destroy);
CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS);
CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback,
file_writer),
CAIRO_STATUS_SUCCESS);
image.WritePng(file_writer);
CHECK(file_writer->Close());
}
@ -196,7 +168,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation,
pixel_data.mean_b = column_data.sum_b / column_data.count;
++pixel_data.num_occupied_cells_in_column;
}
WritePng(image, file_writer);
WriteImage(image, file_writer);
}
void XRayPointsProcessor::Insert(const PointsBatch& batch,