932 lines
35 KiB
C#
932 lines
35 KiB
C#
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.SLAM;
|
|
using RobotNet10.Shared.Geometry;
|
|
// using RobotNet10.Shared.Numbers;
|
|
using RobotNet10.Shared.Sensor;
|
|
|
|
namespace RobotNet10.RobotApp.Detection;
|
|
|
|
/// <summary>
|
|
/// Session for detecting reflective markers based on shape matching with laser scan data
|
|
/// Supports 2, 3, or 4 reference points
|
|
/// IMPORTANT: Marker origin (0,0) in marker frame MUST be at the centroid of reference points
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// Create a new shape reflective marker detection session
|
|
/// </remarks>
|
|
/// <param name="markerReferencePoints">Reference points in marker frame (2, 3, or 4 points). Centroid must be at marker origin (0,0).</param>
|
|
/// <param name="searchRegion">Search region for marker center (in global frame)</param>
|
|
/// <param name="intensityThreshold">Intensity threshold for filtering reflective markers</param>
|
|
/// <param name="clusteringEps">OPTICS epsilon parameter for clustering</param>
|
|
/// <param name="clusteringMinPts">OPTICS minimum points parameter</param>
|
|
/// <param name="clusterThreshold">Reachability threshold for cluster extraction</param>
|
|
/// <param name="maxFitError">Maximum allowed fitting error (meters)</param>
|
|
/// <exception cref="ArgumentException">Thrown when number of reference points is not 2, 3, or 4</exception>
|
|
public class ShapeReflectiveDetector(
|
|
Point2D[] markerReferencePoints,
|
|
RectangleRegion searchRegion,
|
|
ISLAMService sLAMService,
|
|
ILidar lidar,
|
|
Pose lidarPose,
|
|
double intensityThreshold = 2500,
|
|
double clusteringEps = 0.1,
|
|
int clusteringMinPts = 3,
|
|
double clusterThreshold = 0.5,
|
|
double maxFitError = 0.05) : IDetector
|
|
{
|
|
// Validate number of reference points (must be 2, 3, or 4)
|
|
private readonly Point2D[] _validatedMarkerPoints = markerReferencePoints.Length is >= 1 and <= 4
|
|
? markerReferencePoints
|
|
: throw new ArgumentException(
|
|
$"Number of reference points must be between 2 and 4, but got {markerReferencePoints.Length}",
|
|
nameof(markerReferencePoints));
|
|
|
|
private readonly OpticsClusteringAlgorithm _optics = new(clusteringEps, clusteringMinPts);
|
|
|
|
private readonly Lock _lockPose = new();
|
|
private readonly Lock _lockScan = new();
|
|
|
|
private bool _isActive = false;
|
|
private bool _isProcessing = false;
|
|
private Thread? _processingThread;
|
|
private AutoResetEvent? _scanReceivedEvent;
|
|
private LaserScan? _latestScan;
|
|
private DateTime _lastScanTime = DateTime.MinValue;
|
|
|
|
/// <summary>
|
|
/// Detected marker pose in global frame
|
|
/// Initialized with search region center, updated when marker is detected
|
|
/// </summary>
|
|
public Pose MarkerPose
|
|
{
|
|
get
|
|
{
|
|
lock (_lockPose)
|
|
{
|
|
return field;
|
|
}
|
|
}
|
|
private set
|
|
{
|
|
lock (_lockPose)
|
|
{
|
|
field = value;
|
|
}
|
|
}
|
|
} = new Pose(new Vector3(searchRegion.Center.X, searchRegion.Center.Y, 0), CreateQuaternionFromYaw(searchRegion.RotationAngle));
|
|
|
|
/// <summary>
|
|
/// Timestamp of the last successful marker detection
|
|
/// </summary>
|
|
public DateTime DetectionTime { get; private set; }
|
|
|
|
/// <summary>
|
|
/// Activate the marker detection session
|
|
/// Starts listening to laser scan data and processes it in background
|
|
/// </summary>
|
|
public void Active()
|
|
{
|
|
if (_isActive)
|
|
return;
|
|
|
|
_isActive = true;
|
|
_scanReceivedEvent = new AutoResetEvent(false);
|
|
|
|
// Create and start processing thread
|
|
_processingThread = new Thread(ProcessingThreadLoop)
|
|
{
|
|
Name = "ShapeReflectiveDetection",
|
|
IsBackground = true
|
|
};
|
|
_processingThread.Start();
|
|
|
|
// Subscribe to laser scan data
|
|
lidar.ScanDataReceived += OnScanDataReceived;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Disable the marker detection session
|
|
/// Stops processing laser scan data and unsubscribes from events
|
|
/// </summary>
|
|
public void Disable()
|
|
{
|
|
if (!_isActive)
|
|
return;
|
|
|
|
_isActive = false;
|
|
|
|
// Unsubscribe from laser scan data
|
|
lidar.ScanDataReceived -= OnScanDataReceived;
|
|
|
|
// Signal the thread to wake up and exit
|
|
_scanReceivedEvent?.Set();
|
|
|
|
// Wait for thread to finish
|
|
_processingThread?.Join();
|
|
_processingThread = null;
|
|
|
|
// Dispose wait handle
|
|
_scanReceivedEvent?.Dispose();
|
|
_scanReceivedEvent = null;
|
|
|
|
// Clear latest scan
|
|
lock (_lockScan)
|
|
{
|
|
_latestScan = null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Event handler for laser scan data received
|
|
/// Stores the latest scan and signals the processing thread
|
|
/// If already processing, skip this scan to avoid overload
|
|
/// </summary>
|
|
private void OnScanDataReceived(object? _, LidarScanDataEventArgs e)
|
|
{
|
|
// Check if already processing, skip if busy
|
|
lock (_lockScan)
|
|
{
|
|
if (_isProcessing)
|
|
return;
|
|
|
|
// Store latest scan data
|
|
_latestScan = e.MeasurementData;
|
|
_lastScanTime = e.Timestamp;
|
|
}
|
|
|
|
// Signal the processing thread that new data is available
|
|
_scanReceivedEvent?.Set();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Processing thread loop that waits for new scan data and processes it
|
|
/// </summary>
|
|
private void ProcessingThreadLoop()
|
|
{
|
|
while (_isActive)
|
|
{
|
|
// Wait for signal that new scan data is available
|
|
if (_scanReceivedEvent?.WaitOne() == true)
|
|
{
|
|
// Check if still active (might have been disabled)
|
|
if (!_isActive)
|
|
break;
|
|
|
|
// Get the latest scan data
|
|
LaserScan? scanToProcess;
|
|
lock (_lockScan)
|
|
{
|
|
scanToProcess = _latestScan;
|
|
_latestScan = null; // Clear after reading
|
|
|
|
// Set processing flag
|
|
if (scanToProcess != null)
|
|
_isProcessing = true;
|
|
}
|
|
|
|
// Process the scan if available
|
|
if (scanToProcess is LaserScan scan)
|
|
{
|
|
try
|
|
{
|
|
var currentPose = sLAMService.CurrentPose;
|
|
|
|
var markerReferenceSearchRegionsInGlobal = CalculateMarkerReferenceSearchRegionsInGlobal(searchRegion);
|
|
|
|
var markerReferenceSearchRegionsInRobot = TransformRegionsFromGlobalToRobot(markerReferenceSearchRegionsInGlobal, currentPose);
|
|
|
|
var markerReferenceSearchRegions = TransformRegionsFromRobotToLidar(markerReferenceSearchRegionsInRobot, lidarPose);
|
|
|
|
var (angleStart, angleEnd) = CalculateAngleRangeFromRegions(markerReferenceSearchRegions);
|
|
|
|
var points = ConvertLaserScanToPoints(scan, intensityThreshold, angleStart, angleEnd);
|
|
|
|
_optics.ClearPoints();
|
|
_optics.AddPoints(points);
|
|
_optics.Run();
|
|
|
|
// Extract cluster indices for this region
|
|
var clusters = _optics.GetClusters(clusterThreshold);
|
|
|
|
// Extract centroids from clusters in each region
|
|
var centroids = clusters.Select(cluster => CalculateCentroid([.. cluster])).ToList();
|
|
|
|
// Match centroids to their corresponding search regions
|
|
var regionCentroids = new List<List<Point2D>>();
|
|
for (int i = 0; i < markerReferenceSearchRegions.Count; i++)
|
|
{
|
|
var region = markerReferenceSearchRegions[i];
|
|
var matchingCentroids = new List<Point2D>();
|
|
|
|
foreach (var centroid in centroids)
|
|
{
|
|
if (region.ContainsPoint(centroid.X, centroid.Y))
|
|
{
|
|
matchingCentroids.Add(centroid);
|
|
}
|
|
}
|
|
|
|
regionCentroids.Add(matchingCentroids);
|
|
}
|
|
|
|
// Find best matching pose in lidar frame
|
|
var poseInLidar = FindMatchingPoses(regionCentroids);
|
|
|
|
if (poseInLidar.HasValue)
|
|
{
|
|
// Transform pose from lidar frame -> robot frame -> global frame
|
|
var poseInRobot = TransformPose(poseInLidar.Value, lidarPose);
|
|
var poseInGlobal = TransformPose(poseInRobot, currentPose);
|
|
|
|
// Update marker pose and detection time
|
|
MarkerPose = poseInGlobal;
|
|
DetectionTime = _lastScanTime;
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
// Clear processing flag
|
|
lock (_lockScan)
|
|
{
|
|
_isProcessing = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform search region from global frame to robot frame (inverse transform)
|
|
/// </summary>
|
|
/// <param name="searchRegion">Search region in global frame</param>
|
|
/// <param name="robotPose">Robot pose in global frame</param>
|
|
/// <returns>Transformed search region in robot frame</returns>
|
|
private static RectangleRegion TransformSearchRegionFromGlobalToRobot(RectangleRegion searchRegion, Pose robotPose)
|
|
{
|
|
// Get yaw angle from robot pose
|
|
double robotYaw = robotPose.Orientation.ToYawRadian();
|
|
|
|
// Inverse transform: global frame → robot frame
|
|
// Translate center from global to robot origin (inverse)
|
|
double dx = searchRegion.Center.X - robotPose.Position.X;
|
|
double dy = searchRegion.Center.Y - robotPose.Position.Y;
|
|
|
|
// Rotate by negative robot yaw (inverse rotation)
|
|
double cosInv = Math.Cos(-robotYaw);
|
|
double sinInv = Math.Sin(-robotYaw);
|
|
|
|
double newCenterX = dx * cosInv - dy * sinInv;
|
|
double newCenterY = dx * sinInv + dy * cosInv;
|
|
|
|
// Transform rotation angle (subtract robot yaw)
|
|
double newRotation = searchRegion.RotationAngle - robotYaw;
|
|
|
|
// Normalize angle to [-π, π]
|
|
newRotation = NormalizeAngle(newRotation);
|
|
|
|
return new RectangleRegion(
|
|
new Point2D(newCenterX, newCenterY),
|
|
searchRegion.Width,
|
|
searchRegion.Height,
|
|
newRotation);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform search region from robot frame to lidar frame (inverse transform)
|
|
/// </summary>
|
|
/// <param name="searchRegion">Search region in robot frame</param>
|
|
/// <param name="lidarPose">Lidar pose relative to robot base</param>
|
|
/// <returns>Transformed search region in lidar frame</returns>
|
|
private static RectangleRegion TransformSearchRegionInverse(RectangleRegion searchRegion, Pose lidarPose)
|
|
{
|
|
// Get yaw angle from lidar pose
|
|
double lidarYaw = lidarPose.Orientation.ToYawRadian();
|
|
|
|
// Inverse transform: robot frame → lidar frame
|
|
// Translate center from robot origin to lidar origin (inverse)
|
|
double dx = searchRegion.Center.X - lidarPose.Position.X;
|
|
double dy = searchRegion.Center.Y - lidarPose.Position.Y;
|
|
|
|
// Rotate by negative lidar yaw (inverse rotation)
|
|
double cosInv = Math.Cos(-lidarYaw);
|
|
double sinInv = Math.Sin(-lidarYaw);
|
|
|
|
double newCenterX = dx * cosInv - dy * sinInv;
|
|
double newCenterY = dx * sinInv + dy * cosInv;
|
|
|
|
// Transform rotation angle (subtract lidar yaw)
|
|
double newRotation = searchRegion.RotationAngle - lidarYaw;
|
|
|
|
// Normalize angle to [-π, π]
|
|
newRotation = NormalizeAngle(newRotation);
|
|
|
|
return new RectangleRegion(
|
|
new Point2D(newCenterX, newCenterY),
|
|
searchRegion.Width,
|
|
searchRegion.Height,
|
|
newRotation);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate search regions for each marker reference point in global frame
|
|
/// Transforms each reference point from marker frame to global frame
|
|
/// based on predicted marker pose, then creates search region around it
|
|
/// Each search region inherits size from the main search region to account for pose uncertainty
|
|
/// </summary>
|
|
/// <param name="searchRegion">Predicted marker pose (center + rotation) in global frame</param>
|
|
/// <returns>List of search regions for each reference point in global frame</returns>
|
|
private List<RectangleRegion> CalculateMarkerReferenceSearchRegionsInGlobal(RectangleRegion searchRegion)
|
|
{
|
|
var regions = new List<RectangleRegion>();
|
|
|
|
// Use search region center and rotation as predicted marker pose in global frame
|
|
double markerX = searchRegion.Center.X;
|
|
double markerY = searchRegion.Center.Y;
|
|
double markerRotation = searchRegion.RotationAngle;
|
|
|
|
double cosTheta = Math.Cos(markerRotation);
|
|
double sinTheta = Math.Sin(markerRotation);
|
|
|
|
// Transform each reference point from marker frame to global frame
|
|
for (int i = 0; i < _validatedMarkerPoints.Length; i++)
|
|
{
|
|
var refPoint = _validatedMarkerPoints[i];
|
|
|
|
// Apply rotation and translation to transform from marker frame to global frame
|
|
double pointX = markerX + refPoint.X * cosTheta - refPoint.Y * sinTheta;
|
|
double pointY = markerY + refPoint.X * sinTheta + refPoint.Y * cosTheta;
|
|
|
|
// Create search region centered at this predicted point
|
|
// Use same size as main search region to account for pose uncertainty
|
|
regions.Add(new RectangleRegion(
|
|
new Point2D(pointX, pointY),
|
|
searchRegion.Width,
|
|
searchRegion.Height,
|
|
0)); // Axis-aligned for simplicity
|
|
}
|
|
|
|
return regions;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform a list of rectangle regions from global frame to robot frame
|
|
/// </summary>
|
|
/// <param name="regionsInGlobal">List of regions in global frame</param>
|
|
/// <param name="robotPose">Robot pose in global frame</param>
|
|
/// <returns>List of regions in robot frame</returns>
|
|
private static List<RectangleRegion> TransformRegionsFromGlobalToRobot(List<RectangleRegion> regionsInGlobal, Pose robotPose)
|
|
{
|
|
var regionsInRobot = new List<RectangleRegion>();
|
|
|
|
foreach (var region in regionsInGlobal)
|
|
{
|
|
var transformedRegion = TransformSearchRegionFromGlobalToRobot(region, robotPose);
|
|
regionsInRobot.Add(transformedRegion);
|
|
}
|
|
|
|
return regionsInRobot;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform a list of rectangle regions from robot frame to lidar frame
|
|
/// </summary>
|
|
/// <param name="regionsInRobot">List of regions in robot frame</param>
|
|
/// <param name="lidarPose">Lidar pose relative to robot base</param>
|
|
/// <returns>List of regions in lidar frame</returns>
|
|
private static List<RectangleRegion> TransformRegionsFromRobotToLidar(List<RectangleRegion> regionsInRobot, Pose lidarPose)
|
|
{
|
|
var regionsInLidar = new List<RectangleRegion>();
|
|
|
|
foreach (var region in regionsInRobot)
|
|
{
|
|
var transformedRegion = TransformSearchRegionInverse(region, lidarPose);
|
|
regionsInLidar.Add(transformedRegion);
|
|
}
|
|
|
|
return regionsInLidar;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate the angle range needed to cover all search regions
|
|
/// This optimizes laser scan processing by only considering relevant angles
|
|
/// </summary>
|
|
/// <param name="regions">Search regions to analyze</param>
|
|
/// <returns>Tuple of (angleStart, angleEnd) in radians</returns>
|
|
private static (double angleStart, double angleEnd) CalculateAngleRangeFromRegions(List<RectangleRegion> regions)
|
|
{
|
|
double minAngle = double.MaxValue;
|
|
double maxAngle = double.MinValue;
|
|
|
|
foreach (var region in regions)
|
|
{
|
|
// Get the 4 corners of the rectangle
|
|
var corners = GetRectangleCorners(region);
|
|
|
|
// Calculate angle to each corner from origin (lidar position at 0,0)
|
|
foreach (var corner in corners)
|
|
{
|
|
double angle = Math.Atan2(corner.Y, corner.X);
|
|
|
|
if (angle < minAngle) minAngle = angle;
|
|
if (angle > maxAngle) maxAngle = angle;
|
|
}
|
|
}
|
|
|
|
// Add small margin (5 degrees) to ensure we don't miss any points at the boundaries
|
|
const double margin = 5.0 * Math.PI / 180.0; // 5 degrees in radians
|
|
minAngle -= margin;
|
|
maxAngle += margin;
|
|
|
|
// Clamp to valid angle range [-π, π]
|
|
minAngle = Math.Max(minAngle, -Math.PI);
|
|
maxAngle = Math.Min(maxAngle, Math.PI);
|
|
|
|
return (minAngle, maxAngle);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the 4 corners of a rectangle region
|
|
/// </summary>
|
|
/// <param name="region">Rectangle region</param>
|
|
/// <returns>List of 4 corner points in the same frame as the input region</returns>
|
|
private static List<Point2D> GetRectangleCorners(RectangleRegion region)
|
|
{
|
|
double halfWidth = region.Width / 2.0;
|
|
double halfHeight = region.Height / 2.0;
|
|
|
|
// Define 4 corners in local frame (before rotation)
|
|
var localCorners = new List<(double x, double y)>
|
|
{
|
|
(-halfWidth, -halfHeight),
|
|
(halfWidth, -halfHeight),
|
|
(halfWidth, halfHeight),
|
|
(-halfWidth, halfHeight)
|
|
};
|
|
|
|
// Transform to region frame (apply rotation and translation)
|
|
var corners = new List<Point2D>();
|
|
double cosTheta = Math.Cos(region.RotationAngle);
|
|
double sinTheta = Math.Sin(region.RotationAngle);
|
|
|
|
foreach (var (x, y) in localCorners)
|
|
{
|
|
double worldX = region.Center.X + x * cosTheta - y * sinTheta;
|
|
double worldY = region.Center.Y + x * sinTheta + y * cosTheta;
|
|
corners.Add(new Point2D(worldX, worldY));
|
|
}
|
|
|
|
return corners;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find best matching pose from region centroids
|
|
/// Returns the pose with the lowest fitting error
|
|
/// </summary>
|
|
/// <param name="regionCentroids">Centroids from each search region</param>
|
|
/// <returns>Best detected pose, or null if no valid match found</returns>
|
|
private Pose? FindMatchingPoses(List<List<Point2D>> regionCentroids)
|
|
{
|
|
// Generate all combinations of centroids (one from each region)
|
|
// GenerateCombinations already validates that all regions have clusters
|
|
var combinations = GenerateCombinations(regionCentroids);
|
|
|
|
if (combinations.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
Pose? bestPose = null;
|
|
double bestError = double.MaxValue;
|
|
int validPoseCount = 0;
|
|
int acceptableErrorCount = 0;
|
|
|
|
foreach (var combination in combinations)
|
|
{
|
|
// Try to estimate pose from this combination
|
|
var pose = EstimatePoseFromPoints(_validatedMarkerPoints, combination);
|
|
|
|
if (pose.HasValue)
|
|
{
|
|
validPoseCount++;
|
|
// Calculate fitting error
|
|
double error = CalculateFitError(_validatedMarkerPoints, combination, pose.Value);
|
|
|
|
// Check if error is acceptable and better than previous best
|
|
if (error <= maxFitError && error < bestError)
|
|
{
|
|
acceptableErrorCount++;
|
|
bestPose = pose.Value;
|
|
bestError = error;
|
|
}
|
|
}
|
|
}
|
|
|
|
return bestPose;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Generate all valid combinations of centroids from different regions
|
|
/// IMPORTANT: Each combination[i] must come from regionCentroids[i] to maintain correct correspondence
|
|
/// with _validatedMarkerPoints[i]. Skipping regions is NOT allowed.
|
|
/// </summary>
|
|
/// <param name="regionCentroids">Centroids from each region</param>
|
|
/// <returns>List of point combinations</returns>
|
|
private List<List<Point2D>> GenerateCombinations(List<List<Point2D>> regionCentroids)
|
|
{
|
|
var combinations = new List<List<Point2D>>();
|
|
|
|
// Ensure we have exactly N regions (one per reference point)
|
|
int numReferencePoints = _validatedMarkerPoints.Length;
|
|
|
|
if (regionCentroids.Count != numReferencePoints)
|
|
{
|
|
return combinations; // Return empty if mismatch
|
|
}
|
|
|
|
// Check if ALL required regions have at least one cluster
|
|
// If ANY region is empty, we cannot form valid combinations
|
|
for (int i = 0; i < numReferencePoints; i++)
|
|
{
|
|
if (regionCentroids[i].Count == 0)
|
|
{
|
|
return combinations; // Return empty - missing required points
|
|
}
|
|
}
|
|
|
|
// Generate combinations where combination[i] comes from regionCentroids[i]
|
|
void GenerateRecursive(int regionIndex, List<Point2D> current)
|
|
{
|
|
// Base case: processed all regions
|
|
if (regionIndex == numReferencePoints)
|
|
{
|
|
combinations.Add([.. current]);
|
|
return;
|
|
}
|
|
|
|
// Try each centroid from the CURRENT region only (no skipping!)
|
|
foreach (var centroid in regionCentroids[regionIndex])
|
|
{
|
|
current.Add(centroid);
|
|
GenerateRecursive(regionIndex + 1, current);
|
|
current.RemoveAt(current.Count - 1);
|
|
}
|
|
}
|
|
|
|
GenerateRecursive(0, []);
|
|
return combinations;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Estimate marker pose from reference points and measured points
|
|
/// Uses a simplified point set registration algorithm
|
|
/// </summary>
|
|
/// <param name="referencePoints">Reference points in marker frame</param>
|
|
/// <param name="measuredPoints">Measured points in lidar frame</param>
|
|
/// <returns>Estimated pose in lidar frame, or null if estimation fails</returns>
|
|
private static Pose? EstimatePoseFromPoints(Point2D[] referencePoints, List<Point2D> measuredPoints)
|
|
{
|
|
if (referencePoints.Length != measuredPoints.Count)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Handle single-point detection
|
|
if (referencePoints.Length == 1)
|
|
{
|
|
// For single point: marker position = measured position - reference point offset
|
|
// Since we don't know rotation, assume rotation = 0
|
|
// Marker position = measured point - reference point (with rotation 0)
|
|
double marker_X = measuredPoints[0].X - referencePoints[0].X;
|
|
double marker_Y = measuredPoints[0].Y - referencePoints[0].Y;
|
|
|
|
return new Pose
|
|
{
|
|
Position = new Vector3
|
|
{
|
|
X = marker_X,
|
|
Y = marker_Y,
|
|
Z = 0
|
|
},
|
|
Orientation = CreateQuaternionFromYaw(0) // Cannot determine rotation from single point
|
|
};
|
|
}
|
|
|
|
if (referencePoints.Length < 2)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Calculate centroids
|
|
var refCentroid = CalculateCentroid(referencePoints);
|
|
var measCentroid = CalculateCentroid([.. measuredPoints]);
|
|
|
|
// Center the point sets
|
|
var refCentered = referencePoints.Select(p => new Point2D(p.X - refCentroid.X, p.Y - refCentroid.Y)).ToList();
|
|
var measCentered = measuredPoints.Select(p => new Point2D(p.X - measCentroid.X, p.Y - measCentroid.Y)).ToList();
|
|
|
|
// Calculate rotation using SVD-like approach (simplified for 2D)
|
|
double theta = EstimateRotation(refCentered, measCentered);
|
|
|
|
// Calculate marker origin position in lidar frame
|
|
// Formula: t = M_centroid - R(theta) * R_centroid
|
|
// This accounts for cases where reference centroid is not at marker origin (0,0)
|
|
double cosTheta = Math.Cos(theta);
|
|
double sinTheta = Math.Sin(theta);
|
|
|
|
// R(theta) * refCentroid
|
|
double rotatedRefX = refCentroid.X * cosTheta - refCentroid.Y * sinTheta;
|
|
double rotatedRefY = refCentroid.X * sinTheta + refCentroid.Y * cosTheta;
|
|
|
|
// t = measCentroid - R(theta) * refCentroid
|
|
double markerX = measCentroid.X - rotatedRefX;
|
|
double markerY = measCentroid.Y - rotatedRefY;
|
|
|
|
var pose = new Pose
|
|
{
|
|
Position = new Vector3
|
|
{
|
|
X = markerX,
|
|
Y = markerY,
|
|
Z = 0
|
|
},
|
|
Orientation = CreateQuaternionFromYaw(theta)
|
|
};
|
|
|
|
return pose;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Estimate rotation angle between two centered point sets
|
|
/// </summary>
|
|
private static double EstimateRotation(List<Point2D> refCentered, List<Point2D> measCentered)
|
|
{
|
|
// Use cross-covariance method
|
|
double sxx = 0, sxy = 0, syx = 0, syy = 0;
|
|
|
|
for (int i = 0; i < refCentered.Count; i++)
|
|
{
|
|
sxx += measCentered[i].X * refCentered[i].X;
|
|
sxy += measCentered[i].X * refCentered[i].Y;
|
|
syx += measCentered[i].Y * refCentered[i].X;
|
|
syy += measCentered[i].Y * refCentered[i].Y;
|
|
}
|
|
|
|
// Calculate rotation angle using atan2
|
|
double theta = Math.Atan2(syx - sxy, sxx + syy);
|
|
|
|
return theta;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate fitting error between reference and measured points given a pose
|
|
/// Error represents the average Euclidean distance between:
|
|
/// - Predicted positions: where reference points SHOULD be (based on estimated pose)
|
|
/// - Measured positions: where reflective markers were ACTUALLY detected by lidar
|
|
///
|
|
/// Lower error = better match between model and reality
|
|
/// </summary>
|
|
/// <param name="referencePoints">Reference points in marker frame (model)</param>
|
|
/// <param name="measuredPoints">Measured points in lidar frame (reality)</param>
|
|
/// <param name="pose">Estimated marker pose to validate</param>
|
|
/// <returns>Average distance error in meters</returns>
|
|
private static double CalculateFitError(Point2D[] referencePoints, List<Point2D> measuredPoints, Pose pose)
|
|
{
|
|
if (referencePoints.Length != measuredPoints.Count)
|
|
return double.MaxValue;
|
|
|
|
double totalError = 0;
|
|
double theta = pose.Orientation.ToYawRadian();
|
|
|
|
// Pre-calculate cos and sin (optimization - computed once instead of per iteration)
|
|
double cosTheta = Math.Cos(theta);
|
|
double sinTheta = Math.Sin(theta);
|
|
|
|
for (int i = 0; i < referencePoints.Length; i++)
|
|
{
|
|
// Transform reference point from marker frame to lidar frame using the estimated pose
|
|
// Formula: P_predicted = t + R(theta) * P_reference
|
|
double transformedX = pose.Position.X + referencePoints[i].X * cosTheta - referencePoints[i].Y * sinTheta;
|
|
double transformedY = pose.Position.Y + referencePoints[i].X * sinTheta + referencePoints[i].Y * cosTheta;
|
|
|
|
// Calculate Euclidean distance between predicted and measured positions
|
|
double dx = transformedX - measuredPoints[i].X;
|
|
double dy = transformedY - measuredPoints[i].Y;
|
|
double distance = Math.Sqrt(dx * dx + dy * dy);
|
|
|
|
totalError += distance;
|
|
}
|
|
|
|
double averageError = totalError / referencePoints.Length;
|
|
|
|
return averageError;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create a quaternion from yaw angle (rotation around Z axis)
|
|
/// </summary>
|
|
private static RobotNet10.Shared.Geometry.Quaternion CreateQuaternionFromYaw(double yaw)
|
|
{
|
|
double halfYaw = yaw / 2.0;
|
|
return new RobotNet10.Shared.Geometry.Quaternion
|
|
{
|
|
X = 0,
|
|
Y = 0,
|
|
Z = Math.Sin(halfYaw),
|
|
W = Math.Cos(halfYaw)
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Normalize angle to [-π, π]
|
|
/// </summary>
|
|
private static double NormalizeAngle(double angle)
|
|
{
|
|
while (angle > Math.PI) angle -= 2 * Math.PI;
|
|
while (angle < -Math.PI) angle += 2 * Math.PI;
|
|
return angle;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform a pose by another pose (pose composition)
|
|
/// result = parentPose * childPose
|
|
/// </summary>
|
|
private static Pose TransformPose(Pose childPose, Pose parentPose)
|
|
{
|
|
// Get yaw angles
|
|
double parentYaw = parentPose.Orientation.ToYawRadian();
|
|
double childYaw = childPose.Orientation.ToYawRadian();
|
|
|
|
// Rotate child position by parent orientation
|
|
double cosParent = Math.Cos(parentYaw);
|
|
double sinParent = Math.Sin(parentYaw);
|
|
|
|
double globalX = parentPose.Position.X + childPose.Position.X * cosParent - childPose.Position.Y * sinParent;
|
|
double globalY = parentPose.Position.Y + childPose.Position.X * sinParent + childPose.Position.Y * cosParent;
|
|
|
|
// Combine orientations
|
|
double globalYaw = NormalizeAngle(parentYaw + childYaw);
|
|
|
|
return new Pose
|
|
{
|
|
Position = new Vector3
|
|
{
|
|
X = globalX,
|
|
Y = globalY,
|
|
Z = parentPose.Position.Z + childPose.Position.Z
|
|
},
|
|
Orientation = CreateQuaternionFromYaw(globalYaw)
|
|
};
|
|
}
|
|
|
|
private static List<Point> ConvertLaserScanToPoints(LaserScan scan, double intensityThreshold, double angleStart, double angleEnd)
|
|
{
|
|
var points = new List<Point>();
|
|
|
|
// Skip invalid scans
|
|
if (scan.Ranges.Length == 0)
|
|
{
|
|
return points;
|
|
}
|
|
|
|
int invalidRanges = 0;
|
|
int filteredByAngle = 0;
|
|
int filteredByIntensity = 0;
|
|
|
|
double normalizedStart = NormalizeAngle(angleStart);
|
|
double normalizedEnd = NormalizeAngle(angleEnd);
|
|
|
|
for (int i = 0; i < scan.Ranges.Length; i++)
|
|
{
|
|
double range = scan.Ranges[i];
|
|
|
|
// Skip invalid ranges (out of bounds or NaN/Infinity)
|
|
if (double.IsNaN(range) || double.IsInfinity(range) ||
|
|
range < scan.RangeMin || range > scan.RangeMax)
|
|
{
|
|
invalidRanges++;
|
|
continue;
|
|
}
|
|
|
|
// Calculate angle for this measurement
|
|
double alpha = scan.AngleMin + (i * scan.AngleIncrement);
|
|
|
|
// Normalize alpha to [-π, π] for proper comparison
|
|
double normalizedAlpha = NormalizeAngle(alpha);
|
|
|
|
// Handle angle wrapping around (e.g., from -π to π)
|
|
if (normalizedStart <= normalizedEnd)
|
|
{
|
|
// Normal case: angleStart < angleEnd
|
|
if (normalizedAlpha < normalizedStart || normalizedAlpha > normalizedEnd)
|
|
{
|
|
filteredByAngle++;
|
|
continue;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Wrapped case: angleEnd < angleStart (e.g., 3π/4 to -3π/4)
|
|
if (normalizedAlpha < normalizedStart && normalizedAlpha > normalizedEnd)
|
|
{
|
|
filteredByAngle++;
|
|
continue;
|
|
}
|
|
}
|
|
|
|
// Filter by intensity if threshold is set and intensities are available
|
|
if (scan.Intensities.Length > i)
|
|
{
|
|
if (scan.Intensities[i] < intensityThreshold)
|
|
{
|
|
filteredByIntensity++;
|
|
continue;
|
|
}
|
|
}
|
|
|
|
points.Add(new Point(range * Math.Cos(alpha), range * Math.Sin(alpha), range, alpha));
|
|
}
|
|
|
|
return points;
|
|
}
|
|
|
|
public static Point2D CalculateCentroid(Point2D[] cluster)
|
|
{
|
|
if (cluster.Length == 0)
|
|
return new Point2D(0, 0);
|
|
|
|
double sumX = 0;
|
|
double sumY = 0;
|
|
|
|
foreach (var point in cluster)
|
|
{
|
|
sumX += point.X;
|
|
sumY += point.Y;
|
|
}
|
|
|
|
return new Point2D(sumX / cluster.Length, sumY / cluster.Length);
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
// Disable detector (stops thread, unsubscribes events, disposes resources)
|
|
Disable();
|
|
|
|
// Suppress finalization since we've cleaned up
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
}
|
|
|
|
public readonly struct RectangleRegion(Point2D center, double width, double height, double rotationAngle)
|
|
{
|
|
/// <summary>
|
|
/// Center point of the rectangle
|
|
/// </summary>
|
|
public Point2D Center { get; init; } = center;
|
|
|
|
/// <summary>
|
|
/// Width of the rectangle (meters)
|
|
/// </summary>
|
|
public double Width { get; init; } = width;
|
|
|
|
/// <summary>
|
|
/// Height of the rectangle (meters)
|
|
/// </summary>
|
|
public double Height { get; init; } = height;
|
|
|
|
/// <summary>
|
|
/// Rotation angle in radians (counterclockwise from positive X-axis)
|
|
/// </summary>
|
|
public double RotationAngle { get; init; } = rotationAngle;
|
|
|
|
public RectangleRegion(double centerX, double centerY, double width, double height, double rotationAngle)
|
|
: this(new Point2D(centerX, centerY), width, height, rotationAngle)
|
|
{
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if a point is contained within this rectangle
|
|
/// Uses coordinate transformation to handle rotation efficiently
|
|
/// </summary>
|
|
/// <param name="point">Point to check</param>
|
|
/// <returns>True if point is inside the rectangle</returns>
|
|
public bool ContainsPoint(double x, double y)
|
|
{
|
|
// Translate point to rectangle's local coordinate system (center at origin)
|
|
double dx = x - Center.X;
|
|
double dy = y - Center.Y;
|
|
|
|
// Rotate point by negative rotation angle to align with rectangle axes
|
|
double cosTheta = Math.Cos(-RotationAngle);
|
|
double sinTheta = Math.Sin(-RotationAngle);
|
|
|
|
double localX = dx * cosTheta - dy * sinTheta;
|
|
double localY = dx * sinTheta + dy * cosTheta;
|
|
|
|
// Check if point is within rectangle bounds
|
|
double halfWidth = Width / 2.0;
|
|
double halfHeight = Height / 2.0;
|
|
|
|
return Math.Abs(localX) <= halfWidth && Math.Abs(localY) <= halfHeight;
|
|
}
|
|
} |