Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/CovarianceCalculator.cs

@@ -0,0 +1,81 @@
+using CartographerSharp.Mapping;
+using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for calculating covariance from Cartographer constraints
+/// </summary>
+public static class CovarianceCalculator
+{
+    /// <summary>
+    /// Result of covariance calculation
+    /// </summary>
+    public record Result(
+        Matrix3x3? Covariance,
+        int ConstraintCount,
+        double AverageConstraintQuality);
+
+    /// <summary>
+    /// Calculate covariance and quality metrics from constraints
+    /// </summary>
+    /// <param name="constraints">List of pose graph constraints</param>
+    /// <returns>Covariance calculation result</returns>
+    public static Result Calculate(IList<IPoseGraph.Constraint> constraints)
+    {
+        if (constraints == null || constraints.Count == 0)
+        {
+            return new Result(null, 0, 0.0);
+        }
+
+        int constraintCount = constraints.Count;
+        
+        // Calculate average constraint weights
+        var avgTranslationWeight = constraints.Average(c => c.ConstraintPose.TranslationWeight);
+        var avgRotationWeight = constraints.Average(c => c.ConstraintPose.RotationWeight);
+
+        // Calculate constraint quality score (0.0 - 1.0)
+        var normalizedTranslationQuality = Math.Min(1.0, avgTranslationWeight / 100.0);
+        var normalizedRotationQuality = Math.Min(1.0, avgRotationWeight / 100.0);
+        var averageConstraintQuality = (normalizedTranslationQuality + normalizedRotationQuality) / 2.0;
+
+        // Calculate covariance from constraint weights
+        var translationWeightVariance = constraints
+            .Select(c => c.ConstraintPose.TranslationWeight)
+            .Select(w => Math.Pow(w - avgTranslationWeight, 2))
+            .Average();
+            
+        var rotationWeightVariance = constraints
+            .Select(c => c.ConstraintPose.RotationWeight)
+            .Select(w => Math.Pow(w - avgRotationWeight, 2))
+            .Average();
+
+        // Convert weights to covariance (inverse relationship)
+        var translationCovBase = 1.0 / (avgTranslationWeight + 1e-6);
+        var rotationCovBase = 1.0 / (avgRotationWeight + 1e-6);
+
+        // Adjust covariance based on variance
+        var translationVarianceFactor = 1.0 + (translationWeightVariance / (avgTranslationWeight * avgTranslationWeight + 1e-6));
+        var rotationVarianceFactor = 1.0 + (rotationWeightVariance / (avgRotationWeight * avgRotationWeight + 1e-6));
+
+        var translationCov = translationCovBase * translationVarianceFactor;
+        var rotationCov = rotationCovBase * rotationVarianceFactor;
+
+        // Adjust covariance based on number of constraints
+        var constraintCountFactor = 1.0 / (1.0 + Math.Log10(Math.Max(1, constraintCount)));
+        translationCov *= constraintCountFactor;
+        rotationCov *= constraintCountFactor;
+
+        // Create covariance matrix for 2D pose (x, y, theta)
+        var covariance = Matrix3x3.Covariance(
+            xx: translationCov,
+            yy: translationCov,
+            tt: rotationCov,
+            xy: 0.0,
+            xt: 0.0,
+            yt: 0.0
+        );
+
+        return new Result(covariance, constraintCount, averageConstraintQuality);
+    }
+}