diff --git a/cartographer/mapping/internal/tsd_value_converter.cc b/cartographer/mapping/internal/tsd_value_converter.cc new file mode 100644 index 0000000..d7dceee --- /dev/null +++ b/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 TSDValueConverter::PrecomputeValueToTSD() { + std::vector result; + size_t num_values = std::numeric_limits::max() + 1; + result.reserve(num_values); + for (size_t value = 0; value != num_values; ++value) { + result.push_back( + SlowValueToTSD(static_cast(value) & ~update_marker_)); + } + return result; +} + +// 0 is unknown, [1, 32767] maps to [min_weight_ max_weight_]. +float TSDValueConverter::SlowValueToWeight(const uint16 value) const { + CHECK_GE(value, 0); + CHECK_LE(value, 32767); + if (value == unknown_weight_value_) { + // Unknown cells have min_weight_. + return min_weight_; + } + const float kScale = (max_weight_ - min_weight_) / 32766.f; + return value * kScale + (min_weight_ - kScale); +} + +std::vector TSDValueConverter::PrecomputeValueToWeight() { + std::vector result; + size_t num_values = std::numeric_limits::max() + 1; + result.reserve(num_values); + for (size_t value = 0; value != num_values; ++value) { + result.push_back( + SlowValueToWeight(static_cast(value) & ~update_marker_)); + } + return result; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/tsd_value_converter.h b/cartographer/mapping/internal/tsd_value_converter.h new file mode 100644 index 0000000..56bd1f5 --- /dev/null +++ b/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 +#include + +#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 PrecomputeValueToTSD(); + float SlowValueToWeight(const uint16 value) const; + std::vector 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 value_to_tsd_; + std::vector value_to_weight_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_TSD_VALUE_CONVERTER_H_ diff --git a/cartographer/mapping/internal/tsd_value_converter_test.cc b/cartographer/mapping/internal/tsd_value_converter_test.cc new file mode 100644 index 0000000..98ff6d5 --- /dev/null +++ b/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