Initial commit

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

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