Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/3D/ActiveSubmaps3D.cs

@@ -0,0 +1,156 @@
+/*
+ * 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.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D3D;
+
+/// <summary>
+/// Options for 3D submaps.
+/// </summary>
+public struct SubmapsOptions3D(
+    double highResolution,
+    double highResolutionMaxRange,
+    double lowResolution,
+    int numRangeData,
+    RangeDataInserterOptions3D rangeDataInserterOptions)
+{
+    public double HighResolution { get; set; } = highResolution;
+    public double HighResolutionMaxRange { get; set; } = highResolutionMaxRange;
+    public double LowResolution { get; set; } = lowResolution;
+    public int NumRangeData { get; set; } = numRangeData;
+    public RangeDataInserterOptions3D RangeDataInserterOptions { get; set; } = rangeDataInserterOptions;
+}
+
+/// <summary>
+/// The first active submap will be created on the insertion of the first range
+/// data. Except during this initialization when no or only one single submap
+/// exists, there are always two submaps into which range data is inserted: an
+/// old submap that is used for matching, and a new one, which will be used for
+/// matching next, that is being initialized.
+///
+/// Once a certain number of range data have been inserted, the new submap is
+/// considered initialized: the old submap is no longer changed, the "new" submap
+/// is now the "old" submap and is used for scan-to-map matching. Moreover, a
+/// "new" submap gets created. The "old" submap is forgotten by this object.
+/// </summary>
+public class ActiveSubmaps3D
+{
+    private readonly SubmapsOptions3D _options;
+    private readonly List<Submap3D> _submaps = [];
+    private readonly RangeDataInserter3D _rangeDataInserter;
+
+    public ActiveSubmaps3D(SubmapsOptions3D options)
+    {
+        if (options.NumRangeData <= 0)
+        {
+            throw new ArgumentException("num_range_data must be greater than 0", nameof(options));
+        }
+
+        _options = options;
+        _rangeDataInserter = new RangeDataInserter3D(options.RangeDataInserterOptions);
+    }
+
+    /// <summary>
+    /// Inserts 'range_data_in_local' into the Submap collection.
+    /// 'local_from_gravity_aligned' is used for the orientation of new submaps so
+    /// that the z axis approximately aligns with gravity.
+    /// 'rotational_scan_matcher_histogram_in_gravity' will be accumulated in all
+    /// submaps of the Submap collection.
+    /// </summary>
+    public List<Submap3D> InsertData(
+        RangeData rangeDataInLocal,
+        Quaternion localFromGravityAligned,
+        List<double> rotationalScanMatcherHistogramInGravity)
+    {
+        // Create new submap if needed
+        if (_submaps.Count == 0 ||
+            _submaps[^1].NumRangeData == _options.NumRangeData)
+        {
+            var localSubmapPose = new Rigid3d(
+                (Vector3)rangeDataInLocal.Origin,
+                localFromGravityAligned);
+            AddSubmap(localSubmapPose, rotationalScanMatcherHistogramInGravity.Count);
+        }
+
+        // Insert into all active submaps
+        foreach (var submap in _submaps)
+        {
+            submap.InsertData(
+                rangeDataInLocal,
+                _rangeDataInserter,
+                _options.HighResolutionMaxRange,
+                localFromGravityAligned,
+                rotationalScanMatcherHistogramInGravity);
+        }
+
+        // Finish the first submap if it has reached 2 * num_range_data
+        if (_submaps.Count > 0 && _submaps[0].NumRangeData == 2 * _options.NumRangeData)
+        {
+            _submaps[0].Finish();
+        }
+
+        return [.. _submaps];
+    }
+
+    /// <summary>
+    /// Gets the current active submaps.
+    /// </summary>
+    public List<Submap3D> Submaps()
+    {
+        return [.. _submaps];
+    }
+
+    /// <summary>
+    /// Adds a new submap to the collection.
+    /// </summary>
+    private void AddSubmap(Rigid3d localSubmapPose, int rotationalScanMatcherHistogramSize)
+    {
+        if (_submaps.Count >= 2)
+        {
+            // This will crop the finished Submap before inserting a new Submap to
+            // reduce peak memory usage a bit.
+            if (!_submaps[0].InsertionFinished)
+            {
+                throw new InvalidOperationException(
+                    "First submap must be finished before adding a new one");
+            }
+
+            // We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use
+            // active submaps and their associated intensity hybrid grids for scan
+            // matching, we call `ForgetIntensityHybridGrid` once we remove the submap
+            // from active submaps and no longer need the intensity hybrid grid.
+            _submaps[0].ForgetIntensityHybridGrid();
+            _submaps.RemoveAt(0);
+        }
+
+        var initialRotationalScanMatcherHistogram = new List<double>(rotationalScanMatcherHistogramSize);
+        for (int i = 0; i < rotationalScanMatcherHistogramSize; i++)
+        {
+            initialRotationalScanMatcherHistogram.Add(0.0);
+        }
+
+        var submap = new Submap3D(
+            _options.HighResolution,
+            _options.LowResolution,
+            localSubmapPose,
+            initialRotationalScanMatcherHistogram);
+
+        _submaps.Add(submap);
+    }
+}