Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/Submap2D.cs

@@ -0,0 +1,184 @@
+/*
+ * 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.Models.Transform;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using System;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping.D2D;
+
+/// <summary>
+/// 2D Submap implementation.
+/// </summary>
+public class Submap2D : Submap
+{
+    private Grid2D? _grid;
+    private Grid2D? _highResGrid;
+    private readonly ValueConversionTables _conversionTables;
+
+    public Submap2D(Rigid3d localSubmapPose, Grid2D grid, ValueConversionTables conversionTables, Grid2D? highResGrid = null)
+        : base(localSubmapPose) // Pass the full Rigid3d pose including rotation
+    {
+        _grid = grid;
+        _highResGrid = highResGrid;
+        _conversionTables = conversionTables;
+    }
+    
+    // Keep old constructor for backward compatibility (if needed)
+    public Submap2D(Vector2 origin, Grid2D grid, ValueConversionTables conversionTables)
+        : this(new Rigid3d(new Vector3(origin.X, origin.Y, 0.0), Quaternion.Identity), grid, conversionTables)
+    {
+    }
+
+    public Submap2D(Models.Mapping.Submap2D proto, ValueConversionTables conversionTables)
+        : base((Rigid3d)proto.LocalPose)
+    {
+        _conversionTables = conversionTables;
+        NumRangeData = proto.NumRangeData;
+        InsertionFinished = proto.Finished;
+
+        // Match C++: if (proto.has_grid()) - no resolution check
+        if (proto.Grid.HasValue)
+        {
+            var gridProto = proto.Grid.Value;
+            if (gridProto.ProbabilityGrid2D.HasValue)
+            {
+                _grid = new ProbabilityGrid(gridProto, conversionTables);
+            }
+            else if (gridProto.Tsdf2D.HasValue)
+            {
+                _grid = new TSDF2D(gridProto, conversionTables);
+            }
+            else
+            {
+                throw new ArgumentException("proto::Submap2D has grid with unknown type.", nameof(proto));
+            }
+        }
+    }
+
+    /// <summary>
+    /// Gets the grid.
+    /// </summary>
+    public Grid2D? Grid => _grid;
+
+    /// <summary>
+    /// Gets the optional high resolution grid for fine-grained Ceres scan matching.
+    /// </summary>
+    public Grid2D? HighResGrid => _highResGrid;
+
+    /// <summary>
+    /// Insert 'range_data' into this submap using 'range_data_inserter'. The
+    /// submap must not be finished yet.
+    /// </summary>
+    public void InsertRangeData(RangeData rangeData, IRangeDataInserter rangeDataInserter)
+    {
+        if (InsertionFinished)
+        {
+            throw new InvalidOperationException("Cannot insert range data into finished submap");
+        }
+        if (_grid == null)
+        {
+            throw new InvalidOperationException("Grid is not initialized");
+        }
+        // Insert range data directly (already transformed to submap frame in ActiveSubmaps2D)
+        if (_highResGrid != null)
+        {
+            // Main grid and high-res grid are independent - insert in parallel.
+            var highResGrid = _highResGrid;
+            var highResTask = Task.Run(() => rangeDataInserter.Insert(rangeData, (IGrid)highResGrid));
+            rangeDataInserter.Insert(rangeData, (IGrid)_grid);
+            highResTask.GetAwaiter().GetResult();
+        }
+        else
+        {
+            rangeDataInserter.Insert(rangeData, (IGrid)_grid);
+        }
+        NumRangeData++;
+    }
+
+    /// <summary>
+    /// Finishes the submap.
+    /// Match C++: grid_ = grid_->ComputeCroppedGrid(); to reduce memory usage
+    /// </summary>
+    public void Finish()
+    {
+        if (_grid == null)
+        {
+            throw new InvalidOperationException("Grid is not initialized");
+        }
+        if (InsertionFinished)
+        {
+            throw new InvalidOperationException("Submap is already finished");
+        }
+        // Match C++: Crop grid to reduce memory usage when submap is finished
+        _grid = _grid.ComputeCroppedGrid();
+        _highResGrid = _highResGrid?.ComputeCroppedGrid();
+        InsertionFinished = true;
+    }
+
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public override Models.Mapping.Submap ToProto(bool includeGridData)
+    {
+        var submap2D = new Models.Mapping.Submap2D(
+            (Rigid3dProto)LocalPose,
+            NumRangeData,
+            InsertionFinished,
+            includeGridData && _grid != null ? _grid.ToProto() : null
+        );
+
+        // Note: SubmapId will be set by caller
+        // Note: SubmapId will be set by caller when creating full Submap
+        return new Models.Mapping.Submap(new Models.Mapping.PoseGraph.SubmapId(0, 0), submap2D, null);
+    }
+
+    /// <summary>
+    /// Updates from proto representation.
+    /// Match C++: CHECK(proto.has_submap_2d())
+    /// </summary>
+    public override void UpdateFromProto(Models.Mapping.Submap proto)
+    {
+        if (!proto.Submap2D.HasValue)
+        {
+            throw new ArgumentException("Proto must contain Submap2D", nameof(proto));
+        }
+
+        var submap2D = proto.Submap2D.Value;
+        NumRangeData = submap2D.NumRangeData;
+        InsertionFinished = submap2D.Finished;
+        
+        // Match C++: if (proto.submap_2d().has_grid()) - no resolution check
+        if (submap2D.Grid.HasValue)
+        {
+            var gridProto = submap2D.Grid.Value;
+            if (gridProto.ProbabilityGrid2D.HasValue)
+            {
+                _grid = new ProbabilityGrid(gridProto, _conversionTables);
+            }
+            else if (gridProto.Tsdf2D.HasValue)
+            {
+                _grid = new TSDF2D(gridProto, _conversionTables);
+            }
+            else
+            {
+                throw new ArgumentException("proto::Submap2D has grid with unknown type.", nameof(proto));
+            }
+        }
+    }
+}