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

Add value conversion tables (#1255) 

Adds value conversion tables to perform lazy computations of lookup tables for mapping from a uint16 value to a float in [`lower_bound`, `upper_bound`].
Owners of the value conversion tables are `SubmapController`, `PoseGraph2D`, `ActiveSubmaps` and `ProbabilityGridPointsProcessor`. For concurrency reasons, having only a single owner is not possible.
Follow up PR in `cartographer_ros`  is prepared.
master
Kevin Daun 2018-07-12 22:11:55 +02:00 committed by Wally B. Feed
parent 8602bf9430
commit 5abd413310
35 changed files with 410 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cartographer/io/probability_grid_points_processor.cc
View File

@ -91,7 +91,8 @@ ProbabilityGridPointsProcessor::ProbabilityGridPointsProcessor(
file_writer_(std::move(file_writer)),
next_(next),
range_data_inserter_(probability_grid_range_data_inserter_options),
probability_grid_(CreateProbabilityGrid(resolution)) {
probability_grid_(
CreateProbabilityGrid(resolution, &conversion_tables_)) {
LOG_IF(WARNING, output_type == OutputType::kPb &&
draw_trajectories_ == DrawTrajectories::kYes)
<< "Drawing the trajectories is not supported when writing the "
@ -192,14 +193,17 @@ std::unique_ptr<Image> DrawProbabilityGrid(
return image;
}
mapping::ProbabilityGrid CreateProbabilityGrid(const double resolution) {
mapping::ProbabilityGrid CreateProbabilityGrid(
const double resolution,
mapping::ValueConversionTables* conversion_tables) {
constexpr int kInitialProbabilityGridSize = 100;
Eigen::Vector2d max =
0.5 * kInitialProbabilityGridSize * resolution * Eigen::Vector2d::Ones();
return mapping::ProbabilityGrid(
mapping::MapLimits(resolution, max,
mapping::CellLimits(kInitialProbabilityGridSize,
kInitialProbabilityGridSize)));
kInitialProbabilityGridSize)),
conversion_tables);
}
} // namespace io

5
cartographer/io/probability_grid_points_processor.h
View File

@ -27,6 +27,7 @@
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
#include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h"
#include "cartographer/mapping/proto/trajectory.pb.h"
#include "cartographer/mapping/value_conversion_tables.h"
namespace cartographer {
namespace io {
@ -71,6 +72,7 @@ class ProbabilityGridPointsProcessor : public PointsProcessor {
std::unique_ptr<FileWriter> file_writer_;
PointsProcessor* const next_;
mapping::ProbabilityGridRangeDataInserter2D range_data_inserter_;
mapping::ValueConversionTables conversion_tables_;
mapping::ProbabilityGrid probability_grid_;
};
@ -81,7 +83,8 @@ std::unique_ptr<Image> DrawProbabilityGrid(
// Create an initially empty probability grid with 'resolution' and a small
// size, suitable for a PointsBatchProcessor.
mapping::ProbabilityGrid CreateProbabilityGrid(const double resolution);
mapping::ProbabilityGrid CreateProbabilityGrid(
const double resolution, mapping::ValueConversionTables* conversion_tables);
} // namespace io
} // namespace cartographer

6
cartographer/io/probability_grid_points_processor_test.cc
View File

@ -23,6 +23,7 @@
#include "cartographer/io/fake_file_writer.h"
#include "cartographer/io/points_processor_pipeline_builder.h"
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
#include "cartographer/mapping/value_conversion_tables.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
@ -81,8 +82,9 @@ std::vector<char> CreateExpectedProbabilityGrid(
CreateProbabilityGridRangeDataInserterOptions2D(
probability_grid_options->GetDictionary("range_data_inserter")
.get()));
auto probability_grid =
CreateProbabilityGrid(probability_grid_options->GetDouble("resolution"));
mapping::ValueConversionTables conversion_tables;
auto probability_grid = CreateProbabilityGrid(
probability_grid_options->GetDouble("resolution"), &conversion_tables);
range_data_inserter.Insert({points_batch->origin, points_batch->points, {}},
&probability_grid);

6
cartographer/io/submap_painter.cc
View File

@ -110,7 +110,8 @@ PaintSubmapSlicesResult PaintSubmapSlices(
void FillSubmapSlice(
const ::cartographer::transform::Rigid3d& global_submap_pose,
const ::cartographer::mapping::proto::Submap& proto,
SubmapSlice* const submap_slice) {
SubmapSlice* const submap_slice,
mapping::ValueConversionTables* conversion_tables) {
::cartographer::mapping::proto::SubmapQuery::Response response;
::cartographer::transform::Rigid3d local_pose;
if (proto.has_submap_3d()) {
@ -118,7 +119,8 @@ void FillSubmapSlice(
local_pose = submap.local_pose();
submap.ToResponseProto(global_submap_pose, &response);
} else {
::cartographer::mapping::Submap2D submap(proto.submap_2d());
::cartographer::mapping::Submap2D submap(proto.submap_2d(),
conversion_tables);
local_pose = submap.local_pose();
submap.ToResponseProto(global_submap_pose, &response);
}

4
cartographer/io/submap_painter.h
View File

@ -22,6 +22,7 @@
#include "cartographer/io/image.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/value_conversion_tables.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
@ -80,7 +81,8 @@ PaintSubmapSlicesResult PaintSubmapSlices(
void FillSubmapSlice(
const ::cartographer::transform::Rigid3d& global_submap_pose,
const ::cartographer::mapping::proto::Submap& proto,
SubmapSlice* const submap_slice);
SubmapSlice* const submap_slice,
mapping::ValueConversionTables* conversion_tables);
// Unpacks cell data as provided by the backend into 'intensity' and 'alpha'.
SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells,

67
cartographer/mapping/2d/grid_2d.cc
View File

@ -19,6 +19,34 @@
namespace cartographer {
namespace mapping {
namespace {
float MinCorrespondenceCostFromProto(const proto::Grid2D& proto) {
if (proto.min_correspondence_cost() == 0.f &&
proto.max_correspondence_cost() == 0.f) {
LOG(WARNING)
<< "proto::Grid2D: max_correspondence_cost and min_correspondence_cost "
"are initialized with 0 indicating an older version of the "
"protobuf format. Loading default values.";
return kMinCorrespondenceCost;
} else {
return proto.min_correspondence_cost();
}
}
float MaxCorrespondenceCostFromProto(const proto::Grid2D& proto) {
if (proto.min_correspondence_cost() == 0.f &&
proto.max_correspondence_cost() == 0.f) {
LOG(WARNING)
<< "proto::Grid2D: max_correspondence_cost and min_correspondence_cost "
"are initialized with 0 indicating an older version of the "
"protobuf format. Loading default values.";
return kMaxCorrespondenceCost;
} else {
return proto.max_correspondence_cost();
}
}
} // namespace
proto::GridOptions2D CreateGridOptions2D(
common::LuaParameterDictionary* const parameter_dictionary) {
@ -34,18 +62,30 @@ proto::GridOptions2D CreateGridOptions2D(
}
Grid2D::Grid2D(const MapLimits& limits, float min_correspondence_cost,
float max_correspondence_cost)
float max_correspondence_cost,
ValueConversionTables* conversion_tables)
: limits_(limits),
correspondence_cost_cells_(
limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
kUnknownCorrespondenceValue),
min_correspondence_cost_(min_correspondence_cost),
max_correspondence_cost_(max_correspondence_cost) {
max_correspondence_cost_(max_correspondence_cost),
value_to_correspondence_cost_table_(conversion_tables->GetConversionTable(
max_correspondence_cost, min_correspondence_cost,
max_correspondence_cost)) {
CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
}
Grid2D::Grid2D(const proto::Grid2D& proto)
: limits_(proto.limits()), correspondence_cost_cells_() {
Grid2D::Grid2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables)
: limits_(proto.limits()),
correspondence_cost_cells_(),
min_correspondence_cost_(MinCorrespondenceCostFromProto(proto)),
max_correspondence_cost_(MaxCorrespondenceCostFromProto(proto)),
value_to_correspondence_cost_table_(conversion_tables->GetConversionTable(
max_correspondence_cost_, min_correspondence_cost_,
max_correspondence_cost_)) {
CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
if (proto.has_known_cells_box()) {
const auto& box = proto.known_cells_box();
known_cells_box_ =
@ -57,19 +97,6 @@ Grid2D::Grid2D(const proto::Grid2D& proto)
CHECK_LE(cell, std::numeric_limits<uint16>::max());
correspondence_cost_cells_.push_back(cell);
}
if (proto.min_correspondence_cost() == 0.f &&
proto.max_correspondence_cost() == 0.f) {
LOG(WARNING)
<< "proto::Grid2D: max_correspondence_cost and min_correspondence_cost "
"are initialized with 0 indicating an older version of the "
"protobuf format. Loading default values.";
min_correspondence_cost_ = kMinCorrespondenceCost;
max_correspondence_cost_ = kMaxCorrespondenceCost;
} else {
min_correspondence_cost_ = proto.min_correspondence_cost();
max_correspondence_cost_ = proto.max_correspondence_cost();
}
CHECK_LT(min_correspondence_cost_, max_correspondence_cost_);
}
// Finishes the update sequence.
@ -84,9 +111,9 @@ void Grid2D::FinishUpdate() {
// Returns the correspondence cost of the cell with 'cell_index'.
float Grid2D::GetCorrespondenceCost(const Eigen::Array2i& cell_index) const {
if (!limits().Contains(cell_index)) return kMaxCorrespondenceCost;
return ValueToCorrespondenceCost(
correspondence_cost_cells()[ToFlatIndex(cell_index)]);
if (!limits().Contains(cell_index)) return max_correspondence_cost_;
return (*value_to_correspondence_cost_table_)
[correspondence_cost_cells()[ToFlatIndex(cell_index)]];
}
// Returns true if the correspondence cost at the specified index is known.

8
cartographer/mapping/2d/grid_2d.h
View File

@ -24,6 +24,7 @@
#include "cartographer/mapping/proto/2d/grid_2d.pb.h"
#include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/value_conversion_tables.h"
namespace cartographer {
namespace mapping {
@ -34,8 +35,10 @@ proto::GridOptions2D CreateGridOptions2D(
class Grid2D : public GridInterface {
public:
Grid2D(const MapLimits& limits, float min_correspondence_cost,
float max_correspondence_cost);
explicit Grid2D(const proto::Grid2D& proto);
float max_correspondence_cost,
ValueConversionTables* conversion_tables);
explicit Grid2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables);
// Returns the limits of this Grid2D.
const MapLimits& limits() const { return limits_; }
@ -103,6 +106,7 @@ class Grid2D : public GridInterface {
// Bounding box of known cells to efficiently compute cropping limits.
Eigen::AlignedBox2i known_cells_box_;
const std::vector<float>* value_to_correspondence_cost_table_;
};
} // namespace mapping

13
cartographer/mapping/2d/probability_grid.cc
View File

@ -24,10 +24,15 @@
namespace cartographer {
namespace mapping {
ProbabilityGrid::ProbabilityGrid(const MapLimits& limits)
: Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost) {}
ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
ValueConversionTables* conversion_tables)
: Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost,
conversion_tables),
conversion_tables_(conversion_tables) {}
ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto) : Grid2D(proto) {
ProbabilityGrid::ProbabilityGrid(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables)
: Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
CHECK(proto.has_probability_grid_2d());
}
@ -88,7 +93,7 @@ std::unique_ptr<Grid2D> ProbabilityGrid::ComputeCroppedGrid() const {
limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
std::unique_ptr<ProbabilityGrid> cropped_grid =
common::make_unique<ProbabilityGrid>(
MapLimits(resolution, max, cell_limits));
MapLimits(resolution, max, cell_limits), conversion_tables_);
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
if (!IsKnown(xy_index + offset)) continue;
cropped_grid->SetProbability(xy_index, GetProbability(xy_index + offset));

9
cartographer/mapping/2d/probability_grid.h
View File

@ -30,8 +30,10 @@ namespace mapping {
// Represents a 2D grid of probabilities.
class ProbabilityGrid : public Grid2D {
public:
explicit ProbabilityGrid(const MapLimits& limits);
explicit ProbabilityGrid(const proto::Grid2D& proto);
explicit ProbabilityGrid(const MapLimits& limits,
ValueConversionTables* conversion_tables);
explicit ProbabilityGrid(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables);
// Sets the probability of the cell at 'cell_index' to the given
// 'probability'. Only allowed if the cell was unknown before.
@ -56,6 +58,9 @@ class ProbabilityGrid : public Grid2D {
virtual bool DrawToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture,
transform::Rigid3d local_pose) const override;
private:
ValueConversionTables* conversion_tables_;
};
} // namespace mapping

23
cartographer/mapping/2d/probability_grid_test.cc
View File

@ -41,7 +41,8 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
proto.mutable_known_cells_box()->set_min_y(22);
proto.mutable_probability_grid_2d();
ProbabilityGrid grid(proto);
ValueConversionTables conversion_tables;
ProbabilityGrid grid(proto, &conversion_tables);
EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
// TODO(macmason): Figure out how to test the contents of cells_ and
@ -49,8 +50,10 @@ TEST(ProbabilityGridTest, ProtoConstructor) {
}
TEST(ProbabilityGridTest, ToProto) {
ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid(
MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
&conversion_tables);
const auto proto = probability_grid.ToProto();
EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(),
@ -61,8 +64,10 @@ TEST(ProbabilityGridTest, ToProto) {
}
TEST(ProbabilityGridTest, ApplyOdds) {
ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid(
MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
&conversion_tables);
const MapLimits& limits = probability_grid.limits();
EXPECT_TRUE(limits.Contains(Array2i(0, 0)));
@ -108,8 +113,10 @@ TEST(ProbabilityGridTest, ApplyOdds) {
}
TEST(ProbabilityGridTest, GetProbability) {
ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid(
MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)));
MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)),
&conversion_tables);
const MapLimits& limits = probability_grid.limits();
EXPECT_EQ(1., limits.max().x());
@ -133,8 +140,10 @@ TEST(ProbabilityGridTest, GetProbability) {
}
TEST(ProbabilityGridTest, GetCellIndex) {
ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid(
MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)));
MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)),
&conversion_tables);
const MapLimits& limits = probability_grid.limits();
const CellLimits& cell_limits = limits.cell_limits();
@ -167,8 +176,10 @@ TEST(ProbabilityGridTest, CorrectCropping) {
std::mt19937 rng(42);
std::uniform_real_distribution<float> value_distribution(kMinProbability,
kMaxProbability);
ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)),
&conversion_tables);
for (const Array2i& xy_index :
XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) {
probability_grid.SetProbability(xy_index, value_distribution(rng));

4
cartographer/mapping/2d/range_data_inserter_2d_test.cc
View File

@ -33,7 +33,8 @@ class RangeDataInserterTest2D : public ::testing::Test {
protected:
RangeDataInserterTest2D()
: probability_grid_(
MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) {
MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5)),
&conversion_tables_) {
auto parameter_dictionary = common::MakeDictionary(
"return { "
"insert_free_space = true, "
@ -58,6 +59,7 @@ class RangeDataInserterTest2D : public ::testing::Test {
probability_grid_.FinishUpdate();
}
ValueConversionTables conversion_tables_;
ProbabilityGrid probability_grid_;
std::unique_ptr<ProbabilityGridRangeDataInserter2D> range_data_inserter_;
proto::ProbabilityGridRangeDataInserterOptions2D options_;

28
cartographer/mapping/2d/submap_2d.cc
View File

@ -61,17 +61,22 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D(
return options;
}
Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid)
Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid,
ValueConversionTables* conversion_tables)
: Submap(transform::Rigid3d::Translation(
Eigen::Vector3d(origin.x(), origin.y(), 0.))) {
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
conversion_tables_(conversion_tables) {
grid_ = std::move(grid);
}
Submap2D::Submap2D(const proto::Submap2D& proto)
: Submap(transform::ToRigid3(proto.local_pose())) {
Submap2D::Submap2D(const proto::Submap2D& proto,
ValueConversionTables* conversion_tables)
: Submap(transform::ToRigid3(proto.local_pose())),
conversion_tables_(conversion_tables) {
if (proto.has_grid()) {
CHECK(proto.grid().has_probability_grid_2d());
grid_ = common::make_unique<ProbabilityGrid>(proto.grid());
grid_ =
common::make_unique<ProbabilityGrid>(proto.grid(), conversion_tables_);
}
set_num_range_data(proto.num_range_data());
set_finished(proto.finished());
@ -96,7 +101,8 @@ void Submap2D::UpdateFromProto(const proto::Submap& proto) {
set_finished(submap_2d.finished());
if (proto.submap_2d().has_grid()) {
CHECK(proto.submap_2d().grid().has_probability_grid_2d());
grid_ = common::make_unique<ProbabilityGrid>(submap_2d.grid());
grid_ = common::make_unique<ProbabilityGrid>(submap_2d.grid(),
conversion_tables_);
}
}
@ -164,7 +170,8 @@ std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
MapLimits(resolution,
origin.cast<double>() + 0.5 * kInitialSubmapSize * resolution *
Eigen::Vector2d::Ones(),
CellLimits(kInitialSubmapSize, kInitialSubmapSize)));
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
&conversion_tables_);
}
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
@ -174,10 +181,11 @@ void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
CHECK(submaps_.front()->finished());
submaps_.erase(submaps_.begin());
}
submaps_.push_back(common::make_unique<Submap2D>(
origin, std::unique_ptr<Grid2D>(
static_cast<Grid2D*>(CreateGrid(origin).release()))));
origin,
std::unique_ptr<Grid2D>(
static_cast<Grid2D*>(CreateGrid(origin).release())),
&conversion_tables_));
}
} // namespace mapping

9
cartographer/mapping/2d/submap_2d.h
View File

@ -30,6 +30,7 @@
#include "cartographer/mapping/range_data_inserter_interface.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/mapping/value_conversion_tables.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
@ -41,8 +42,10 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D(
class Submap2D : public Submap {
public:
Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid);
explicit Submap2D(const proto::Submap2D& proto);
Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid,
ValueConversionTables* conversion_tables);
explicit Submap2D(const proto::Submap2D& proto,
ValueConversionTables* conversion_tables);
void ToProto(proto::Submap* proto,
bool include_probability_grid_data) const override;
@ -61,6 +64,7 @@ class Submap2D : public Submap {
private:
std::unique_ptr<Grid2D> grid_;
ValueConversionTables* conversion_tables_;
};
// The first active submap will be created on the insertion of the first range
@ -95,6 +99,7 @@ class ActiveSubmaps2D {
const proto::SubmapsOptions2D options_;
std::vector<std::shared_ptr<Submap2D>> submaps_;
std::unique_ptr<RangeDataInserterInterface> range_data_inserter_;
ValueConversionTables conversion_tables_;
};
} // namespace mapping

7
cartographer/mapping/2d/submap_2d_test.cc
View File

@ -98,13 +98,16 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
TEST(Submap2DTest, ToFromProto) {
MapLimits expected_map_limits(1., Eigen::Vector2d(2., 3.),
CellLimits(100, 110));
ValueConversionTables conversion_tables;
Submap2D expected(Eigen::Vector2f(4.f, 5.f),
common::make_unique<ProbabilityGrid>(expected_map_limits));
common::make_unique<ProbabilityGrid>(expected_map_limits,
&conversion_tables),
&conversion_tables);
proto::Submap proto;
expected.ToProto(&proto, true /* include_probability_grid_data */);
EXPECT_TRUE(proto.has_submap_2d());
EXPECT_FALSE(proto.has_submap_3d());
const auto actual = Submap2D(proto.submap_2d());
const auto actual = Submap2D(proto.submap_2d(), &conversion_tables);
EXPECT_TRUE(expected.local_pose().translation().isApprox(
actual.local_pose().translation(), 1e-6));
EXPECT_TRUE(expected.local_pose().rotation().isApprox(

17
cartographer/mapping/2d/tsdf_2d.cc
View File

@ -20,18 +20,23 @@ namespace cartographer {
namespace mapping {
TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance,
float max_weight)
: Grid2D(limits, -truncation_distance, truncation_distance),
float max_weight, ValueConversionTables* conversion_tables)
: Grid2D(limits, -truncation_distance, truncation_distance,
conversion_tables),
conversion_tables_(conversion_tables),
value_converter_(common::make_unique<TSDValueConverter>(
truncation_distance, max_weight)),
truncation_distance, max_weight, conversion_tables_)),
weight_cells_(
limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells,
value_converter_->getUnknownWeightValue()) {}
TSDF2D::TSDF2D(const proto::Grid2D& proto) : Grid2D(proto) {
TSDF2D::TSDF2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables)
: Grid2D(proto, conversion_tables), conversion_tables_(conversion_tables) {
CHECK(proto.has_tsdf_2d());
value_converter_ = common::make_unique<TSDValueConverter>(
proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight());
proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight(),
conversion_tables_);
weight_cells_.reserve(proto.tsdf_2d().weight_cells_size());
for (const auto& cell : proto.tsdf_2d().weight_cells()) {
CHECK_LE(cell, std::numeric_limits<uint16>::max());
@ -115,7 +120,7 @@ std::unique_ptr<Grid2D> TSDF2D::ComputeCroppedGrid() const {
limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
std::unique_ptr<TSDF2D> cropped_grid = common::make_unique<TSDF2D>(
MapLimits(resolution, max, cell_limits), value_converter_->getMaxTSD(),
value_converter_->getMaxWeight());
value_converter_->getMaxWeight(), conversion_tables_);
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
if (!IsKnown(xy_index + offset)) continue;
cropped_grid->SetCell(xy_index, GetTSD(xy_index + offset),

7
cartographer/mapping/2d/tsdf_2d.h
View File

@ -30,8 +30,10 @@ namespace mapping {
// Represents a 2D grid of truncated signed distances and weights.
class TSDF2D : public Grid2D {
public:
TSDF2D(const MapLimits& limits, float truncation_distance, float max_weight);
explicit TSDF2D(const proto::Grid2D& proto);
TSDF2D(const MapLimits& limits, float truncation_distance, float max_weight,
ValueConversionTables* conversion_tables);
explicit TSDF2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables);
void SetCell(const Eigen::Array2i& cell_index, const float tsd,
const float weight);
@ -49,6 +51,7 @@ class TSDF2D : public Grid2D {
bool CellIsUpdated(const Eigen::Array2i& cell_index) const;
private:
ValueConversionTables* conversion_tables_;
std::unique_ptr<TSDValueConverter> value_converter_;
std::vector<uint16> weight_cells_; // Highest bit is update marker.
};

16
cartographer/mapping/2d/tsdf_2d_test.cc
View File

@ -18,6 +18,7 @@
#include <random>
#include "cartographer/mapping/value_conversion_tables.h"
#include "gtest/gtest.h"
namespace cartographer {
@ -45,21 +46,24 @@ TEST(TSDF2DTest, ProtoConstructor) {
proto.set_max_correspondence_cost(1.0);
proto.set_min_correspondence_cost(-1.0);
TSDF2D grid(proto);
ValueConversionTables conversion_tables;
TSDF2D grid(proto, &conversion_tables);
EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
}
TEST(TSDF2DTest, ToProto) {
ValueConversionTables conversion_tables;
TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f,
10.0f);
10.0f, &conversion_tables);
const auto proto = tsdf.ToProto();
EXPECT_EQ(ToProto(tsdf.limits()).DebugString(), proto.limits().DebugString());
}
TEST(TSDF2DTest, GetCellIndex) {
ValueConversionTables conversion_tables;
TSDF2D tsdf(MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)), 1.f,
10.f);
10.f, &conversion_tables);
const MapLimits& limits = tsdf.limits();
const CellLimits& cell_limits = limits.cell_limits();
@ -90,8 +94,9 @@ TEST(TSDF2DTest, GetCellIndex) {
TEST(TSDF2DTest, WriteRead) {
const float truncation_distance = 1.f;
const float max_weight = 10.f;
ValueConversionTables conversion_tables;
TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)),
truncation_distance, max_weight);
truncation_distance, max_weight, &conversion_tables);
const MapLimits& limits = tsdf.limits();
EXPECT_EQ(1., limits.max().x());
@ -136,8 +141,9 @@ TEST(TSDF2DTest, CorrectCropping) {
std::uniform_real_distribution<float> tsdf_distribution(-truncation_distance,
truncation_distance);
std::uniform_real_distribution<float> weight_distribution(0.f, max_weight);
ValueConversionTables conversion_tables;
TSDF2D tsdf(MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)),
truncation_distance, max_weight);
truncation_distance, max_weight, &conversion_tables);
for (const Array2i& xy_index :
XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) {
tsdf.SetCell(xy_index, tsdf_distribution(rng), weight_distribution(rng));

3
cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc
View File

@ -28,7 +28,7 @@ class RangeDataInserterTest2DTSDF : public ::testing::Test {
protected:
RangeDataInserterTest2DTSDF()
: tsdf_(MapLimits(1., Eigen::Vector2d(0., 7.), CellLimits(8, 1)), 2.0,
10.0) {
10.0, &conversion_tables_) {
auto parameter_dictionary = common::MakeDictionary(
"return { "
"truncation_distance = 2.0,"
@ -57,6 +57,7 @@ class RangeDataInserterTest2DTSDF : public ::testing::Test {
tsdf_.FinishUpdate();
}
ValueConversionTables conversion_tables_;
proto::TSDFRangeDataInserterOptions2D options_;
TSDF2D tsdf_;
std::unique_ptr<TSDFRangeDataInserter2D> range_data_inserter_;

3
cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc
View File

@ -69,7 +69,7 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
grid->mutable_probability_grid_2d();
fake_pose_graph_.mutable_submap_data()->Insert(
{0 /* trajectory_id */, submap_index},
{std::make_shared<const Submap2D>(submap_2d),
{std::make_shared<const Submap2D>(submap_2d, &conversion_tables_),
transform::Embed3D(global_to_submap_frame)});
}
@ -91,6 +91,7 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
: PoseGraphInterface::Constraint::INTER_SUBMAP});
}
ValueConversionTables conversion_tables_;
testing::FakeTrimmable fake_pose_graph_;
};

8
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -547,12 +547,12 @@ void PoseGraph2D::AddSubmapFromProto(
const SubmapId submap_id = {submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()};
std::shared_ptr<const Submap2D> submap_ptr =
std::make_shared<const Submap2D>(submap.submap_2d());
const transform::Rigid2d global_submap_pose_2d =
transform::Project2D(global_submap_pose);
common::MutexLocker locker(&mutex_);
std::shared_ptr<const Submap2D> submap_ptr =
std::make_shared<const Submap2D>(submap.submap_2d(), &conversion_tables_);
const transform::Rigid2d global_submap_pose_2d =
transform::Project2D(global_submap_pose);
AddTrajectoryIfNeeded(submap_id.trajectory_id);
if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
data_.submap_data.Insert(submap_id, InternalSubmapData());

2
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -244,6 +244,8 @@ class PoseGraph2D : public PoseGraph {
PoseGraphData data_ GUARDED_BY(mutex_);
ValueConversionTables conversion_tables_;
// Allows querying and manipulating the pose graph by the 'trimmers_'. The
// 'mutex_' of the pose graph is held while this class is used.
class TrimmingHandle : public Trimmable {

4
cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc
View File

@ -36,7 +36,8 @@ class CeresScanMatcherTest : public ::testing::Test {
protected:
CeresScanMatcherTest()
: probability_grid_(
MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) {
MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20)),
&conversion_tables_) {
probability_grid_.SetProbability(
probability_grid_.limits().GetCellIndex(Eigen::Vector2f(-3.5f, 2.5f)),
kMaxProbability);
@ -73,6 +74,7 @@ class CeresScanMatcherTest : public ::testing::Test {
<< "\nExpected: " << transform::ToProto(expected_pose).DebugString();
}
ValueConversionTables conversion_tables_;
ProbabilityGrid probability_grid_;
sensor::PointCloud point_cloud_;
std::unique_ptr<CeresScanMatcher2D> ceres_scan_matcher_;

16
cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc
View File

@ -39,8 +39,10 @@ TEST(PrecomputationGridTest, CorrectValues) {
// represented by uint8 values.
std::mt19937 prng(42);
std::uniform_int_distribution<int> distribution(0, 255);
ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)),
&conversion_tables);
std::vector<float> reusable_intermediate_grid;
PrecomputationGrid2D precomputation_grid_dummy(
probability_grid, probability_grid.limits().cell_limits(), 1,
@ -77,8 +79,10 @@ TEST(PrecomputationGridTest, CorrectValues) {
TEST(PrecomputationGridTest, TinyProbabilityGrid) {
std::mt19937 prng(42);
std::uniform_int_distribution<int> distribution(0, 255);
ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)),
&conversion_tables);
std::vector<float> reusable_intermediate_grid;
PrecomputationGrid2D precomputation_grid_dummy(
probability_grid, probability_grid.limits().cell_limits(), 1,
@ -158,8 +162,10 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
{2. * distribution(prng), 2. * distribution(prng)},
0.5 * distribution(prng));
ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)),
&conversion_tables);
range_data_inserter.Insert(
sensor::RangeData{
Eigen::Vector3f(expected_pose.translation().x(),
@ -212,8 +218,10 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
0.5 * distribution(prng)) *
perturbation.inverse();
ValueConversionTables conversion_tables;
ProbabilityGrid probability_grid(
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)),
&conversion_tables);
range_data_inserter.Insert(
sensor::RangeData{
transform::Embed3D(expected_pose * perturbation).translation(),

5
cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc
View File

@ -31,8 +31,9 @@ using ::testing::DoubleEq;
using ::testing::ElementsAre;
TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) {
ProbabilityGrid grid(
MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
ValueConversionTables conversion_tables;
ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)),
&conversion_tables);
sensor::PointCloud point_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}};
ceres::Problem problem;
std::unique_ptr<ceres::CostFunction> cost_function(

4
cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc
View File

@ -38,7 +38,8 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
protected:
RealTimeCorrelativeScanMatcherTest()
: probability_grid_(
MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) {
MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)),
&conversion_tables_) {
{
auto parameter_dictionary = common::MakeDictionary(
"return { "
@ -77,6 +78,7 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
}
}
ValueConversionTables conversion_tables_;
ProbabilityGrid probability_grid_;
std::unique_ptr<ProbabilityGridRangeDataInserter2D> range_data_inserter_;
sensor::PointCloud point_cloud_;

7
cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc
View File

@ -75,8 +75,11 @@ TEST_F(ConstraintBuilder2DTest, FindsConstraints) {
node_data.local_pose = transform::Rigid3d::Identity();
SubmapId submap_id{0, 1};
MapLimits map_limits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110));
Submap2D submap(Eigen::Vector2f(4.f, 5.f),
common::make_unique<ProbabilityGrid>(map_limits));
ValueConversionTables conversion_tables;
Submap2D submap(
Eigen::Vector2f(4.f, 5.f),
common::make_unique<ProbabilityGrid>(map_limits, &conversion_tables),
&conversion_tables);
int expected_nodes = 0;
for (int i = 0; i < 2; ++i) {
EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes);

3
cartographer/mapping/internal/submap_controller.cc
View File

@ -23,7 +23,8 @@ template <>
std::shared_ptr<mapping::Submap2D>
SubmapController<mapping::Submap2D>::CreateSubmap(
const mapping::proto::Submap& proto) {
return std::make_shared<mapping::Submap2D>(proto.submap_2d());
return std::make_shared<mapping::Submap2D>(proto.submap_2d(),
&conversion_tables_);
}
template <>

2
cartographer/mapping/internal/submap_controller.h
View File

@ -60,6 +60,8 @@ class SubmapController {
mapping::MapById<mapping::SubmapId, std::shared_ptr<SubmapType>>
unfinished_submaps_;
ValueConversionTables conversion_tables_;
};
template <>

54
cartographer/mapping/internal/tsd_value_converter.cc
View File

@ -19,59 +19,17 @@
namespace cartographer {
namespace mapping {
TSDValueConverter::TSDValueConverter(float max_tsd, float max_weight)
TSDValueConverter::TSDValueConverter(float max_tsd, float max_weight,
ValueConversionTables* conversion_tables)
: max_tsd_(max_tsd),
min_tsd_(-max_tsd),
max_weight_(max_weight),
tsd_resolution_(32766.f / (max_tsd_ - min_tsd_)),
weight_resolution_(32766.f / (max_weight_ - min_weight_)),
value_to_tsd_(PrecomputeValueToTSD()),
value_to_weight_(PrecomputeValueToWeight()) {}
// 0 is unknown, [1, 32767] maps to [min_tsd_ max_tsd_].
float TSDValueConverter::SlowValueToTSD(const uint16 value) const {
CHECK_GE(value, 0);
CHECK_LE(value, 32767);
if (value == unknown_tsd_value_) {
return min_tsd_;
}
const float kScale = (max_tsd_ - min_tsd_) / 32766.f;
return value * kScale + (min_tsd_ - kScale);
}
std::vector<float> TSDValueConverter::PrecomputeValueToTSD() {
std::vector<float> result;
size_t num_values = std::numeric_limits<uint16>::max() + 1;
result.reserve(num_values);
for (size_t value = 0; value != num_values; ++value) {
result.push_back(
SlowValueToTSD(static_cast<uint16>(value) & ~update_marker_));
}
return result;
}
// 0 is unknown, [1, 32767] maps to [min_weight_ max_weight_].
float TSDValueConverter::SlowValueToWeight(const uint16 value) const {
CHECK_GE(value, 0);
CHECK_LE(value, 32767);
if (value == unknown_weight_value_) {
// Unknown cells have min_weight_.
return min_weight_;
}
const float kScale = (max_weight_ - min_weight_) / 32766.f;
return value * kScale + (min_weight_ - kScale);
}
std::vector<float> TSDValueConverter::PrecomputeValueToWeight() {
std::vector<float> result;
size_t num_values = std::numeric_limits<uint16>::max() + 1;
result.reserve(num_values);
for (size_t value = 0; value != num_values; ++value) {
result.push_back(
SlowValueToWeight(static_cast<uint16>(value) & ~update_marker_));
}
return result;
}
value_to_tsd_(conversion_tables->GetConversionTable(unknown_tsd_value_,
min_tsd_, max_tsd_)),
value_to_weight_(conversion_tables->GetConversionTable(
unknown_weight_value_, min_weight_, max_weight)) {}
} // namespace mapping
} // namespace cartographer

16
cartographer/mapping/internal/tsd_value_converter.h
View File

@ -22,6 +22,7 @@
#include "cartographer/common/math.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping/value_conversion_tables.h"
#include "glog/logging.h"
namespace cartographer {
@ -31,7 +32,8 @@ namespace mapping {
// truncated signed distance values and weights.
class TSDValueConverter {
public:
TSDValueConverter(float max_tsd, float max_weight);
TSDValueConverter(float max_tsd, float max_weight,
ValueConversionTables* conversion_tables);
// Converts a tsd to a uint16 in the [1, 32767] range.
inline uint16 TSDToValue(const float tsd) const {
@ -55,13 +57,13 @@ class TSDValueConverter {
// Converts a uint16 (which may or may not have the update marker set) to a
// value in the range [min_tsd_, max_tsd_].
inline float ValueToTSD(const uint16 value) const {
return value_to_tsd_[value];
return (*value_to_tsd_)[value];
}
// Converts a uint16 (which may or may not have the update marker set) to a
// value in the range [min_weight_, max_weight_].
inline float ValueToWeight(const uint16 value) const {
return value_to_weight_[value];
return (*value_to_weight_)[value];
}
static uint16 getUnknownTSDValue() { return unknown_tsd_value_; }
@ -81,10 +83,6 @@ class TSDValueConverter {
inline float ClampWeight(const float weight) const {
return common::Clamp(weight, min_weight_, max_weight_);
}
float SlowValueToTSD(const uint16 value) const;
std::vector<float> PrecomputeValueToTSD();
float SlowValueToWeight(const uint16 value) const;
std::vector<float> PrecomputeValueToWeight();
float max_tsd_;
float min_tsd_;
@ -96,8 +94,8 @@ class TSDValueConverter {
static constexpr uint16 unknown_weight_value_ = 0;
static constexpr uint16 update_marker_ = 1u << 15;
std::vector<float> value_to_tsd_;
std::vector<float> value_to_weight_;
const std::vector<float>* value_to_tsd_;
const std::vector<float>* value_to_weight_;
};
} // namespace mapping

4
cartographer/mapping/internal/tsd_value_converter_test.cc
View File

@ -27,7 +27,9 @@ class TSDValueConverterTest : public ::testing::Test {
TSDValueConverterTest()
: truncation_distance_(0.1f),
max_weight_(10.0f),
tsd_value_converter_(truncation_distance_, max_weight_) {}
tsd_value_converter_(truncation_distance_, max_weight_,
&conversion_tables) {}
ValueConversionTables conversion_tables;
float truncation_distance_;
float max_weight_;
TSDValueConverter tsd_value_converter_;

1
cartographer/mapping/probability_values.cc
View File

@ -28,7 +28,6 @@ float SlowValueToBoundedFloat(const uint16 value, const uint16 unknown_value,
const float unknown_result,
const float lower_bound,
const float upper_bound) {
CHECK_GE(value, 0);
CHECK_LE(value, 32767);
if (value == unknown_value) return unknown_result;
const float kScale = (upper_bound - lower_bound) / 32766.f;

70
cartographer/mapping/value_conversion_tables.cc Normal file
View File

@ -0,0 +1,70 @@
/*
* Copyright 2018 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/mapping/value_conversion_tables.h"
#include "cartographer/common/make_unique.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping {
namespace {
constexpr uint16 kUpdateMarker = 1u << 15;
// 0 is unknown, [1, 32767] maps to [lower_bound, upper_bound].
float SlowValueToBoundedFloat(const uint16 value, const uint16 unknown_value,
const float unknown_result,
const float lower_bound,
const float upper_bound) {
CHECK_LE(value, 32767);
if (value == unknown_value) return unknown_result;
const float kScale = (upper_bound - lower_bound) / 32766.f;
return value * kScale + (lower_bound - kScale);
}
std::unique_ptr<std::vector<float>> PrecomputeValueToBoundedFloat(
const uint16 unknown_value, const float unknown_result,
const float lower_bound, const float upper_bound) {
auto result = common::make_unique<std::vector<float>>();
size_t num_values = std::numeric_limits<uint16>::max() + 1;
result->reserve(num_values);
for (size_t value = 0; value != num_values; ++value) {
result->push_back(SlowValueToBoundedFloat(
static_cast<uint16>(value) & ~kUpdateMarker, unknown_value,
unknown_result, lower_bound, upper_bound));
}
return result;
}
} // namespace
const std::vector<float>* ValueConversionTables::GetConversionTable(
float unknown_result, float lower_bound, float upper_bound) {
std::tuple<float, float, float> bounds =
std::make_tuple(unknown_result, lower_bound, upper_bound);
auto lookup_table_iterator = bounds_to_lookup_table_.find(bounds);
if (lookup_table_iterator == bounds_to_lookup_table_.end()) {
auto insertion_result = bounds_to_lookup_table_.emplace(
bounds, PrecomputeValueToBoundedFloat(0, unknown_result, lower_bound,
upper_bound));
return insertion_result.first->second.get();
} else {
return lookup_table_iterator->second.get();
}
}
} // namespace mapping
} // namespace cartographer

48
cartographer/mapping/value_conversion_tables.h Normal file
View File

@ -0,0 +1,48 @@
/*
* Copyright 2018 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_MAPPING_VALUE_CONVERSION_TABLES_H_
#define CARTOGRAPHER_MAPPING_VALUE_CONVERSION_TABLES_H_
#include <map>
#include <vector>
#include "cartographer/common/port.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping {
// Performs lazy computations of lookup tables for mapping from a uint16 value
// to a float in ['lower_bound', 'upper_bound']. The first element of the table
// is set to 'unknown_result'.
class ValueConversionTables {
public:
const std::vector<float>* GetConversionTable(float unknown_result,
float lower_bound,
float upper_bound);
private:
std::map<const std::tuple<float /* unknown_result */, float /* lower_bound */,
float /* upper_bound */>,
std::unique_ptr<const std::vector<float>>>
bounds_to_lookup_table_;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_VALUE_CONVERSION_TABLES_H_

68
cartographer/mapping/value_conversion_tables_test.cc Normal file
View File

@ -0,0 +1,68 @@
/*
* Copyright 2018 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/mapping/value_conversion_tables.h"
#include <random>
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping {
namespace {
TEST(ValueConversionTablesTest, EqualTables) {
ValueConversionTables value_conversion_tables;
const std::vector<float>* reference_table =
value_conversion_tables.GetConversionTable(0.1f, 0.1f, 0.5f);
const std::vector<float>* test_table =
value_conversion_tables.GetConversionTable(0.1f, 0.1f, 0.5f);
EXPECT_EQ(reference_table, test_table);
}
TEST(ValueConversionTablesTest, InequalTables) {
ValueConversionTables value_conversion_tables;
const std::vector<float>* reference_table =
value_conversion_tables.GetConversionTable(0.1f, 0.1f, 0.5f);
const std::vector<float>* test_table =
value_conversion_tables.GetConversionTable(0.1f, 0.4f, 0.5f);
EXPECT_FALSE(reference_table == test_table);
}
TEST(ValueConversionTablesTest, ValueConversion) {
ValueConversionTables value_conversion_tables;
std::mt19937 rng(42);
std::uniform_real_distribution<float> bound_distribution(-10.f, 10.f);
for (size_t sample_index = 0; sample_index < 100; ++sample_index) {
const float bound_sample_0 = bound_distribution(rng);
const float bound_sample_1 = bound_distribution(rng);
const float lower_bound = std::min(bound_sample_0, bound_sample_1);
const float upper_bound = std::max(bound_sample_0, bound_sample_1);
const float undefined_value = bound_distribution(rng);
const std::vector<float>* conversion_table =
value_conversion_tables.GetConversionTable(undefined_value, lower_bound,
upper_bound);
EXPECT_EQ((*conversion_table)[0], undefined_value);
const float scale = (upper_bound - lower_bound) / 32766.f;
for (uint16 i = 1; i < 32768; ++i) {
EXPECT_EQ((*conversion_table)[i], i * scale + (lower_bound - scale));
}
}
}
} // namespace
} // namespace mapping
} // namespace cartographer