109 lines
3.9 KiB
C++
Executable File
109 lines
3.9 KiB
C++
Executable File
/*
|
|
* Copyright 2016 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/probability_values.h"
|
|
|
|
#include "absl/memory/memory.h"
|
|
|
|
namespace cartographer {
|
|
namespace mapping {
|
|
|
|
namespace {
|
|
|
|
constexpr int kValueCount = 32768;
|
|
|
|
// 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_LT(value, kValueCount);
|
|
if (value == unknown_value) return unknown_result;
|
|
const float kScale = (upper_bound - lower_bound) / (kValueCount - 2.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>>();
|
|
// Repeat two times, so that both values with and without the update marker
|
|
// can be converted to a probability.
|
|
constexpr int kRepetitionCount = 2;
|
|
result->reserve(kRepetitionCount * kValueCount);
|
|
for (int repeat = 0; repeat != kRepetitionCount; ++repeat) {
|
|
for (int value = 0; value != kValueCount; ++value) {
|
|
result->push_back(SlowValueToBoundedFloat(
|
|
value, unknown_value, unknown_result, lower_bound, upper_bound));
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
std::unique_ptr<std::vector<float>> PrecomputeValueToProbability() {
|
|
return PrecomputeValueToBoundedFloat(kUnknownProbabilityValue,
|
|
kMinProbability, kMinProbability,
|
|
kMaxProbability);
|
|
}
|
|
|
|
std::unique_ptr<std::vector<float>> PrecomputeValueToCorrespondenceCost() {
|
|
return PrecomputeValueToBoundedFloat(
|
|
kUnknownCorrespondenceValue, kMaxCorrespondenceCost,
|
|
kMinCorrespondenceCost, kMaxCorrespondenceCost);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
const std::vector<float>* const kValueToProbability =
|
|
PrecomputeValueToProbability().release();
|
|
|
|
const std::vector<float>* const kValueToCorrespondenceCost =
|
|
PrecomputeValueToCorrespondenceCost().release();
|
|
|
|
std::vector<uint16> ComputeLookupTableToApplyOdds(const float odds) {
|
|
std::vector<uint16> result;
|
|
result.reserve(kValueCount);
|
|
result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) +
|
|
kUpdateMarker);
|
|
for (int cell = 1; cell != kValueCount; ++cell) {
|
|
result.push_back(ProbabilityToValue(ProbabilityFromOdds(
|
|
odds * Odds((*kValueToProbability)[cell]))) +
|
|
kUpdateMarker);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(
|
|
float odds) {
|
|
std::vector<uint16> result;
|
|
result.reserve(kValueCount);
|
|
result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
|
|
ProbabilityFromOdds(odds))) +
|
|
kUpdateMarker);
|
|
for (int cell = 1; cell != kValueCount; ++cell) {
|
|
result.push_back(
|
|
CorrespondenceCostToValue(
|
|
ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
|
|
odds * Odds(CorrespondenceCostToProbability(
|
|
(*kValueToCorrespondenceCost)[cell]))))) +
|
|
kUpdateMarker);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
} // namespace mapping
|
|
} // namespace cartographer
|