Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/ValueConversionTables.cs

@@ -0,0 +1,95 @@
+/*
+ * 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);
+    }
+}