GS-SLAM + VLA: Real-Time Gaussian Splatting SLAM with Vision-Language-Action Models for Robotic Manipulation

Step 1: GPU Gaussian Map with Semantic Feature Fields

from __future__ import annotations

import math
from dataclasses import dataclass
from typing import Tuple, Optional
import torch
import torch.nn.functional as F


@dataclass
class SemanticGaussianMap:
    means: torch.Tensor       # (N, 3) Centroid coordinates
    quats: torch.Tensor       # (N, 4) Unit quaternions [w, x, y, z]
    scales: torch.Tensor      # (N, 3) Log-scales
    opacities: torch.Tensor   # (N,)   Logit-opacities
    colors: torch.Tensor      # (N, 3) Pre-evaluated RGB or SH_0
    semantics: torch.Tensor   # (N, D) Semantic feature embeddings

    @property
    def N(self) -> int:
        return self.means.shape[0]

    @classmethod
    def create_empty(cls, feature_dim: int = 64, device: str = "cuda") -> SemanticGaussianMap:
        return cls(
            means=torch.zeros((0, 3), device=device),
            quats=torch.zeros((0, 4), device=device),
            scales=torch.zeros((0, 3), device=device),
            opacities=torch.zeros((0,), device=device),
            colors=torch.zeros((0, 3), device=device),
            semantics=torch.zeros((0, feature_dim), device=device),
        )

    def add_primitives(
        self,
        xyz: torch.Tensor,
        rgb: torch.Tensor,
        semantic_feats: torch.Tensor,
    ) -> None:
        num_new = xyz.shape[0]
        device = xyz.device

        quats_new = torch.tensor([1.0, 0.0, 0.0, 0.0], device=device).repeat(num_new, 1)
        scales_new = torch.full((num_new, 3), math.log(0.012), device=device)
        opacities_new = torch.full((num_new,), float(math.log(0.2 / 0.8)), device=device)

        self.means = torch.cat([self.means, xyz], dim=0)
        self.quats = torch.cat([self.quats, quats_new], dim=0)
        self.scales = torch.cat([self.scales, scales_new], dim=0)
        self.opacities = torch.cat([self.opacities, opacities_new], dim=0)
        self.colors = torch.cat([self.colors, rgb], dim=0)
        self.semantics = torch.cat([self.semantics, semantic_feats], dim=0)

    def query_semantic_cluster(self, text_embedding: torch.Tensor, threshold: float = 0.65) -> Tuple[torch.Tensor, torch.Tensor]:
        """Queries 3D centroid and surface normal corresponding to text embedding."""
        if self.N == 0:
            return torch.zeros(3, device=self.means.device), torch.tensor([0.0, 0.0, 1.0], device=self.means.device)

        norm_semantics = F.normalize(self.semantics, dim=-1)
        norm_query = F.normalize(text_embedding, dim=-1)
        sim = torch.mv(norm_semantics, norm_query)

        mask = sim > threshold
        if mask.sum() == 0:
            mask = sim > sim.max() * 0.85

        cluster_pts = self.means[mask]
        cluster_weights = torch.sigmoid(self.opacities[mask]).unsqueeze(-1)

        centroid = (cluster_pts * cluster_weights).sum(dim=0) / cluster_weights.sum().clamp_min(1e-6)

        # Estimate surface normal via PCA on local covariance
        centered = cluster_pts - centroid
        cov = (centered.T @ (centered * cluster_weights)) / cluster_weights.sum().clamp_min(1e-6)
        eigvals, eigvecs = torch.linalg.eigh(cov)
        normal = eigvecs[:, 0]  # Minimum variance direction

        return centroid, normal

Step 2: 3D Spatial Feature Adapter

from __future__ import annotations

import torch
import torch.nn as nn


class SpatialFeatureAdapter(nn.Module):
    """Encodes explicit 3D Gaussian map queries into VLA token space."""

    def __init__(self, vla_dim: int = 1024, num_freqs: int = 8):
        super().__init__()
        self.num_freqs = num_freqs
        pos_dim = 3 * num_freqs * 2

        # Projection layer to match VLA token dimensions
        self.proj = nn.Sequential(
            nn.Linear(pos_dim + 3, 256),
            nn.SiLU(),
            nn.Linear(256, vla_dim),
        )

        # Pre-compute Fourier frequency scales
        self.register_buffer("freq_bands", 2.0 ** torch.arange(num_freqs))

    def encode_fourier(self, pos: torch.Tensor) -> torch.Tensor:
        """Applies sinusoidal positional encoding to 3D point (3,)."""
        scaled = pos.unsqueeze(-1) * self.freq_bands.unsqueeze(0) * torch.pi  # (3, num_freqs)
        sin_part = torch.sin(scaled).flatten()
        cos_part = torch.cos(scaled).flatten()
        return torch.cat([sin_part, cos_part], dim=-1)

    def forward(self, centroid: torch.Tensor, normal: torch.Tensor) -> torch.Tensor:
        fourier_pos = self.encode_fourier(centroid)
        combined = torch.cat([fourier_pos, normal], dim=-1)  # (pos_dim + 3,)
        return self.proj(combined[None, None])  # (1, 1, vla_dim)

Step 3: Asynchronous Closed-Loop Controller

from __future__ import annotations

from typing import Dict, Any
import torch
from ..map.gaussian_map import SemanticGaussianMap
from ..adapter.spatial_adapter import SpatialFeatureAdapter


class GSVLAController:
    def __init__(
        self,
        gmap: SemanticGaussianMap,
        adapter: SpatialFeatureAdapter,
        vla_policy: Any,
        control_freq_hz: int = 20,
    ):
        self.gmap = gmap
        self.adapter = adapter
        self.vla = vla_policy
        self.dt = 1.0 / control_freq_hz

    def step(
        self,
        obs: Dict[str, torch.Tensor],
        text_query_emb: torch.Tensor,
        instruction_str: str,
    ) -> torch.Tensor:
        """Executes a single closed-loop policy step with 3D Gaussian spatial grounding."""
        # 1. Query current 3D map for target centroid and normal
        centroid, normal = self.gmap.query_semantic_cluster(text_query_emb)

        # 2. Encode 3D spatial token
        spatial_token = self.adapter(centroid, normal)

        # 3. Policy forward pass: Vision Tokens + Language + 3D Spatial Token
        action_chunk = self.vla.predict_action(
            rgb=obs["rgb"],
            instruction=instruction_str,
            proprio=obs["proprio"],
            extra_tokens=spatial_token,
        )

        return action_chunk[0]  # Return next immediate 6-DoF action delta

Experimental Benchmark Results

We evaluated the GS-SLAM + VLA architecture across standard manipulation benchmarks (custom physical Franka Emika setup + RoboSuite simulation):

Figure 3: Attention distribution comparison. Conditioning the VLA transformer on 3D Gaussian map tokens prevents attention dispersion across background clutter, focusing directly on the physical object centroid.

Production Deployment & Sim-to-Real Considerations

1. Depth Sensor Noise Filtering: RealSense and ToF sensors exhibit edge dropout and flying pixels on reflective cutlery or transparent glassware. Applying a bilateral depth gradient filter prior to back-projection prevents corrupted 3D Gaussian initialization.
2. Asynchronous Thread Isolation: To prevent VLA inference latency from bottlenecking state estimation, isolate the Visual Odometry loop (runs at 30 Hz on CPU/CUDA Core) from the heavy Transformer Action Chunking loop (runs at 5–10 Hz via TensorRT-LLM).
3. Action Chunking Alignment: Predicting action trajectories in chunks of 8–16 steps provides temporal smoothing while allowing the GS-SLAM thread to refine object centroids between policy queries.

Troubleshooting Deployment Failures

1. Spatial Token Misalignment

• Symptom: The robot reaches to a point offset by $5\sim 10\text{ cm}$ from the true target object.
• Root Cause: Extrinsics between robot base frame and camera frame drifted.
• Solution: Execute automated hand-eye calibration (Tsai-Lenz solver) at initialization and anchor Gaussian centroids in base coordinates.

2. Semantic Cluster Confusion in Dense Scenes

• Symptom: The query returns a centroid midway between two adjacent identical objects.
• Root Cause: Semantic feature clustering lacked spatial proximity filtering.
• Solution: Apply DBSCAN clustering to semantic candidate points $\mathcal{S}(c^{\ast })$ and select the cluster with the highest aggregate opacity.