Initial commit

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

@@ -0,0 +1,205 @@
+/*
+ * Copyright 2018 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.GroundTruth;
+using CartographerSharp.Models.Mapping;
+using CartographerSharp.Models.Transform;
+using CartographerSharp.Transform;
+using GroundTruthProto = CartographerSharp.Models.GroundTruth.GroundTruth;
+using PoseGraphProto = CartographerSharp.Models.Mapping.PoseGraph;
+
+namespace CartographerSharp.GroundTruth;
+
+/// <summary>
+/// Generates GroundTruth proto from the given pose graph using the specified
+/// criteria parameters. See
+/// 'https://google-cartographer.readthedocs.io/en/latest/evaluation.html' for
+/// more details.
+/// </summary>
+public static class AutogenerateGroundTruth
+{
+    /// <summary>
+    /// Generates ground truth from pose graph.
+    /// </summary>
+    /// <param name="poseGraph">Pose graph proto.</param>
+    /// <param name="minCoveredDistance">Minimum covered distance between nodes.</param>
+    /// <param name="outlierThresholdMeters">Outlier threshold in meters.</param>
+    /// <param name="outlierThresholdRadians">Outlier threshold in radians.</param>
+    /// <returns>GroundTruth proto with relations.</returns>
+    public static GroundTruthProto GenerateGroundTruth(
+        PoseGraph poseGraph,
+        double minCoveredDistance,
+        double outlierThresholdMeters,
+        double outlierThresholdRadians)
+    {
+        if (poseGraph.Trajectories == null || poseGraph.Trajectories.Count == 0)
+        {
+            return new GroundTruthProto { Relations = [] };
+        }
+
+        var trajectory = poseGraph.Trajectories[0];
+        if (trajectory.Nodes == null || trajectory.Nodes.Count == 0)
+        {
+            return new GroundTruthProto { Relations = [] };
+        }
+
+        var coveredDistance = ComputeCoveredDistance(trajectory);
+        var submapToNodeIndex = ComputeSubmapRepresentativeNode(poseGraph);
+
+        int numOutliers = 0;
+        var groundTruth = new GroundTruthProto
+        {
+            Relations = []
+        };
+
+        if (poseGraph.Constraints == null)
+        {
+            return groundTruth;
+        }
+
+        foreach (var constraint in poseGraph.Constraints)
+        {
+            // We're only interested in loop closure constraints.
+            if (constraint.ConstraintTag == PoseGraphProto.Constraint.Tag.IntraSubmap)
+            {
+                continue;
+            }
+
+            // For some submaps at the very end, we have not chosen a representative
+            // node, but those should not be part of loop closure anyway.
+            if (constraint.SubmapId.TrajectoryId != 0 ||
+                constraint.NodeId.TrajectoryId != 0)
+            {
+                continue;
+            }
+
+            if (constraint.SubmapId.SubmapIndex >= submapToNodeIndex.Count)
+            {
+                continue;
+            }
+
+            var matchedNode = constraint.NodeId.NodeIndex;
+            var representativeNode = submapToNodeIndex[constraint.SubmapId.SubmapIndex];
+
+            // Covered distance between the two should not be too small.
+            var coveredDistanceInConstraint = Math.Abs(
+                coveredDistance[matchedNode] - coveredDistance[representativeNode]);
+            if (coveredDistanceInConstraint < minCoveredDistance)
+            {
+                continue;
+            }
+
+            // Compute the transform between the nodes according to the solution and
+            // the constraint.
+            var solutionPose1 = (Rigid3d)trajectory.Nodes[representativeNode].Pose;
+            var solutionPose2 = (Rigid3d)trajectory.Nodes[matchedNode].Pose;
+            var solution = solutionPose1.Inverse() * solutionPose2;
+
+            var submapSolution = (Rigid3d)trajectory.Submaps[constraint.SubmapId.SubmapIndex].Pose;
+            var submapSolutionToNodeSolution = solutionPose1.Inverse() * submapSolution;
+            var nodeToSubmapConstraint = (Rigid3d)constraint.RelativePose;
+            var expected = submapSolutionToNodeSolution * nodeToSubmapConstraint;
+
+            var error = solution * expected.Inverse();
+
+            if (error.Translation.Length() > outlierThresholdMeters ||
+                TransformOperations.GetAngle(error) > outlierThresholdRadians)
+            {
+                numOutliers++;
+                continue;
+            }
+
+            var relation = new Relation
+            {
+                Timestamp1 = trajectory.Nodes[representativeNode].Timestamp,
+                Timestamp2 = trajectory.Nodes[matchedNode].Timestamp,
+                Expected = (Rigid3dProto)expected,
+                CoveredDistance = coveredDistanceInConstraint
+            };
+
+            groundTruth.Relations.Add(relation);
+        }
+
+        // Log number of relations and outliers for debugging and analysis
+        return groundTruth;
+    }
+
+    /// <summary>
+    /// Computes covered distance for each node in the trajectory.
+    /// </summary>
+    private static List<double> ComputeCoveredDistance(Trajectory trajectory)
+    {
+        var coveredDistance = new List<double> { 0.0 };
+
+        if (trajectory.Nodes == null || trajectory.Nodes.Count == 0)
+        {
+            return coveredDistance;
+        }
+
+        for (int i = 1; i < trajectory.Nodes.Count; i++)
+        {
+            var lastPose = (Rigid3d)trajectory.Nodes[i - 1].Pose;
+            var thisPose = (Rigid3d)trajectory.Nodes[i].Pose;
+            var relativeTransform = lastPose.Inverse() * thisPose;
+            coveredDistance.Add(coveredDistance[^1] + relativeTransform.Translation.Length());
+        }
+
+        return coveredDistance;
+    }
+
+    /// <summary>
+    /// We pick the representative node in the middle of the submap.
+    /// </summary>
+    private static List<int> ComputeSubmapRepresentativeNode(PoseGraphProto poseGraph)
+    {
+        var submapToNodeIndex = new List<int>();
+
+        if (poseGraph.Constraints == null)
+        {
+            return submapToNodeIndex;
+        }
+
+        foreach (var constraint in poseGraph.Constraints)
+        {
+            if (constraint.ConstraintTag != PoseGraphProto.Constraint.Tag.IntraSubmap)
+            {
+                continue;
+            }
+
+            if (constraint.SubmapId.TrajectoryId != 0 ||
+                constraint.NodeId.TrajectoryId != 0)
+            {
+                continue;
+            }
+
+            var nextSubmapIndex = submapToNodeIndex.Count;
+            var submapIndex = constraint.SubmapId.SubmapIndex;
+            if (submapIndex <= nextSubmapIndex)
+            {
+                continue;
+            }
+
+            if (submapIndex != nextSubmapIndex + 1)
+            {
+                continue;
+            }
+
+            submapToNodeIndex.Add(constraint.NodeId.NodeIndex);
+        }
+
+        return submapToNodeIndex;
+    }
+}

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

@@ -0,0 +1,305 @@
+/*
+ * 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.Math;
+using CartographerSharp.Transform;
+using System.Text.Json;
+using GroundTruthProto = CartographerSharp.Models.GroundTruth.GroundTruth;
+using PoseGraphProto = CartographerSharp.Models.Mapping.PoseGraph;
+
+namespace CartographerSharp.GroundTruth;
+
+/// <summary>
+/// Error structure for computing metrics.
+/// </summary>
+internal struct Error
+{
+    public double TranslationalSquared { get; set; }
+    public double RotationalSquared { get; set; }
+}
+
+/// <summary>
+/// Computes relations metrics from pose graph and ground truth.
+/// </summary>
+public static class ComputeRelationsMetrics
+{
+    private static readonly JsonSerializerOptions JsonOptions = new()
+    {
+        PropertyNameCaseInsensitive = true
+    };
+
+    /// <summary>
+    /// Computes error between two poses and expected transform.
+    /// </summary>
+    private static Error ComputeError(
+        Rigid3d pose1,
+        Rigid3d pose2,
+        Rigid3d expected)
+    {
+        var error = (pose1.Inverse() * pose2) * expected.Inverse();
+        return new Error
+        {
+            TranslationalSquared = error.Translation.Length() * error.Translation.Length(),
+            RotationalSquared = MathUtils.Pow2(TransformOperations.GetAngle(error))
+        };
+    }
+
+    /// <summary>
+    /// Computes mean and standard deviation string from values.
+    /// </summary>
+    private static string MeanAndStdDevString(List<double> values)
+    {
+        if (values.Count < 2)
+            return "N/A";
+
+        var mean = values.Average();
+        var sumOfSquaredDifferences = values.Sum(v => MathUtils.Pow2(v - mean));
+        var standardDeviation = Math.Sqrt(sumOfSquaredDifferences / (values.Count - 1));
+        return $"{mean:F5} +/- {standardDeviation:F5}";
+    }
+
+    /// <summary>
+    /// Computes statistics string from errors.
+    /// </summary>
+    private static string StatisticsString(List<Error> errors)
+    {
+        var translationalErrors = errors.Select(e => Math.Sqrt(e.TranslationalSquared)).ToList();
+        var squaredTranslationalErrors = errors.Select(e => e.TranslationalSquared).ToList();
+        var rotationalErrorsDegrees = errors.Select(e => MathUtils.RadToDeg(Math.Sqrt(e.RotationalSquared))).ToList();
+        var squaredRotationalErrorsDegrees = errors.Select(e => MathUtils.Pow2(MathUtils.RadToDeg(Math.Sqrt(e.RotationalSquared)))).ToList();
+
+        return $"Translational error (m): {MeanAndStdDevString(translationalErrors)}\n" +
+               $"Translational error squared (m²): {MeanAndStdDevString(squaredTranslationalErrors)}\n" +
+               $"Rotational error (deg): {MeanAndStdDevString(rotationalErrorsDegrees)}\n" +
+               $"Rotational error squared (deg²): {MeanAndStdDevString(squaredRotationalErrorsDegrees)}";
+    }
+
+    /// <summary>
+    /// Computes relations metrics from pose graph and ground truth.
+    /// </summary>
+    /// <param name="poseGraph">Pose graph proto.</param>
+    /// <param name="groundTruth">Ground truth relations.</param>
+    /// <returns>Metrics string.</returns>
+    public static string ComputeMetrics(
+        PoseGraphProto poseGraph,
+        GroundTruthProto groundTruth)
+    {
+        if (poseGraph.Trajectories == null || poseGraph.Trajectories.Count == 0)
+        {
+            return "No trajectories in pose graph.";
+        }
+
+        var trajectory = poseGraph.Trajectories[0];
+        if (trajectory.Nodes == null || trajectory.Nodes.Count == 0)
+        {
+            return "No nodes in trajectory.";
+        }
+
+        var errors = new List<Error>();
+
+        if (groundTruth.Relations == null)
+        {
+            return "No relations in ground truth.";
+        }
+
+        foreach (var relation in groundTruth.Relations)
+        {
+            // Find nodes with matching timestamps
+            var node1 = trajectory.Nodes.FirstOrDefault(n => n.Timestamp == relation.Timestamp1);
+            var node2 = trajectory.Nodes.FirstOrDefault(n => n.Timestamp == relation.Timestamp2);
+
+            // Check if nodes were found (Node is a struct, so FirstOrDefault returns default(Node) if not found)
+            // We check if any node with matching timestamp exists
+            var found1 = trajectory.Nodes.Any(n => n.Timestamp == relation.Timestamp1);
+            var found2 = trajectory.Nodes.Any(n => n.Timestamp == relation.Timestamp2);
+            if (!found1 || !found2)
+            {
+                continue; // Nodes not found
+            }
+
+            var pose1 = (Rigid3d)node1.Pose;
+            var pose2 = (Rigid3d)node2.Pose;
+            var expected = (Rigid3d)relation.Expected;
+
+            var error = ComputeError(pose1, pose2, expected);
+            errors.Add(error);
+        }
+
+        if (errors.Count == 0)
+        {
+            return "No matching relations found.";
+        }
+
+        return $"Number of relations: {errors.Count}\n" +
+               StatisticsString(errors);
+    }
+
+    /// <summary>
+    /// Computes relations metrics from pose graph and ground truth file.
+    /// Note: Pose graph should be obtained from MapBuilder.ToProto() method.
+    /// </summary>
+    /// <param name="poseGraph">Pose graph proto (obtained from MapBuilder.ToProto()).</param>
+    /// <param name="relationsFilename">Path to relations text file or ground truth proto file.</param>
+    /// <param name="readTextFileWithUnixTimestamps">Whether to read text file with Unix timestamps.</param>
+    /// <param name="writeRelationMetrics">Whether to write relation metrics to CSV file.</param>
+    /// <param name="outputCsvFilename">Output CSV filename (optional).</param>
+    /// <returns>Metrics string.</returns>
+    public static string ComputeMetricsFromFiles(
+        PoseGraphProto poseGraph,
+        string relationsFilename,
+        bool readTextFileWithUnixTimestamps = false,
+        bool writeRelationMetrics = false,
+        string? outputCsvFilename = null)
+    {
+        if (string.IsNullOrEmpty(relationsFilename))
+            throw new ArgumentException("Relations filename cannot be null or empty", nameof(relationsFilename));
+
+        // Load ground truth
+        GroundTruthProto groundTruth;
+        if (readTextFileWithUnixTimestamps)
+        {
+            groundTruth = RelationsTextFile.ReadRelationsTextFile(relationsFilename);
+        }
+        else
+        {
+            // Try to read as proto file first, fall back to text file if it fails
+            try
+            {
+                groundTruth = ReadGroundTruthProto(relationsFilename);
+            }
+            catch
+            {
+                // Fall back to text file if proto reading fails
+                groundTruth = RelationsTextFile.ReadRelationsTextFile(relationsFilename);
+            }
+        }
+
+        var metrics = ComputeMetrics(poseGraph, groundTruth);
+
+        // Write metrics to CSV if requested
+        if (writeRelationMetrics)
+        {
+            var csvFilename = outputCsvFilename ?? "relation_metrics.csv";
+            WriteMetricsToCsv(csvFilename, poseGraph, groundTruth);
+        }
+
+        return metrics;
+    }
+
+    /// <summary>
+    /// Reads ground truth from proto file.
+    /// </summary>
+    /// <param name="filename">Path to ground truth proto file.</param>
+    /// <returns>GroundTruth proto.</returns>
+    private static GroundTruthProto ReadGroundTruthProto(string filename)
+    {
+        if (!File.Exists(filename))
+        {
+            throw new FileNotFoundException($"Ground truth file not found: {filename}", filename);
+        }
+
+        // Try reading as proto stream format first (pbstream or compressed format)
+        try
+        {
+            using var reader = new IO.ProtoStreamReader(filename);
+            if (reader.ReadProto<GroundTruthProto>(out var proto))
+            {
+                return proto;
+            }
+        }
+        catch
+        {
+            // If proto stream reading fails, try JSON deserialization
+        }
+
+        // Try reading as JSON file (if not proto stream format)
+        try
+        {
+            var jsonContent = File.ReadAllText(filename);
+            var proto = JsonSerializer.Deserialize<GroundTruthProto>(jsonContent, JsonOptions);
+
+            // Check if deserialization was successful (struct has default relations list if failed)
+            if (proto.Relations != null && proto.Relations.Count > 0)
+            {
+                return proto;
+            }
+            // Also check if empty relations list is valid (could be empty ground truth)
+            if (proto.Relations != null)
+            {
+                return proto;
+            }
+        }
+        catch (Exception ex)
+        {
+            throw new InvalidOperationException($"Failed to read ground truth from file '{filename}'. " +
+                "Expected either proto stream format (.pbstream) or JSON format. " +
+                $"Error: {ex.Message}", ex);
+        }
+
+        throw new InvalidOperationException($"Failed to read ground truth from file '{filename}'. " +
+            "File format not recognized or file is empty.");
+    }
+
+    /// <summary>
+    /// Writes relation metrics to CSV file.
+    /// </summary>
+    private static void WriteMetricsToCsv(
+        string csvFilename,
+        PoseGraphProto poseGraph,
+        GroundTruthProto groundTruth)
+    {
+        using var writer = new StreamWriter(csvFilename);
+        writer.WriteLine("timestamp1,timestamp2,translational_error_m,rotational_error_deg,covered_distance");
+
+        if (poseGraph.Trajectories == null || poseGraph.Trajectories.Count == 0 ||
+            groundTruth.Relations == null)
+        {
+            return;
+        }
+
+        var trajectory = poseGraph.Trajectories[0];
+        if (trajectory.Nodes == null)
+        {
+            return;
+        }
+
+        foreach (var relation in groundTruth.Relations)
+        {
+            var node1 = trajectory.Nodes.FirstOrDefault(n => n.Timestamp == relation.Timestamp1);
+            var node2 = trajectory.Nodes.FirstOrDefault(n => n.Timestamp == relation.Timestamp2);
+
+            // Check if nodes were found
+            var found1 = trajectory.Nodes.Any(n => n.Timestamp == relation.Timestamp1);
+            var found2 = trajectory.Nodes.Any(n => n.Timestamp == relation.Timestamp2);
+            if (!found1 || !found2)
+            {
+                continue;
+            }
+
+            var pose1 = (Rigid3d)node1.Pose;
+            var pose2 = (Rigid3d)node2.Pose;
+            var expected = (Rigid3d)relation.Expected;
+
+            var error = ComputeError(pose1, pose2, expected);
+            var translationalError = Math.Sqrt(error.TranslationalSquared);
+            var rotationalErrorDeg = MathUtils.RadToDeg(Math.Sqrt(error.RotationalSquared));
+
+            writer.WriteLine($"{relation.Timestamp1},{relation.Timestamp2}," +
+                            $"{translationalError:F6},{rotationalErrorDeg:F6}," +
+                            $"{relation.CoveredDistance:F6}");
+        }
+    }
+}

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);
+    }
+}
+}