82 lines
3.3 KiB
C#
82 lines
3.3 KiB
C#
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);
|
|
}
|
|
}
|