diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index 4b28864..f404fe1 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -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 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 diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h index 7b6ba5d..fe7e74f 100644 --- a/cartographer/io/probability_grid_points_processor.h +++ b/cartographer/io/probability_grid_points_processor.h @@ -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 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 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 diff --git a/cartographer/io/probability_grid_points_processor_test.cc b/cartographer/io/probability_grid_points_processor_test.cc index 8ceac5e..0b01977 100644 --- a/cartographer/io/probability_grid_points_processor_test.cc +++ b/cartographer/io/probability_grid_points_processor_test.cc @@ -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 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); diff --git a/cartographer/io/submap_painter.cc b/cartographer/io/submap_painter.cc index 1c5855c..dbaa6df 100644 --- a/cartographer/io/submap_painter.cc +++ b/cartographer/io/submap_painter.cc @@ -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); } diff --git a/cartographer/io/submap_painter.h b/cartographer/io/submap_painter.h index 9bbcab9..c322b73 100644 --- a/cartographer/io/submap_painter.h +++ b/cartographer/io/submap_painter.h @@ -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, diff --git a/cartographer/mapping/2d/grid_2d.cc b/cartographer/mapping/2d/grid_2d.cc index 9672a59..5eb5af5 100644 --- a/cartographer/mapping/2d/grid_2d.cc +++ b/cartographer/mapping/2d/grid_2d.cc @@ -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::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. diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index c7506e4..89ad674 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/cartographer/mapping/2d/grid_2d.h @@ -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* value_to_correspondence_cost_table_; }; } // namespace mapping diff --git a/cartographer/mapping/2d/probability_grid.cc b/cartographer/mapping/2d/probability_grid.cc index 82761d4..10a62bc 100644 --- a/cartographer/mapping/2d/probability_grid.cc +++ b/cartographer/mapping/2d/probability_grid.cc @@ -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 ProbabilityGrid::ComputeCroppedGrid() const { limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x()); std::unique_ptr cropped_grid = common::make_unique( - 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)); diff --git a/cartographer/mapping/2d/probability_grid.h b/cartographer/mapping/2d/probability_grid.h index 540f47e..dd2b767 100644 --- a/cartographer/mapping/2d/probability_grid.h +++ b/cartographer/mapping/2d/probability_grid.h @@ -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 diff --git a/cartographer/mapping/2d/probability_grid_test.cc b/cartographer/mapping/2d/probability_grid_test.cc index c42ba3c..99c3eb7 100644 --- a/cartographer/mapping/2d/probability_grid_test.cc +++ b/cartographer/mapping/2d/probability_grid_test.cc @@ -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 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)); diff --git a/cartographer/mapping/2d/range_data_inserter_2d_test.cc b/cartographer/mapping/2d/range_data_inserter_2d_test.cc index f3b5227..5a72022 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/range_data_inserter_2d_test.cc @@ -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 range_data_inserter_; proto::ProbabilityGridRangeDataInserterOptions2D options_; diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index 53d1770..5fd4cf9 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -61,17 +61,22 @@ proto::SubmapsOptions2D CreateSubmapsOptions2D( return options; } -Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr grid) +Submap2D::Submap2D(const Eigen::Vector2f& origin, std::unique_ptr 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(proto.grid()); + grid_ = + common::make_unique(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(submap_2d.grid()); + grid_ = common::make_unique(submap_2d.grid(), + conversion_tables_); } } @@ -164,7 +170,8 @@ std::unique_ptr ActiveSubmaps2D::CreateGrid( MapLimits(resolution, origin.cast() + 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( - origin, std::unique_ptr( - static_cast(CreateGrid(origin).release())))); + origin, + std::unique_ptr( + static_cast(CreateGrid(origin).release())), + &conversion_tables_)); } } // namespace mapping diff --git a/cartographer/mapping/2d/submap_2d.h b/cartographer/mapping/2d/submap_2d.h index b2cd8f7..2e97113 100644 --- a/cartographer/mapping/2d/submap_2d.h +++ b/cartographer/mapping/2d/submap_2d.h @@ -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 grid); - explicit Submap2D(const proto::Submap2D& proto); + Submap2D(const Eigen::Vector2f& origin, std::unique_ptr 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 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> submaps_; std::unique_ptr range_data_inserter_; + ValueConversionTables conversion_tables_; }; } // namespace mapping diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index 855ff16..1508971 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -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(expected_map_limits)); + common::make_unique(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( diff --git a/cartographer/mapping/2d/tsdf_2d.cc b/cartographer/mapping/2d/tsdf_2d.cc index f74ab6a..1264146 100644 --- a/cartographer/mapping/2d/tsdf_2d.cc +++ b/cartographer/mapping/2d/tsdf_2d.cc @@ -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( - 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( - 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::max()); @@ -115,7 +120,7 @@ std::unique_ptr TSDF2D::ComputeCroppedGrid() const { limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x()); std::unique_ptr cropped_grid = common::make_unique( 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), diff --git a/cartographer/mapping/2d/tsdf_2d.h b/cartographer/mapping/2d/tsdf_2d.h index a4985a0..415eeb3 100644 --- a/cartographer/mapping/2d/tsdf_2d.h +++ b/cartographer/mapping/2d/tsdf_2d.h @@ -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 value_converter_; std::vector weight_cells_; // Highest bit is update marker. }; diff --git a/cartographer/mapping/2d/tsdf_2d_test.cc b/cartographer/mapping/2d/tsdf_2d_test.cc index f1b3420..65f78e2 100644 --- a/cartographer/mapping/2d/tsdf_2d_test.cc +++ b/cartographer/mapping/2d/tsdf_2d_test.cc @@ -18,6 +18,7 @@ #include +#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 tsdf_distribution(-truncation_distance, truncation_distance); std::uniform_real_distribution 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)); diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc b/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc index 0a9f120..c8c3136 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc @@ -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 range_data_inserter_; diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc index 7e5c90c..db63c60 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc @@ -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(submap_2d), + {std::make_shared(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_; }; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index c3825e0..de95104 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -547,12 +547,12 @@ void PoseGraph2D::AddSubmapFromProto( const SubmapId submap_id = {submap.submap_id().trajectory_id(), submap.submap_id().submap_index()}; - std::shared_ptr submap_ptr = - std::make_shared(submap.submap_2d()); - const transform::Rigid2d global_submap_pose_2d = - transform::Project2D(global_submap_pose); common::MutexLocker locker(&mutex_); + std::shared_ptr submap_ptr = + std::make_shared(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()); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 1d67db1..07ffee9 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -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 { diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc index 51cb1c6..7503aa8 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc @@ -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 ceres_scan_matcher_; diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc index d4ab3a2..ad6e7f0 100644 --- a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc @@ -39,8 +39,10 @@ TEST(PrecomputationGridTest, CorrectValues) { // represented by uint8 values. std::mt19937 prng(42); std::uniform_int_distribution 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 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 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 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(), diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc index 37281a7..a52f1df 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc @@ -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 cost_function( diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index 5b3d136..cf4af1a 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -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 range_data_inserter_; sensor::PointCloud point_cloud_; diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc index c19fcaa..088a70d 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc @@ -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(map_limits)); + ValueConversionTables conversion_tables; + Submap2D submap( + Eigen::Vector2f(4.f, 5.f), + common::make_unique(map_limits, &conversion_tables), + &conversion_tables); int expected_nodes = 0; for (int i = 0; i < 2; ++i) { EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes); diff --git a/cartographer/mapping/internal/submap_controller.cc b/cartographer/mapping/internal/submap_controller.cc index 16a60f9..64d39c8 100644 --- a/cartographer/mapping/internal/submap_controller.cc +++ b/cartographer/mapping/internal/submap_controller.cc @@ -23,7 +23,8 @@ template <> std::shared_ptr SubmapController::CreateSubmap( const mapping::proto::Submap& proto) { - return std::make_shared(proto.submap_2d()); + return std::make_shared(proto.submap_2d(), + &conversion_tables_); } template <> diff --git a/cartographer/mapping/internal/submap_controller.h b/cartographer/mapping/internal/submap_controller.h index 74e26ac..c3ad51d 100644 --- a/cartographer/mapping/internal/submap_controller.h +++ b/cartographer/mapping/internal/submap_controller.h @@ -60,6 +60,8 @@ class SubmapController { mapping::MapById> unfinished_submaps_; + + ValueConversionTables conversion_tables_; }; template <> diff --git a/cartographer/mapping/internal/tsd_value_converter.cc b/cartographer/mapping/internal/tsd_value_converter.cc index d7dceee..c35f92b 100644 --- a/cartographer/mapping/internal/tsd_value_converter.cc +++ b/cartographer/mapping/internal/tsd_value_converter.cc @@ -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 TSDValueConverter::PrecomputeValueToTSD() { - std::vector result; - size_t num_values = std::numeric_limits::max() + 1; - result.reserve(num_values); - for (size_t value = 0; value != num_values; ++value) { - result.push_back( - SlowValueToTSD(static_cast(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 TSDValueConverter::PrecomputeValueToWeight() { - std::vector result; - size_t num_values = std::numeric_limits::max() + 1; - result.reserve(num_values); - for (size_t value = 0; value != num_values; ++value) { - result.push_back( - SlowValueToWeight(static_cast(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 diff --git a/cartographer/mapping/internal/tsd_value_converter.h b/cartographer/mapping/internal/tsd_value_converter.h index 56bd1f5..f01d826 100644 --- a/cartographer/mapping/internal/tsd_value_converter.h +++ b/cartographer/mapping/internal/tsd_value_converter.h @@ -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 PrecomputeValueToTSD(); - float SlowValueToWeight(const uint16 value) const; - std::vector 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 value_to_tsd_; - std::vector value_to_weight_; + const std::vector* value_to_tsd_; + const std::vector* value_to_weight_; }; } // namespace mapping diff --git a/cartographer/mapping/internal/tsd_value_converter_test.cc b/cartographer/mapping/internal/tsd_value_converter_test.cc index 98ff6d5..8a720f6 100644 --- a/cartographer/mapping/internal/tsd_value_converter_test.cc +++ b/cartographer/mapping/internal/tsd_value_converter_test.cc @@ -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_; diff --git a/cartographer/mapping/probability_values.cc b/cartographer/mapping/probability_values.cc index cae2357..8643853 100644 --- a/cartographer/mapping/probability_values.cc +++ b/cartographer/mapping/probability_values.cc @@ -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; diff --git a/cartographer/mapping/value_conversion_tables.cc b/cartographer/mapping/value_conversion_tables.cc new file mode 100644 index 0000000..bd978bc --- /dev/null +++ b/cartographer/mapping/value_conversion_tables.cc @@ -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> PrecomputeValueToBoundedFloat( + const uint16 unknown_value, const float unknown_result, + const float lower_bound, const float upper_bound) { + auto result = common::make_unique>(); + size_t num_values = std::numeric_limits::max() + 1; + result->reserve(num_values); + for (size_t value = 0; value != num_values; ++value) { + result->push_back(SlowValueToBoundedFloat( + static_cast(value) & ~kUpdateMarker, unknown_value, + unknown_result, lower_bound, upper_bound)); + } + return result; +} +} // namespace + +const std::vector* ValueConversionTables::GetConversionTable( + float unknown_result, float lower_bound, float upper_bound) { + std::tuple 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 diff --git a/cartographer/mapping/value_conversion_tables.h b/cartographer/mapping/value_conversion_tables.h new file mode 100644 index 0000000..56924f0 --- /dev/null +++ b/cartographer/mapping/value_conversion_tables.h @@ -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 +#include + +#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* GetConversionTable(float unknown_result, + float lower_bound, + float upper_bound); + + private: + std::map, + std::unique_ptr>> + bounds_to_lookup_table_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_VALUE_CONVERSION_TABLES_H_ diff --git a/cartographer/mapping/value_conversion_tables_test.cc b/cartographer/mapping/value_conversion_tables_test.cc new file mode 100644 index 0000000..3009ecd --- /dev/null +++ b/cartographer/mapping/value_conversion_tables_test.cc @@ -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 + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +TEST(ValueConversionTablesTest, EqualTables) { + ValueConversionTables value_conversion_tables; + const std::vector* reference_table = + value_conversion_tables.GetConversionTable(0.1f, 0.1f, 0.5f); + const std::vector* 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* reference_table = + value_conversion_tables.GetConversionTable(0.1f, 0.1f, 0.5f); + const std::vector* 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 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* 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