Skip to content
Python
14.2 KB
Raw Blame
"""MHR kinematic fit as a static, export-ready PyTorch module (ONNX -> TensorRT / ORT CUDA / ORT QNN, fp16).

Used by ``tools/export_onnx.py --fit``: the network's MHR pose is refined so that its 30 body keypoints, projected
into the frame, land on the network's own 2D (SimCC) keypoints, weighted by their spread.

The pose (root rotation + 62 body angles, MHR parameters 3:68) and the camera translation are moved so that the
30 projected body keypoints land on given 2D targets, weighted by their spread, with a quadratic prior pulling
towards the network's prediction. Levenberg-Marquardt, a fixed number of iterations, no data-dependent control
flow:

* forward kinematics as in the MHR rig (prefix-multiplication levels, quaternions), from constants read once out
  of ``checkpoints/mhr_model.pt``;
* an exact Jacobian: parameters 3:68 drive only joint rotations, so the derivative of a joint position along a
  rotation axis w of an ancestor j is w x (p - p_j); keypoints are a fixed linear map of joint positions;
* the 68x68 normal equations solved by Newton-Schulz iterations (matrix products only: no solve / inverse
  operator exists in ONNX, and phone NPUs are slow on long chains of tiny operations).
"""

from __future__ import annotations

import math
from pathlib import Path

import numpy as np
import torch
from torch import nn

BODY_KP = list(range(21)) + [41, 62] + list(range(63, 70))
POSE = slice(3, 68)
N_POSE = 65
LN2 = math.log(2.0)


def _qmul(a, b):
    x1, y1, z1, w1 = a.unbind(-1)
    x2, y2, z2, w2 = b.unbind(-1)
    return torch.stack((w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
                        w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2), -1)


def _cross(a, b):
    ax, ay, az = a.unbind(-1)
    bx, by, bz = b.unbind(-1)
    return torch.stack((ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx), -1)


def _rotate(q, v):
    u, w = q[..., :3], q[..., 3:]
    uv = _cross(u, v)
    return v + 2 * (uv * w + _cross(u, uv))


def _normalize(q):
    return q / torch.sqrt((q * q).sum(-1, keepdim=True)).clamp_min(1e-6)


def _half_angle_quats(e):
    """Euler XYZ (roll, pitch, yaw) -> the three elementary quaternions qx, qy, qz (x, y, z, w)."""
    h = e * 0.5
    c, s = torch.cos(h), torch.sin(h)
    z = c[..., 0] * 0
    qx = torch.stack((s[..., 0], z, z, c[..., 0]), -1)
    qy = torch.stack((z, s[..., 1], z, c[..., 1]), -1)
    qz = torch.stack((z, z, s[..., 2], c[..., 2]), -1)
    return qx, qy, qz


def rig_constants(mhr_path: str | Path, kp_regressor_path: str | Path) -> dict:
    """Read the TorchScript rig once: what forward kinematics and the keypoint readout need."""
    rig = torch.jit.load(str(mhr_path), map_location="cpu").eval()
    ct = rig.character_torch
    sk = ct.skeleton
    parents = sk.joint_parents.numpy().astype(np.int64)
    chains = []
    for j in range(len(parents)):
        c = [j]
        while parents[c[-1]] >= 0:
            c.append(int(parents[c[-1]]))
        chains.append(c[::-1])
    partners, actives, level = [], [], 0
    while True:                                   # pymomentum's prefix-multiplication levels
        partner, active = np.arange(len(parents)), np.zeros(len(parents), bool)
        for c in chains:
            i = len(c) - 1
            if (i >> level) & 1:
                partner[c[i]] = c[((i >> level) << level) - 1]
                active[c[i]] = True
        if not active.any():
            break
        partners.append(partner); actives.append(active); level += 1
    desc = np.zeros((len(parents), len(parents)), np.float32)   # desc[j, k] = 1 if k is a strict descendant of j
    for k, c in enumerate(chains):
        for j in c[:-1]:
            desc[j, k] = 1.0
    return dict(parameter_transform=ct.parameter_transform.parameter_transform[:, :204].numpy(),
                joint_offsets=sk.joint_translation_offsets.numpy(), joint_prerotations=sk.joint_prerotations.numpy(),
                fk_partner=np.stack(partners), fk_active=np.stack(actives), joint_parents=parents, descendants=desc,
                kp_regressor=np.load(kp_regressor_path).astype(np.float32))


class MHRKinematics(nn.Module):
    """Forward kinematics of the MHR skeleton (raw units: cm, Y-up) and the 30 body keypoints (vision frame)."""

    def __init__(self, constants: dict):
        super().__init__()
        P = torch.as_tensor(constants["parameter_transform"], dtype=torch.float32)       # (889, 204)
        n_j = P.shape[0] // 7
        rot = P.reshape(n_j, 7, 204)[:, 3:6, POSE]                                         # (127, 3, 65)
        self.register_buffer("P", P)
        self.register_buffer("P_rot", rot.reshape(n_j * 3, N_POSE))
        self.register_buffer("offsets", torch.as_tensor(constants["joint_offsets"], dtype=torch.float32))
        self.register_buffer("prerot", torch.as_tensor(constants["joint_prerotations"], dtype=torch.float32))
        self.register_buffer("partner", torch.as_tensor(constants["fk_partner"], dtype=torch.long))
        self.register_buffer("active", torch.as_tensor(constants["fk_active"], dtype=torch.bool))
        A = torch.as_tensor(constants["kp_regressor"], dtype=torch.float32)[BODY_KP]        # (30, 127)
        D = torch.as_tensor(constants["descendants"], dtype=torch.float32)                  # (127, 127)
        self.register_buffer("A", A)
        self.register_buffer("AD", (A[:, None, :] * D[None]).reshape(-1, n_j))               # (30*127, 127)
        self.register_buffer("AD_sum", (A[:, None, :] * D[None]).sum(-1))                   # (30, 127)
        self.register_buffer("vision", torch.tensor([0.01, -0.01, -0.01]))
        self.register_buffer("axes", torch.eye(3))                                         # e_x, e_y, e_z
        self.register_buffer("conj", torch.tensor([-1.0, -1.0, -1.0, 1.0]))
        self.n_j = n_j

    @classmethod
    def from_rig(cls, mhr_path: str | Path, kp_regressor_path: str | Path) -> "MHRKinematics":
        return cls(rig_constants(mhr_path, kp_regressor_path))

    def forward(self, mp):
        """(B, 204) -> joint positions (B, 127, 3) raw, global quaternions (B, 127, 4), local Euler quats."""
        jp = (mp @ self.P.T).unflatten(-1, (self.n_j, 7))
        qx, qy, qz = _half_angle_quats(jp[..., 3:6])
        q = _qmul(self.prerot, _qmul(qz, _qmul(qy, qx)))
        t, s = jp[..., :3] + self.offsets, torch.exp(LN2 * jp[..., 6:])
        for lvl in range(self.partner.shape[0]):                # global = parent_global o local, by levels
            idx = self.partner[lvl]
            tp, qp, sp = t.index_select(-2, idx), _normalize(q.index_select(-2, idx)), s.index_select(-2, idx)
            tn, qn, sn = tp + sp * _rotate(qp, t), _qmul(qp, _normalize(q)), sp * s
            a = self.active[lvl][:, None]
            t, q, s = torch.where(a, tn, t), torch.where(a, qn, q), torch.where(a, sn, s)
        return t, _normalize(q), qx, qy

    def keypoints(self, t):
        """(B, 127, 3) raw joint positions -> (B, 30, 3) body keypoints, vision frame (metres, Y-down)."""
        return (self.A @ t) * self.vision

    def keypoint_jacobian(self, t, q, qx, qy):
        """d(30 body keypoints, vision frame) / d(MHR params 3:68): (B, 30, 3, 65)."""
        B = t.shape[0]
        ex, ey, ez = (self.axes[i].expand_as(t) for i in range(3))
        cx, cy = qx * self.conj, qy * self.conj                    # conjugates
        wx = _rotate(q, ex)                                         # world axes of the three Euler rotations
        wy = _rotate(q, _rotate(cx, ey))
        wz = _rotate(q, _rotate(cx, _rotate(cy, ez)))
        w = torch.stack((wx, wy, wz), 2)                            # (B, 127, 3 axes, 3)
        S = (self.AD @ t).reshape(B, -1, self.n_j, 3)               # (B, 30, 127, 3): sum_k A D p_k
        lever = S - self.AD_sum[None, :, :, None] * t[:, None]      # (B, 30, 127, 3)
        M = _cross(w[:, None], lever[:, :, :, None])                # (B, 30, 127, 3 axes, 3 coords)
        M = M.permute(0, 1, 4, 2, 3).reshape(B, -1, self.n_j * 3)   # (B, 30*3, 127*3)
        J = (M @ self.P_rot).reshape(B, -1, 3, N_POSE)              # (B, 30, 3, 65)
        return J * self.vision[None, None, :, None]


def newton_schulz_solve(H, g, iters: int, eye):
    """x = H^-1 g for SPD H (B, n, n), by Jacobi scaling and Newton-Schulz on the inverse."""
    d = torch.rsqrt((H * eye).sum(-1).clamp_min(1e-12))                         # (B, n): 1 / sqrt(diag H)
    Hs = H * d[:, :, None] * d[:, None, :]
    X = eye / Hs.abs().sum(-1).amax(-1)[:, None, None]                           # ||I - X0 Hs|| < 1
    for _ in range(iters):
        X = X @ (2 * eye - Hs @ X)
    return d * (X @ (d * g)[..., None])[..., 0]


class KinematicFit(nn.Module):
    """Levenberg-Marquardt on (MHR params 3:68, camera translation), fixed iterations, export-ready."""

    def __init__(self, kin: MHRKinematics, lam_pose: float = 30.0, lam_t: float = 1e-2, iters: int = 1,
                 ns_iters: int = 10, damping: float = 1e-3):
        super().__init__()
        self.kin = kin
        self.register_buffer("prior", torch.cat([torch.full((N_POSE,), lam_pose), torch.full((3,), lam_t)]))
        self.register_buffer("eye", torch.eye(N_POSE + 3)[None])
        self.register_buffer("zero", torch.zeros(1))
        self.iters, self.ns_iters, self.damping = iters, ns_iters, damping

    def forward(self, mp, t, target, sigma, cam):
        """mp (B, 204), t (B, 3), target / sigma (B, 30, 2), cam (B, 3) = (focal, cx, cy), all in one unit.

        Residuals and Jacobian are scaled by 1/16 and the prior by 1/256 (the same problem): with a 1 % of the crop
        sigma floor the weighted Jacobian reaches ~100 per entry, and J^T J would overflow fp16 (65504) unscaled."""
        inv_sigma = (1.0 / 16.0) / sigma
        x0 = torch.cat([mp[:, POSE], t], 1)
        x, mu = x0, x0[:, :1] * 0 + self.damping
        prior, eye = self.prior * (1.0 / 256.0), self.eye
        for i in range(self.iters):
            m = torch.cat([mp[:, :3], x[:, :N_POSE], mp[:, 68:]], 1)
            pos, q, qx, qy = self.kin(m)
            X = self.kin.keypoints(pos) + x[:, None, N_POSE:]
            z = X[..., 2:].clamp_min(1e-3)
            f = cam[:, None, :1]
            uv = f * X[..., :2] / z + cam[:, None, 1:]
            r = ((uv - target) * inv_sigma).flatten(1)
            Jk = self.kin.keypoint_jacobian(pos, q, qx, qy)                       # (B, 30, 3, 65)
            zero = z * 0                                                          # d(u, v) / d(X, Y, Z)
            du = torch.stack((f / z, zero, -f * X[..., :1] / z ** 2), -1)[..., 0, :]
            dv = torch.stack((zero, f / z, -f * X[..., 1:2] / z ** 2), -1)[..., 0, :]
            Jp = torch.stack((du, dv), 2)                                         # (B, 30, 2, 3)
            Jx = (torch.cat([Jp @ Jk, Jp], -1) * inv_sigma[..., None]).flatten(1, 2)   # (B, 60, 68)
            JtJ = Jx.transpose(1, 2) @ Jx
            H = JtJ + eye * (prior + mu * (JtJ * eye).sum(-1))[:, None, :]
            g = (Jx.transpose(1, 2) @ r[..., None])[..., 0] + prior * (x - x0)
            step = newton_schulz_solve(H, g, self.ns_iters, eye)
            x = x - torch.where(step == step, step, self.zero)                    # a NaN step is dropped
        return torch.cat([mp[:, :3], x[:, :N_POSE], mp[:, 68:]], 1), x[:, N_POSE:]


class FitHead(nn.Module):
    """One person's network outputs on ``n_crops`` crops -> fused 2D keypoints -> KinematicFit.

    Crop 0 is the main crop (its MHR parameters and camera translation start the fit). With two crops, crop 1 is
    the zoomed-in one: each body keypoint's 2D is the inverse-variance mean of the two SimCC estimates, keypoints
    outside crop 1 using crop 0 alone. ``crop_box`` (n_crops, 4) is each crop's pixel square (x1, y1, w, h) in the
    full image, i.e. what the 288 px input was resized from; ``cam`` (1, 3) is (focal, cx, cy).
    """

    def __init__(self, kin: MHRKinematics, bin_centers: torch.Tensor, n_crops: int, sigma_floor: float = 0.01,
                 **fit_kwargs):
        super().__init__()
        self.fit = KinematicFit(kin, **fit_kwargs)
        self.register_buffer("centers", bin_centers.detach().float().clone())
        self.register_buffer("body", torch.as_tensor(BODY_KP))
        self.n_crops, self.sigma_floor = n_crops, sigma_floor

    def forward(self, mhr_params, cam_trans, logits, crop_box, cam):
        """Returns the fitted (1, 204) parameters and (1, 3) translation, the fused (1, 70, 2) 2D in units of crop 0
        (origin at its centre), and crop 0's centre (1, 2) and side (1, 1) in pixels: ``pixels = c0 + 2D * s0``.

        Every 2D quantity is expressed in crop-0 units so that it stays near 1 whatever the frame resolution: in
        pixels, a broad SimCC on a 2000 px close-up crop gives sigma^2 ~ 1e5, beyond fp16 (65504), and absolute
        coordinates near 4000 px keep only 2 px of fp16 precision."""
        prob = torch.softmax(logits, -1)                                       # (N, 70, 2, bins)
        mu = (prob * self.centers).sum(-1)                                     # crop units, [-1, 1] = the crop
        sd = torch.sqrt((prob * (self.centers - mu[..., None]) ** 2).sum(-1).clamp_min(1e-6))
        s0 = crop_box[:1, 2:3]
        c0 = crop_box[:1, :2] + 0.5 * crop_box[:1, 2:]
        rel = (crop_box[:, None, 2:] / s0)                                     # (N, 1, 2) crop size / crop 0's
        off = (crop_box[:, None, :2] + 0.5 * crop_box[:, None, 2:] - c0) / s0  # (N, 1, 2) crop centre, crop-0 units
        xy = off + mu * 0.5 * rel                                              # (N, 70, 2)
        sig = torch.clamp(sd, min=2 * self.sigma_floor) * 0.5 * rel            # floor: sigma_floor of the crop side
        if self.n_crops == 2:
            inside = (mu[1].abs() <= 1).all(-1, keepdim=True).to(xy.dtype)    # keypoint within the zoomed crop
            r2 = (sig[0] / sig[1]) ** 2 * inside                               # w1 / w0
            fused = xy[0] + (r2 / (1 + r2)) * (xy[1] - xy[0])                  # inverse-variance mean
            sigma = sig[0] * torch.rsqrt(1 + r2)
        else:
            fused, sigma = xy[0], sig[0]
        cam_n = torch.cat([cam[:, :1] / s0, (cam[:, 1:] - c0) / s0], 1)
        mp, t = self.fit(mhr_params[:1], cam_trans[:1], fused[None].index_select(1, self.body),
                         sigma[None].index_select(1, self.body), cam_n)
        return mp, t, fused[None], c0, s0