Add TSDF RangeDataInserter (#1236)

Adds TSDF RangeDataInserter, tests and integrates the configuration files.

cartographer/mapping/2d/map_limits.h

@@ -75,6 +75,12 @@ class MapLimits {
         common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
   }
 
+  // Returns the center of the cell at 'cell_index'.
+  Eigen::Vector2f GetCellCenter(const Eigen::Array2i cell_index) const {
+    return {max_.x() - resolution() * (cell_index[1] + 0.5),
+            max_.y() - resolution() * (cell_index[0] + 0.5)};
+  }
+
   // Returns true if the ProbabilityGrid contains 'cell_index'.
   bool Contains(const Eigen::Array2i& cell_index) const {
     return (Eigen::Array2i(0, 0) <= cell_index).all() &&

cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc

@@ -35,6 +35,7 @@ constexpr int kSubpixelScale = 1000;
 void GrowAsNeeded(const sensor::RangeData& range_data,
                   ProbabilityGrid* const probability_grid) {
   Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
+  // Padding around bounding box to avoid numerical issues at cell boundaries.
   constexpr float kPadding = 1e-6f;
   for (const Eigen::Vector3f& hit : range_data.returns) {
     bounding_box.extend(hit.head<2>());

cartographer/mapping/2d/submap_2d_test.cc

@@ -50,6 +50,19 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
       "hit_probability = 0.53, "
       "miss_probability = 0.495, "
       "},"
+      "tsdf_range_data_inserter = { "
+      "truncation_distance = 2.0,"
+      "maximum_weight = 10.,"
+      "update_free_space = false,"
+      "normal_estimation_options = {"
+      "num_normal_samples = 2,"
+      "sample_radius = 10.,"
+      "},"
+      "project_sdf_distance_to_scan_normal = false,"
+      "update_weight_range_exponent = 0,"
+      "update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,"
+      "update_weight_distance_cell_to_hit_kernel_bandwith = 0,"
+      "},"
       "},"
       "}");
   ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())};

cartographer/mapping/2d/tsdf_2d.cc

@@ -16,10 +16,6 @@
 
 #include "cartographer/mapping/2d/tsdf_2d.h"
 
-#include <limits>
-
-#include "cartographer/common/make_unique.h"
-
 namespace cartographer {
 namespace mapping {
 
@@ -43,11 +39,16 @@ TSDF2D::TSDF2D(const proto::Grid2D& proto) : Grid2D(proto) {
   }
 }
 
+bool TSDF2D::CellIsUpdated(const Eigen::Array2i& cell_index) const {
+  const int flat_index = ToFlatIndex(cell_index);
+  uint16 tsdf_cell = correspondence_cost_cells()[flat_index];
+  return tsdf_cell >= value_converter_->getUpdateMarker();
+}
+
 void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd,
                      float weight) {
   const int flat_index = ToFlatIndex(cell_index);
   uint16* tsdf_cell = &(*mutable_correspondence_cost_cells())[flat_index];
-  uint16* weight_cell = &weight_cells_[flat_index];
   if (*tsdf_cell >= value_converter_->getUpdateMarker()) {
     return;
   }
@@ -55,8 +56,8 @@ void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd,
   mutable_known_cells_box()->extend(cell_index.matrix());
   *tsdf_cell =
       value_converter_->TSDToValue(tsd) + value_converter_->getUpdateMarker();
+  uint16* weight_cell = &weight_cells_[flat_index];
   *weight_cell = value_converter_->WeightToValue(weight);
-  return;
 }
 
 float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const {

cartographer/mapping/2d/tsdf_2d.h

@@ -46,6 +46,7 @@ class TSDF2D : public Grid2D {
   virtual bool DrawToSubmapTexture(
       proto::SubmapQuery::Response::SubmapTexture* const texture,
       transform::Rigid3d local_pose) const override;
+  bool CellIsUpdated(const Eigen::Array2i& cell_index) const;
 
  private:
   std::unique_ptr<TSDValueConverter> value_converter_;

cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc

@@ -0,0 +1,234 @@
+/*
+ * 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/2d/tsdf_range_data_inserter_2d.h"
+
+#include "cartographer/mapping/internal/2d/normal_estimation_2d.h"
+#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+// Factor for subpixel accuracy of start and end point for ray casts.
+constexpr int kSubpixelScale = 1000;
+// Minimum distance between range observation and origin. Otherwise, range
+// observations are discarded.
+constexpr float kMinRangeMeters = 1e-6f;
+constexpr float kSqrtTwoPi = std::sqrt(2.0 * M_PI);
+
+void GrowAsNeeded(const sensor::RangeData& range_data,
+                  const float truncation_distance, TSDF2D* const tsdf) {
+  Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
+  for (const Eigen::Vector3f& hit : range_data.returns) {
+    const Eigen::Vector3f direction = (hit - range_data.origin).normalized();
+    const Eigen::Vector3f end_position = hit + truncation_distance * direction;
+    bounding_box.extend(end_position.head<2>());
+  }
+  // Padding around bounding box to avoid numerical issues at cell boundaries.
+  constexpr float kPadding = 1e-6f;
+  tsdf->GrowLimits(bounding_box.min() - kPadding * Eigen::Vector2f::Ones());
+  tsdf->GrowLimits(bounding_box.max() + kPadding * Eigen::Vector2f::Ones());
+}
+
+float GaussianKernel(const float x, const float sigma) {
+  return 1.0 / (kSqrtTwoPi * sigma) * std::exp(-0.5 * x * x / (sigma * sigma));
+}
+
+std::pair<Eigen::Array2i, Eigen::Array2i> SuperscaleRay(
+    const Eigen::Vector2f& begin, const Eigen::Vector2f& end,
+    TSDF2D* const tsdf) {
+  const MapLimits& limits = tsdf->limits();
+  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
+  const MapLimits superscaled_limits(
+      superscaled_resolution, limits.max(),
+      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
+                 limits.cell_limits().num_y_cells * kSubpixelScale));
+
+  const Eigen::Array2i superscaled_begin =
+      superscaled_limits.GetCellIndex(begin);
+  const Eigen::Array2i superscaled_end = superscaled_limits.GetCellIndex(end);
+  return std::make_pair(superscaled_begin, superscaled_end);
+}
+
+struct RangeDataSorter {
+  RangeDataSorter(Eigen::Vector3f origin) { origin_ = origin.head<2>(); }
+  bool operator()(Eigen::Vector3f lhs, Eigen::Vector3f rhs) {
+    const Eigen::Vector2f delta_lhs = (lhs.head<2>() - origin_).normalized();
+    const Eigen::Vector2f delta_rhs = (lhs.head<2>() - origin_).normalized();
+    if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) {
+      return delta_lhs[1] < 0.f;
+    } else if (delta_lhs[1] < 0.f) {
+      return delta_lhs[0] < delta_rhs[0];
+    } else {
+      return delta_lhs[0] > delta_rhs[0];
+    }
+  }
+
+ private:
+  Eigen::Vector2f origin_;
+};
+
+float ComputeRangeWeightFactor(float range, int exponent) {
+  float weight = 0.f;
+  if (std::abs(range) > kMinRangeMeters) {
+    weight = 1.f / (std::pow(range, exponent));
+  }
+  return weight;
+}
+}  // namespace
+
+proto::TSDFRangeDataInserterOptions2D CreateTSDFRangeDataInserterOptions2D(
+    common::LuaParameterDictionary* parameter_dictionary) {
+  proto::TSDFRangeDataInserterOptions2D options;
+  options.set_truncation_distance(
+      parameter_dictionary->GetDouble("truncation_distance"));
+  options.set_maximum_weight(parameter_dictionary->GetDouble("maximum_weight"));
+  options.set_update_free_space(
+      parameter_dictionary->GetBool("update_free_space"));
+  *options
+       .mutable_normal_estimation_options() = CreateNormalEstimationOptions2D(
+      parameter_dictionary->GetDictionary("normal_estimation_options").get());
+  options.set_project_sdf_distance_to_scan_normal(
+      parameter_dictionary->GetBool("project_sdf_distance_to_scan_normal"));
+  options.set_update_weight_range_exponent(
+      parameter_dictionary->GetInt("update_weight_range_exponent"));
+  options.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith(
+      parameter_dictionary->GetDouble(
+          "update_weight_angle_scan_normal_to_ray_kernel_bandwith"));
+  options.set_update_weight_distance_cell_to_hit_kernel_bandwith(
+      parameter_dictionary->GetDouble(
+          "update_weight_distance_cell_to_hit_kernel_bandwith"));
+  return options;
+}
+
+TSDFRangeDataInserter2D::TSDFRangeDataInserter2D(
+    const proto::TSDFRangeDataInserterOptions2D& options)
+    : options_(options) {}
+
+// Casts a ray from origin towards hit for each hit in range data.
+// If 'options.update_free_space' is 'true', all cells along the ray
+// until 'truncation_distance' behind hit are updated. Otherwise, only the cells
+// within 'truncation_distance' around hit are updated.
+void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data,
+                                     GridInterface* grid) const {
+  const float truncation_distance =
+      static_cast<float>(options_.truncation_distance());
+  TSDF2D* tsdf = static_cast<TSDF2D*>(grid);
+  GrowAsNeeded(range_data, truncation_distance, tsdf);
+
+  // Compute normals if needed.
+  bool scale_update_weight_angle_scan_normal_to_ray =
+      options_.update_weight_angle_scan_normal_to_ray_kernel_bandwith() != 0.f;
+  sensor::RangeData sorted_range_data = range_data;
+  std::vector<float> normals;
+  if (options_.project_sdf_distance_to_scan_normal() ||
+      scale_update_weight_angle_scan_normal_to_ray) {
+    std::sort(sorted_range_data.returns.begin(),
+              sorted_range_data.returns.end(),
+              RangeDataSorter(sorted_range_data.origin));
+    normals = EstimateNormals(sorted_range_data,
+                              options_.normal_estimation_options());
+  }
+
+  const Eigen::Vector2f origin = sorted_range_data.origin.head<2>();
+  for (size_t hit_index = 0; hit_index < sorted_range_data.returns.size();
+       ++hit_index) {
+    const Eigen::Vector2f hit = sorted_range_data.returns[hit_index].head<2>();
+    const float normal = normals.empty()
+                             ? std::numeric_limits<float>::quiet_NaN()
+                             : normals[hit_index];
+    InsertHit(options_, hit, origin, normal, tsdf);
+  }
+  tsdf->FinishUpdate();
+}
+
+void TSDFRangeDataInserter2D::InsertHit(
+    const proto::TSDFRangeDataInserterOptions2D& options,
+    const Eigen::Vector2f& hit, const Eigen::Vector2f& origin, float normal,
+    TSDF2D* tsdf) const {
+  const Eigen::Vector2f ray = hit - origin;
+  const float range = ray.norm();
+  const float truncation_distance =
+      static_cast<float>(options_.truncation_distance());
+  if (range < truncation_distance) return;
+  const float truncation_ratio = truncation_distance / range;
+  const Eigen::Vector2f ray_begin =
+      options_.update_free_space() ? origin
+                                   : origin + (1.0f - truncation_ratio) * ray;
+  const Eigen::Vector2f ray_end = origin + (1.0f + truncation_ratio) * ray;
+  std::pair<Eigen::Array2i, Eigen::Array2i> superscaled_ray =
+      SuperscaleRay(ray_begin, ray_end, tsdf);
+  std::vector<Eigen::Array2i> ray_mask = RayToPixelMask(
+      superscaled_ray.first, superscaled_ray.second, kSubpixelScale);
+
+  // Precompute weight factors.
+  float weight_factor_angle_ray_normal = 1.f;
+  if (options_.update_weight_angle_scan_normal_to_ray_kernel_bandwith() !=
+      0.f) {
+    const Eigen::Vector2f negative_ray = -ray;
+    float angle_ray_normal =
+        common::NormalizeAngleDifference(normal - common::atan2(negative_ray));
+    weight_factor_angle_ray_normal = GaussianKernel(
+        angle_ray_normal,
+        options_.update_weight_angle_scan_normal_to_ray_kernel_bandwith());
+  }
+  float weight_factor_range = 1.f;
+  if (options_.update_weight_range_exponent() != 0) {
+    weight_factor_range = ComputeRangeWeightFactor(
+        range, options_.update_weight_range_exponent());
+  }
+
+  // Update Cells.
+  for (const Eigen::Array2i& cell_index : ray_mask) {
+    if (tsdf->CellIsUpdated(cell_index)) continue;
+    Eigen::Vector2f cell_center = tsdf->limits().GetCellCenter(cell_index);
+    float distance_cell_to_origin = (cell_center - origin).norm();
+    float update_tsd = range - distance_cell_to_origin;
+    if (options_.project_sdf_distance_to_scan_normal()) {
+      float normal_orientation = normal;
+      update_tsd = (cell_center - hit)
+                       .dot(Eigen::Vector2f{std::cos(normal_orientation),
+                                            std::sin(normal_orientation)});
+    }
+    update_tsd =
+        common::Clamp(update_tsd, -truncation_distance, truncation_distance);
+    float update_weight = weight_factor_range * weight_factor_angle_ray_normal;
+    if (options_.update_weight_distance_cell_to_hit_kernel_bandwith() != 0.f) {
+      update_weight *= GaussianKernel(
+          update_tsd,
+          options_.update_weight_distance_cell_to_hit_kernel_bandwith());
+    }
+    UpdateCell(cell_index, update_tsd, update_weight, tsdf);
+  }
+}
+
+void TSDFRangeDataInserter2D::UpdateCell(const Eigen::Array2i& cell,
+                                         float update_sdf, float update_weight,
+                                         TSDF2D* tsdf) const {
+  if (update_weight == 0.f) return;
+  const std::pair<float, float> tsd_and_weight = tsdf->GetTSDAndWeight(cell);
+  float updated_weight = tsd_and_weight.second + update_weight;
+  float updated_sdf = (tsd_and_weight.first * tsd_and_weight.second +
+                       update_sdf * update_weight) /
+                      updated_weight;
+  updated_weight =
+      std::min(updated_weight, static_cast<float>(options_.maximum_weight()));
+  tsdf->SetCell(cell, updated_sdf, updated_weight);
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/2d/tsdf_range_data_inserter_2d.h

@@ -0,0 +1,60 @@
+/*
+ * 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_2D_TSDF_RANGE_DATA_INSERTER_2D_H_
+#define CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/mapping/2d/tsdf_2d.h"
+#include "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.pb.h"
+#include "cartographer/mapping/range_data_inserter_interface.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/sensor/range_data.h"
+
+namespace cartographer {
+namespace mapping {
+
+proto::TSDFRangeDataInserterOptions2D CreateTSDFRangeDataInserterOptions2D(
+    common::LuaParameterDictionary* parameter_dictionary);
+
+class TSDFRangeDataInserter2D : public RangeDataInserterInterface {
+ public:
+  explicit TSDFRangeDataInserter2D(
+      const proto::TSDFRangeDataInserterOptions2D& options);
+
+  TSDFRangeDataInserter2D(const TSDFRangeDataInserter2D&) = delete;
+  TSDFRangeDataInserter2D& operator=(const TSDFRangeDataInserter2D&) = delete;
+
+  // Casts a ray from origin towards hit for each hit in range data.
+  // If 'options.update_free_space' is 'true', all cells along the ray
+  // until 'truncation_distance' behind hit are updated. Otherwise, only the
+  // cells within 'truncation_distance' around hit are updated.
+  virtual void Insert(const sensor::RangeData& range_data,
+                      GridInterface* grid) const override;
+
+ private:
+  void InsertHit(const proto::TSDFRangeDataInserterOptions2D& options,
+                 const Eigen::Vector2f& hit, const Eigen::Vector2f& origin,
+                 float normal, TSDF2D* tsdf) const;
+  void UpdateCell(const Eigen::Array2i& cell, float update_sdf,
+                  float update_weight, TSDF2D* tsdf) const;
+  const proto::TSDFRangeDataInserterOptions2D options_;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_

cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc

@@ -0,0 +1,348 @@
+/*
+ * 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/2d/tsdf_range_data_inserter_2d.h"
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
+#include "gmock/gmock.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+class RangeDataInserterTest2DTSDF : public ::testing::Test {
+ protected:
+  RangeDataInserterTest2DTSDF()
+      : tsdf_(MapLimits(1., Eigen::Vector2d(0., 7.), CellLimits(8, 1)), 2.0,
+              10.0) {
+    auto parameter_dictionary = common::MakeDictionary(
+        "return { "
+        "truncation_distance = 2.0,"
+        "maximum_weight = 10.,"
+        "update_free_space = false,"
+        "normal_estimation_options = {"
+        "num_normal_samples = 2,"
+        "sample_radius = 10.,"
+        "},"
+        "project_sdf_distance_to_scan_normal = false,"
+        "update_weight_range_exponent = 0,"
+        "update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,"
+        "update_weight_distance_cell_to_hit_kernel_bandwith = 0,"
+        "}");
+    options_ = CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get());
+    range_data_inserter_ =
+        common::make_unique<TSDFRangeDataInserter2D>(options_);
+  }
+
+  void InsertPoint() {
+    sensor::RangeData range_data;
+    range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
+    range_data.origin.x() = -0.5f;
+    range_data.origin.y() = -0.5f;
+    range_data_inserter_->Insert(range_data, &tsdf_);
+    tsdf_.FinishUpdate();
+  }
+
+  proto::TSDFRangeDataInserterOptions2D options_;
+  TSDF2D tsdf_;
+  std::unique_ptr<TSDFRangeDataInserter2D> range_data_inserter_;
+};
+
+class MockCellProperties {
+ public:
+  MockCellProperties(const Eigen::Array2i& cell_index, const TSDF2D& tsdf)
+      : is_known_(tsdf.IsKnown(cell_index)),
+        tsd_(tsdf.GetTSD(cell_index)),
+        weight_(tsdf.GetWeight(cell_index)){};
+
+  bool is_known_;
+  float tsd_;
+  float weight_;
+};
+
+::std::ostream& operator<<(::std::ostream& os, const MockCellProperties& bar) {
+  return os << std::to_string(bar.is_known_) + "\t" + std::to_string(bar.tsd_) +
+                   "\t" + std::to_string(bar.weight_);
+}
+
+MATCHER_P3(EqualCellProperties, expected_is_known, expected_tsd,
+           expected_weight,
+           std::string("Expected ") + std::to_string(expected_is_known) + "\t" +
+               std::to_string(expected_tsd) + "\t" +
+               std::to_string(expected_weight)) {
+  bool result = expected_is_known == arg.is_known_;
+  result = result && (std::abs(expected_tsd - arg.tsd_ < 1e-4));
+  result = result && (std::abs(expected_weight - arg.weight_ < 1e-2));
+  return result;
+}
+
+TEST_F(RangeDataInserterTest2DTSDF, InsertPoint) {
+  InsertPoint();
+  const float truncation_distance =
+      static_cast<float>(options_.truncation_distance());
+  const float maximum_weight = static_cast<float>(options_.maximum_weight());
+
+  for (float y = 1.5; y < 6.; ++y) {
+    // Cell intersects with ray.
+    float x = -0.5f;
+    Eigen::Array2i cell_index =
+        tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    float expected_tsdf =
+        std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
+    float expected_weight = 1.f;
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(true, expected_tsdf, expected_weight));
+    // Cells don't intersect with ray.
+    x = 0.5f;
+    cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    expected_tsdf = -truncation_distance;
+    expected_weight = 0.;
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(false, expected_tsdf, expected_weight));
+    x = 1.5f;
+    cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(false, expected_tsdf, expected_weight));
+  }
+
+  // Cells don't intersect with ray.
+  Eigen::Array2i cell_index =
+      tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.5, 6.5));
+  EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+              EqualCellProperties(false, -truncation_distance, 0.));
+  cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, -1.5));
+  EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+              EqualCellProperties(false, -truncation_distance, 0.));
+  for (int i = 0; i < 1000; ++i) {
+    InsertPoint();
+  }
+  for (float y = 1.5; y < 6.; ++y) {
+    // Cell intersects with ray.
+    float x = -0.5f;
+    Eigen::Array2i cell_index =
+        tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    float expected_tsdf =
+        std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(true, expected_tsdf, maximum_weight));
+  }
+}
+
+TEST_F(RangeDataInserterTest2DTSDF, InsertPointWithFreeSpaceUpdate) {
+  options_.set_update_free_space(true);
+  range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
+  InsertPoint();
+  const float truncation_distance =
+      static_cast<float>(options_.truncation_distance());
+  const float maximum_weight = static_cast<float>(options_.maximum_weight());
+
+  for (float y = -0.5; y < 6.; ++y) {
+    // Cells intersect with ray.
+    float x = -0.5f;
+    Eigen::Array2i cell_index =
+        tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    float expected_tsdf =
+        std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
+    float expected_weight = 1.f;
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(true, expected_tsdf, expected_weight));
+    // Cells don't intersect with ray.
+    x = 0.5f;
+    cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    expected_tsdf = -truncation_distance;
+    expected_weight = 0.;
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(false, expected_tsdf, expected_weight));
+    x = 1.5f;
+    cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(false, expected_tsdf, expected_weight));
+  }
+
+  // Cells don't intersect with the ray.
+  Eigen::Array2i cell_index =
+      tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, 6.5));
+  EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+              EqualCellProperties(false, -truncation_distance, 0.));
+  cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, -1.5));
+  EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+              EqualCellProperties(false, -truncation_distance, 0.));
+  for (int i = 0; i < 1000; ++i) {
+    InsertPoint();
+  }
+  for (float y = -0.5; y < 6.; ++y) {
+    // Cell intersects with ray.
+    float x = -0.5f;
+    Eigen::Array2i cell_index =
+        tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    float expected_tsdf =
+        std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(true, expected_tsdf, maximum_weight));
+  }
+}
+
+TEST_F(RangeDataInserterTest2DTSDF, InsertPointLinearWeight) {
+  options_.set_update_weight_range_exponent(1);
+  range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
+  InsertPoint();
+  const float truncation_distance =
+      static_cast<float>(options_.truncation_distance());
+  for (float y = 1.5; y < 6.; ++y) {
+    // Cell intersects with ray.
+    float x = -0.5f;
+    Eigen::Array2i cell_index =
+        tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    float expected_tsdf =
+        std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
+    float expected_weight = 1.f / 4.f;
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(true, expected_tsdf, expected_weight));
+  }
+}
+
+TEST_F(RangeDataInserterTest2DTSDF, InsertPointQuadraticWeight) {
+  options_.set_update_weight_range_exponent(2);
+  range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
+  InsertPoint();
+  const float truncation_distance =
+      static_cast<float>(options_.truncation_distance());
+  for (float y = 1.5; y < 6.; ++y) {
+    // Cell intersects with ray.
+    float x = -0.5f;
+    Eigen::Array2i cell_index =
+        tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    float expected_tsdf =
+        std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
+    float expected_weight = 1.f / std::pow(4.f, 2);
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(true, expected_tsdf, expected_weight));
+  }
+}
+
+TEST_F(RangeDataInserterTest2DTSDF,
+       InsertSmallAnglePointWithoutNormalProjection) {
+  sensor::RangeData range_data;
+  range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
+  range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
+  range_data.returns.emplace_back(10.5f, 3.5f, 0.f);
+  range_data.origin.x() = -0.5f;
+  range_data.origin.y() = -0.5f;
+  range_data_inserter_->Insert(range_data, &tsdf_);
+  tsdf_.FinishUpdate();
+  float x = 4.5f;
+  float y = 2.5f;
+  Eigen::Array2i cell_index =
+      tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+  Eigen::Vector2f ray =
+      Eigen::Vector2f(-0.5f, -0.5f) - Eigen::Vector2f(5.5f, 3.5f);
+  float ray_length = ray.norm();
+  Eigen::Vector2f origin_to_cell =
+      Eigen::Vector2f(x, y) - Eigen::Vector2f(-0.5f, -0.5f);
+  float distance_origin_to_cell = origin_to_cell.norm();
+  float expected_tsdf = ray_length - distance_origin_to_cell;
+  float expected_weight = 1.f;
+  EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+              EqualCellProperties(true, expected_tsdf, expected_weight));
+}
+
+TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWitNormalProjection) {
+  options_.set_project_sdf_distance_to_scan_normal(true);
+  range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
+  sensor::RangeData range_data;
+  range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
+  range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
+  range_data.origin.x() = -0.5f;
+  range_data.origin.y() = -0.5f;
+  range_data_inserter_->Insert(range_data, &tsdf_);
+  tsdf_.FinishUpdate();
+  float x = 4.5f;
+  float y = 2.5f;
+  Eigen::Array2i cell_index =
+      tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+  float expected_tsdf = 1.f;
+  float expected_weight = 1.f;
+  EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+              EqualCellProperties(true, expected_tsdf, expected_weight));
+  x = 6.5f;
+  y = 4.5f;
+  cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+  expected_tsdf = -1.f;
+  expected_weight = 1.f;
+  EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+              EqualCellProperties(true, expected_tsdf, expected_weight));
+}
+
+TEST_F(RangeDataInserterTest2DTSDF,
+       InsertPointsWithAngleScanNormalToRayWeight) {
+  float bandwith = 10.f;
+  options_.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith(bandwith);
+  range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
+  sensor::RangeData range_data;
+  range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
+  range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
+  range_data.origin.x() = -0.5f;
+  range_data.origin.y() = -0.5f;
+  range_data_inserter_->Insert(range_data, &tsdf_);
+  tsdf_.FinishUpdate();
+  float x = -0.5f;
+  float y = 3.5f;
+  // Ray is perpendicular to surface.
+  Eigen::Array2i cell_index =
+      tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+  float expected_weight = 1.f / (std::sqrt(2 * M_PI) * bandwith);
+  EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3);
+  x = 6.5f;
+  y = 4.5f;
+  cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+  EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3);
+  // Ray is inclined relative to surface.
+  cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+  float angle = std::atan(7.f / 5.f);
+  expected_weight = 1.f / (std::sqrt(2 * M_PI) * bandwith) *
+                    std::exp(angle * angle / (2 * std::pow(bandwith, 2)));
+  EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3);
+  x = 6.5f;
+  y = 4.5f;
+  cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+  EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3);
+}
+
+TEST_F(RangeDataInserterTest2DTSDF, InsertPointsWithDistanceCellToHit) {
+  float bandwith = 10.f;
+  options_.set_update_weight_distance_cell_to_hit_kernel_bandwith(bandwith);
+  range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
+  InsertPoint();
+  const float truncation_distance =
+      static_cast<float>(options_.truncation_distance());
+  for (float y = 1.5; y < 6.; ++y) {
+    float x = -0.5f;
+    Eigen::Array2i cell_index =
+        tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
+    float expected_tsdf =
+        std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
+    float expected_weight =
+        1.f / (std::sqrt(2 * M_PI) * bandwith) *
+        std::exp(std::pow(expected_tsdf, 2) / (2 * std::pow(bandwith, 2)));
+    EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
+                EqualCellProperties(true, expected_tsdf, expected_weight));
+  }
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/internal/2d/pose_graph_2d_test.cc

@@ -62,8 +62,21 @@ class PoseGraph2DTest : public ::testing::Test {
                 hit_probability = 0.53,
                 miss_probability = 0.495,
               },
+            tsdf_range_data_inserter = {
+              truncation_distance = 0.3,
+              maximum_weight = 10.,
+              update_free_space = false,
+              normal_estimation_options = {
+                num_normal_samples = 4,
+                sample_radius = 0.5,
+              },
+              project_sdf_distance_to_scan_normal = false,
+              update_weight_range_exponent = 0,
+              update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,
+              update_weight_distance_cell_to_hit_kernel_bandwith = 0,
             },
-          })text");
+          },
+        })text");
       active_submaps_ = common::make_unique<ActiveSubmaps2D>(
           mapping::CreateSubmapsOptions2D(parameter_dictionary.get()));
     }

cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto

@@ -0,0 +1,47 @@
+// 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.
+
+syntax = "proto3";
+
+package cartographer.mapping.proto;
+
+import "cartographer/mapping/proto/2d/normal_estimation_options_2d.proto";
+
+message TSDFRangeDataInserterOptions2D {
+  // Distance to the surface within the signed distance function is evaluated.
+  double truncation_distance = 1;
+  // Maximum weight that can be stored in a cell.
+  double maximum_weight = 2;
+
+  // Enables updating cells between the sensor origin and the range observation
+  // as free space.
+  bool update_free_space = 3;
+
+  NormalEstimationOptions2D normal_estimation_options = 4;
+
+  // Project the distance between the updated cell und the range observation to
+  // the estimated scan normal.
+  bool project_sdf_distance_to_scan_normal = 5;
+
+  // Update weight is scaled with 1/distance(origin,hit)^range_exponent.
+  int32 update_weight_range_exponent = 6;
+
+  // Kernel bandwith of the weight factor based on the angle between.
+  // scan normal and ray
+  double update_weight_angle_scan_normal_to_ray_kernel_bandwith = 7;
+
+  // Kernel bandwith of the weight factor based on the distance between
+  // cell and scan observation.
+  double update_weight_distance_cell_to_hit_kernel_bandwith = 8;
+}

cartographer/mapping/proto/range_data_inserter_options.proto

@@ -15,6 +15,7 @@
 syntax = "proto3";
 
 import "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto";
+import "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto";
 
 package cartographer.mapping.proto;
 
@@ -22,9 +23,11 @@ message RangeDataInserterOptions {
   enum RangeDataInserterType {
     INVALID_INSERTER = 0;
     PROBABILITY_GRID_INSERTER_2D = 1;
+    TSDF_INSERTER_2D = 2;
   }
 
   RangeDataInserterType range_data_inserter_type = 1;
   ProbabilityGridRangeDataInserterOptions2D
       probability_grid_range_data_inserter_options_2d = 2;
+  TSDFRangeDataInserterOptions2D tsdf_range_data_inserter_options_2d = 3;
 }

cartographer/mapping/range_data_inserter_interface.cc

@@ -16,6 +16,7 @@
 
 #include "cartographer/mapping/range_data_inserter_interface.h"
 #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
+#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h"
 
 namespace cartographer {
 namespace mapping {
@@ -37,6 +38,10 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
           parameter_dictionary
               ->GetDictionary("probability_grid_range_data_inserter")
               .get());
+  *options.mutable_tsdf_range_data_inserter_options_2d() =
+      CreateTSDFRangeDataInserterOptions2D(
+          parameter_dictionary->GetDictionary("tsdf_range_data_inserter")
+              .get());
   return options;
 }
 }  // namespace mapping

configuration_files/trajectory_builder_2d.lua

@@ -74,6 +74,19 @@ TRAJECTORY_BUILDER_2D = {
         hit_probability = 0.55,
         miss_probability = 0.49,
       },
+      tsdf_range_data_inserter = {
+        truncation_distance = 0.3,
+        maximum_weight = 10.,
+        update_free_space = false,
+        normal_estimation_options = {
+          num_normal_samples = 4,
+          sample_radius = 0.5,
+        },
+        project_sdf_distance_to_scan_normal = false,
+        update_weight_range_exponent = 0,
+        update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,
+        update_weight_distance_cell_to_hit_kernel_bandwith = 0,
+      },
     },
   },
 }