Initial commit
This commit is contained in:
@@ -0,0 +1,932 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user