96 lines
3.8 KiB
C#
96 lines
3.8 KiB
C#
/*
|
|
* 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.
|
|
*/
|
|
|
|
namespace CartographerSharp.Mapping;
|
|
|
|
/// <summary>
|
|
/// Performs lazy computations of lookup tables for mapping from a uint16 value
|
|
/// to a double in ['lower_bound', 'upper_bound']. The first element of the table
|
|
/// is set to 'unknown_result'.
|
|
/// </summary>
|
|
public class ValueConversionTables
|
|
{
|
|
private readonly Dictionary<(double unknownResult, double lowerBound, double upperBound), double[]> _boundsToLookupTable;
|
|
|
|
public ValueConversionTables()
|
|
{
|
|
_boundsToLookupTable = [];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets or creates a conversion table for the given bounds.
|
|
/// </summary>
|
|
public double[] GetConversionTable(double unknownResult, double lowerBound, double upperBound)
|
|
{
|
|
var key = (unknownResult, lowerBound, upperBound);
|
|
|
|
if (!_boundsToLookupTable.TryGetValue(key, out var table))
|
|
{
|
|
table = PrecomputeValueToBoundedFloat(unknownResult, lowerBound, upperBound);
|
|
_boundsToLookupTable[key] = table;
|
|
}
|
|
|
|
return table;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Precomputes value to bounded double lookup table.
|
|
/// Match C++: size_t num_values = std::numeric_limits<uint16>::max() + 1; (65536)
|
|
/// C++: PrecomputeValueToBoundedFloat masks out update marker: value & ~kUpdateMarker
|
|
/// </summary>
|
|
private static double[] PrecomputeValueToBoundedFloat(
|
|
double unknownResult, double lowerBound, double upperBound)
|
|
{
|
|
// Match C++: num_values = std::numeric_limits<uint16>::max() + 1 = 65536
|
|
const int kValueCount = 65536; // All possible ushort values (0-65535)
|
|
const ushort kUpdateMarker = (ushort)(1u << 15); // Highest bit
|
|
var result = new double[kValueCount];
|
|
|
|
// Match C++: for (size_t value = 0; value != num_values; ++value)
|
|
// C++: SlowValueToBoundedFloat(static_cast<uint16>(value) & ~kUpdateMarker, ...)
|
|
// Mask out update marker before computing conversion
|
|
for (int value = 0; value != kValueCount; value++)
|
|
{
|
|
// Match C++: Mask out update marker: value & ~kUpdateMarker
|
|
var valueWithoutMarker = (ushort)(value & ~kUpdateMarker);
|
|
result[value] = SlowValueToBoundedFloat(
|
|
valueWithoutMarker, unknownResult, lowerBound, upperBound);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Slow conversion from value to bounded double (used for precomputation).
|
|
/// Match C++: CHECK_LE(value, 32767) and kScale calculation.
|
|
/// </summary>
|
|
private static double SlowValueToBoundedFloat(
|
|
ushort value, double unknownResult, double lowerBound, double upperBound)
|
|
{
|
|
// Match C++: CHECK_LE(value, 32767) - value should be <= 32767 after masking
|
|
// (value is already masked in PrecomputeValueToBoundedFloat)
|
|
const ushort kUnknownValue = 0;
|
|
if (value == kUnknownValue)
|
|
{
|
|
return unknownResult;
|
|
}
|
|
|
|
// Match C++: const double kScale = (upper_bound - lower_bound) / 32766.f;
|
|
var kScale = (upperBound - lowerBound) / 32766.0;
|
|
return value * kScale + (lowerBound - kScale);
|
|
}
|
|
}
|