Extract road from lidar scan

lidarroad/README.md

@@ -0,0 +1,29 @@
+# LidarRoad
+
+Extract road boundary from WanJee down pointing lidar (10 degrees).
+
+## Usage
+
+You can run the script in single scan analysis mode or in batch processing mode.
+
+### Single Scan Analysis
+
+Analyzes and visualizes the smoothness of a road from a single scan:
+```bash
+uv run python lidarroad.py <logfile> [--jump <seconds>] [--width <meters>] [--tolerance <millimeters>] [--fast]
+```
+
+- `<logfile>`: Path to the OSGAR logfile.
+- `--jump`, `-j`: Jump forward to the specified time in seconds before starting analysis.
+- `--width`, `-w`: Window size/width in meters (default is `3.0`).
+- `--tolerance`, `-t`: Smoothness difference tolerance in millimeters between neighboring scan points (default is `10`).
+- `--fast`: Bypass the legacy comparative calculation and dual assertions for maximum execution speed.
+
+### Batch Processing Mode
+
+Analyzes scans over a specified time interval and draws a unified plot of the best matching boundaries over time:
+```bash
+uv run python lidarroad.py <logfile> --batch <start_sec> <end_sec> [--width <meters>] [--tolerance <millimeters>] [--fast]
+```
+
+- `--batch`: Runs in batch mode, taking exact `<start_sec>` and `<end_sec>` limits.

lidarroad/__init__.py

@@ -0,0 +1,8 @@
+from .lidarroad import (
+    analyze_scan,
+    batch_processing,
+    draw_batch,
+    draw_scan,
+    get_best_match,
+    slow_get_best_match,
+)

lidarroad/lidarroad.py

@@ -0,0 +1,133 @@
+import argparse
+from datetime import timedelta
+
+import numpy as np
+from osgar.logger import LogReaderEx
+
+
+def slow_get_best_match(mask, window_size):
+    best_i = 0
+    best_sum = None
+    for i in range(0, len(mask) - window_size + 1):
+        value = sum(mask[i:i+window_size])
+        if best_sum is None or value > best_sum:
+            best_sum = value
+            best_i = i
+    return best_i, best_i + window_size
+
+
+def get_best_match(mask, window_size):
+    if len(mask) <= window_size:
+        return 0, window_size
+    cum = np.cumsum(np.asarray(mask))
+    window_sums = np.empty(len(mask) - window_size + 1, dtype=cum.dtype)
+    window_sums[0] = cum[window_size - 1]
+    window_sums[1:] = cum[window_size:] - cum[:-window_size]
+    best_i = int(np.argmax(window_sums))
+    return best_i, best_i + window_size
+
+
+def analyze_scan(scan, tolerance=10, window_size = 300, fast=False):
+    assert len(scan)==1800, len(scan)
+    diff = np.diff(scan) #[450:-450])
+    mask = np.abs(diff) < tolerance
+    from_i, to_i = get_best_match(mask, window_size)
+    if not fast:
+        from_i_slow, to_i_slow = slow_get_best_match(mask, window_size)
+        assert (from_i, to_i) == (from_i_slow, to_i_slow), (
+            f"Optimization mismatch: {(from_i, to_i)} != {(from_i_slow, to_i_slow)}"
+        )
+    return diff, from_i, to_i
+
+
+def draw_scan(scan, tolerance=None, interval=None):
+    import matplotlib.pyplot as plt
+
+    plt.plot(scan)
+    if tolerance is not None:
+        plt.axhline(y=tolerance, color='r', linestyle='--')
+        plt.axhline(y=-tolerance, color='r', linestyle='--')
+    if interval is not None:
+        from_i, to_i = interval
+        plt.axvline(x=from_i, color='g', linestyle='--')
+        plt.axvline(x=to_i, color='g', linestyle='--')
+    plt.show()
+
+
+def draw_batch(timestamps, from_indices, to_indices):
+    import matplotlib.pyplot as plt
+
+    plt.plot(timestamps, from_indices, 'g.-', label='from_i')
+    plt.plot(timestamps, to_indices, 'b.-', label='to_i')
+    plt.xlabel('Time (s)')
+    plt.ylabel('Scan Index')
+    plt.legend()
+    plt.title('Best Matching Interval Boundaries Over Time')
+    plt.show()
+
+
+def batch_processing(log, start_sec, end_sec, tolerance, window_size, fast=False):
+    start_time = timedelta(seconds=start_sec)
+    end_time = timedelta(seconds=end_sec)
+
+    times = []
+    from_indices = []
+    to_indices = []
+
+    for timestamp, name, data in log:
+        if timestamp < start_time:
+            continue
+        if timestamp > end_time:
+            break
+
+        diff, from_i, to_i = analyze_scan(data, tolerance=tolerance, window_size=window_size, fast=fast)
+        print(timestamp, len(data), from_i, to_i)
+
+        times.append(timestamp.total_seconds())
+        from_indices.append(from_i)
+        to_indices.append(to_i)
+
+    return times, from_indices, to_indices
+
+
+def main():
+    parser = argparse.ArgumentParser(description='Analyze smoothness of the road/scan10')
+    parser.add_argument('logfile', help='logfile path')
+    parser.add_argument('--jump', '-j', help='jump in seconds', type=float)
+    parser.add_argument('--width', '-w', help='width in meters', type=float, default=3.0)
+    parser.add_argument('--tolerance', '-t', help='tolerance in millimeters', type=int, default=10)
+    parser.add_argument(
+        '--batch', nargs=2, type=float, metavar=('START', 'END'),
+        help='run in batch mode for time range in seconds'
+    )
+    parser.add_argument('--fast', action='store_true', help='skip slow match calculation and assertion')
+    args = parser.parse_args()
+
+    window_size = int(args.width * 100)  # simplified conversion to scan indexes - TODO proper calibration
+    tolerance = args.tolerance
+    fast = args.fast
+
+    with LogReaderEx(args.logfile, ['vanjee.scan10']) as log:
+        if args.batch is not None:
+            start_sec, end_sec = args.batch
+            times, from_indices, to_indices = batch_processing(
+                log, start_sec, end_sec, tolerance, window_size, fast=fast
+            )
+            if times:
+                draw_batch(times, from_indices, to_indices)
+            else:
+                print("No scans found in the specified time range.")
+        else:
+            for timestamp, name, data in log:
+                if args.jump is not None and timestamp < timedelta(seconds=args.jump):
+                    continue
+                print(timestamp, len(data))
+                diff, from_i, to_i = analyze_scan(data, tolerance=tolerance, window_size=window_size, fast=fast)
+                print(from_i, to_i)
+                draw_scan(diff, tolerance=tolerance, interval=(from_i, to_i))
+                break
+
+
+
+if __name__ == '__main__':
+    main()

lidarroad/test_lidarroad.py

@@ -0,0 +1,149 @@
+import unittest
+from datetime import timedelta
+from unittest.mock import patch
+
+import numpy as np
+
+from lidarroad import analyze_scan, batch_processing, draw_batch, draw_scan, get_best_match, slow_get_best_match
+
+
+class TestLidarRoad(unittest.TestCase):
+    def test_get_best_match(self):
+        # Basic validation case
+        mask = [0, 0, 1, 1, 1, 0, 0]
+        from_i, to_i = get_best_match(mask, 3)
+        self.assertEqual(from_i, 2)
+        self.assertEqual(to_i, 5)
+
+        # Bug reproduction: optimal window is at the very end of the array
+        mask_end = [0, 0, 1, 1, 1]
+        from_i_end, to_i_end = get_best_match(mask_end, 3)
+        self.assertEqual(from_i_end, 2)
+        self.assertEqual(to_i_end, 5)
+
+        from_i_slow, to_i_slow = slow_get_best_match(mask_end, 3)
+        self.assertEqual(from_i_slow, 2)
+        self.assertEqual(to_i_slow, 5)
+
+        # Extended comparative validation between fast and slow implementations
+        np.random.seed(42)  # For deterministic reproducibility
+        masks = [
+            [0, 0, 1, 1, 1, 0, 0],
+            [1, 1, 1, 1, 1, 1, 1],
+            [0, 0, 0, 0, 0, 0, 0],
+            [1, 0, 1, 0, 1, 1, 0, 0, 1, 1, 1, 0, 1],
+            np.random.randint(0, 2, 1000).tolist()
+        ]
+        for m in masks:
+            for window_size in [1, 3, 5, 50, 100]:
+                from_fast, to_fast = get_best_match(m, window_size)
+                from_slow, to_slow = slow_get_best_match(m, window_size)
+                self.assertEqual(
+                    (from_fast, to_fast), (from_slow, to_slow),
+                    f"Mismatch for window_size {window_size} on mask {m if len(m) < 20 else 'random'}"
+                )
+
+    @patch('matplotlib.pyplot.show')
+    @patch('matplotlib.pyplot.axvline')
+    @patch('matplotlib.pyplot.axhline')
+    @patch('matplotlib.pyplot.plot')
+    def test_draw_scan_with_tolerance_and_interval(self, mock_plot, mock_axhline, mock_axvline, mock_show):
+        scan = [1, 2, 3]
+        draw_scan(scan, tolerance=15, interval=(10, 20))
+        mock_plot.assert_called_once_with(scan)
+
+        # Check horizontal lines
+        self.assertEqual(mock_axhline.call_count, 2)
+        mock_axhline.assert_any_call(y=15, color='r', linestyle='--')
+        mock_axhline.assert_any_call(y=-15, color='r', linestyle='--')
+
+        # Check vertical lines
+        self.assertEqual(mock_axvline.call_count, 2)
+        mock_axvline.assert_any_call(x=10, color='g', linestyle='--')
+        mock_axvline.assert_any_call(x=20, color='g', linestyle='--')
+
+        mock_show.assert_called_once()
+
+    @patch('matplotlib.pyplot.show')
+    @patch('matplotlib.pyplot.axvline')
+    @patch('matplotlib.pyplot.axhline')
+    @patch('matplotlib.pyplot.plot')
+    def test_draw_scan_without_tolerance_and_interval(self, mock_plot, mock_axhline, mock_axvline, mock_show):
+        scan = [1, 2, 3]
+        draw_scan(scan, tolerance=None, interval=None)
+        mock_plot.assert_called_once_with(scan)
+        mock_axhline.assert_not_called()
+        mock_axvline.assert_not_called()
+        mock_show.assert_called_once()
+
+    @patch('matplotlib.pyplot.show')
+    @patch('matplotlib.pyplot.legend')
+    @patch('matplotlib.pyplot.ylabel')
+    @patch('matplotlib.pyplot.xlabel')
+    @patch('matplotlib.pyplot.title')
+    @patch('matplotlib.pyplot.plot')
+    def test_draw_batch(self, mock_plot, mock_title, mock_xlabel, mock_ylabel, mock_legend, mock_show):
+        times = [1.0, 2.0, 3.0]
+        from_i = [100, 110, 120]
+        to_i = [400, 410, 420]
+        draw_batch(times, from_i, to_i)
+
+        self.assertEqual(mock_plot.call_count, 2)
+        mock_plot.assert_any_call(times, from_i, 'g.-', label='from_i')
+        mock_plot.assert_any_call(times, to_i, 'b.-', label='to_i')
+
+        mock_xlabel.assert_called_once_with('Time (s)')
+        mock_ylabel.assert_called_once_with('Scan Index')
+        mock_title.assert_called_once_with('Best Matching Interval Boundaries Over Time')
+        mock_legend.assert_called_once()
+        mock_show.assert_called_once()
+
+    def test_batch_processing(self):
+        mock_scan = [10] * 1800
+        log = [
+            (timedelta(seconds=1.0), 'vanjee.scan10', mock_scan),
+            (timedelta(seconds=2.0), 'vanjee.scan10', mock_scan),
+            (timedelta(seconds=3.0), 'vanjee.scan10', mock_scan),
+            (timedelta(seconds=4.0), 'vanjee.scan10', mock_scan)
+        ]
+
+        # Test standard batch processing
+        times, from_indices, to_indices = batch_processing(
+            log, start_sec=1.5, end_sec=3.5, tolerance=10, window_size=500, fast=False
+        )
+        self.assertEqual(times, [2.0, 3.0])
+        self.assertEqual(from_indices, [0, 0])
+        self.assertEqual(to_indices, [500, 500])
+
+        # Test fast batch processing
+        times_f, from_indices_f, to_indices_f = batch_processing(
+            log, start_sec=1.5, end_sec=3.5, tolerance=10, window_size=500, fast=True
+        )
+        self.assertEqual(times_f, [2.0, 3.0])
+        self.assertEqual(from_indices_f, [0, 0])
+        self.assertEqual(to_indices_f, [500, 500])
+
+    @patch('lidarroad.slow_get_best_match')
+    def test_analyze_scan(self, mock_slow):
+        mock_slow.return_value = (0, 500)
+        scan = [10] * 1800
+
+        # Standard mode (runs and asserts slow version matches)
+        diff, from_i, to_i = analyze_scan(scan, tolerance=10, window_size=500, fast=False)
+        np.testing.assert_array_equal(diff, np.zeros(1799))
+        self.assertEqual(from_i, 0)
+        self.assertEqual(to_i, 500)
+        mock_slow.assert_called_once()
+
+        mock_slow.reset_mock()
+
+        # Fast mode (skips slow match calculation and assertion)
+        diff_fast, from_i_fast, to_i_fast = analyze_scan(scan, tolerance=10, window_size=500, fast=True)
+        np.testing.assert_array_equal(diff_fast, np.zeros(1799))
+        self.assertEqual(from_i_fast, 0)
+        self.assertEqual(to_i_fast, 500)
+        mock_slow.assert_not_called()
+
+
+if __name__ == '__main__':
+    unittest.main()