Towards a ROS map writing PointsProcessor. (#598)

- Refactor ProbabilityGridPointsProcessor for code reuse.
- Adds Image::width and height.
- Adds GetFilename for FileWriter.

Related to googlecartographer/cartographer_ros#475.
master
Holger Rapp 2017-10-19 12:30:19 +02:00 committed by GitHub
parent 0c9f2c4f65
commit 49ead6055c
5 changed files with 133 additions and 56 deletions

View File

@ -20,7 +20,7 @@ namespace cartographer {
namespace io { namespace io {
StreamFileWriter::StreamFileWriter(const string& filename) StreamFileWriter::StreamFileWriter(const string& filename)
: out_(filename, std::ios::out | std::ios::binary) {} : filename_(filename), out_(filename, std::ios::out | std::ios::binary) {}
StreamFileWriter::~StreamFileWriter() {} StreamFileWriter::~StreamFileWriter() {}
@ -49,5 +49,7 @@ bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) {
return Write(data, len); return Write(data, len);
} }
string StreamFileWriter::GetFilename() { return filename_; }
} // namespace io } // namespace io
} // namespace cartographer } // namespace cartographer

View File

@ -42,6 +42,7 @@ class FileWriter {
virtual bool Write(const char* data, size_t len) = 0; virtual bool Write(const char* data, size_t len) = 0;
virtual bool Close() = 0; virtual bool Close() = 0;
virtual string GetFilename() = 0;
}; };
// An Implementation of file using std::ofstream. // An Implementation of file using std::ofstream.
@ -54,8 +55,10 @@ class StreamFileWriter : public FileWriter {
bool Write(const char* data, size_t len) override; bool Write(const char* data, size_t len) override;
bool WriteHeader(const char* data, size_t len) override; bool WriteHeader(const char* data, size_t len) override;
bool Close() override; bool Close() override;
string GetFilename() override;
private: private:
const string filename_;
std::ofstream out_; std::ofstream out_;
}; };

View File

@ -1,3 +1,19 @@
/*
* 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_IMAGE_H_ #ifndef CARTOGRAPHER_IO_IMAGE_H_
#define CARTOGRAPHER_IO_IMAGE_H_ #define CARTOGRAPHER_IO_IMAGE_H_
@ -41,6 +57,9 @@ class Image {
// to this surface is alive. // to this surface is alive.
UniqueCairoSurfacePtr GetCairoSurface(); UniqueCairoSurfacePtr GetCairoSurface();
int width() const { return width_; }
int height() const { return height_; }
private: private:
int width_; int width_;
int height_; int height_;

View File

@ -1,3 +1,19 @@
/*
* 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/probability_grid_points_processor.h" #include "cartographer/io/probability_grid_points_processor.h"
#include "Eigen/Core" #include "Eigen/Core"
@ -12,63 +28,28 @@ namespace cartographer {
namespace io { namespace io {
namespace { namespace {
void WriteGrid( void DrawTrajectoriesIntoImage(
const mapping_2d::ProbabilityGrid& probability_grid, const mapping_2d::ProbabilityGrid& probability_grid,
const ProbabilityGridPointsProcessor::DrawTrajectories& draw_trajectories, const Eigen::Array2i& offset,
const std::vector<mapping::proto::Trajectory>& trajectories, const std::vector<mapping::proto::Trajectory>& trajectories,
FileWriter* const file_writer) { cairo_surface_t* cairo_surface) {
Eigen::Array2i offset; for (size_t i = 0; i < trajectories.size(); ++i) {
mapping_2d::CellLimits cell_limits; DrawTrajectory(trajectories[i], GetColor(i),
probability_grid.ComputeCroppedLimits(&offset, &cell_limits); [&probability_grid,
if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) { &offset](const transform::Rigid3d& pose) -> Eigen::Array2i {
LOG(WARNING) << "Not writing output: empty probability grid"; return probability_grid.limits().GetCellIndex(
return; pose.cast<float>().translation().head<2>()) -
offset;
},
cairo_surface);
} }
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>(
255 * ((probability - mapping::kMinProbability) /
(mapping::kMaxProbability - mapping::kMinProbability)));
} else {
constexpr uint8 kUnknownValue = 128;
return kUnknownValue;
}
};
Image image(cell_limits.num_x_cells, cell_limits.num_y_cells);
for (const auto& xy_index :
cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) {
const auto index = xy_index + offset;
uint8 value = compute_color_value(index);
image.SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}});
}
if (draw_trajectories ==
ProbabilityGridPointsProcessor::DrawTrajectories::kYes) {
for (size_t i = 0; i < trajectories.size(); ++i) {
DrawTrajectory(trajectories[i], GetColor(i),
[&probability_grid, &offset](
const transform::Rigid3d& pose) -> Eigen::Array2i {
return probability_grid.limits().GetCellIndex(
pose.cast<float>().translation().head<2>()) -
offset;
},
image.GetCairoSurface().get());
}
}
image.WritePng(file_writer);
CHECK(file_writer->Close());
} }
mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution) { uint8 ProbabilityToColor(float probability_from_grid) {
constexpr int kInitialProbabilityGridSize = 100; const float probability = 1.f - probability_from_grid;
Eigen::Vector2d max = return ::cartographer::common::RoundToInt(
0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones(); 255 * ((probability - mapping::kMinProbability) /
return mapping_2d::ProbabilityGrid(cartographer::mapping_2d::MapLimits( (mapping::kMaxProbability - mapping::kMinProbability)));
resolution, max,
mapping_2d::CellLimits(kInitialProbabilityGridSize,
kInitialProbabilityGridSize)));
} }
} // namespace } // namespace
@ -115,8 +96,19 @@ void ProbabilityGridPointsProcessor::Process(
} }
PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() { PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
WriteGrid(probability_grid_, draw_trajectories_, trajectories_, Eigen::Array2i offset;
file_writer_.get()); std::unique_ptr<Image> image =
DrawProbabilityGrid(probability_grid_, &offset);
if (image != nullptr) {
if (draw_trajectories_ ==
ProbabilityGridPointsProcessor::DrawTrajectories::kYes) {
DrawTrajectoriesIntoImage(probability_grid_, offset, trajectories_,
image->GetCairoSurface().get());
}
image->WritePng(file_writer_.get());
CHECK(file_writer_->Close());
}
switch (next_->Flush()) { switch (next_->Flush()) {
case FlushResult::kRestartStream: case FlushResult::kRestartStream:
LOG(FATAL) << "ProbabilityGrid generation must be configured to occur " LOG(FATAL) << "ProbabilityGrid generation must be configured to occur "
@ -128,5 +120,40 @@ PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
LOG(FATAL); LOG(FATAL);
} }
std::unique_ptr<Image> DrawProbabilityGrid(
const mapping_2d::ProbabilityGrid& probability_grid,
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 nullptr;
}
auto image = common::make_unique<Image>(cell_limits.num_x_cells,
cell_limits.num_y_cells);
for (const auto& xy_index :
cartographer::mapping_2d::XYIndexRangeIterator(cell_limits)) {
const auto index = xy_index + *offset;
constexpr uint8 kUnknownValue = 128;
const uint8 value =
probability_grid.IsKnown(index)
? ProbabilityToColor(probability_grid.GetProbability(index))
: kUnknownValue;
;
image->SetPixel(xy_index.x(), xy_index.y(), {{value, value, value}});
}
return image;
}
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 io } // namespace io
} // namespace cartographer } // namespace cartographer

View File

@ -1,10 +1,26 @@
/*
* Copyright 2016 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_PROBABILITY_GRID_POINTS_PROCESSOR_H_ #ifndef CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_
#define CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_
#include <memory> #include <memory>
#include <string>
#include "cartographer/io/file_writer.h" #include "cartographer/io/file_writer.h"
#include "cartographer/io/image.h"
#include "cartographer/io/points_batch.h" #include "cartographer/io/points_batch.h"
#include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor.h"
#include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/mapping/proto/trajectory.pb.h"
@ -56,6 +72,16 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
mapping_2d::ProbabilityGrid probability_grid_; mapping_2d::ProbabilityGrid probability_grid_;
}; };
// Draws 'probability_grid' into an image and fills in 'offset' with the cropped
// map limits. Returns 'nullptr' if probability_grid was empty.
std::unique_ptr<Image> DrawProbabilityGrid(
const mapping_2d::ProbabilityGrid& probability_grid,
Eigen::Array2i* offset);
// Create an initially empty probability grid with 'resolution' and a small
// size, suitable for a PointsBatchProcessor.
mapping_2d::ProbabilityGrid CreateProbabilityGrid(const double resolution);
} // namespace io } // namespace io
} // namespace cartographer } // namespace cartographer