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.

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<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

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<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

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<char> CreateExpectedProbabilityGrid(
               CreateProbabilityGridRangeDataInserterOptions2D(
                   probability_grid_options->GetDictionary("range_data_inserter")
                       .get()));
-  auto probability_grid =
+  mapping::ValueConversionTables conversion_tables;
-      CreateProbabilityGrid(probability_grid_options->GetDouble("resolution"));
+  auto probability_grid = CreateProbabilityGrid(
+      probability_grid_options->GetDouble("resolution"), &conversion_tables);
   range_data_inserter.Insert({points_batch->origin, points_batch->points, {}},
                              &probability_grid);
 

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);
   }

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,

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)
+Grid2D::Grid2D(const proto::Grid2D& proto,
-    : limits_(proto.limits()), correspondence_cost_cells_() {
+               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;
+  if (!limits().Contains(cell_index)) return max_correspondence_cost_;
-  return ValueToCorrespondenceCost(
+  return (*value_to_correspondence_cost_table_)
-      correspondence_cost_cells()[ToFlatIndex(cell_index)]);
+      [correspondence_cost_cells()[ToFlatIndex(cell_index)]];
 }
 
 // Returns true if the correspondence cost at the specified index is known.

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);
+         float max_correspondence_cost,
-  explicit Grid2D(const proto::Grid2D& proto);
+         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

cartographer/mapping/2d/probability_grid.cc

@@ -24,10 +24,15 @@
 namespace cartographer {
 namespace mapping {
 
-ProbabilityGrid::ProbabilityGrid(const MapLimits& limits)
+ProbabilityGrid::ProbabilityGrid(const MapLimits& limits,
-    : Grid2D(limits, kMinCorrespondenceCost, kMaxCorrespondenceCost) {}
+                                 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));

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 MapLimits& limits,
-  explicit ProbabilityGrid(const proto::Grid2D& proto);
+                           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

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<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));

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<ProbabilityGridRangeDataInserter2D> range_data_inserter_;
   proto::ProbabilityGridRangeDataInserterOptions2D options_;

cartographer/mapping/2d/submap_2d.cc

@@ -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)
+Submap2D::Submap2D(const proto::Submap2D& proto,
-    : Submap(transform::ToRigid3(proto.local_pose())) {
+                   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>(
+      origin,
-                  static_cast<Grid2D*>(CreateGrid(origin).release()))));
+      std::unique_ptr<Grid2D>(
+          static_cast<Grid2D*>(CreateGrid(origin).release())),
+      &conversion_tables_));
 }
 
 }  // namespace mapping

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<Grid2D> grid);
+  Submap2D(const Eigen::Vector2f& origin, std::unique_ptr<Grid2D> grid,
-  explicit Submap2D(const proto::Submap2D& proto);
+           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

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<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(

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)
+               float max_weight, ValueConversionTables* conversion_tables)
-    : Grid2D(limits, -truncation_distance, truncation_distance),
+    : 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),

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);
+  TSDF2D(const MapLimits& limits, float truncation_distance, float max_weight,
-  explicit TSDF2D(const proto::Grid2D& proto);
+         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.
 };

cartographer/mapping/2d/tsdf_2d_test.cc

@@ -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));

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<TSDFRangeDataInserter2D> range_data_inserter_;

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<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_;
 };
 

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<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());

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 {

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<CeresScanMatcher2D> ceres_scan_matcher_;

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<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(),

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(
+  ValueConversionTables conversion_tables;
-      MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
+  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(

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<ProbabilityGridRangeDataInserter2D> range_data_inserter_;
   sensor::PointCloud point_cloud_;

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),
+  ValueConversionTables conversion_tables;
-                  common::make_unique<ProbabilityGrid>(map_limits));
+  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);

cartographer/mapping/internal/submap_controller.cc

@@ -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 <>

cartographer/mapping/internal/submap_controller.h

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

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_tsd_(conversion_tables->GetConversionTable(unknown_tsd_value_,
-      value_to_weight_(PrecomputeValueToWeight()) {}
+                                                          min_tsd_, max_tsd_)),
-
+      value_to_weight_(conversion_tables->GetConversionTable(
-// 0 is unknown, [1, 32767] maps to [min_tsd_ max_tsd_].
+          unknown_weight_value_, min_weight_, max_weight)) {}
-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;
-}
 
 }  // namespace mapping
 }  // namespace cartographer

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<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_;
+  const std::vector<float>* value_to_tsd_;
-  std::vector<float> value_to_weight_;
+  const std::vector<float>* value_to_weight_;
 };
 
 }  // namespace mapping

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_;

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;

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<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

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 <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_

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 <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