Feature/minkowski workarround

AVR/PCProcess.py

@@ -10,7 +10,11 @@
 import carla
 import imageio
 
-from MinkowskiEngine.utils import sparse_quantize
+try:
+    from MinkowskiEngine.utils import sparse_quantize
+except ImportError:
+    print("MinkowskiEngine not found, using stub implementation")
+    from AVR.minkowski_stub import sparse_quantize
 # from lidar_cython import fast_lidar
 from numba import jit
 

AVR/minkowski_stub.py

@@ -0,0 +1,87 @@
+"""
+Minimal stub for MinkowskiEngine.utils.sparse_quantize
+This provides the sparse_quantize function without requiring full MinkowskiEngine installation.
+Based on the original MinkowskiEngine implementation.
+"""
+
+import numpy as np
+from typing import Tuple, Union, Optional
+
+
+def sparse_quantize(
+    coordinates: np.ndarray,
+    features: Optional[np.ndarray] = None,
+    labels: Optional[np.ndarray] = None,
+    ignore_label: int = -100,
+    return_index: bool = False,
+    return_inverse: bool = False,
+    quantization_size: Optional[Union[float, np.ndarray]] = None,
+) -> Union[np.ndarray, Tuple]:
+    """
+    Given coordinates, and features (optionally labels), the function
+    generates quantized (voxelized) coordinates and respective features and labels.
+
+    Args:
+        coordinates: a numpy array of size N x D where N is the number of points
+                     and D is the dimension of the space.
+        features: a numpy array of size N x F where F is the number of features per point.
+        labels: a numpy array of size N. Points with label == ignore_label are filtered out.
+        ignore_label: label value to ignore/filter out.
+        return_index: if True, return the index of the unique coordinates.
+        return_inverse: if True, return the inverse mapping.
+        quantization_size: voxel size for quantization. If None, no quantization is applied.
+
+    Returns:
+        quantized_coordinates: voxelized coordinates (N' x D).
+        unique_features: (if features provided) features corresponding to unique coordinates.
+        unique_labels: (if labels provided) labels corresponding to unique coordinates.
+        unique_indices: (if return_index=True) indices into original array.
+        inverse_indices: (if return_inverse=True) inverse mapping for reconstruction.
+    """
+    coordinates = np.array(coordinates)
+    features = np.array(features) if features is not None else None
+    labels = np.array(labels) if labels is not None else None
+
+    # Filter out ignored labels if labels are provided
+    if labels is not None:
+        valid_mask = labels != ignore_label
+        coordinates = coordinates[valid_mask]
+        if features is not None:
+            features = features[valid_mask]
+        labels = labels[valid_mask]
+
+    # Apply quantization if specified
+    if quantization_size is not None:
+        if isinstance(quantization_size, (list, tuple, np.ndarray)):
+            quantization_size = np.array(quantization_size)
+        quantized_coords = np.floor(coordinates / quantization_size).astype(np.int32)
+    else:
+        quantized_coords = coordinates.astype(np.int32)
+
+    # Find unique coordinates
+    _, unique_indices, inverse_indices = np.unique(
+        quantized_coords, axis=0, return_index=True, return_inverse=True
+    )
+
+    # Get unique features and labels
+    unique_coords = quantized_coords[unique_indices]
+
+    results = [unique_coords]
+
+    if features is not None:
+        unique_features = features[unique_indices]
+        results.append(unique_features)
+
+    if labels is not None:
+        unique_labels = labels[unique_indices]
+        results.append(unique_labels)
+
+    if return_index:
+        results.append(unique_indices)
+
+    if return_inverse:
+        results.append(inverse_indices)
+
+    if len(results) == 1:
+        return results[0]
+    return tuple(results)

test_minkowski_logic.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+"""
+Quick inline test of minkowski_stub - designed to work with minimal setup
+"""
+
+# Inline minimal numpy replacement for testing
+class SimpleArray:
+    def __init__(self, data):
+        self.data = data
+    def __getitem__(self, idx):
+        return self.data[idx]
+    def __len__(self):
+        return len(self.data)
+    def __repr__(self):
+        return f"Array({self.data})"
+
+# Test the logic without imports
+def test_sparse_quantize_logic():
+    """Test the core quantization logic"""
+    print("Testing sparse_quantize logic...")
+
+    # Simulate: coordinates = [[0.1, 0.1], [0.5, 0.5], [1.1, 1.1]]
+    # With quantization_size=1.0, should voxelize to: [[0, 0], [0, 0], [1, 1]]
+    # Which has 2 unique: [[0, 0], [1, 1]]
+
+    raw_coords = [
+        [0.1, 0.1],
+        [0.5, 0.5],  # Should map to same voxel as first
+        [1.1, 1.1],
+    ]
+
+    # Manual quantization
+    quantized = []
+    for coord in raw_coords:
+        q = [int(c / 1.0) for c in coord]  # floor(c / 1.0)
+        quantized.append(tuple(q))
+
+    print(f"Raw coords: {raw_coords}")
+    print(f"Quantized: {quantized}")
+
+    # Find unique
+    unique = list(set(quantized))
+    print(f"Unique voxels: {unique}")
+    print(f"Count: {len(unique)}")
+
+    assert len(unique) == 2, f"Expected 2 unique voxels, got {len(unique)}"
+    print("✓ Test passed!\n")
+
+
+def test_ignore_label_logic():
+    """Test ignore_label filtering logic"""
+    print("Testing ignore_label filtering logic...")
+
+    coords = [
+        [0.0, 0.0],
+        [1.0, 1.0],
+        [2.0, 2.0],
+        [3.0, 3.0],
+    ]
+
+    labels = [0, -100, 1, -100]
+    ignore_label = -100
+
+    # Filter
+    filtered_coords = []
+    filtered_labels = []
+    for c, l in zip(coords, labels):
+        if l != ignore_label:
+            filtered_coords.append(c)
+            filtered_labels.append(l)
+
+    print(f"Original: {len(coords)} points")
+    print(f"After filtering ignore_label={ignore_label}: {len(filtered_coords)} points")
+    print(f"Remaining labels: {filtered_labels}")
+
+    assert len(filtered_coords) == 2, f"Expected 2 filtered points, got {len(filtered_coords)}"
+    assert all(l != ignore_label for l in filtered_labels), "Found ignore_label in results"
+    print("✓ Test passed!\n")
+
+
+if __name__ == "__main__":
+    print("=" * 60)
+    print("MinkowskiEngine Stub - Logic Verification")
+    print("=" * 60 + "\n")
+
+    test_sparse_quantize_logic()
+    test_ignore_label_logic()
+
+    print("=" * 60)
+    print("All logic tests passed ✓")
+    print("=" * 60)

test_minkowski_stub.py

@@ -0,0 +1,183 @@
+"""
+Standalone tests for minkowski_stub.sparse_quantize function
+No CARLA dependency required.
+"""
+
+import numpy as np
+import sys
+from AVR.minkowski_stub import sparse_quantize
+
+
+def test_basic_quantization():
+    """Test basic quantization without features or labels"""
+    print("Test 1: Basic quantization")
+    coordinates = np.array([
+        [0.1, 0.1, 0.1],
+        [0.2, 0.2, 0.2],
+        [1.1, 1.1, 1.1],
+        [1.2, 1.2, 1.2],
+    ], dtype=np.float32)
+
+    result = sparse_quantize(coordinates, quantization_size=1.0)
+    print(f"  Input shape: {coordinates.shape}")
+    print(f"  Output shape: {result.shape}")
+    print(f"  Result:\n{result}")
+    assert result.shape[0] == 2, "Should have 2 unique voxels"
+    print("  ✓ PASSED\n")
+
+
+def test_with_features():
+    """Test quantization with features"""
+    print("Test 2: Quantization with features")
+    coordinates = np.array([
+        [0.0, 0.0, 0.0],
+        [0.5, 0.5, 0.5],
+        [1.0, 1.0, 1.0],
+        [1.5, 1.5, 1.5],
+    ], dtype=np.float32)
+
+    features = np.array([
+        [1.0, 2.0],
+        [3.0, 4.0],
+        [5.0, 6.0],
+        [7.0, 8.0],
+    ], dtype=np.float32)
+
+    coords, feats = sparse_quantize(coordinates, features=features, quantization_size=1.0)
+    print(f"  Coordinates shape: {coords.shape}")
+    print(f"  Features shape: {feats.shape}")
+    assert coords.shape[0] == feats.shape[0], "Coords and features should have same count"
+    print("  ✓ PASSED\n")
+
+
+def test_with_labels():
+    """Test quantization with labels and ignore filtering"""
+    print("Test 3: Quantization with labels and ignore_label filtering")
+    coordinates = np.array([
+        [0.0, 0.0, 0.0],
+        [0.5, 0.5, 0.5],
+        [1.0, 1.0, 1.0],
+        [1.5, 1.5, 1.5],
+    ], dtype=np.float32)
+
+    labels = np.array([0, -100, 1, -100], dtype=np.int32)  # ignore points with label -100
+
+    coords, labs = sparse_quantize(coordinates, labels=labels, ignore_label=-100, quantization_size=1.0)
+    print(f"  Input count: {len(coordinates)}")
+    print(f"  Output count: {len(coords)}")
+    print(f"  Output labels: {labs}")
+    assert len(coords) == 2, "Should only have 2 points after filtering ignore_label"
+    assert np.all(labs != -100), "Should not contain ignore_label"
+    print("  ✓ PASSED\n")
+
+
+def test_return_index():
+    """Test return_index flag"""
+    print("Test 4: return_index flag")
+    coordinates = np.array([
+        [0.0, 0.0, 0.0],
+        [0.1, 0.1, 0.1],  # Same voxel as first
+        [1.0, 1.0, 1.0],
+    ], dtype=np.float32)
+
+    coords, indices = sparse_quantize(coordinates, return_index=True, quantization_size=1.0)
+    print(f"  Unique coordinates count: {len(coords)}")
+    print(f"  Indices: {indices}")
+    assert len(indices) == len(coords), "Should have one index per unique coordinate"
+    assert len(coords) == 2, "Should have 2 unique voxels"
+    print("  ✓ PASSED\n")
+
+
+def test_return_inverse():
+    """Test return_inverse flag"""
+    print("Test 5: return_inverse flag")
+    coordinates = np.array([
+        [0.0, 0.0, 0.0],
+        [0.1, 0.1, 0.1],  # Same voxel as first
+        [1.0, 1.0, 1.0],
+    ], dtype=np.float32)
+
+    coords, inverse = sparse_quantize(coordinates, return_inverse=True, quantization_size=1.0)
+    print(f"  Original count: {len(coordinates)}")
+    print(f"  Unique count: {len(coords)}")
+    print(f"  Inverse mapping: {inverse}")
+    # Can reconstruct: coords[inverse] should give same voxels as input
+    reconstructed = coords[inverse]
+    print(f"  Reconstructed matches quantized input: {np.allclose(reconstructed, np.floor(coordinates).astype(np.int32))}")
+    print("  ✓ PASSED\n")
+
+
+def test_all_flags():
+    """Test with all flags enabled"""
+    print("Test 6: All flags enabled (coordinates, features, labels, return_index, return_inverse)")
+    coordinates = np.array([
+        [0.0, 0.0, 0.0],
+        [0.5, 0.5, 0.5],
+        [1.0, 1.0, 1.0],
+    ], dtype=np.float32)
+
+    features = np.array([[1, 2], [3, 4], [5, 6]], dtype=np.float32)
+    labels = np.array([10, 20, 30], dtype=np.int32)
+
+    result = sparse_quantize(
+        coordinates,
+        features=features,
+        labels=labels,
+        return_index=True,
+        return_inverse=True,
+        quantization_size=1.0
+    )
+
+    coords, feats, labs, indices, inverse = result
+    print(f"  Coordinates shape: {coords.shape}")
+    print(f"  Features shape: {feats.shape}")
+    print(f"  Labels shape: {labs.shape}")
+    print(f"  Indices shape: {indices.shape}")
+    print(f"  Inverse shape: {inverse.shape}")
+    assert len(coords) == len(feats) == len(labs) == len(indices), "All should have same length"
+    assert len(inverse) == len(coordinates), "Inverse should match original length"
+    print("  ✓ PASSED\n")
+
+
+def test_no_quantization():
+    """Test without quantization (quantization_size=None)"""
+    print("Test 7: No quantization (raw coordinate deduplication)")
+    coordinates = np.array([
+        [0.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0],  # Duplicate
+        [1.0, 1.0, 1.0],
+    ], dtype=np.float32)
+
+    result = sparse_quantize(coordinates, quantization_size=None)
+    print(f"  Input: {len(coordinates)} points")
+    print(f"  Output: {len(result)} unique points")
+    assert len(result) == 2, "Should have 2 unique points after dedup"
+    print("  ✓ PASSED\n")
+
+
+if __name__ == "__main__":
+    print("=" * 60)
+    print("MinkowskiEngine Stub - Standalone Tests")
+    print("=" * 60 + "\n")
+
+    try:
+        test_basic_quantization()
+        test_with_features()
+        test_with_labels()
+        test_return_index()
+        test_return_inverse()
+        test_all_flags()
+        test_no_quantization()
+
+        print("=" * 60)
+        print("ALL TESTS PASSED ✓")
+        print("=" * 60)
+        sys.exit(0)
+    except AssertionError as e:
+        print(f"\n TEST FAILED: {e}")
+        sys.exit(1)
+    except Exception as e:
+        print(f"\n ERROR: {e}")
+        import traceback
+        traceback.print_exc()
+        sys.exit(1)