Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/GroundTruth/RelationsTextFile.cs

@@ -0,0 +1,111 @@
+/*
+ * 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.Common.Time;
+using CartographerSharp.Models.GroundTruth;
+using CartographerSharp.Models.Transform;
+using CartographerSharp.Transform;
+using System.Globalization;
+using  RobotNet10.Shared.Numbers;
+using GroundTruthProto = CartographerSharp.Models.GroundTruth.GroundTruth;
+using QuaternionUtils = CartographerSharp.Transform.QuaternionUtils;
+
+namespace CartographerSharp.GroundTruth
+{
+/// <summary>
+/// Reads a text file and converts it to a GroundTruth proto. Each line contains:
+/// time1 time2 x y z roll pitch yaw
+/// using Unix epoch timestamps.
+/// 
+/// This is the format used in the relations files provided for:
+/// R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, C. Stachniss,
+/// and A. Kleiner, "On measuring the accuracy of SLAM algorithms," Autonomous
+/// Robots, vol. 27, no. 4, pp. 387–407, 2009.
+/// </summary>
+public static class RelationsTextFile
+{
+    /// <summary>
+    /// Reads relations from a text file.
+    /// </summary>
+    /// <param name="relationsFilename">Path to the relations text file.</param>
+    /// <returns>GroundTruth proto with relations.</returns>
+    public static GroundTruthProto ReadRelationsTextFile(string relationsFilename)
+    {
+        if (string.IsNullOrEmpty(relationsFilename))
+            throw new ArgumentException("Relations filename cannot be null or empty", nameof(relationsFilename));
+        if (!File.Exists(relationsFilename))
+            throw new FileNotFoundException("Relations file not found", relationsFilename);
+
+        var groundTruth = new GroundTruthProto
+        {
+            Relations = []
+        };
+
+        var lines = File.ReadAllLines(relationsFilename);
+        foreach (var line in lines)
+        {
+            if (string.IsNullOrWhiteSpace(line))
+                continue;
+
+            var parts = line.Split([' ', '\t'], StringSplitOptions.RemoveEmptyEntries);
+            if (parts.Length < 8)
+                continue;
+
+            if (!double.TryParse(parts[0], NumberStyles.Float, CultureInfo.InvariantCulture, out var unixTime1) ||
+                !double.TryParse(parts[1], NumberStyles.Float, CultureInfo.InvariantCulture, out var unixTime2) ||
+                !double.TryParse(parts[2], NumberStyles.Float, CultureInfo.InvariantCulture, out var x) ||
+                !double.TryParse(parts[3], NumberStyles.Float, CultureInfo.InvariantCulture, out var y) ||
+                !double.TryParse(parts[4], NumberStyles.Float, CultureInfo.InvariantCulture, out var z) ||
+                !double.TryParse(parts[5], NumberStyles.Float, CultureInfo.InvariantCulture, out var roll) ||
+                !double.TryParse(parts[6], NumberStyles.Float, CultureInfo.InvariantCulture, out var pitch) ||
+                !double.TryParse(parts[7], NumberStyles.Float, CultureInfo.InvariantCulture, out var yaw))
+            {
+                continue;
+            }
+
+            var commonTime1 = UnixToCommonTime(unixTime1);
+            var commonTime2 = UnixToCommonTime(unixTime2);
+
+            // Create expected transform from translation and Euler angles
+            var expected = new Rigid3d( new Vector3(x, y, z), QuaternionUtils.RollPitchYaw(roll, pitch, yaw));
+
+            // Convert TimeSpan to universal ticks (same as ToUniversal for DateTime)
+            var relation = new Relation
+            {
+                Timestamp1 = commonTime1.Ticks,
+                Timestamp2 = commonTime2.Ticks,
+                Expected = (Rigid3dProto)expected, // Explicit conversion from Rigid3d to Rigid3dProto
+                CoveredDistance = 0.0 // Will be computed later if needed
+            };
+
+            groundTruth.Relations.Add(relation);
+        }
+
+        return groundTruth;
+    }
+
+    /// <summary>
+    /// Converts Unix timestamp to Common time (TimeSpan since epoch).
+    /// </summary>
+    private static TimeSpan UnixToCommonTime(double unixTime)
+    {
+        const long kUtsTicksPerSecond = 10000000;
+        var utsEpochOffsetTicks = TimeUtils.UtsEpochOffsetFromUnixEpochInSeconds * kUtsTicksPerSecond;
+        var unixTimeTicks = (long)(unixTime * kUtsTicksPerSecond);
+        return TimeSpan.FromTicks(utsEpochOffsetTicks + unixTimeTicks);
+    }
+}
+}