Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/DetectSession.cs

@@ -0,0 +1,204 @@
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.SLAM;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.Detection;
+
+public class DetectSession(Guid id, ISLAMService sLAMService) : IDetectSession
+{
+    private static uint SequenceDetection = 1;
+    private readonly List<DetectorInfo> _detectors = [];
+    private readonly Lock _lockGoal = new();
+    private Thread? _updateThread;
+    private bool _isRunning = false;
+    private const int UPDATE_FREQUENCY_HZ = 30;
+    private const double DETECTION_TIMEOUT_SECONDS = 2.0;
+
+    public Guid SessionId { get; } = id;
+
+    public PoseStamped? Goal
+    {
+        get
+        {
+            lock (_lockGoal)
+            {
+                return field;
+            }
+        }
+        private set
+        {
+            lock (_lockGoal)
+            {
+                field = value;
+            }
+        }
+    }
+
+    public void AddShapeReflectiveDetector(string markerId, int priority, Point2D[] markerReferencePoints, RectangleRegion searchRegion, ILidar lidar, Pose lidarPose, double intensityThreshold = 1000)
+    {
+        // Create detector with configurable intensity threshold
+        var detector = new ShapeReflectiveDetector(
+            markerReferencePoints,
+            searchRegion,
+            sLAMService,
+            lidar,
+            lidarPose,
+            intensityThreshold);
+
+        // Add to list with priority
+        _detectors.Add(new DetectorInfo(markerId, priority, detector));
+
+        // Sort by priority (higher priority first)
+        _detectors.Sort((a, b) => b.Priority.CompareTo(a.Priority));
+    }
+
+    public void AddQRDetector(string markerId, int priority, string qrCode, ICameraQr camera, Pose cameraPose)
+    {
+        // Create QR detector
+        var detector = new QRDetector(
+            qrCode,
+            sLAMService,
+            camera,
+            cameraPose);
+
+        // Add to list with priority
+        _detectors.Add(new DetectorInfo(markerId, priority, detector));
+
+        // Sort by priority (higher priority first)
+        _detectors.Sort((a, b) => b.Priority.CompareTo(a.Priority));
+    }
+
+    public void Start()
+    {
+        if (_isRunning)
+            return;
+
+        _isRunning = true;
+
+        // Activate all detectors
+        foreach (var detectorInfo in _detectors)
+        {
+            detectorInfo.Detector.Active();
+        }
+
+        // Create and start goal update thread (30Hz)
+        _updateThread = new Thread(UpdateGoalLoop)
+        {
+            Name = "DetectSession_UpdateGoal",
+            IsBackground = true
+        };
+        _updateThread.Start();
+    }
+
+    public void Dispose()
+    {
+        if (!_isRunning)
+            return;
+
+        _isRunning = false;
+
+        // Disable all detectors
+        foreach (var detectorInfo in _detectors)
+        {
+            detectorInfo.Detector.Disable();
+            detectorInfo.Detector.Dispose();
+        }
+
+        // Wait for thread to finish
+        _updateThread?.Join();
+        _updateThread = null;
+
+        // Clear detectors list
+        _detectors.Clear();
+
+        GC.SuppressFinalize(this);
+    }
+
+    /// <summary>
+    /// Thread loop to update Goal based on detector priorities at 30Hz
+    /// Filters detectors by detection time, pose validity, and priority
+    /// </summary>
+    private void UpdateGoalLoop()
+    {
+        int delayMs = 1000 / UPDATE_FREQUENCY_HZ; // ~33ms for 30Hz
+
+        while (_isRunning)
+        {
+            try
+            {
+                var now = DateTime.UtcNow;
+                PoseStamped? bestGoal = null;
+                int bestPriority = int.MinValue;
+                DateTime bestDetectionTime = DateTime.MinValue;
+
+                // Find best detector based on priority and detection freshness
+                foreach (var detectorInfo in _detectors)
+                {
+                    var markerPose = detectorInfo.Detector.MarkerPose;
+                    var detectionTime = detectorInfo.Detector.DetectionTime;
+
+                    // Skip if pose is invalid (default value)
+                    if (IsDefaultPose(markerPose))
+                        continue;
+
+                    // Skip if detection is too old (timeout)
+                    var timeSinceDetection = (now - detectionTime).TotalSeconds;
+                    if (timeSinceDetection > DETECTION_TIMEOUT_SECONDS)
+                        continue;
+
+                    // Select detector with highest priority
+                    // If same priority, prefer more recent detection
+                    if (detectorInfo.Priority > bestPriority ||
+                        (detectorInfo.Priority == bestPriority && detectionTime > bestDetectionTime))
+                    {
+                        bestGoal = new PoseStamped()
+                        {
+                            Header = new RobotNet10.Shared.Header()
+                            {
+                                FrameId = detectorInfo.MakerId,
+                                Stamp = detectionTime,
+                                Seq = ++SequenceDetection,
+                            },
+                            Pose = markerPose,
+                        };
+                        bestPriority = detectorInfo.Priority;
+                        bestDetectionTime = detectionTime;
+                    }
+                }
+
+                // Update goal if we found a valid one
+                if (bestGoal.HasValue)
+                {
+                    Goal = bestGoal.Value;
+                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [DetectSession] Updated Goal from detector '{bestGoal.Value.Header.FrameId}' with priority {bestPriority} at pose: [{bestGoal.Value.Pose.Position.X}, {bestGoal.Value.Pose.Position.Y}, {bestGoal.Value.Pose.Orientation.ToYawDegrees()}deg]");
+                }
+            }
+            catch
+            {
+                // Ignore errors in update loop
+            }
+
+            // Sleep to maintain 30Hz update rate
+            Thread.Sleep(delayMs);
+        }
+    }
+
+    /// <summary>
+    /// Check if pose is default/invalid
+    /// </summary>
+    private static bool IsDefaultPose(Pose pose)
+    {
+        return pose.Position.X == 0 &&
+               pose.Position.Y == 0 &&
+               pose.Position.Z == 0 &&
+               pose.Orientation.X == 0 &&
+               pose.Orientation.Y == 0 &&
+               pose.Orientation.Z == 0 &&
+               pose.Orientation.W == 0;
+    }
+
+    /// <summary>
+    /// Internal class to store detector with priority
+    /// </summary>
+    private record DetectorInfo(string MakerId, int Priority, IDetector Detector);
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/IDetectSession.cs

@@ -0,0 +1,13 @@
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.Detection;
+
+/// <summary>
+/// Interface for a marker detection session
+/// Manages detection of multiple markers with priority-based aggregation
+/// </summary>
+public interface IDetectSession : IDisposable
+{
+    Guid SessionId { get; }
+    PoseStamped? Goal { get; }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/IDetector.cs

@@ -0,0 +1,11 @@
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.Detection;
+
+public interface IDetector : IDisposable
+{
+    Pose MarkerPose { get; }
+    DateTime DetectionTime { get; }
+    void Active();
+    void Disable();
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/IMarkerDetector.cs

@@ -0,0 +1,36 @@
+using RobotNet10.Shared.Detection;
+
+namespace RobotNet10.RobotApp.Detection;
+
+/// <summary>
+/// Main service interface for marker and landmark detection
+/// Manages detection sessions and device transforms
+/// </summary>
+public interface IMarkerDetector : IDisposable
+{
+    /// <summary>
+    /// Create a new detection session
+    /// </summary>
+    /// <param name="config">Session configuration with search area and marker list</param>
+    /// <returns>Created detection session instance</returns>
+    /// <exception cref="ArgumentException">
+    /// Thrown if:
+    /// - Marker IDs don't exist in database
+    /// - Device IDs don't exist in device provider
+    /// - Invalid configuration parameters
+    /// </exception>
+    Task<IDetectSession> CreateSessionAsync(MarkersSearchRequest request);
+
+    /// <summary>
+    /// Get all active sessions
+    /// </summary>
+    /// <returns>Read-only list of active sessions</returns>
+    IReadOnlyList<IDetectSession> GetActiveSessions();
+
+    /// <summary>
+    /// Get session by ID
+    /// </summary>
+    /// <param name="sessionId">Session ID to find</param>
+    /// <returns>Session if found, null otherwise</returns>
+    IDetectSession? GetSession(Guid sessionId);
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/MarkerDetector.cs

@@ -0,0 +1,178 @@
+using Microsoft.Extensions.Configuration;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.SLAM;
+using RobotNet10.Shared.Detection;
+using RobotNet10.Shared.Enum;
+using RobotNet10.Shared.Geometry;
+using SkiaSharp;
+
+namespace RobotNet10.RobotApp.Detection;
+
+public class MarkerDetector(
+    IConfiguration configuration,
+    IDeviceProvider deviceProvider,
+    ISLAMService slamService) : IMarkerDetector
+{
+    private readonly MarkerDetectorConfiguration _config = BindConfig(configuration);
+    private readonly List<IDetectSession> _sessions = [];
+    private readonly Lock _lock = new();
+
+    private static MarkerDetectorConfiguration BindConfig(IConfiguration configuration)
+    {
+        var section = configuration.GetSection("Detection:MarkerDetector");
+        if (!section.Exists())
+            throw new InvalidOperationException("Configuration section 'Detection:MarkerDetector' not found.");
+
+        var config = new MarkerDetectorConfiguration();
+        section.Bind(config);
+        return config;
+    }
+
+    public Task<IDetectSession> CreateSessionAsync(MarkersSearchRequest request)
+    {
+        var session = new DetectSession(Guid.NewGuid(), slamService);
+
+        // Create search region from request in global frame
+        // (X, Y as center, Yaw as rotation - all in global/world coordinates)
+        var searchRegion = new RectangleRegion(
+            request.X,
+            request.Y,
+            request.Width,
+            request.Length,
+            request.Yaw);
+
+        foreach (var entry in request.MarkerSearchRequests)
+        {
+            switch (entry.Type)
+            {
+                case MarkerType.ShapeReflective:
+                    AddShapeReflectiveDetector(session, entry, searchRegion);
+                    break;
+
+                case MarkerType.QRCode:
+                    AddQRDetector(session, entry);
+                    break;
+
+                case MarkerType.ArUco:
+                    // TODO: Implement ArUco detector
+                    break;
+
+                default:
+                    // TODO: Handle other marker types
+                    break;
+            }
+        }
+
+        session.Start();
+
+        lock (_lock)
+        {
+            _sessions.Add(session);
+        }
+
+        return Task.FromResult<IDetectSession>(session);
+    }
+
+    public IReadOnlyList<IDetectSession> GetActiveSessions()
+    {
+        lock (_lock)
+        {
+            return [.. _sessions];
+        }
+    }
+
+    public IDetectSession? GetSession(Guid sessionId)
+    {
+        lock (_lock)
+        {
+            return _sessions.FirstOrDefault(s => s.SessionId == sessionId);
+        }
+    }
+
+    public void Dispose()
+    {
+        lock (_lock)
+        {
+            foreach (var session in _sessions)
+            {
+                session.Dispose();
+            }
+            _sessions.Clear();
+        }
+        GC.SuppressFinalize(this);
+    }
+
+    private void AddShapeReflectiveDetector(DetectSession session, MarkerEntry entry, RectangleRegion searchRegion)
+    {
+        // Get device from provider and cast to ILidar
+        var device = deviceProvider.GetDevice(entry.DeviceId)
+            ?? throw new ArgumentException($"Device '{entry.DeviceId}' not found.");
+
+        if (device is not ILidar lidar)
+            throw new ArgumentException($"Device '{entry.DeviceId}' is not an ILidar.");
+
+        // Get lidar transform from configuration (includes pose and intensity threshold)
+        var lidarTransform = GetDeviceTransform(_config.LidarDevices, entry.DeviceId);
+        var lidarPose = new Pose(
+            lidarTransform.Position,
+            new RobotNet10.Shared.Geometry.Quaternion(
+                lidarTransform.Orientation.X,
+                lidarTransform.Orientation.Y,
+                lidarTransform.Orientation.Z,
+                lidarTransform.Orientation.W));
+
+        // Parse reference points from Parameters [x1, y1, x2, y2, ...]
+        var referencePoints = entry.ReferencePoints.Select(p => new Point2D(p.X, p.Y)).ToArray();
+
+        session.AddShapeReflectiveDetector(entry.MarkerId,
+            entry.Priority,
+            referencePoints,
+            searchRegion,
+            lidar,
+            lidarPose,
+            lidarTransform.IntensityThreshold);
+    }
+
+    private void AddQRDetector(DetectSession session, MarkerEntry entry)
+    {
+        // Get device from provider and cast to ICameraQr
+        var device = deviceProvider.GetDevice(entry.DeviceId)
+            ?? throw new ArgumentException($"Device '{entry.DeviceId}' not found.");
+
+        if (device is not ICameraQr camera)
+            throw new ArgumentException($"Device '{entry.DeviceId}' is not an ICameraQr.");
+
+        // Get camera pose from configuration
+        var cameraPose = GetDevicePose(_config.QrDevices, entry.DeviceId);
+
+        // Use MarkerId as QR code string to detect
+        var qrCode = entry.Code;
+
+        session.AddQRDetector(entry.MarkerId,
+            entry.Priority,
+            qrCode,
+            camera,
+            cameraPose);
+    }
+
+    private static Pose GetDevicePose(DeviceTransform[] devices, string deviceId)
+    {
+        var transform = GetDeviceTransform(devices, deviceId);
+        return new Pose(
+            transform.Position,
+            new RobotNet10.Shared.Geometry.Quaternion(
+                transform.Orientation.X,
+                transform.Orientation.Y,
+                transform.Orientation.Z,
+                transform.Orientation.W));
+    }
+
+    private static DeviceTransform GetDeviceTransform(DeviceTransform[] devices, string deviceId)
+    {
+        var transform = Array.Find(devices, d => d.DeviceId == deviceId);
+        if (string.IsNullOrEmpty(transform.DeviceId))
+            throw new ArgumentException($"Device transform for '{deviceId}' not found in configuration.");
+
+        return transform;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/MarkerDetectorConfiguration.cs

@@ -0,0 +1,43 @@
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.Detection;
+
+#region Service Configuration
+
+/// <summary>
+/// Configuration for MarkerLandmarkDetector service
+/// Loaded from RobotConfig JSON (e.g., ModelConfigs/test.json)
+/// </summary>
+public class MarkerDetectorConfiguration
+{
+    /// <summary>
+    /// Device transforms: deviceId -> Pose3D in base_link frame
+    /// </summary>
+    public DeviceTransform[] LidarDevices { get; set; } = [];
+    public DeviceTransform[] QrDevices { get; set; } = [];
+}
+
+/// <summary>
+/// Transform configuration for a device (camera/LiDAR) relative to base_link
+/// </summary>
+public struct DeviceTransform
+{
+    public string DeviceId { get; set; }
+    /// <summary>
+    /// Position (X, Y, Z in meters) in base_link frame
+    /// </summary>
+    public Vector3 Position { get; set; }
+
+    /// <summary>
+    /// Orientation (quaternion) in base_link frame
+    /// </summary>
+    public Quaternion Orientation { get; set; }
+
+    /// <summary>
+    /// Intensity threshold for reflective marker detection (LiDAR devices only)
+    /// </summary>
+    public double IntensityThreshold { get; set; }
+}
+
+#endregion
+

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/OpticsClusteringAlgorithm.cs

@@ -0,0 +1,416 @@
+namespace RobotNet10.RobotApp.Detection;
+
+/// <summary>
+/// OPTICS (Ordering Points To Identify the Clustering Structure) clustering algorithm
+/// Implements density-based clustering with spatial indexing optimizations
+/// </summary>
+public class OpticsClusteringAlgorithm(double eps, int minPts)
+{
+    private readonly List<Point> _points = [];
+    private readonly List<int> _orderedList = [];
+    private readonly Dictionary<long, List<int>> _grid = [];
+    private double _gridCellSize = 0;
+    private KDTree? _kdTree;
+
+    /// <summary>
+    /// KD-tree implementation for efficient radius searches
+    /// </summary>
+    private class KDTree
+    {
+        private class Node
+        {
+            public int Idx { get; set; }
+            public int Left { get; set; } = -1;
+            public int Right { get; set; } = -1;
+        }
+
+        private readonly List<Point> _pts;
+        private readonly List<Node> _nodes;
+        private readonly int _root;
+
+        public KDTree(List<Point> points)
+        {
+            _pts = points;
+            _nodes = [];
+            _root = -1;
+
+            if (points.Count > 0)
+            {
+                var idxs = Enumerable.Range(0, points.Count).ToList();
+                _root = BuildRec(idxs, 0, idxs.Count - 1, 0);
+            }
+        }
+
+        private int BuildRec(List<int> idxs, int l, int r, int depth)
+        {
+            if (l > r) return -1;
+
+            int axis = depth % 2;
+            int m = (l + r) / 2;
+
+            // Partition based on axis
+            idxs.Sort(l, r - l + 1, Comparer<int>.Create((a, b) =>
+            {
+                if (axis == 0)
+                    return _pts[a].X.CompareTo(_pts[b].X);
+                return _pts[a].Y.CompareTo(_pts[b].Y);
+            }));
+
+            int nodeIdx = _nodes.Count;
+            _nodes.Add(new Node { Idx = idxs[m] });
+            _nodes[nodeIdx].Left = BuildRec(idxs, l, m - 1, depth + 1);
+            _nodes[nodeIdx].Right = BuildRec(idxs, m + 1, r, depth + 1);
+            return nodeIdx;
+        }
+
+        public List<int> RadiusSearch(Point q, double radius)
+        {
+            var result = new List<int>();
+            double r2 = radius * radius;
+            SearchRec(_root, q, r2, 0, result);
+            return result;
+        }
+
+        private void SearchRec(int nodeIdx, Point q, double r2, int depth, List<int> output)
+        {
+            if (nodeIdx < 0) return;
+
+            var node = _nodes[nodeIdx];
+            var p = _pts[node.Idx];
+
+            double dx = q.X - p.X;
+            double dy = q.Y - p.Y;
+            double dist2 = dx * dx + dy * dy;
+
+            if (dist2 <= r2)
+                output.Add(node.Idx);
+
+            int axis = depth % 2;
+            double diff = axis == 0 ? dx : dy;
+
+            if (diff <= 0)
+            {
+                SearchRec(node.Left, q, r2, depth + 1, output);
+                if (diff * diff <= r2)
+                    SearchRec(node.Right, q, r2, depth + 1, output);
+            }
+            else
+            {
+                SearchRec(node.Right, q, r2, depth + 1, output);
+                if (diff * diff <= r2)
+                    SearchRec(node.Left, q, r2, depth + 1, output);
+            }
+        }
+    }
+
+    /// <summary>
+    /// Add a single point to the dataset
+    /// </summary>
+    public void AddPoint(double x, double y, double range, double alpha)
+    {
+        _points.Add(new Point(x, y, range, alpha));
+
+        // If grid is enabled, insert into the grid
+        if (_gridCellSize > 0)
+        {
+            int ix = (int)Math.Floor(x / _gridCellSize);
+            int iy = (int)Math.Floor(y / _gridCellSize);
+            long key = CellKey(ix, iy);
+
+            if (!_grid.ContainsKey(key))
+                _grid[key] = [];
+
+            _grid[key].Add(_points.Count - 1);
+        }
+    }
+
+    /// <summary>
+    /// Add multiple points to the dataset
+    /// </summary>
+    public void AddPoints(List<Point> newPoints)
+    {
+        _points.Clear();
+        _points.AddRange(newPoints);
+
+        // Build spatial index using eps as cell size
+        _grid.Clear();
+        _gridCellSize = eps;
+        BuildSpatialIndex();
+        BuildKDTree();
+    }
+
+    /// <summary>
+    /// Clear all points and reset the algorithm state
+    /// </summary>
+    public void ClearPoints()
+    {
+        _points.Clear();
+        _orderedList.Clear();
+        _grid.Clear();
+        _gridCellSize = 0;
+        _kdTree = null;
+    }
+
+    /// <summary>
+    /// Run the OPTICS clustering algorithm
+    /// </summary>
+    public void Run()
+    {
+        _orderedList.Clear();
+        _orderedList.Capacity = _points.Count;
+
+        for (int i = 0; i < _points.Count; i++)
+        {
+            if (!_points[i].Processed)
+            {
+                ExpandClusterOrder(i);
+            }
+        }
+    }
+
+    /// <summary>
+    /// Get the ordered list of point indices after running OPTICS
+    /// </summary>
+    public IReadOnlyList<int> GetClusterOrder() => _orderedList.AsReadOnly();
+
+    /// <summary>
+    /// Extract clusters using a reachability distance threshold
+    /// </summary>
+    public List<List<int>> ExtractClusters(double clusterThreshold)
+    {
+        var clusters = new List<List<int>>();
+        var currentCluster = new List<int>();
+        double thrSq = clusterThreshold * clusterThreshold;
+
+        for (int i = 0; i < _orderedList.Count; i++)
+        {
+            int pointIdx = _orderedList[i];
+
+            if (_points[pointIdx].ReachabilityDistance > thrSq)
+            {
+                if (currentCluster.Count > 0)
+                {
+                    clusters.Add(currentCluster);
+                    currentCluster = [];
+                }
+            }
+            currentCluster.Add(pointIdx);
+        }
+
+        if (currentCluster.Count > 0)
+        {
+            clusters.Add(currentCluster);
+        }
+
+        return clusters;
+    }
+
+    /// <summary>
+    /// Get clustered points as Point2D structures
+    /// </summary>
+    public List<List<Point2D>> GetClusters(double clusterThreshold)
+    {
+        var result = new List<List<Point2D>>();
+        var clusters = ExtractClusters(clusterThreshold);
+
+        foreach (var cluster in clusters)
+        {
+            var clusteredPoints = new List<Point2D>();
+            foreach (int pointIdx in cluster)
+            {
+                var pt = _points[pointIdx];
+                clusteredPoints.Add(new Point2D(pt.X, pt.Y));
+            }
+            result.Add(clusteredPoints);
+        }
+
+        return result;
+    }
+
+    private void ExpandClusterOrder(int pointIdx)
+    {
+        var neighbors = GetNeighbors(pointIdx);
+        _points[pointIdx].Processed = true;
+        _orderedList.Add(pointIdx);
+
+        if (neighbors.Count >= minPts)
+        {
+            // Compute core distance for point_idx
+            double coreDistPoint = double.MaxValue;
+            if (neighbors.Count >= minPts)
+            {
+                var tmp = new List<(double, int)>(neighbors);
+                tmp.Sort((a, b) => a.Item1.CompareTo(b.Item1));
+                coreDistPoint = tmp[minPts - 1].Item1;
+            }
+
+            // Priority queue ordered by reachability distance
+            var seeds = new SortedSet<(double, int)>(Comparer<(double, int)>.Create((a, b) =>
+            {
+                int cmp = a.Item1.CompareTo(b.Item1);
+                return cmp != 0 ? cmp : a.Item2.CompareTo(b.Item2);
+            }));
+
+            foreach (var (dist, neighborIdx) in neighbors)
+            {
+                if (!_points[neighborIdx].Processed)
+                {
+                    double newReachDist = Math.Max(dist, coreDistPoint);
+                    if (_points[neighborIdx].ReachabilityDistance == double.MaxValue)
+                    {
+                        _points[neighborIdx].ReachabilityDistance = newReachDist;
+                        seeds.Add((newReachDist, neighborIdx));
+                    }
+                    else if (newReachDist < _points[neighborIdx].ReachabilityDistance)
+                    {
+                        _points[neighborIdx].ReachabilityDistance = newReachDist;
+                        seeds.Add((newReachDist, neighborIdx));
+                    }
+                }
+            }
+
+            while (seeds.Count > 0)
+            {
+                var (_, current) = seeds.Min;
+                seeds.Remove(seeds.Min);
+
+                var currentNeighbors = GetNeighbors(current);
+                _points[current].Processed = true;
+                _orderedList.Add(current);
+
+                if (currentNeighbors.Count >= minPts)
+                {
+                    // Compute core distance for current
+                    double coreDistCurrent = double.MaxValue;
+                    if (currentNeighbors.Count >= minPts)
+                    {
+                        var tmp2 = new List<(double, int)>(currentNeighbors);
+                        tmp2.Sort((a, b) => a.Item1.CompareTo(b.Item1));
+                        coreDistCurrent = tmp2[minPts - 1].Item1;
+                    }
+
+                    foreach (var (dist, neighborIdx) in currentNeighbors)
+                    {
+                        if (!_points[neighborIdx].Processed)
+                        {
+                            double newReachDist = Math.Max(dist, coreDistCurrent);
+                            if (_points[neighborIdx].ReachabilityDistance == double.MaxValue)
+                            {
+                                _points[neighborIdx].ReachabilityDistance = newReachDist;
+                                seeds.Add((newReachDist, neighborIdx));
+                            }
+                            else if (newReachDist < _points[neighborIdx].ReachabilityDistance)
+                            {
+                                _points[neighborIdx].ReachabilityDistance = newReachDist;
+                                seeds.Add((newReachDist, neighborIdx));
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    private List<(double, int)> GetNeighbors(int pointIdx)
+    {
+        var neighbors = new List<(double, int)>(32);
+
+        // If we have a KD-tree, prefer it for radius queries
+        if (_kdTree != null)
+        {
+            var ids = _kdTree.RadiusSearch(_points[pointIdx], eps);
+            foreach (int idx in ids)
+            {
+                if (idx == pointIdx) continue;
+                double distanceSq = EuclideanDistance(_points[pointIdx], _points[idx]);
+                neighbors.Add((distanceSq, idx));
+            }
+            return neighbors;
+        }
+
+        if (_gridCellSize <= 0 || _grid.Count == 0)
+        {
+            // Fallback to brute-force
+            for (int i = 0; i < _points.Count; i++)
+            {
+                if (i == pointIdx) continue;
+                double distanceSq = EuclideanDistance(_points[pointIdx], _points[i]);
+                if (distanceSq <= eps * eps)
+                {
+                    neighbors.Add((distanceSq, i));
+                }
+            }
+            return neighbors;
+        }
+
+        var p = _points[pointIdx];
+        int cx = (int)Math.Floor(p.X / _gridCellSize);
+        int cy = (int)Math.Floor(p.Y / _gridCellSize);
+
+        // Search neighbor cells around (cx, cy)
+        for (int dx = -1; dx <= 1; dx++)
+        {
+            for (int dy = -1; dy <= 1; dy++)
+            {
+                long key = CellKey(cx + dx, cy + dy);
+                if (_grid.TryGetValue(key, out var cellPoints))
+                {
+                    foreach (int idx in cellPoints)
+                    {
+                        if (idx == pointIdx) continue;
+                        double distanceSq = EuclideanDistance(p, _points[idx]);
+                        if (distanceSq <= eps * eps)
+                        {
+                            neighbors.Add((distanceSq, idx));
+                        }
+                    }
+                }
+            }
+        }
+
+        return neighbors;
+    }
+
+    private static double EuclideanDistance(Point p1, Point p2)
+    {
+        double dx = p1.X - p2.X;
+        double dy = p1.Y - p2.Y;
+        return dx * dx + dy * dy; // Returns squared distance
+    }
+
+    private void BuildSpatialIndex()
+    {
+        if (_gridCellSize <= 0) return;
+
+        _grid.Clear();
+
+        for (int i = 0; i < _points.Count; i++)
+        {
+            int ix = (int)Math.Floor(_points[i].X / _gridCellSize);
+            int iy = (int)Math.Floor(_points[i].Y / _gridCellSize);
+            long key = CellKey(ix, iy);
+
+            if (!_grid.ContainsKey(key))
+                _grid[key] = [];
+
+            _grid[key].Add(i);
+        }
+    }
+
+    private void BuildKDTree()
+    {
+        if (_points.Count == 0)
+        {
+            _kdTree = null;
+            return;
+        }
+
+        _kdTree = new KDTree(_points);
+    }
+
+    private static long CellKey(int ix, int iy)
+    {
+        // Pack two 32-bit ints into one 64-bit key
+        return ((long)ix << 32) ^ (uint)iy;
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/Point2D.cs

@@ -0,0 +1,90 @@
+namespace RobotNet10.RobotApp.Detection;
+
+/// <summary>
+/// Represents a 2D point with additional laser scan metadata
+/// </summary>
+public class Point(double x, double y, double range, double alpha)
+{
+    /// <summary>
+    /// X coordinate in meters
+    /// </summary>
+    public double X { get; set; } = x;
+
+    /// <summary>
+    /// Y coordinate in meters
+    /// </summary>
+    public double Y { get; set; } = y;
+
+    /// <summary>
+    /// Range of point from Lidar sensor in meters
+    /// </summary>
+    public double Range { get; set; } = range;
+
+    /// <summary>
+    /// Angle of laser beam to the point in radians
+    /// </summary>
+    public double Alpha { get; set; } = alpha;
+
+    /// <summary>
+    /// Cluster ID assigned by clustering algorithm (-1 if unassigned)
+    /// </summary>
+    public int Cluster { get; set; } = -1;
+
+    /// <summary>
+    /// Reachability distance for OPTICS algorithm
+    /// </summary>
+    public double ReachabilityDistance { get; set; } = double.MaxValue;
+
+    /// <summary>
+    /// Whether this point has been processed by the algorithm
+    /// </summary>
+    public bool Processed { get; set; } = false;
+}
+
+/// <summary>
+/// Simple 2D point structure for cluster results
+/// </summary>
+public struct Point2D(double x, double y) : IEquatable<Point2D>, IComparable<Point2D>
+{
+    /// <summary>
+    /// X coordinate in meters
+    /// </summary>
+    public double X { get; set; } = x;
+
+    /// <summary>
+    /// Y coordinate in meters
+    /// </summary>
+    public double Y { get; set; } = y;
+
+    public readonly bool Equals(Point2D other)
+    {
+        return Math.Abs(X - other.X) < 3e-3 && Math.Abs(Y - other.Y) < 3e-3;
+    }
+
+    public override readonly bool Equals(object? obj)
+    {
+        return obj is Point2D other && Equals(other);
+    }
+
+    public override readonly int GetHashCode()
+    {
+        return HashCode.Combine(X, Y);
+    }
+
+    public readonly int CompareTo(Point2D other)
+    {
+        if (!X.Equals(other.X))
+            return X.CompareTo(other.X);
+        return Y.CompareTo(other.Y);
+    }
+
+    public static bool operator ==(Point2D left, Point2D right)
+    {
+        return left.Equals(right);
+    }
+
+    public static bool operator !=(Point2D left, Point2D right)
+    {
+        return !left.Equals(right);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/QRDetector.cs

@@ -0,0 +1,213 @@
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.SLAM;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.Detection;
+
+/// <summary>
+/// Detector for QR code markers using camera
+/// Polls QR camera for specific QR code and transforms pose to global frame
+/// </summary>
+/// <remarks>
+/// Create a new QR code detection session
+/// </remarks>
+/// <param name="qrCode">QR code string to detect</param>
+/// <param name="slamService">SLAM service for current robot pose</param>
+/// <param name="camera">QR camera device</param>
+/// <param name="cameraPose">Camera pose relative to robot base</param>
+public class QRDetector(
+    string qrCode,
+    ISLAMService slamService,
+    ICameraQr camera,
+    Pose cameraPose) : IDetector
+{
+    private readonly Lock _lockPose = new();
+    private bool _isActive = false;
+    private Thread? _pollingThread;
+    private const int POLLING_FREQUENCY_HZ = 30; // Poll at 30Hz
+
+    /// <summary>
+    /// Detected marker pose in global frame
+    /// Initialized with default pose, updated when QR code is detected
+    /// </summary>
+    public Pose MarkerPose
+    {
+        get
+        {
+            lock (_lockPose)
+            {
+                return field;
+            }
+        }
+        private set
+        {
+            lock (_lockPose)
+            {
+                field = value;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Timestamp of the last successful QR detection
+    /// </summary>
+    public DateTime DetectionTime { get; private set; } = DateTime.MinValue;
+
+    /// <summary>
+    /// Activate the QR detection
+    /// Starts polling QR camera in background thread
+    /// </summary>
+    public void Active()
+    {
+        if (_isActive)
+            return;
+
+        _isActive = true;
+
+        // Create and start polling thread
+        _pollingThread = new Thread(PollingThreadLoop)
+        {
+            Name = $"QRDetection_{qrCode}",
+            IsBackground = true
+        };
+        _pollingThread.Start();
+    }
+
+    /// <summary>
+    /// Disable the QR detection
+    /// Stops polling thread
+    /// </summary>
+    public void Disable()
+    {
+        if (!_isActive)
+            return;
+
+        _isActive = false;
+
+        // Wait for thread to finish
+        _pollingThread?.Join();
+        _pollingThread = null;
+    }
+
+    /// <summary>
+    /// Polling thread loop that checks QR camera periodically
+    /// Runs at 30Hz to check for QR code detection
+    /// </summary>
+    private void PollingThreadLoop()
+    {
+        int delayMs = 1000 / POLLING_FREQUENCY_HZ; // ~33ms for 30Hz
+
+        while (_isActive)
+        {
+            try
+            {
+                // Check if camera is connected
+                if (!camera.IsConnected)
+                {
+                    Thread.Sleep(delayMs);
+                    continue;
+                }
+
+                // Get QR code pose from camera (in camera frame)
+                var poseStampedInCamera = camera[qrCode];
+
+                if (poseStampedInCamera.HasValue)
+                {
+                    // Get current robot pose in global frame
+                    var currentRobotPose = slamService.CurrentPose;
+
+                    // Transform: camera frame -> robot frame -> global frame
+                    var poseInRobot = TransformPose(poseStampedInCamera.Value.Pose, cameraPose);
+                    var poseInGlobal = TransformPose(poseInRobot, currentRobotPose);
+
+                    // Update marker pose and detection time
+                    MarkerPose = poseInGlobal;
+                    DetectionTime = poseStampedInCamera.Value.Header.Stamp;
+
+                    // Console.WriteLine($"{DetectionTime:HH:mm:ss.ffffff} [QRDetector] Detected found QR code '{qrCode}' at global pose: [{MarkerPose.Position.X}, {MarkerPose.Position.Y}, {MarkerPose.Orientation.ToYawDegrees()}deg]");
+                }
+            }
+            catch
+            {
+                // Ignore errors in polling loop
+            }
+
+            // Sleep to maintain polling rate
+            Thread.Sleep(delayMs);
+        }
+    }
+
+    /// <summary>
+    /// Transform a pose by another pose (pose composition)
+    /// result = parentPose * childPose
+    /// </summary>
+    /// <param name="childPose">Pose in child frame</param>
+    /// <param name="parentPose">Parent frame pose</param>
+    /// <returns>Transformed pose in parent's parent frame</returns>
+    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)
+        };
+    }
+
+    /// <summary>
+    /// Create a quaternion from yaw angle (rotation around Z axis)
+    /// </summary>
+    /// <param name="yaw">Yaw angle in radians</param>
+    /// <returns>Quaternion representing rotation around Z axis</returns>
+    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>
+    /// <param name="angle">Angle in radians</param>
+    /// <returns>Normalized angle in [-π, π]</returns>
+    private static double NormalizeAngle(double angle)
+    {
+        while (angle > Math.PI) angle -= 2 * Math.PI;
+        while (angle < -Math.PI) angle += 2 * Math.PI;
+        return angle;
+    }
+
+    public void Dispose()
+    {
+        // Disable detector (stops thread)
+        Disable();
+
+        // Suppress finalization since we've cleaned up
+        GC.SuppressFinalize(this);
+    }
+}

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/ShapeReflectiveDetector.cs

@@ -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;
+    }
+}