Cuda mapper

[leshy]

dimos --replay run unitree-go2-newmapper

• 1-to-1 global mapper, runs on CUDA if possible but 6ms injest on CPU
• experiment with incline based costmap that we should move to (checked out Pauls costmap files)
• small structure for testing with moment.py (sketch for now)

removed stupid cylinder based map clearing, does perfect correct insertion of new frames

[chatgpt-codex-connector]

Codex usage limits have been reached for code reviews. Please check with the admins of this repo to increase the limits by adding credits.
Credits must be used to enable repository wide code reviews.

[greptile-apps]

Greptile Summary

This PR introduces CUDA-accelerated voxel mapping with occupancy grid generation, along with improvements to decorators, type safety, and testing infrastructure.

Key Changes

• CUDA Voxel Mapper: New SparseVoxelGridMapper with Open3D tensor-based voxelization, supporting GPU acceleration and column carving to handle overlapping observations
• Occupancy Grid Generation: Three occupancy grid algorithms (height_cost_occupancy, general_occupancy, simple_occupancy) for costmap generation from point clouds
• Decorator Enhancements: Added cache invalidation to simple_mcache, new retry decorator with configurable retry logic
• Testing Infrastructure: New Moment classes for timestamp-based sensor replay and output validation
• Type Safety: Improved module typing with ModuleConfigT TypeVar and added frame_id property with prefix support
• Resource Management: Added start()/stop() methods to transport classes for proper cleanup

Issues Found

• Resource Leaks: The timed decorator creates LCMTransport instances that are never cleaned up, causing thread and file descriptor leaks
• Thread Cleanup: The limit decorator's threading.Timer instances lack proper cleanup mechanisms
• Minor Issues: Redundant isinstance check in ensure_tensor_pcd, cache invalidation performance considerations

The mapper implementation is solid with comprehensive tests including carving validation, coordinate roundtrip verification, and occupancy grid image comparisons.

Confidence Score: 3/5

• Safe to merge with attention to resource leak issues that should be addressed
• The core mapping functionality is well-tested and sound, but there are resource leak issues in the decorator utilities (timed and limit) that could accumulate over time. These won't cause immediate failures but should be fixed to prevent long-running process degradation.
• Pay close attention to dimos/utils/metrics.py and dimos/utils/decorators/decorators.py for resource cleanup

Important Files Changed

Filename	Overview
dimos/mapping/voxels.py	New CUDA-accelerated voxel mapper with column carving. Minor unused imports flagged in previous review.
dimos/mapping/pointclouds/occupancy.py	New occupancy grid generation functions (height_cost, general, simple) with solid implementation and comprehensive docstrings.
dimos/utils/decorators/decorators.py	Added cache invalidation to simple_mcache and retry decorator. Potential thread leak in limit decorator with Timer cleanup.
dimos/utils/metrics.py	New timed decorator creates LCMTransport instances that may not be cleaned up, potentially causing resource leaks.
dimos/core/module.py	Added frame_id property and improved typing with TypeVar for better type safety.

Sequence Diagram

sequenceDiagram
    participant Lidar as Lidar Sensor
    participant Mapper as SparseVoxelGridMapper
    participant VBG as VoxelBlockGrid (CUDA)
    participant Cache as Cache Methods
    participant Occupancy as Occupancy Generator
    participant Output as Transport/Publish

    Lidar->>Mapper: LidarMessage (pointcloud)
    Mapper->>Mapper: _on_frame()
    Mapper->>Mapper: add_frame()
    Mapper->>VBG: Convert to tensor PointCloud
    VBG-->>Mapper: CUDA tensor points
    
    alt carve_columns enabled
        Mapper->>Mapper: _carve_and_insert()
        Mapper->>VBG: Extract (X,Y) keys
        Mapper->>VBG: Create temp HashMap for lookup
        Mapper->>VBG: Find existing keys with matching (X,Y)
        Mapper->>VBG: Erase conflicting columns
        Mapper->>VBG: Insert new voxel keys
    else carve_columns disabled
        Mapper->>VBG: Activate voxel keys
    end
    
    Mapper->>Cache: Invalidate cache (pointcloud, occupancy)
    
    alt publish_interval <= 0
        Mapper->>Mapper: publish_global_map()
        Mapper->>Cache: get_global_pointcloud2()
        Cache->>VBG: Get voxel coordinates
        VBG-->>Cache: Voxel centers as PointCloud
        Cache-->>Mapper: Cached PointCloud2
        Mapper->>Output: Publish global_map
        
        alt costmap.publish enabled
            Mapper->>Cache: get_global_occupancygrid()
            Cache->>Occupancy: height_cost_occupancy()
            Occupancy->>Occupancy: Build min/max height maps
            Occupancy->>Occupancy: Determine effective heights
            Occupancy->>Occupancy: Apply Gaussian smoothing
            Occupancy->>Occupancy: Calculate gradients (Sobel)
            Occupancy->>Occupancy: Map gradient to cost (0-100)
            Occupancy-->>Cache: OccupancyGrid
            Cache-->>Mapper: Cached OccupancyGrid
            Mapper->>Output: Publish global_costmap
        end
    else publish_interval > 0
        Note over Mapper: Timer publishes periodically
    end

Loading

[greptile-apps]

_{3 files reviewed, 5 comments}

_{Edit Code Review Agent Settings | Greptile}

[chatgpt-codex-connector]

Codex usage limits have been reached for code reviews. Please check with the admins of this repo to increase the limits by adding credits.
Credits must be used to enable repository wide code reviews.

[leshy]

@greptileai can you review

[greptile-apps]

Additional Comments (1)

1. dimos/utils/decorators/decorators.py, line 106-107 (link)

   logic: Timer threads created here are never explicitly cleaned up. If the decorated function is called repeatedly and then the object is destroyed, these daemon timer threads will remain until they fire. Consider adding a cleanup mechanism or storing timer references for proper cancellation.

_{30 files reviewed, 5 comments}

_{Edit Code Review Agent Settings | Greptile}

[greptile-apps]

style: Cache invalidation on every frame may impact performance. The invalidation happens inside the method that would typically be called in a tight loop (once per lidar frame). Is the cache invalidation pattern here intended, or would it be better to handle this differently?

[paul-nechifor]

I'd move this out of this test file because it's a class what's used in other tests.

[leshy]

me too