Initial commit

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

@@ -0,0 +1,102 @@
+/*
+ * 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.Transform;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// An individual submap, which has a 'local_pose' in the local map frame, keeps
+/// track of how many range data were inserted into it, and sets
+/// 'insertion_finished' when the map no longer changes and is ready for loop
+/// closing.
+/// </summary>
+public abstract class Submap(Rigid3d localSubmapPose)
+{
+    private int _numRangeData = 0;
+    private bool _insertionFinished = false;
+
+    /// <summary>
+    /// Pose of this submap in the local map frame.
+    /// </summary>
+    public Rigid3d LocalPose => localSubmapPose;
+
+    /// <summary>
+    /// Number of RangeData inserted.
+    /// </summary>
+    public int NumRangeData
+    {
+        get => Volatile.Read(ref _numRangeData);
+        set => Volatile.Write(ref _numRangeData, value);
+    }
+
+    /// <summary>
+    /// Whether insertion is finished.
+    /// </summary>
+    public bool InsertionFinished
+    {
+        get => _insertionFinished;
+        set => _insertionFinished = value;
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public abstract Models.Mapping.Submap ToProto(bool includeGridData);
+
+    /// <summary>
+    /// Updates from proto representation.
+    /// </summary>
+    public abstract void UpdateFromProto(Models.Mapping.Submap proto);
+
+    /// <summary>
+    /// Fills data into the response proto.
+    /// Note: SubmapQueryResponse proto will be implemented later.
+    /// </summary>
+    // public abstract void ToResponseProto(Rigid3d globalSubmapPose, SubmapQueryResponse response);
+}
+
+/// <summary>
+/// Probability value conversion utilities for submaps.
+/// </summary>
+public static class SubmapProbabilityUtils
+{
+    /// <summary>
+    /// Converts the given probability to log odds.
+    /// </summary>
+    public static double Logit(double probability)
+    {
+        return Math.Log(probability / (1.0 - probability));
+    }
+
+    public static readonly double kMaxLogOdds = Logit(ProbabilityValues.kMaxProbability);
+    public static readonly double kMinLogOdds = Logit(ProbabilityValues.kMinProbability);
+
+    /// <summary>
+    /// Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds,
+    /// kMaxLogOdds] is mapped to [1, 255].
+    /// </summary>
+    public static byte ProbabilityToLogOddsInteger(double probability)
+    {
+        // 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((Logit(probability) - kMinLogOdds) *
+                                    254.0 / (kMaxLogOdds - kMinLogOdds),
+                                    MidpointRounding.AwayFromZero) + 1;
+        value = Math.Max(1, Math.Min(255, value));
+        return (byte)value;
+    }
+}