Truncated Signed Distance Value Converter (#1204)

Introduces conversions between float and uint16 representations for truncated signed distance values and weights.

cartographer/mapping/internal/tsd_value_converter.cc

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

cartographer/mapping/internal/tsd_value_converter.h

@@ -0,0 +1,106 @@
+/*
+ * 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_TSD_VALUE_CONVERTER_H_
+#define CARTOGRAPHER_MAPPING_TSD_VALUE_CONVERTER_H_
+
+#include <cmath>
+#include <vector>
+
+#include "cartographer/common/math.h"
+#include "cartographer/common/port.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+// Provides conversions between float and uint16 representations for
+// truncated signed distance values and weights.
+class TSDValueConverter {
+ public:
+  TSDValueConverter(float max_tsd, float max_weight);
+
+  // Converts a tsd to a uint16 in the [1, 32767] range.
+  inline uint16 TSDToValue(const float tsd) const {
+    const int value =
+        common::RoundToInt((ClampTSD(tsd) - min_tsd_) * tsd_resolution_) + 1;
+    DCHECK_GE(value, 1);
+    DCHECK_LE(value, 32767);
+    return value;
+  }
+
+  // Converts a weight to a uint16 in the [1, 32767] range.
+  inline uint16 WeightToValue(const float weight) const {
+    const int value = common::RoundToInt((ClampWeight(weight) - min_weight_) *
+                                         weight_resolution_) +
+                      1;
+    DCHECK_GE(value, 1);
+    DCHECK_LE(value, 32767);
+    return value;
+  }
+
+  // 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];
+  }
+
+  // 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];
+  }
+
+  static uint16 getUnknownTSDValue() { return unknown_tsd_value_; }
+  static uint16 getUnknownWeightValue() { return unknown_weight_value_; }
+  static uint16 getUpdateMarker() { return update_marker_; }
+  float getMaxTSD() const { return max_tsd_; }
+  float getMinTSD() const { return min_tsd_; }
+  float getMaxWeight() const { return max_weight_; }
+  float getMinWeight() const { return min_weight_; }
+
+ private:
+  // Clamps TSD to be in the range [min_tsd_, max_tsd_].
+  inline float ClampTSD(const float tsd) const {
+    return common::Clamp(tsd, min_tsd_, max_tsd_);
+  }
+  // Clamps weight to be in the range [min_tsd_, max_tsd_].
+  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_;
+  float max_weight_;
+  float tsd_resolution_;
+  float weight_resolution_;
+  static constexpr float min_weight_ = 0.f;
+  static constexpr uint16 unknown_tsd_value_ = 0;
+  static constexpr uint16 unknown_weight_value_ = 0;
+  static constexpr uint16 update_marker_ = 1u << 15;
+
+  std::vector<float> value_to_tsd_;
+  std::vector<float> value_to_weight_;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_TSD_VALUE_CONVERTER_H_

cartographer/mapping/internal/tsd_value_converter_test.cc

@@ -0,0 +1,122 @@
+/*
+ * 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/internal/tsd_value_converter.h"
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+class TSDValueConverterTest : public ::testing::Test {
+ protected:
+  TSDValueConverterTest()
+      : truncation_distance_(0.1f),
+        max_weight_(10.0f),
+        tsd_value_converter_(truncation_distance_, max_weight_) {}
+  float truncation_distance_;
+  float max_weight_;
+  TSDValueConverter tsd_value_converter_;
+};
+
+TEST_F(TSDValueConverterTest, DefaultValues) {
+  EXPECT_EQ(tsd_value_converter_.getUnknownWeightValue(), 0);
+  EXPECT_EQ(tsd_value_converter_.getUnknownTSDValue(), 0);
+  EXPECT_EQ(tsd_value_converter_.getMinTSD(), -truncation_distance_);
+  EXPECT_EQ(tsd_value_converter_.getMaxTSD(), truncation_distance_);
+  EXPECT_EQ(tsd_value_converter_.getMinWeight(), 0.f);
+  EXPECT_EQ(tsd_value_converter_.getMaxWeight(), max_weight_);
+}
+
+TEST_F(TSDValueConverterTest, ValueToTSDConversions) {
+  for (uint16 i = 1; i < 32768; ++i) {
+    EXPECT_EQ(
+        tsd_value_converter_.TSDToValue(tsd_value_converter_.ValueToTSD(i)), i);
+  }
+}
+
+TEST_F(TSDValueConverterTest, ValueToTSDConversionsWithUpdateMarker) {
+  for (uint16 i = 1; i < 32768; ++i) {
+    EXPECT_EQ(tsd_value_converter_.TSDToValue(tsd_value_converter_.ValueToTSD(
+                  i + tsd_value_converter_.getUpdateMarker())),
+              i);
+  }
+}
+
+TEST_F(TSDValueConverterTest, ValueToWeightConversions) {
+  for (uint16 i = 1; i < 32768; ++i) {
+    EXPECT_EQ(tsd_value_converter_.WeightToValue(
+                  tsd_value_converter_.ValueToWeight(i)),
+              i);
+  }
+}
+
+TEST_F(TSDValueConverterTest, ValueToWeightConversionsWithUpdateMarker) {
+  for (uint16 i = 1; i < 32768; ++i) {
+    EXPECT_EQ(
+        tsd_value_converter_.WeightToValue(tsd_value_converter_.ValueToWeight(
+            i + tsd_value_converter_.getUpdateMarker())),
+        i);
+  }
+}
+
+TEST_F(TSDValueConverterTest, TSDToValueConversions) {
+  uint16 num_samples = 1000;
+  float tolerance = truncation_distance_ * 2.f / 32767.f;
+  for (uint16 i = 0; i < num_samples; ++i) {
+    float sdf_sample =
+        -truncation_distance_ + i * 2.f * truncation_distance_ / num_samples;
+    EXPECT_NEAR(tsd_value_converter_.ValueToTSD(
+                    tsd_value_converter_.TSDToValue(sdf_sample)),
+                sdf_sample, tolerance);
+  }
+}
+
+TEST_F(TSDValueConverterTest, WeightToValueConversions) {
+  uint16 num_samples = 1000;
+  float tolerance = max_weight_ / 32767.f;
+  for (uint16 i = 0; i < num_samples; ++i) {
+    float weight_sample = i * max_weight_ / num_samples;
+    EXPECT_NEAR(tsd_value_converter_.ValueToWeight(
+                    tsd_value_converter_.WeightToValue(weight_sample)),
+                weight_sample, tolerance);
+  }
+}
+
+TEST_F(TSDValueConverterTest, WeightToValueOutOfRangeConversions) {
+  float tolerance = max_weight_ / 32767.f;
+  EXPECT_NEAR(tsd_value_converter_.ValueToWeight(
+                  tsd_value_converter_.WeightToValue(2.f * max_weight_)),
+              max_weight_, tolerance);
+  EXPECT_NEAR(tsd_value_converter_.ValueToWeight(
+                  tsd_value_converter_.WeightToValue(-max_weight_)),
+              0.f, tolerance);
+}
+
+TEST_F(TSDValueConverterTest, TSDToValueOutOfRangeConversions) {
+  float tolerance = truncation_distance_ * 2.f / 32767.f;
+  EXPECT_NEAR(tsd_value_converter_.ValueToTSD(
+                  tsd_value_converter_.TSDToValue(2.f * truncation_distance_)),
+              truncation_distance_, tolerance);
+  EXPECT_NEAR(tsd_value_converter_.ValueToTSD(
+                  tsd_value_converter_.TSDToValue(-2.f * truncation_distance_)),
+              -truncation_distance_, tolerance);
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer