diff --git a/lidarroad/README.md b/lidarroad/README.md new file mode 100644 index 0000000..34cf29e --- /dev/null +++ b/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 [--jump ] [--width ] [--tolerance ] [--fast] +``` + +- ``: 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 --batch [--width ] [--tolerance ] [--fast] +``` + +- `--batch`: Runs in batch mode, taking exact `` and `` limits. diff --git a/lidarroad/__init__.py b/lidarroad/__init__.py new file mode 100644 index 0000000..34e2c1b --- /dev/null +++ b/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, +) diff --git a/lidarroad/lidarroad.py b/lidarroad/lidarroad.py new file mode 100644 index 0000000..6a8cbbc --- /dev/null +++ b/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() diff --git a/lidarroad/test_lidarroad.py b/lidarroad/test_lidarroad.py new file mode 100644 index 0000000..cee72a5 --- /dev/null +++ b/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()