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 {
|
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
|
||||||
|
|
|
@ -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_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
|
@ -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;
|
|
||||||
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) {
|
|
||||||
for (size_t i = 0; i < trajectories.size(); ++i) {
|
for (size_t i = 0; i < trajectories.size(); ++i) {
|
||||||
DrawTrajectory(trajectories[i], GetColor(i),
|
DrawTrajectory(trajectories[i], GetColor(i),
|
||||||
[&probability_grid, &offset](
|
[&probability_grid,
|
||||||
const transform::Rigid3d& pose) -> Eigen::Array2i {
|
&offset](const transform::Rigid3d& pose) -> Eigen::Array2i {
|
||||||
return probability_grid.limits().GetCellIndex(
|
return probability_grid.limits().GetCellIndex(
|
||||||
pose.cast<float>().translation().head<2>()) -
|
pose.cast<float>().translation().head<2>()) -
|
||||||
offset;
|
offset;
|
||||||
},
|
},
|
||||||
image.GetCairoSurface().get());
|
cairo_surface);
|
||||||
}
|
}
|
||||||
}
|
|
||||||
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
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue