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
parent
0c9f2c4f65
commit
49ead6055c
|
@ -20,7 +20,7 @@ namespace cartographer {
|
|||
namespace io {
|
||||
|
||||
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() {}
|
||||
|
||||
|
@ -49,5 +49,7 @@ bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) {
|
|||
return Write(data, len);
|
||||
}
|
||||
|
||||
string StreamFileWriter::GetFilename() { return filename_; }
|
||||
|
||||
} // namespace io
|
||||
} // namespace cartographer
|
||||
|
|
|
@ -42,6 +42,7 @@ class FileWriter {
|
|||
|
||||
virtual bool Write(const char* data, size_t len) = 0;
|
||||
virtual bool Close() = 0;
|
||||
virtual string GetFilename() = 0;
|
||||
};
|
||||
|
||||
// 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 WriteHeader(const char* data, size_t len) override;
|
||||
bool Close() override;
|
||||
string GetFilename() override;
|
||||
|
||||
private:
|
||||
const string filename_;
|
||||
std::ofstream out_;
|
||||
};
|
||||
|
||||
|
|
|
@ -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_
|
||||
#define CARTOGRAPHER_IO_IMAGE_H_
|
||||
|
||||
|
@ -41,6 +57,9 @@ class Image {
|
|||
// to this surface is alive.
|
||||
UniqueCairoSurfacePtr GetCairoSurface();
|
||||
|
||||
int width() const { return width_; }
|
||||
int height() const { return height_; }
|
||||
|
||||
private:
|
||||
int width_;
|
||||
int height_;
|
||||
|
|
|
@ -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 "Eigen/Core"
|
||||
|
@ -12,63 +28,28 @@ namespace cartographer {
|
|||
namespace io {
|
||||
namespace {
|
||||
|
||||
void WriteGrid(
|
||||
void DrawTrajectoriesIntoImage(
|
||||
const mapping_2d::ProbabilityGrid& probability_grid,
|
||||
const ProbabilityGridPointsProcessor::DrawTrajectories& draw_trajectories,
|
||||
const Eigen::Array2i& offset,
|
||||
const std::vector<mapping::proto::Trajectory>& trajectories,
|
||||
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 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) {
|
||||
cairo_surface_t* cairo_surface) {
|
||||
for (size_t i = 0; i < trajectories.size(); ++i) {
|
||||
DrawTrajectory(trajectories[i], GetColor(i),
|
||||
[&probability_grid, &offset](
|
||||
const transform::Rigid3d& pose) -> Eigen::Array2i {
|
||||
[&probability_grid,
|
||||
&offset](const transform::Rigid3d& pose) -> Eigen::Array2i {
|
||||
return probability_grid.limits().GetCellIndex(
|
||||
pose.cast<float>().translation().head<2>()) -
|
||||
offset;
|
||||
},
|
||||
image.GetCairoSurface().get());
|
||||
cairo_surface);
|
||||
}
|
||||
}
|
||||
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)));
|
||||
uint8 ProbabilityToColor(float probability_from_grid) {
|
||||
const float probability = 1.f - probability_from_grid;
|
||||
return ::cartographer::common::RoundToInt(
|
||||
255 * ((probability - mapping::kMinProbability) /
|
||||
(mapping::kMaxProbability - mapping::kMinProbability)));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
@ -115,8 +96,19 @@ void ProbabilityGridPointsProcessor::Process(
|
|||
}
|
||||
|
||||
PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
|
||||
WriteGrid(probability_grid_, draw_trajectories_, trajectories_,
|
||||
file_writer_.get());
|
||||
Eigen::Array2i offset;
|
||||
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()) {
|
||||
case FlushResult::kRestartStream:
|
||||
LOG(FATAL) << "ProbabilityGrid generation must be configured to occur "
|
||||
|
@ -128,5 +120,40 @@ PointsProcessor::FlushResult ProbabilityGridPointsProcessor::Flush() {
|
|||
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 cartographer
|
||||
|
|
|
@ -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_
|
||||
#define CARTOGRAPHER_IO_PROBABILITY_GRID_POINTS_PROCESSOR_H_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "cartographer/io/file_writer.h"
|
||||
#include "cartographer/io/image.h"
|
||||
#include "cartographer/io/points_batch.h"
|
||||
#include "cartographer/io/points_processor.h"
|
||||
#include "cartographer/mapping/proto/trajectory.pb.h"
|
||||
|
@ -56,6 +72,16 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
|
|||
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 cartographer
|
||||
|
||||
|
|
Loading…
Reference in New Issue