Auto Scaling Global Voxel Grid Mapper

[leshy]

For Go2 voxels.py mapper, we have a pre-set voxel grid size and injest frequency, injest frequency is basically lidar frame message frequency (I think set to 0)

This is an issue on weaker machines, rr.log (rerun log) lidar frame injest time to monitor this processing speed

And we should allow setting of ranges for scaling of processing frequency depending on how long the processing loop takes, no need to expect to re-process full res map on each lidar frame, if running non cuda on a shitty cpu - even 1hz (as opposed to 10hz) is fine

Propose the spec first here (for config changes) then when I confirm you can implement

[spomichter]

Analysis & Implementation Plan

Current Behavior

VoxelGridMapper has two publish modes controlled by publish_interval:

• publish_interval=0 (default): publishes global map on every lidar frame via _on_frame → _publish_trigger
• publish_interval>0: publishes on a timer, decoupled from frame rate

The _on_frame callback always calls add_frame() (the expensive part — voxel hashing, column carving, CUDA ops) on every incoming lidar message regardless of publish mode. The backpressure() wrapper only protects the publish path, not the ingest path.

So on a weak CPU without CUDA, if lidar is coming at 10Hz but add_frame() takes 300ms, frames queue up and you get unbounded lag.

Proposed Spec

Add adaptive frequency scaling to Config:

@dataclass
class Config(ModuleConfig):
    # ... existing fields ...

    # Auto-scaling: target processing time range (seconds)
    # When add_frame() exceeds max_process_time, skip frames to reduce load
    # When add_frame() is under min_process_time, process more frames
    min_process_time: float = 0.05   # 50ms — below this, we can afford more frames
    max_process_time: float = 0.15   # 150ms — above this, start dropping frames
    min_frequency: float = 1.0       # Never drop below 1 Hz ingest
    max_frequency: float = 0.0       # 0 = no cap (process every frame if fast enough)

Behavior

1. Instrument add_frame() — measure wall-clock time per call, track rolling average (EMA)
2. Adaptive skip logic in _on_frame():
   • If avg_process_time > max_process_time → increase skip ratio (process fewer frames)
   • If avg_process_time < min_process_time → decrease skip ratio (process more frames)
   • Never go below min_frequency Hz
   • Never exceed max_frequency Hz (if set)
3. Log ingest timing via rr.log (rerun) — rr.log("voxel_mapper/ingest_time", rr.Scalar(elapsed)) so you can visualize processing speed in the Rerun viewer
4. Log effective frequency — rr.log("voxel_mapper/effective_hz", rr.Scalar(current_hz))

Implementation Sketch

def _on_frame(self, frame: PointCloud2) -> None:
    now = time.monotonic()

    # Skip check: enforce minimum interval between processing
    if (now - self._last_process_time) < self._current_interval:
        self._frames_skipped += 1
        return

    # Process
    t0 = time.monotonic()
    self.add_frame(frame)
    elapsed = time.monotonic() - t0

    # Update EMA of processing time
    self._ema_process_time = 0.7 * self._ema_process_time + 0.3 * elapsed

    # Adapt interval
    if self._ema_process_time > self.config.max_process_time:
        # Slow down — increase interval (but respect min_frequency)
        self._current_interval = min(
            1.0 / self.config.min_frequency,
            self._current_interval * 1.5
        )
    elif self._ema_process_time < self.config.min_process_time:
        # Speed up — decrease interval
        max_interval = 1.0 / self.config.max_frequency if self.config.max_frequency > 0 else 0
        self._current_interval = max(max_interval, self._current_interval * 0.8)

    self._last_process_time = now

    # Rerun telemetry
    try:
        import rerun as rr
        rr.log("voxel_mapper/ingest_time_ms", rr.Scalar(elapsed * 1000))
        rr.log("voxel_mapper/effective_hz", rr.Scalar(1.0 / self._current_interval if self._current_interval > 0 else float("inf")))
        rr.log("voxel_mapper/frames_skipped", rr.Scalar(self._frames_skipped))
    except ImportError:
        pass

    if self.config.publish_interval == 0:
        self._publish_trigger.on_next(None)

Concerns

1. Stale map on aggressive skipping — If min_frequency=1 and lidar is 10Hz, we skip 90% of frames. The map will have gaps in fast-moving scenarios. Might want a max_skip_ratio safety cap.

2. EMA smoothing factor — 0.3 is aggressive; a sudden spike (GC pause, CUDA sync) could overcorrect. Maybe configurable or use a longer window (0.1 factor).

3. Column carving correctness — Skipping frames means we miss carving updates. Old voxels that should be cleared (because the robot moved and the column is now empty) will persist longer. This is a quality tradeoff, not a correctness bug, but worth noting.

4. CUDA sync timing — time.monotonic() around add_frame() may not capture true CUDA kernel time if ops are async. May need torch.cuda.synchronize() or Open3D equivalent for accurate measurement on GPU. Without it, the EMA could underestimate actual load.

5. Rerun dependency — rr.log calls should be guarded (already shown with try/except) since rerun is optional. Could also gate behind a config.enable_telemetry flag to avoid the import overhead on every frame.

6. Thread safety — _on_frame is called from the transport subscriber thread. The skip logic with _current_interval / _last_process_time should be fine (single writer) but worth confirming no concurrent add_frame calls are possible.

Questions

• Should the auto-scaling also affect voxel_size dynamically (coarser voxels under load), or strictly frequency only?
• Do you want the rerun logging always-on or behind a config flag?
• Any preference on the EMA smoothing factor?

Let me know if this spec direction looks right and I'll implement it.

[leshy]

Actually no, you need to just see if processing time > required publish frequency (which is lidar freq if 0 right?) and if so, slow down global publish freq / re-process time?

@dataclass
class Config(ModuleConfig):
    autoscale: bool = True # should it autoscale
    autoscale_min_frequency: float = 1.0 # min freqnecy to go to

on 4. CUDA sync timing

can't we just use standard python time module? we are timing standard python loop exec

[spomichter]

Agreed on the simpler config. Revised spec:

@dataclass
class Config(ModuleConfig):
    # ... existing fields ...
    autoscale: bool = True
    autoscale_min_frequency: float = 1.0  # never drop below 1 Hz

Logic

No need to know the lidar rate. Just use processing time directly as the throttle:

1. Time each add_frame() call
2. If last call took 300ms, you're saturated — don't accept another frame for at least 300ms
3. Scale back naturally until autoscale_min_frequency floor is hit
4. If machine is fast enough, every frame gets processed (no change from current behavior)

def _on_frame(self, frame: PointCloud2) -> None:
    if self.config.autoscale:
        now = time.monotonic()
        if (now - self._last_process_time) < self._last_process_duration:
            return  # still saturated, skip
        min_interval = 1.0 / self.config.autoscale_min_frequency
        if (now - self._last_process_time) < min_interval:
            pass  # respect min frequency floor — but we already cleared saturation check above

    t0 = time.monotonic()
    self.add_frame(frame)
    elapsed = time.monotonic() - t0

    if self.config.autoscale:
        self._last_process_time = t0
        self._last_process_duration = elapsed
        rr.log("voxel_mapper/ingest_time_ms", rr.Scalar(elapsed * 1000))

    if self.config.publish_interval == 0:
        self._publish_trigger.on_next(None)

Self-regulating: fast machine eats every frame, slow machine finds its ceiling. autoscale_min_frequency is the safety floor so the map never goes completely stale.

On CUDA timing — confirmed, Open3D Python API is synchronous (blocks until GPU op returns), so time.monotonic() captures real cost. No special sync needed.

Ready to implement on confirmation.

[leshy]

measure injest time and global map publish time separately, to see which one is more costly, I assume global map publish time, so scaling will help here

[spomichter]

Good point. Will instrument both separately:

rr.log("voxel_mapper/ingest_time_ms", rr.Scalar(...))       # add_frame() cost
rr.log("voxel_mapper/publish_time_ms", rr.Scalar(...))      # get_global_pointcloud2() + publish cost
rr.log("voxel_mapper/map_size", rr.Scalar(self.size()))      # voxel count (correlates with publish cost)

If publish is the bottleneck (likely — voxel_coordinates_and_flattened_indices() over entire grid + GPU→CPU copy grows with map size), then autoscaling the publish frequency is where the real win is. Ingest can stay at full rate since per-frame hashing is O(n_points_in_frame), while publish is O(total_map_size).

Will implement with both timers so we have the data to confirm.