71 lines
2.6 KiB
C++
Executable File
71 lines
2.6 KiB
C++
Executable File
/*
|
|
* 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 "absl/memory/memory.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 = absl::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
|