NeuralBoneReg Solved the Hardest Alignment Problem in Robotic Surgery Without a Single Labeled Training Example

The following is a complete PyTorch implementation of NeuralBoneReg covering both stages described in Sections 3.2.1 and 3.2.2 of the paper. It includes the NeuralUDF module with tangent-plane consistency loss, the NeuralReg module with shared MLP backbone and parallel hypothesis generation heads, the coarse-to-fine evaluation strategy, the registration loss function, quaternion-to-rotation-matrix conversion, farthest point sampling initialization in SO(3), and a runnable smoke test demonstrating both the preoperative training stage and the intraoperative registration stage.

# ============================================================
# NeuralBoneReg: Instance-Specific Self-Supervised Multi-Modal
# Bone Surface Registration via Neural Distance Fields
# Paper: Wu et al., Medical Image Analysis 112 (2026) 104133
# Authors: Luohong Wu, Matthias Seibold et al.
# Institution: Balgrist University Hospital / ETH Zurich
# ============================================================

from __future__ import annotations
import math
import torch
import torch.nn as nn
import torch.nn.functional as F
from typing import Tuple, Optional


# ─── SECTION 1: Spatial Normalization Utilities ───────────────────────────────

def normalize_point_cloud(pts: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]:
    """Normalize point cloud to the (-1, 1)^3 cube.
    
    Follows Section 3.1 Equations 2-4 of the paper.
    Scale is defined as the diagonal length of the 3D bounding box.
    Centroid is subtracted before scale normalization.
    
    Parameters
    ----------
    pts : (N, 3) point cloud in original spatial coordinates

    Returns
    -------
    pts_norm : (N, 3) normalized point cloud
    centroid  : (3,) centroid of original cloud
    scale     : scalar bounding box diagonal length
    """
    centroid = pts.mean(dim=0)                                   # (3,)
    pts_c = pts - centroid
    bbox_min = pts_c.min(dim=0).values
    bbox_max = pts_c.max(dim=0).values
    scale = (bbox_max - bbox_min).pow(2).sum().sqrt()              # diagonal length
    pts_norm = pts_c / (scale + 1e-8)
    return pts_norm, centroid, scale


# ─── SECTION 2: NeuralUDF Module ─────────────────────────────────────────────

class NeuralUDF(nn.Module):
    """Learn a neural Unsigned Distance Field from a preoperative point cloud.
    
    Implements Section 3.2.1 of the paper.
    
    The MLP approximates the unsigned distance of any spatial location
    to the underlying bone surface. Training is entirely self-supervised:
    no ground-truth distances are required.
    
    Loss combines:
      (1) Surface loss: query points sampled ON the bone surface should
          have near-zero distance predictions.
      (2) Tangent-plane consistency: query points near the surface should
          have gradients perpendicular to the local surface normal.
      (3) Off-surface loss: random space points should have positive
          predicted distances that increase away from the surface.
    
    After training, the frozen UDF provides smooth, differentiable
    distance queries d(q) for any 3D point q via forward().
    
    Parameters
    ----------
    hidden_dim : MLP hidden dimension (default 256)
    num_layers : number of hidden layers (default 5)
    """
    def __init__(self, hidden_dim: int = 256, num_layers: int = 5):
        super().__init__()
        layers = [nn.Linear(3, hidden_dim), nn.Softplus()]
        for _ in range(num_layers - 1):
            layers += [nn.Linear(hidden_dim, hidden_dim), nn.Softplus()]
        layers.append(nn.Linear(hidden_dim, 1))
        layers.append(nn.Softplus())  # ensures non-negative UDF output
        self.net = nn.Sequential(*layers)

    def forward(self, q: torch.Tensor) -> torch.Tensor:
        """
        Parameters
        ----------
        q : (N, 3) query points in normalized space

        Returns
        -------
        d : (N,) predicted unsigned distances to bone surface
        """
        return self.net(q).squeeze(-1)


def udf_training_loss(
    udf: NeuralUDF,
    surface_pts: torch.Tensor,
    sigma_near: float = 0.01,
    sigma_far: float = 0.5,
    n_near: int = 2048,
    n_far: int = 512,
) -> torch.Tensor:
    """Self-supervised loss for NeuralUDF training.
    
    Three terms from UltraBoneUDF (Wu et al., 2026), used as the
    NeuralUDF module by default.
    
    Surface term: sampled surface points should have distance ≈ 0.
    Near-surface term: slightly perturbed points should have low distance
      with gradient pointing toward the surface (tangent-plane consistency).
    Off-surface term: random points in space should have positive distance.
    
    Parameters
    ----------
    udf          : the NeuralUDF model being trained
    surface_pts  : (M, 3) points sampled from the bone surface
    sigma_near   : Gaussian noise std for near-surface perturbation
    sigma_far    : uniform range for random off-surface sampling
    n_near       : number of near-surface query points
    n_far        : number of off-surface query points
    """
    device = surface_pts.device
    M = surface_pts.shape[0]

    # --- Term 1: surface loss -----------------------------------------
    surface_pts = surface_pts.requires_grad_(True)
    d_surface = udf(surface_pts)
    loss_surface = d_surface.mean()

    # --- Term 2: tangent-plane consistency (near-surface) ---------------
    idx = torch.randint(0, M, (n_near,), device=device)
    near_pts = surface_pts[idx] + sigma_near * torch.randn(n_near, 3, device=device)
    near_pts = near_pts.requires_grad_(True)
    d_near = udf(near_pts)
    grad_near = torch.autograd.grad(
        d_near.sum(), near_pts, create_graph=True
    )[0]                                                           # (n_near, 3)
    # Gradient should be a unit vector -- enforce unit norm for consistency
    grad_norm = grad_near.norm(dim=-1, keepdim=True).clamp(min=1e-6)
    loss_eikonal = (grad_norm - 1).abs().mean()

    # --- Term 3: off-surface repulsion ----------------------------------
    far_pts = torch.zeros(n_far, 3, device=device).uniform_(-sigma_far, sigma_far)
    d_far = udf(far_pts)
    loss_far = F.relu(sigma_far * 0.5 - d_far).mean()

    return loss_surface + 0.1 * loss_eikonal + 0.01 * loss_far


# ─── SECTION 3: Quaternion and SO(3) Utilities ───────────────────────────────

def quaternion_to_rotation_matrix(q: torch.Tensor) -> torch.Tensor:
    """Convert unit quaternions to 3x3 rotation matrices.
    
    Parameters
    ----------
    q : (H, 4) unit quaternions in [w, x, y, z] format

    Returns
    -------
    R : (H, 3, 3) rotation matrices in SO(3)
    """
    q = F.normalize(q, dim=-1)
    w, x, y, z = q[:, 0], q[:, 1], q[:, 2], q[:, 3]
    R = torch.stack([
        1 - 2*(y*y + z*z),   2*(x*y - w*z),     2*(x*z + w*y),
        2*(x*y + w*z),     1 - 2*(x*x + z*z),   2*(y*z - w*x),
        2*(x*z - w*y),       2*(y*z + w*x),     1 - 2*(x*x + y*y),
    ], dim=-1).reshape(-1, 3, 3)
    return R


def fps_so3_init(H: int, device: str = 'cpu') -> torch.Tensor:
    """Initialize H rotation hypotheses via farthest point sampling in SO(3).
    
    Spread initial hypotheses across the full rotation sphere to maximize
    coverage and increase the likelihood that at least one hypothesis
    starts near the correct alignment.
    
    Returns
    -------
    q : (H, 4) unit quaternions uniformly spread over SO(3)
    """
    # Sample random unit quaternions then apply greedy FPS for coverage
    pool_size = max(H * 10, 10000)
    q_pool = torch.randn(pool_size, 4, device=device)
    q_pool = F.normalize(q_pool, dim=-1)                         # uniform on S^3
    selected = [0]
    dists = torch.full((pool_size,), float('inf'), device=device)
    for _ in range(H - 1):
        last = q_pool[selected[-1]].unsqueeze(0)
        dot = (q_pool * last).sum(dim=-1).abs().clamp(0, 1)
        d = 1 - dot                                                 # geodesic-like distance
        dists = torch.minimum(dists, d)
        selected.append(dists.argmax().item())
    return q_pool[selected]                                        # (H, 4)


# ─── SECTION 4: NeuralReg Module ──────────────────────────────────────────────

class NeuralReg(nn.Module):
    """Intraoperative registration via parallel neural hypothesis generation.
    
    Implements Section 3.2.2 of the paper.
    
    Architecture:
      Shared backbone f_theta: (128,) -> (128,) shared latent feature
      H hypothesis heads r_phi_h: (128,) -> (3,) translation + (4,) quaternion
    
    The shared backbone propagates gradient information across all heads,
    enabling cooperative search rather than independent parallel search.
    
    During optimization:
      1. All H hypotheses evaluate their mean UDF distance (coarse stage)
      2. Top-K hypotheses are refined at higher point cloud density
      3. The best hypothesis at inference is selected as final result
    
    Parameters
    ----------
    H            : number of hypothesis generation heads (5000 in paper)
    latent_dim   : backbone input/output dimension (128)
    backbone_depth : shared MLP depth in [0..6], 0 = no sharing
    """
    def __init__(self, H: int = 5000, latent_dim: int = 128,
                 backbone_depth: int = 4):
        super().__init__()
        self.H = H
        self.latent_dim = latent_dim

        # Shared backbone: maps latent code to shared feature
        backbone_layers = []
        for _ in range(backbone_depth):
            backbone_layers += [nn.Linear(latent_dim, latent_dim), nn.Tanh()]
        self.backbone = nn.Sequential(*backbone_layers) if backbone_layers else nn.Identity()

        # H lightweight heads: each outputs translation (3) + quaternion (4)
        # Implemented as a batched single layer for GPU efficiency
        self.heads_weight = nn.Parameter(torch.randn(H, latent_dim, 7) * 0.01)
        self.heads_bias   = nn.Parameter(torch.zeros(H, 7))

        # Fixed random latent code -- held constant during optimization
        self.register_buffer('x_bar', torch.zeros(1, latent_dim).uniform_(-1, 1))

        # Initialize rotation quaternions via FPS; store as parameter
        self._init_rotations()

    def _init_rotations(self):
        """Initialize quaternion component of heads via FPS in SO(3)."""
        q_init = fps_so3_init(self.H)                             # (H, 4)
        self.heads_bias.data[:, 3:] = q_init                      # bias last 4 dims

    def forward(self) -> Tuple[torch.Tensor, torch.Tensor]:
        """Generate H transformation hypotheses.

        Returns
        -------
        t : (H, 3) translation vectors
        R : (H, 3, 3) rotation matrices
        """
        feat = self.backbone(self.x_bar)                            # (1, latent_dim)
        # Batched head computation: (H, latent_dim) @ (H, latent_dim, 7)
        feat_exp = feat.expand(self.H, -1)                         # (H, latent_dim)
        out = torch.tanh(
            torch.einsum('hi,hij->hj', feat_exp, self.heads_weight)
            + self.heads_bias
        )                                                           # (H, 7) in (-1,1)

        t = out[:, :3]                                              # (H, 3) translation
        q = F.normalize(out[:, 3:], dim=-1)                       # (H, 4) unit quat
        R = quaternion_to_rotation_matrix(q)                        # (H, 3, 3)
        return t, R


# ─── SECTION 5: Registration Loss ─────────────────────────────────────────────

def registration_loss(
    udf: NeuralUDF,
    neural_reg: NeuralReg,
    intra_pts: torch.Tensor,
    B: int = 3000,
    B_sparse: int = 32,
    K: int = 64,
) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]:
    """Compute the coarse-to-fine registration loss (Equation 10).
    
    Stage 1 (coarse): evaluate all H hypotheses on B_sparse points.
    Stage 2 (fine): re-evaluate top-K hypotheses on B points.
    
    The loss is the mean UDF value of the transformed intraoperative
    point cloud averaged across all H hypotheses.
    
    Parameters
    ----------
    udf          : frozen NeuralUDF for distance queries
    neural_reg   : NeuralReg to be optimized
    intra_pts    : (N, 3) normalized intraoperative point cloud
    B            : dense batch size for fine-stage evaluation
    B_sparse     : sparse batch size for coarse-stage evaluation
    K            : number of top hypotheses to refine

    Returns
    -------
    loss         : scalar optimization objective
    best_R, best_t : (3,3) and (3,) best hypothesis at this step
    """
    N = intra_pts.shape[0]
    H = neural_reg.H

    t, R = neural_reg()                                             # (H,3), (H,3,3)

    # ── Coarse stage: sparse evaluation of all H hypotheses ──────────────
    idx_sparse = torch.randperm(N, device=intra_pts.device)[:B_sparse]
    pts_sparse = intra_pts[idx_sparse]                              # (B_sparse, 3)

    # Transform: (H, B_sparse, 3) = (H,3,3) @ (B_sparse,3).T + (H,3)
    pts_rot_sparse = torch.einsum('hrc,nc->hnr', R, pts_sparse) + t.unsqueeze(1)
    pts_rot_flat_sparse = pts_rot_sparse.reshape(-1, 3)             # (H*B_sparse, 3)

    with torch.no_grad():
        d_sparse = udf(pts_rot_flat_sparse).reshape(H, B_sparse)   # (H, B_sparse)
    mean_d_sparse = d_sparse.mean(dim=-1)                          # (H,)

    # ── Fine stage: dense evaluation of top-K hypotheses ────────────────
    topk_idx = mean_d_sparse.topk(K, largest=False).indices
    idx_dense = torch.randperm(N, device=intra_pts.device)[:B]
    pts_dense = intra_pts[idx_dense]                                # (B, 3)

    R_topk = R[topk_idx]                                            # (K, 3, 3)
    t_topk = t[topk_idx]                                            # (K, 3)
    pts_rot_fine = torch.einsum('krc,nc->knr', R_topk, pts_dense) + t_topk.unsqueeze(1)
    pts_rot_flat_fine = pts_rot_fine.reshape(-1, 3)                # (K*B, 3)

    d_fine = udf(pts_rot_flat_fine).reshape(K, B)                   # (K, B)
    mean_d_fine = d_fine.mean(dim=-1)                              # (K,)

    # ── Total loss: mean over all H coarse + top-K fine evaluations ──────
    loss = mean_d_sparse.mean() + mean_d_fine.mean()

    # Best hypothesis for inference selection
    best_fine_idx = mean_d_fine.argmin()
    best_R = R_topk[best_fine_idx].detach()
    best_t = t_topk[best_fine_idx].detach()

    return loss, best_R, best_t


# ─── SECTION 6: NeuralBoneReg Full Framework ──────────────────────────────────

class NeuralBoneReg:
    """NeuralBoneReg: Two-stage instance-specific bone surface registration.
    
    Implements the full pipeline from Section 3.2 of the paper.
    
    PREOPERATIVE STAGE (run once per patient before surgery)
      Given: segmented bone surface from CT or MRI
      Action: train NeuralUDF to convergence on the preoperative point cloud
      Output: frozen neural distance field for intraoperative use
    
    INTRAOPERATIVE STAGE (run during surgery, target ~ 5 seconds)
      Given: intraoperative point cloud from ultrasound or RGB-D
      Action: optimize NeuralReg with 5000 parallel hypothesis heads
      Output: best rigid transformation [R | t] aligning intraoperative
              point cloud with the preoperative neural distance field
    
    Parameters
    ----------
    udf_hidden_dim   : NeuralUDF MLP hidden dimension (256)
    udf_layers       : NeuralUDF MLP depth (5)
    H                : number of NeuralReg hypothesis heads (5000)
    latent_dim       : backbone latent dimension (128)
    backbone_depth   : shared backbone depth (4)
    device           : compute device
    """
    def __init__(
        self,
        udf_hidden_dim: int = 256,
        udf_layers: int = 5,
        H: int = 5000,
        latent_dim: int = 128,
        backbone_depth: int = 4,
        device: str = 'cuda',
    ):
        self.device = device
        self.udf = NeuralUDF(udf_hidden_dim, udf_layers).to(device)
        self.neural_reg = NeuralReg(H, latent_dim, backbone_depth).to(device)

    def preoperative_training(
        self,
        preop_pts: torch.Tensor,
        n_iters: int = 10000,
        lr: float = 1e-4,
        verbose: bool = True,
    ) -> None:
        """Preoperative stage: train NeuralUDF on the preoperative point cloud.
        
        This stage is computationally intensive but runs BEFORE surgery,
        so runtime is not critical. After training, the UDF is frozen.
        
        Parameters
        ----------
        preop_pts : (M, 3) normalized preoperative bone surface point cloud
        n_iters   : training iterations (default 10000)
        lr        : Adam learning rate (default 1e-4)
        verbose   : print loss every 1000 iterations
        """
        preop_pts = preop_pts.to(self.device)
        optimizer = torch.optim.Adam(self.udf.parameters(), lr=lr)
        self.udf.train()

        for i in range(n_iters):
            optimizer.zero_grad()
            loss = udf_training_loss(self.udf, preop_pts)
            loss.backward()
            optimizer.step()
            if verbose and (i % 1000 == 0):
                print(ff"  UDF iter {i:5d}  loss {loss.item():.4f}")

        self.udf.eval()
        for p in self.udf.parameters():
            p.requires_grad_(False)
        if verbose:
            print("  NeuralUDF training complete. Weights frozen.")

    def intraoperative_registration(
        self,
        intra_pts: torch.Tensor,
        n_iters: int = 1000,
        lr: float = 1e-3,
        B: int = 3000,
        B_sparse: int = 32,
        K: int = 64,
        verbose: bool = True,
    ) -> Tuple[torch.Tensor, torch.Tensor]:
        """Intraoperative stage: register intraoperative cloud to preoperative UDF.
        
        NeuralReg is re-initialized for each new intraoperative input.
        The rotation hypotheses are initialized via FPS in SO(3) for
        broad coverage of the rotation space from the start.
        
        Parameters
        ----------
        intra_pts : (N, 3) normalized intraoperative bone surface point cloud
        n_iters   : Adam optimization iterations (default 1000)
        lr        : Adam learning rate (0.001 per paper)
        B         : dense point count for fine-stage evaluation (3000)
        B_sparse  : sparse point count for coarse-stage evaluation (32)
        K         : number of hypotheses refined in fine stage (64)

        Returns
        -------
        best_R : (3, 3) estimated rotation matrix
        best_t : (3,) estimated translation vector
        """
        intra_pts = intra_pts.to(self.device)

        # Re-initialize NeuralReg for this instance
        self.neural_reg = NeuralReg(
            self.neural_reg.H,
            self.neural_reg.latent_dim,
            sum(1 for l in self.neural_reg.backbone.children() if hasattr(l, 'weight')),
        ).to(self.device)

        optimizer = torch.optim.Adam(
            self.neural_reg.parameters(), lr=lr, betas=(0.9, 0.999)
        )
        self.neural_reg.train()

        best_R_final, best_t_final = None, None
        best_loss = float('inf')

        for i in range(n_iters):
            optimizer.zero_grad()
            loss, best_R, best_t = registration_loss(
                self.udf, self.neural_reg, intra_pts, B, B_sparse, K
            )
            loss.backward()
            optimizer.step()

            if loss.item() < best_loss:
                best_loss = loss.item()
                best_R_final = best_R.clone()
                best_t_final = best_t.clone()

            if verbose and (i % 200 == 0):
                print(ff"  Reg iter {i:4d}  loss {loss.item():.4f}")

        return best_R_final, best_t_final


# ─── SECTION 7: Smoke Test ─────────────────────────────────────────────────────

def _smoke_test():
    """End-to-end smoke test of NeuralBoneReg on synthetic bone surface data.
    
    Tests:
      - NeuralUDF preoperative training convergence
      - NeuralReg intraoperative optimization with coarse-to-fine strategy
      - Rotation matrix validity (det(R) = +1)
      - Full pipeline gradient flow
    
    Uses reduced model size and iteration counts for fast testing.
    """
    print("=" * 62)
    print("NeuralBoneReg Smoke Test -- Synthetic Bone Surface Data")
    print("Paper: Wu et al., Medical Image Analysis 112 (2026) 104133")
    print("=" * 62)

    device = 'cuda' if torch.cuda.is_available() else 'cpu'
    print(ff"\nDevice: {device}")

    # Synthetic preoperative point cloud: sphere surface as proxy for bone
    M = 1000
    angles = torch.randn(M, 3)
    preop_pts = F.normalize(angles, dim=-1) * 0.5          # sphere of radius 0.5
    preop_pts_norm, centroid, scale = normalize_point_cloud(preop_pts)

    # Synthetic intraoperative cloud: same sphere partially observed + noise
    N = 400
    intra_base = F.normalize(torch.randn(N, 3), dim=-1) * 0.5
    intra_base += 0.02 * torch.randn(N, 3)
    intra_pts_norm, _, _ = normalize_point_cloud(intra_base)

    print(ff"Preoperative cloud: {M} points")
    print(ff"Intraoperative cloud: {N} points")

    # Build NeuralBoneReg with small configuration for fast testing
    framework = NeuralBoneReg(
        udf_hidden_dim=64,
        udf_layers=3,
        H=100,            # paper uses 5000; reduced for smoke test
        latent_dim=64,
        backbone_depth=2,
        device=device,
    )

    total_udf = sum(p.numel() for p in framework.udf.parameters())
    total_reg = sum(p.numel() for p in framework.neural_reg.parameters())
    print(ff"NeuralUDF parameters: {total_udf:,}")
    print(ff"NeuralReg parameters: {total_reg:,}")

    # Preoperative stage
    print(f"\n--- Preoperative Stage (NeuralUDF Training) ---")
    framework.preoperative_training(
        preop_pts_norm.to(device), n_iters=200, lr=1e-3, verbose=True
    )

    # Verify UDF produces valid non-negative distances
    test_q = torch.randn(5, 3, device=device)
    with torch.no_grad():
        test_d = framework.udf(test_q)
    assert (test_d >= 0).all(), "UDF produced negative distances"
    print(ff"UDF test distances (should be >= 0): {test_d.cpu().numpy().round(3)}")

    # Intraoperative stage
    print(f"\n--- Intraoperative Stage (NeuralReg Optimization) ---")
    best_R, best_t = framework.intraoperative_registration(
        intra_pts_norm.to(device),
        n_iters=50,
        lr=1e-3,
        B=200,
        B_sparse=16,
        K=10,
        verbose=True,
    )

    # Validate rotation matrix
    det_R = torch.linalg.det(best_R).item()
    print(ff"\n{'─'*42}")
    print(ff"Best R (3x3 rotation matrix):\n{best_R.cpu().numpy().round(3)}")
    print(ff"Best t (translation vector): {best_t.cpu().numpy().round(3)}")
    print(ff"det(R) = {det_R:.4f} (should be 1.0 for valid rotation)")
    assert abs(det_R - 1.0) < 0.01, "det(R) invalid"
    print(f"{'─'*42}")
    print("Smoke test passed. NeuralBoneReg preop and intraop cycles OK.")
    print("See Section 3.2 in Wu et al. 2026 for the full methodology.")
    print("=" * 62)


if __name__ == '__main__':
    _smoke_test()