Initial commit
This commit is contained in:
@@ -0,0 +1,359 @@
|
||||
using Microsoft.Extensions.Logging;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for detecting walls from point cloud data and calculating alignment angles
|
||||
/// </summary>
|
||||
public static class WallAlignmentHelper
|
||||
{
|
||||
private const double MIN_WALL_LENGTH = 1.0; // Minimum wall length in meters
|
||||
private const double RANSAC_INLIER_THRESHOLD = 0.05; // 5cm tolerance for RANSAC
|
||||
private const int RANSAC_ITERATIONS = 100;
|
||||
private const int MIN_INLIERS = 20; // Minimum points to consider a valid wall
|
||||
|
||||
#region Internal Types
|
||||
|
||||
/// <summary>
|
||||
/// Line representation: ax + by + c = 0 (normalized: a^2 + b^2 = 1)
|
||||
/// </summary>
|
||||
private struct Line
|
||||
{
|
||||
public double A { get; set; }
|
||||
public double B { get; set; }
|
||||
public double C { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Get angle of line relative to X-axis in radians
|
||||
/// Line equation: ax + by + c = 0
|
||||
/// Direction vector: (-b, a)
|
||||
/// Angle = atan2(a, -b)
|
||||
/// </summary>
|
||||
public double GetAngle() => Math.Atan2(A, -B);
|
||||
|
||||
/// <summary>
|
||||
/// Get perpendicular distance from a point to this line
|
||||
/// </summary>
|
||||
public double DistanceToPoint(Vector3 point)
|
||||
{
|
||||
return Math.Abs(A * point.X + B * point.Y + C);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Detected wall information
|
||||
/// </summary>
|
||||
private struct Wall
|
||||
{
|
||||
public Line Line { get; set; }
|
||||
public int InlierCount { get; set; }
|
||||
public double Length { get; set; }
|
||||
public Vector3 StartPoint { get; set; }
|
||||
public Vector3 EndPoint { get; set; }
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public API
|
||||
|
||||
/// <summary>
|
||||
/// Detect the longest wall from a collection of 2D points and calculate the minimum
|
||||
/// rotation angle needed to align it with either the X or Y axis
|
||||
/// </summary>
|
||||
/// <param name="points">Point cloud in base_link frame</param>
|
||||
/// <param name="logger">Logger for debug information</param>
|
||||
/// <returns>
|
||||
/// Compensation angle in radians, or null if no valid wall found.
|
||||
/// This angle should be applied to robot orientation to make the wall parallel to X or Y axis.
|
||||
/// </returns>
|
||||
public static double? DetectWallAndCalculateCompensation(
|
||||
IReadOnlyList<Vector3> points,
|
||||
ILogger logger)
|
||||
{
|
||||
if (points == null || points.Count < MIN_INLIERS)
|
||||
{
|
||||
logger.LogWarning("WallAlignment: Insufficient points for wall detection (count: {Count})", points?.Count ?? 0);
|
||||
return null;
|
||||
}
|
||||
|
||||
logger.LogInformation("WallAlignment: Processing {Count} points for wall detection", points.Count);
|
||||
|
||||
// Detect all walls using RANSAC
|
||||
var walls = DetectWallsRANSAC(points, logger);
|
||||
|
||||
if (walls.Count == 0)
|
||||
{
|
||||
logger.LogWarning("WallAlignment: No walls detected");
|
||||
return null;
|
||||
}
|
||||
|
||||
// Find the longest wall
|
||||
var longestWall = walls.OrderByDescending(w => w.Length).First();
|
||||
|
||||
logger.LogInformation(
|
||||
"WallAlignment: Longest wall found - Length: {Length:F2}m, Inliers: {Inliers}, Angle: {Angle:F2}rad ({AngleDeg:F2}°)",
|
||||
longestWall.Length,
|
||||
longestWall.InlierCount,
|
||||
longestWall.Line.GetAngle(),
|
||||
longestWall.Line.GetAngle() * 180.0 / Math.PI);
|
||||
|
||||
// Calculate compensation angle
|
||||
var wallAngle = longestWall.Line.GetAngle();
|
||||
var compensationAngle = CalculateMinimumRotationToAxis(wallAngle);
|
||||
|
||||
logger.LogInformation(
|
||||
"WallAlignment: Compensation angle: {Angle:F2}rad ({AngleDeg:F2}°)",
|
||||
compensationAngle,
|
||||
compensationAngle * 180.0 / Math.PI);
|
||||
|
||||
return compensationAngle;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region RANSAC Wall Detection
|
||||
|
||||
/// <summary>
|
||||
/// Detect walls using RANSAC line fitting algorithm
|
||||
/// </summary>
|
||||
private static List<Wall> DetectWallsRANSAC(IReadOnlyList<Vector3> points, ILogger logger)
|
||||
{
|
||||
var walls = new List<Wall>();
|
||||
var unusedPoints = points.ToList();
|
||||
var random = new Random(DateTime.Now.Millisecond);
|
||||
|
||||
// Iteratively find walls until not enough points remain
|
||||
while (unusedPoints.Count >= MIN_INLIERS)
|
||||
{
|
||||
Line bestLine = default;
|
||||
int bestInlierCount = 0;
|
||||
List<Vector3> bestInliers = [];
|
||||
|
||||
// RANSAC iterations
|
||||
for (int iter = 0; iter < RANSAC_ITERATIONS; iter++)
|
||||
{
|
||||
// Randomly select 2 points
|
||||
if (unusedPoints.Count < 2) break;
|
||||
|
||||
var idx1 = random.Next(unusedPoints.Count);
|
||||
var idx2 = random.Next(unusedPoints.Count);
|
||||
|
||||
if (idx1 == idx2) continue;
|
||||
|
||||
var p1 = unusedPoints[idx1];
|
||||
var p2 = unusedPoints[idx2];
|
||||
|
||||
// Skip if points are too close
|
||||
var dx = p2.X - p1.X;
|
||||
var dy = p2.Y - p1.Y;
|
||||
var dist = Math.Sqrt(dx * dx + dy * dy);
|
||||
if (dist < 0.1) continue; // Minimum 10cm distance
|
||||
|
||||
// Fit line through these 2 points
|
||||
var line = FitLineThroughPoints(p1, p2);
|
||||
|
||||
// Count inliers
|
||||
var inliers = new List<Vector3>();
|
||||
foreach (var point in unusedPoints)
|
||||
{
|
||||
if (line.DistanceToPoint(point) < RANSAC_INLIER_THRESHOLD)
|
||||
{
|
||||
inliers.Add(point);
|
||||
}
|
||||
}
|
||||
|
||||
// Update best model
|
||||
if (inliers.Count > bestInlierCount)
|
||||
{
|
||||
bestInlierCount = inliers.Count;
|
||||
bestInliers = inliers;
|
||||
bestLine = line;
|
||||
}
|
||||
}
|
||||
|
||||
// Check if we found a valid wall
|
||||
if (bestInlierCount < MIN_INLIERS)
|
||||
{
|
||||
break; // No more walls to find
|
||||
}
|
||||
|
||||
// Calculate wall length (distance between furthest inlier points)
|
||||
var (startPoint, endPoint, length) = CalculateWallExtent(bestInliers);
|
||||
|
||||
if (length < MIN_WALL_LENGTH)
|
||||
{
|
||||
// Wall too short, remove inliers and continue
|
||||
foreach (var inlier in bestInliers)
|
||||
{
|
||||
unusedPoints.Remove(inlier);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
// Valid wall found
|
||||
walls.Add(new Wall
|
||||
{
|
||||
Line = bestLine,
|
||||
InlierCount = bestInlierCount,
|
||||
Length = length,
|
||||
StartPoint = startPoint,
|
||||
EndPoint = endPoint
|
||||
});
|
||||
|
||||
logger.LogDebug(
|
||||
"WallAlignment: Wall detected - Length: {Length:F2}m, Inliers: {Inliers}",
|
||||
length, bestInlierCount);
|
||||
|
||||
// Remove inliers from unused points
|
||||
foreach (var inlier in bestInliers)
|
||||
{
|
||||
unusedPoints.Remove(inlier);
|
||||
}
|
||||
}
|
||||
|
||||
return walls;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Fit a line through two points using line equation: ax + by + c = 0
|
||||
/// where a^2 + b^2 = 1 (normalized)
|
||||
/// </summary>
|
||||
private static Line FitLineThroughPoints(Vector3 p1, Vector3 p2)
|
||||
{
|
||||
var dx = p2.X - p1.X;
|
||||
var dy = p2.Y - p1.Y;
|
||||
var length = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (length < 1e-6)
|
||||
{
|
||||
// Points are identical, return arbitrary line
|
||||
return new Line { A = 1, B = 0, C = -p1.X };
|
||||
}
|
||||
|
||||
// Normal to line: (dy, -dx) / length (perpendicular to direction vector)
|
||||
var a = dy / length;
|
||||
var b = -dx / length;
|
||||
var c = -(a * p1.X + b * p1.Y);
|
||||
|
||||
return new Line { A = a, B = b, C = c };
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate wall extent (start point, end point, and length)
|
||||
/// </summary>
|
||||
private static (Vector3 StartPoint, Vector3 EndPoint, double Length) CalculateWallExtent(
|
||||
List<Vector3> inliers)
|
||||
{
|
||||
if (inliers.Count < 2)
|
||||
{
|
||||
return (Vector3.Zero, Vector3.Zero, 0);
|
||||
}
|
||||
|
||||
// Find two points that are furthest apart
|
||||
var maxDist = 0.0;
|
||||
var startIdx = 0;
|
||||
var endIdx = 0;
|
||||
|
||||
for (int i = 0; i < inliers.Count; i++)
|
||||
{
|
||||
for (int j = i + 1; j < inliers.Count; j++)
|
||||
{
|
||||
var dx = inliers[j].X - inliers[i].X;
|
||||
var dy = inliers[j].Y - inliers[i].Y;
|
||||
var dist = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (dist > maxDist)
|
||||
{
|
||||
maxDist = dist;
|
||||
startIdx = i;
|
||||
endIdx = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (inliers[startIdx], inliers[endIdx], maxDist);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Angle Compensation
|
||||
|
||||
/// <summary>
|
||||
/// Calculate minimum rotation angle to align the wall with X or Y axis
|
||||
/// </summary>
|
||||
/// <param name="wallAngle">Wall angle in radians (relative to X-axis)</param>
|
||||
/// <returns>Compensation angle in radians</returns>
|
||||
private static double CalculateMinimumRotationToAxis(double wallAngle)
|
||||
{
|
||||
// Normalize angle to [-pi, pi]
|
||||
while (wallAngle > Math.PI) wallAngle -= 2 * Math.PI;
|
||||
while (wallAngle < -Math.PI) wallAngle += 2 * Math.PI;
|
||||
|
||||
// Calculate rotation needed for each axis
|
||||
// For X-axis: wall should be at 0° or ±180°
|
||||
// For Y-axis: wall should be at ±90°
|
||||
|
||||
var rotations = new[]
|
||||
{
|
||||
-wallAngle, // Align with X-axis (0°)
|
||||
Math.PI - wallAngle, // Align with X-axis (180°)
|
||||
-Math.PI - wallAngle, // Align with X-axis (-180°)
|
||||
Math.PI / 2 - wallAngle, // Align with Y-axis (90°)
|
||||
-Math.PI / 2 - wallAngle // Align with Y-axis (-90°)
|
||||
};
|
||||
|
||||
// Find the smallest absolute rotation
|
||||
var minRotation = rotations.OrderBy(Math.Abs).First();
|
||||
|
||||
// Normalize result to [-pi, pi]
|
||||
while (minRotation > Math.PI) minRotation -= 2 * Math.PI;
|
||||
while (minRotation < -Math.PI) minRotation += 2 * Math.PI;
|
||||
|
||||
return minRotation;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Quaternion Helpers
|
||||
|
||||
/// <summary>
|
||||
/// Create a quaternion from a yaw angle (rotation around Z-axis)
|
||||
/// </summary>
|
||||
public static Quaternion CreateQuaternionFromYaw(double yawRadians)
|
||||
{
|
||||
// Quaternion for rotation around Z-axis:
|
||||
// q = [0, 0, sin(yaw/2), cos(yaw/2)]
|
||||
var halfYaw = yawRadians / 2.0;
|
||||
return new Quaternion(
|
||||
x: 0,
|
||||
y: 0,
|
||||
z: Math.Sin(halfYaw),
|
||||
w: Math.Cos(halfYaw)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Extract yaw angle from a quaternion
|
||||
/// </summary>
|
||||
public static double GetYawFromQuaternion(Quaternion q)
|
||||
{
|
||||
// Yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))
|
||||
return Math.Atan2(
|
||||
2.0 * (q.W * q.Z + q.X * q.Y),
|
||||
1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Combine current robot yaw with compensation angle
|
||||
/// </summary>
|
||||
public static Quaternion ApplyCompensation(Quaternion currentOrientation, double compensationAngle)
|
||||
{
|
||||
var currentYaw = GetYawFromQuaternion(currentOrientation);
|
||||
var newYaw = currentYaw + compensationAngle;
|
||||
return CreateQuaternionFromYaw(newYaw);
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
Reference in New Issue
Block a user