Initial commit

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

@@ -0,0 +1,170 @@
+/*
+ * 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.Mapping;
+using CartographerSharp.Models.Transform;
+using CartographerSharp.Sensor;
+using CartographerSharp.Transform;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Constant pose data (time and local pose).
+/// </summary>
+public struct ConstantPoseData
+{
+    public long Time { get; set; } // Universal Time Scale ticks
+    public Rigid3d LocalPose { get; set; }
+}
+
+/// <summary>
+/// Trajectory node pose information.
+/// </summary>
+public struct TrajectoryNodePose
+{
+    /// <summary>
+    /// The node pose in the global SLAM frame.
+    /// </summary>
+    public Rigid3d GlobalPose { get; set; }
+
+    /// <summary>
+    /// Optional constant pose data.
+    /// </summary>
+    public ConstantPoseData? ConstantPoseData { get; set; }
+}
+
+/// <summary>
+/// Trajectory node containing sensor data and pose information.
+/// </summary>
+public class TrajectoryNode
+{
+    /// <summary>
+    /// Constant data associated with a trajectory node.
+    /// </summary>
+    public class Data
+    {
+        public long Time { get; set; } // Universal Time Scale ticks
+
+        /// <summary>
+        /// Transform to approximately gravity align the tracking frame as
+        /// determined by local SLAM.
+        /// </summary>
+        public Quaternion GravityAlignment { get; set; }
+
+        /// <summary>
+        /// Used for loop closure in 2D: voxel filtered returns in the
+        /// 'gravity_alignment' frame.
+        /// </summary>
+        public PointCloud? FilteredGravityAlignedPointCloud { get; set; }
+
+        /// <summary>
+        /// Used for loop closure in 3D.
+        /// </summary>
+        public PointCloud? HighResolutionPointCloud { get; set; }
+        public PointCloud? LowResolutionPointCloud { get; set; }
+        public double[]? RotationalScanMatcherHistogram { get; set; }
+
+        /// <summary>
+        /// The node pose in the local SLAM frame.
+        /// </summary>
+        public Rigid3d LocalPose { get; set; }
+    }
+
+    /// <summary>
+    /// Gets the time of this node.
+    /// </summary>
+    public long Time => ConstantData?.Time ?? 0;
+
+    /// <summary>
+    /// This must be a shared reference. If the data is used for visualization while the
+    /// node is being trimmed, it must survive until all use finishes.
+    /// </summary>
+    public Data? ConstantData { get; set; }
+
+    /// <summary>
+    /// The node pose in the global SLAM frame.
+    /// </summary>
+    public Rigid3d GlobalPose { get; set; }
+
+    /// <summary>
+    /// Creates a shallow clone of this TrajectoryNode.
+    /// The ConstantData reference is shared (not deep cloned) since it's immutable after creation.
+    /// Only GlobalPose is independent in the clone.
+    /// Used for Copy-on-Write optimization to avoid data races.
+    /// </summary>
+    public TrajectoryNode ShallowClone()
+    {
+        return new TrajectoryNode
+        {
+            ConstantData = this.ConstantData,  // Shared reference (immutable)
+            GlobalPose = this.GlobalPose       // Value type copy
+        };
+    }
+}
+
+/// <summary>
+/// Conversion utilities for TrajectoryNode.
+/// </summary>
+public static class TrajectoryNodeOperations
+{
+    /// <summary>
+    /// Converts to proto representation.
+    /// </summary>
+    public static TrajectoryNodeData ToProto(TrajectoryNode.Data constantData)
+    {
+        var proto = new TrajectoryNodeData(
+            constantData.Time,
+            (Quaterniond)constantData.GravityAlignment,
+            (Rigid3dProto)constantData.LocalPose,
+            constantData.FilteredGravityAlignedPointCloud != null
+                ? new Sensor.CompressedPointCloud(constantData.FilteredGravityAlignedPointCloud).ToProto()
+                : null,
+            constantData.HighResolutionPointCloud != null
+                ? new Sensor.CompressedPointCloud(constantData.HighResolutionPointCloud).ToProto()
+                : null,
+            constantData.LowResolutionPointCloud != null
+                ? new Sensor.CompressedPointCloud(constantData.LowResolutionPointCloud).ToProto()
+                : null,
+            constantData.RotationalScanMatcherHistogram?.ToList()
+        );
+
+        return proto;
+    }
+
+    /// <summary>
+    /// Creates from proto representation.
+    /// </summary>
+    public static TrajectoryNode.Data FromProto(TrajectoryNodeData proto)
+    {
+        return new TrajectoryNode.Data
+        {
+            Time = proto.Timestamp,
+            GravityAlignment = (Quaternion)proto.GravityAlignment,
+            LocalPose = (Rigid3d)proto.LocalPose,
+            FilteredGravityAlignedPointCloud = proto.FilteredGravityAlignedPointCloud.HasValue
+                ? Sensor.CompressedPointCloud.FromProto(proto.FilteredGravityAlignedPointCloud.Value).Decompress()
+                : null,
+            HighResolutionPointCloud = proto.HighResolutionPointCloud.HasValue
+                ? Sensor.CompressedPointCloud.FromProto(proto.HighResolutionPointCloud.Value).Decompress()
+                : null,
+            LowResolutionPointCloud = proto.LowResolutionPointCloud.HasValue
+                ? Sensor.CompressedPointCloud.FromProto(proto.LowResolutionPointCloud.Value).Decompress()
+                : null,
+            RotationalScanMatcherHistogram = proto.RotationalScanMatcherHistogram?.ToArray()
+        };
+    }
+}