Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Sensor/RangeData.cs

@@ -0,0 +1,140 @@
+/*
+ * 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.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Sensor;
+
+/// <summary>
+/// Range data structure.
+/// Rays begin at 'origin'. 'returns' are the points where obstructions were
+/// detected. 'misses' are points in the direction of rays for which no return
+/// was detected, and were inserted at a configured distance. It is assumed that
+/// between the 'origin' and 'misses' is free space.
+/// </summary>
+public struct RangeData(Vector3 origin, PointCloud returns, PointCloud misses)
+{
+    public Vector3 Origin { get; set; } = origin;
+    public PointCloud Returns { get; set; } = returns;
+    public PointCloud Misses { get; set; } = misses;
+}
+
+/// <summary>
+/// Operations on RangeData.
+/// </summary>
+public static class RangeDataOperations
+{
+    /// <summary>
+    /// Transforms range data according to a transform.
+    /// </summary>
+    public static RangeData Transform(RangeData rangeData, Rigid3f transform)
+    {
+        return new RangeData(
+            transform.TransformPoint(rangeData.Origin),
+            PointCloudOperations.Transform(rangeData.Returns, transform),
+            PointCloudOperations.Transform(rangeData.Misses, transform)
+        );
+    }
+
+    /// <summary>
+    /// Normalizes range data origin to a target origin by translating all points.
+    /// This is used in 2D SLAM to ensure consistent origin for ray casting in submap.
+    /// </summary>
+    public static RangeData NormalizeOrigin(RangeData rangeData, Vector3 targetOrigin)
+    {
+        var originOffset = targetOrigin - rangeData.Origin;
+        return new RangeData(
+            targetOrigin,
+            PointCloudOperations.Translate(rangeData.Returns, originOffset),
+            PointCloudOperations.Translate(rangeData.Misses, originOffset)
+        );
+    }
+
+    /// <summary>
+    /// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'.
+    /// </summary>
+    public static RangeData Crop(RangeData rangeData, double minZ, double maxZ)
+    {
+        return new RangeData(
+            rangeData.Origin,
+            PointCloudOperations.Crop(rangeData.Returns, minZ, maxZ),
+            PointCloudOperations.Crop(rangeData.Misses, minZ, maxZ)
+        );
+    }
+
+    /// <summary>
+    /// Converts 'range_data' to a proto::RangeData.
+    /// </summary>
+    public static Models.Sensor.RangeData ToProto(RangeData rangeData)
+    {
+        var proto = new Models.Sensor.RangeData
+        {
+            Origin = new Vector3f(rangeData.Origin.X, rangeData.Origin.Y, rangeData.Origin.Z),
+            Returns = [],
+            Misses = []
+        };
+
+        foreach (var point in rangeData.Returns.Points)
+        {
+            proto.Returns.Add(new Models.Sensor.RangefinderPoint
+            {
+                Position = new Vector3f(point.Position.X, point.Position.Y, point.Position.Z)
+            });
+        }
+
+        foreach (var point in rangeData.Misses.Points)
+        {
+            proto.Misses.Add(new Models.Sensor.RangefinderPoint
+            {
+                Position = new Vector3f(point.Position.X, point.Position.Y, point.Position.Z)
+            });
+        }
+
+        return proto;
+    }
+
+    /// <summary>
+    /// Converts 'proto' to RangeData.
+    /// </summary>
+    public static RangeData FromProto(Models.Sensor.RangeData proto)
+    {
+        var returns = new List<Sensor.RangefinderPoint>();
+        if (proto.Returns.Count > 0)
+        {
+            foreach (var protoPoint in proto.Returns)
+            {
+                returns.Add(new Sensor.RangefinderPoint(new Vector3(protoPoint.Position.X, protoPoint.Position.Y, protoPoint.Position.Z)));
+            }
+        }
+
+        var misses = new List<Sensor.RangefinderPoint>();
+        if (proto.Misses.Count > 0)
+        {
+            foreach (var protoPoint in proto.Misses)
+            {
+                misses.Add(new Sensor.RangefinderPoint(new Vector3(protoPoint.Position.X, protoPoint.Position.Y, protoPoint.Position.Z)));
+            }
+        }
+
+        return new RangeData(
+            new Vector3(proto.Origin.X, proto.Origin.Y, proto.Origin.Z),
+            new PointCloud(returns),
+            new PointCloud(misses)
+        );
+    }
+}