308 lines
10 KiB
C#
308 lines
10 KiB
C#
/*
|
|
* 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.
|
|
*/
|
|
|
|
using CartographerSharp.Common.Math;
|
|
|
|
namespace CartographerSharp.Mapping;
|
|
|
|
/// <summary>
|
|
/// Probability value conversion utilities.
|
|
/// </summary>
|
|
public static class ProbabilityValues
|
|
{
|
|
public const double kMinProbability = 0.1;
|
|
public const double kMaxProbability = 1.0 - kMinProbability;
|
|
public const double kMinCorrespondenceCost = 1.0 - kMaxProbability;
|
|
public const double kMaxCorrespondenceCost = 1.0 - kMinProbability;
|
|
|
|
private const ushort kUnknownProbabilityValue = 0;
|
|
private const ushort kUnknownCorrespondenceValue = kUnknownProbabilityValue;
|
|
private const ushort kUpdateMarker = (ushort)(1u << 15);
|
|
|
|
private const int kValueCount = 32768;
|
|
|
|
// Precomputed lookup tables
|
|
private static readonly double[] kValueToProbability;
|
|
private static readonly double[] kValueToCorrespondenceCost;
|
|
|
|
static ProbabilityValues()
|
|
{
|
|
kValueToProbability = PrecomputeValueToProbability();
|
|
kValueToCorrespondenceCost = PrecomputeValueToCorrespondenceCost();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts probability to odds.
|
|
/// </summary>
|
|
public static double Odds(double probability)
|
|
{
|
|
return probability / (1.0 - probability);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts odds to probability.
|
|
/// </summary>
|
|
public static double ProbabilityFromOdds(double odds)
|
|
{
|
|
return odds / (odds + 1.0);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts probability to correspondence cost.
|
|
/// </summary>
|
|
public static double ProbabilityToCorrespondenceCost(double probability)
|
|
{
|
|
return 1.0 - probability;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts correspondence cost to probability.
|
|
/// </summary>
|
|
public static double CorrespondenceCostToProbability(double correspondenceCost)
|
|
{
|
|
return 1.0 - correspondenceCost;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clamps probability to be in the range [kMinProbability, kMaxProbability].
|
|
/// </summary>
|
|
public static double ClampProbability(double probability)
|
|
{
|
|
return MathUtils.Clamp(probability, kMinProbability, kMaxProbability);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clamps correspondence cost to be in the range [kMinCorrespondenceCost, kMaxCorrespondenceCost].
|
|
/// </summary>
|
|
public static double ClampCorrespondenceCost(double correspondenceCost)
|
|
{
|
|
return MathUtils.Clamp(correspondenceCost, kMinCorrespondenceCost, kMaxCorrespondenceCost);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts a correspondence_cost to a uint16 in the [1, 32767] range.
|
|
/// </summary>
|
|
public static ushort CorrespondenceCostToValue(double correspondenceCost)
|
|
{
|
|
return BoundedFloatToValue(correspondenceCost, kMinCorrespondenceCost, kMaxCorrespondenceCost);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts a probability to a uint16 in the [1, 32767] range.
|
|
/// </summary>
|
|
public static ushort ProbabilityToValue(double probability)
|
|
{
|
|
return BoundedFloatToValue(probability, kMinProbability, kMaxProbability);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts a uint16 (which may or may not have the update marker set) to a
|
|
/// probability in the range [kMinProbability, kMaxProbability].
|
|
/// </summary>
|
|
public static double ValueToProbability(ushort value)
|
|
{
|
|
if (value >= kValueToProbability.Length)
|
|
{
|
|
return kMinProbability;
|
|
}
|
|
return kValueToProbability[value];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts a uint16 (which may or may not have the update marker set) to a
|
|
/// correspondence cost in the range [kMinCorrespondenceCost, kMaxCorrespondenceCost].
|
|
/// </summary>
|
|
public static double ValueToCorrespondenceCost(ushort value)
|
|
{
|
|
if (value >= kValueToCorrespondenceCost.Length)
|
|
{
|
|
return kMaxCorrespondenceCost;
|
|
}
|
|
return kValueToCorrespondenceCost[value];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts probability value to correspondence cost value.
|
|
/// </summary>
|
|
public static ushort ProbabilityValueToCorrespondenceCostValue(ushort probabilityValue)
|
|
{
|
|
if (probabilityValue == kUnknownProbabilityValue)
|
|
{
|
|
return kUnknownCorrespondenceValue;
|
|
}
|
|
|
|
bool updateCarry = false;
|
|
if (probabilityValue > kUpdateMarker)
|
|
{
|
|
probabilityValue -= kUpdateMarker;
|
|
updateCarry = true;
|
|
}
|
|
|
|
ushort result = CorrespondenceCostToValue(
|
|
ProbabilityToCorrespondenceCost(ValueToProbability(probabilityValue)));
|
|
if (updateCarry)
|
|
{
|
|
result += kUpdateMarker;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts correspondence cost value to probability value.
|
|
/// </summary>
|
|
public static ushort CorrespondenceCostValueToProbabilityValue(ushort correspondenceCostValue)
|
|
{
|
|
if (correspondenceCostValue == kUnknownCorrespondenceValue)
|
|
{
|
|
return kUnknownProbabilityValue;
|
|
}
|
|
|
|
bool updateCarry = false;
|
|
if (correspondenceCostValue > kUpdateMarker)
|
|
{
|
|
correspondenceCostValue -= kUpdateMarker;
|
|
updateCarry = true;
|
|
}
|
|
|
|
ushort result = ProbabilityToValue(CorrespondenceCostToProbability(
|
|
ValueToCorrespondenceCost(correspondenceCostValue)));
|
|
if (updateCarry)
|
|
{
|
|
result += kUpdateMarker;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes lookup table to apply odds.
|
|
/// </summary>
|
|
public static List<ushort> ComputeLookupTableToApplyOdds(double odds)
|
|
{
|
|
var result = new List<ushort>(kValueCount)
|
|
{
|
|
(ushort)(ProbabilityToValue(ProbabilityFromOdds(odds)) + kUpdateMarker)
|
|
};
|
|
|
|
for (int cell = 1; cell != kValueCount; cell++)
|
|
{
|
|
result.Add((ushort)(ProbabilityToValue(ProbabilityFromOdds(
|
|
odds * Odds(ValueToProbability((ushort)cell)))) + kUpdateMarker));
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes lookup table to apply correspondence cost odds.
|
|
/// </summary>
|
|
public static List<ushort> ComputeLookupTableToApplyCorrespondenceCostOdds(double odds)
|
|
{
|
|
var result = new List<ushort>(kValueCount)
|
|
{
|
|
(ushort)(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
|
|
ProbabilityFromOdds(odds))) + kUpdateMarker)
|
|
};
|
|
|
|
for (int cell = 1; cell != kValueCount; cell++)
|
|
{
|
|
result.Add((ushort)(CorrespondenceCostToValue(
|
|
ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
|
|
odds * Odds(CorrespondenceCostToProbability(
|
|
ValueToCorrespondenceCost((ushort)cell)))))) + kUpdateMarker));
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts a bounded double value to a uint16 value.
|
|
/// </summary>
|
|
private static ushort BoundedFloatToValue(double floatValue, double lowerBound, double upperBound)
|
|
{
|
|
var clamped = MathUtils.Clamp(floatValue, lowerBound, upperBound);
|
|
// Match C++: common::RoundToInt uses std::lround which rounds to nearest integer
|
|
// with rounding half away from zero (not banker's rounding)
|
|
var value = (int)Math.Round(
|
|
(clamped - lowerBound) * (32766.0 / (upperBound - lowerBound)),
|
|
MidpointRounding.AwayFromZero) + 1;
|
|
|
|
// Clamp to valid range
|
|
value = Math.Max(1, Math.Min(32767, value));
|
|
return (ushort)value;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Precomputes value to probability lookup table.
|
|
/// </summary>
|
|
private static double[] PrecomputeValueToProbability()
|
|
{
|
|
return PrecomputeValueToBoundedFloat(
|
|
kUnknownProbabilityValue, kMinProbability, kMinProbability, kMaxProbability);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Precomputes value to correspondence cost lookup table.
|
|
/// </summary>
|
|
private static double[] PrecomputeValueToCorrespondenceCost()
|
|
{
|
|
return PrecomputeValueToBoundedFloat(
|
|
kUnknownCorrespondenceValue, kMaxCorrespondenceCost,
|
|
kMinCorrespondenceCost, kMaxCorrespondenceCost);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Precomputes value to bounded double lookup table.
|
|
/// </summary>
|
|
private static double[] PrecomputeValueToBoundedFloat(
|
|
ushort unknownValue, double unknownResult, double lowerBound, double upperBound)
|
|
{
|
|
// Repeat two times, so that both values with and without the update marker
|
|
// can be converted to a probability.
|
|
const int kRepetitionCount = 2;
|
|
var result = new double[kRepetitionCount * kValueCount];
|
|
|
|
for (int repeat = 0; repeat != kRepetitionCount; repeat++)
|
|
{
|
|
for (int value = 0; value != kValueCount; value++)
|
|
{
|
|
result[repeat * kValueCount + value] = SlowValueToBoundedFloat(
|
|
(ushort)value, unknownValue, unknownResult, lowerBound, upperBound);
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Slow conversion from value to bounded double (used for precomputation).
|
|
/// </summary>
|
|
private static double SlowValueToBoundedFloat(
|
|
ushort value, ushort unknownValue, double unknownResult,
|
|
double lowerBound, double upperBound)
|
|
{
|
|
if (value == unknownValue)
|
|
{
|
|
return unknownResult;
|
|
}
|
|
|
|
// Match C++: const double kScale = (upper_bound - lower_bound) / 32766.f;
|
|
// kValueCount = 32768, so kValueCount - 2.0 = 32766.0
|
|
// Use explicit 32766.0 for consistency with C++ and ValueConversionTables
|
|
var kScale = (upperBound - lowerBound) / 32766.0;
|
|
return value * kScale + (lowerBound - kScale);
|
|
}
|
|
}
|