Initial snapshot: hand motion pipeline (Dyn-HaMR + dex-retargeting + SPIDER)

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>

third_party/Dyn-HaMR/third-party/DROID-SLAM/droid_slam/geom/ba.py

@@ -0,0 +1,158 @@
+import lietorch
+import torch
+import torch.nn.functional as F
+
+from .chol import block_solve, schur_solve
+import geom.projective_ops as pops
+
+from torch_scatter import scatter_sum
+
+
+# utility functions for scattering ops
+def safe_scatter_add_mat(A, ii, jj, n, m):
+    v = (ii >= 0) & (jj >= 0) & (ii < n) & (jj < m)
+    return scatter_sum(A[:,v], ii[v]*m + jj[v], dim=1, dim_size=n*m)
+
+def safe_scatter_add_vec(b, ii, n):
+    v = (ii >= 0) & (ii < n)
+    return scatter_sum(b[:,v], ii[v], dim=1, dim_size=n)
+
+# apply retraction operator to inv-depth maps
+def disp_retr(disps, dz, ii):
+    ii = ii.to(device=dz.device)
+    return disps + scatter_sum(dz, ii, dim=1, dim_size=disps.shape[1])
+
+# apply retraction operator to poses
+def pose_retr(poses, dx, ii):
+    ii = ii.to(device=dx.device)
+    return poses.retr(scatter_sum(dx, ii, dim=1, dim_size=poses.shape[1]))
+
+
+def BA(target, weight, eta, poses, disps, intrinsics, ii, jj, fixedp=1, rig=1):
+    """ Full Bundle Adjustment """
+
+    B, P, ht, wd = disps.shape
+    N = ii.shape[0]
+    D = poses.manifold_dim
+
+    ### 1: commpute jacobians and residuals ###
+    coords, valid, (Ji, Jj, Jz) = pops.projective_transform(
+        poses, disps, intrinsics, ii, jj, jacobian=True)
+
+    r = (target - coords).view(B, N, -1, 1)
+    w = .001 * (valid * weight).view(B, N, -1, 1)
+
+    ### 2: construct linear system ###
+    Ji = Ji.reshape(B, N, -1, D)
+    Jj = Jj.reshape(B, N, -1, D)
+    wJiT = (w * Ji).transpose(2,3)
+    wJjT = (w * Jj).transpose(2,3)
+
+    Jz = Jz.reshape(B, N, ht*wd, -1)
+
+    Hii = torch.matmul(wJiT, Ji)
+    Hij = torch.matmul(wJiT, Jj)
+    Hji = torch.matmul(wJjT, Ji)
+    Hjj = torch.matmul(wJjT, Jj)
+
+    vi = torch.matmul(wJiT, r).squeeze(-1)
+    vj = torch.matmul(wJjT, r).squeeze(-1)
+
+    Ei = (wJiT.view(B,N,D,ht*wd,-1) * Jz[:,:,None]).sum(dim=-1)
+    Ej = (wJjT.view(B,N,D,ht*wd,-1) * Jz[:,:,None]).sum(dim=-1)
+
+    w = w.view(B, N, ht*wd, -1)
+    r = r.view(B, N, ht*wd, -1)
+    wk = torch.sum(w*r*Jz, dim=-1)
+    Ck = torch.sum(w*Jz*Jz, dim=-1)
+
+    kx, kk = torch.unique(ii, return_inverse=True)
+    M = kx.shape[0]
+
+    # only optimize keyframe poses
+    P = P // rig - fixedp
+    ii = ii // rig - fixedp
+    jj = jj // rig - fixedp
+
+    H = safe_scatter_add_mat(Hii, ii, ii, P, P) + \
+        safe_scatter_add_mat(Hij, ii, jj, P, P) + \
+        safe_scatter_add_mat(Hji, jj, ii, P, P) + \
+        safe_scatter_add_mat(Hjj, jj, jj, P, P)
+
+    E = safe_scatter_add_mat(Ei, ii, kk, P, M) + \
+        safe_scatter_add_mat(Ej, jj, kk, P, M)
+
+    v = safe_scatter_add_vec(vi, ii, P) + \
+        safe_scatter_add_vec(vj, jj, P)
+
+    C = safe_scatter_add_vec(Ck, kk, M)
+    w = safe_scatter_add_vec(wk, kk, M)
+
+    C = C + eta.view(*C.shape) + 1e-7
+
+    H = H.view(B, P, P, D, D)
+    E = E.view(B, P, M, D, ht*wd)
+
+    ### 3: solve the system ###
+    dx, dz = schur_solve(H, E, C, v, w)
+    
+    ### 4: apply retraction ###
+    poses = pose_retr(poses, dx, torch.arange(P) + fixedp)
+    disps = disp_retr(disps, dz.view(B,-1,ht,wd), kx)
+
+    disps = torch.where(disps > 10, torch.zeros_like(disps), disps)
+    disps = disps.clamp(min=0.0)
+
+    return poses, disps
+
+
+def MoBA(target, weight, eta, poses, disps, intrinsics, ii, jj, fixedp=1, rig=1):
+    """ Motion only bundle adjustment """
+
+    B, P, ht, wd = disps.shape
+    N = ii.shape[0]
+    D = poses.manifold_dim
+
+    ### 1: commpute jacobians and residuals ###
+    coords, valid, (Ji, Jj, Jz) = pops.projective_transform(
+        poses, disps, intrinsics, ii, jj, jacobian=True)
+
+    r = (target - coords).view(B, N, -1, 1)
+    w = .001 * (valid * weight).view(B, N, -1, 1)
+
+    ### 2: construct linear system ###
+    Ji = Ji.reshape(B, N, -1, D)
+    Jj = Jj.reshape(B, N, -1, D)
+    wJiT = (w * Ji).transpose(2,3)
+    wJjT = (w * Jj).transpose(2,3)
+
+    Hii = torch.matmul(wJiT, Ji)
+    Hij = torch.matmul(wJiT, Jj)
+    Hji = torch.matmul(wJjT, Ji)
+    Hjj = torch.matmul(wJjT, Jj)
+
+    vi = torch.matmul(wJiT, r).squeeze(-1)
+    vj = torch.matmul(wJjT, r).squeeze(-1)
+
+    # only optimize keyframe poses
+    P = P // rig - fixedp
+    ii = ii // rig - fixedp
+    jj = jj // rig - fixedp
+
+    H = safe_scatter_add_mat(Hii, ii, ii, P, P) + \
+        safe_scatter_add_mat(Hij, ii, jj, P, P) + \
+        safe_scatter_add_mat(Hji, jj, ii, P, P) + \
+        safe_scatter_add_mat(Hjj, jj, jj, P, P)
+
+    v = safe_scatter_add_vec(vi, ii, P) + \
+        safe_scatter_add_vec(vj, jj, P)
+    
+    H = H.view(B, P, P, D, D)
+
+    ### 3: solve the system ###
+    dx = block_solve(H, v)
+
+    ### 4: apply retraction ###
+    poses = pose_retr(poses, dx, torch.arange(P) + fixedp)
+    return poses
+

third_party/Dyn-HaMR/third-party/DROID-SLAM/droid_slam/geom/chol.py

@@ -0,0 +1,73 @@
+import torch
+import torch.nn.functional as F
+import geom.projective_ops as pops
+
+class CholeskySolver(torch.autograd.Function):
+    @staticmethod
+    def forward(ctx, H, b):
+        # don't crash training if cholesky decomp fails
+        try:
+            U = torch.linalg.cholesky(H)
+            xs = torch.cholesky_solve(b, U)
+            ctx.save_for_backward(U, xs)
+            ctx.failed = False
+        except Exception as e:
+            print(e)
+            ctx.failed = True
+            xs = torch.zeros_like(b)
+
+        return xs
+
+    @staticmethod
+    def backward(ctx, grad_x):
+        if ctx.failed:
+            return None, None
+
+        U, xs = ctx.saved_tensors
+        dz = torch.cholesky_solve(grad_x, U)
+        dH = -torch.matmul(xs, dz.transpose(-1,-2))
+
+        return dH, dz
+
+def block_solve(H, b, ep=0.1, lm=0.0001):
+    """ solve normal equations """
+    B, N, _, D, _ = H.shape
+    I = torch.eye(D).to(H.device)
+    H = H + (ep + lm*H) * I
+
+    H = H.permute(0,1,3,2,4)
+    H = H.reshape(B, N*D, N*D)
+    b = b.reshape(B, N*D, 1)
+
+    x = CholeskySolver.apply(H,b)
+    return x.reshape(B, N, D)
+
+
+def schur_solve(H, E, C, v, w, ep=0.1, lm=0.0001, sless=False):
+    """ solve using shur complement """
+    
+    B, P, M, D, HW = E.shape
+    H = H.permute(0,1,3,2,4).reshape(B, P*D, P*D)
+    E = E.permute(0,1,3,2,4).reshape(B, P*D, M*HW)
+    Q = (1.0 / C).view(B, M*HW, 1)
+
+    # damping
+    I = torch.eye(P*D).to(H.device)
+    H = H + (ep + lm*H) * I
+    
+    v = v.reshape(B, P*D, 1)
+    w = w.reshape(B, M*HW, 1)
+
+    Et = E.transpose(1,2)
+    S = H - torch.matmul(E, Q*Et)
+    v = v - torch.matmul(E, Q*w)
+
+    dx = CholeskySolver.apply(S, v)
+    if sless:
+        return dx.reshape(B, P, D)
+
+    dz = Q * (w - Et @ dx)    
+    dx = dx.reshape(B, P, D)
+    dz = dz.reshape(B, M, HW)
+
+    return dx, dz

third_party/Dyn-HaMR/third-party/DROID-SLAM/droid_slam/geom/graph_utils.py

@@ -0,0 +1,113 @@
+
+import torch
+import numpy as np
+from collections import OrderedDict
+
+import lietorch
+from data_readers.rgbd_utils import compute_distance_matrix_flow, compute_distance_matrix_flow2
+
+
+def graph_to_edge_list(graph):
+    ii, jj, kk = [], [], []
+    for s, u in enumerate(graph):
+        for v in graph[u]:
+            ii.append(u)
+            jj.append(v)
+            kk.append(s)
+
+    ii = torch.as_tensor(ii)
+    jj = torch.as_tensor(jj)
+    kk = torch.as_tensor(kk)
+    return ii, jj, kk
+
+def keyframe_indicies(graph):
+    return torch.as_tensor([u for u in graph])
+
+def meshgrid(m, n, device='cuda'):
+    ii, jj = torch.meshgrid(torch.arange(m), torch.arange(n))
+    return ii.reshape(-1).to(device), jj.reshape(-1).to(device)
+
+def neighbourhood_graph(n, r):
+    ii, jj = meshgrid(n, n)
+    d = (ii - jj).abs()
+    keep = (d >= 1) & (d <= r)
+    return ii[keep], jj[keep]
+
+
+def build_frame_graph(poses, disps, intrinsics, num=16, thresh=24.0, r=2):
+    """ construct a frame graph between co-visible frames """
+    N = poses.shape[1]
+    poses = poses[0].cpu().numpy()
+    disps = disps[0][:,3::8,3::8].cpu().numpy()
+    intrinsics = intrinsics[0].cpu().numpy() / 8.0
+    d = compute_distance_matrix_flow(poses, disps, intrinsics)
+
+    count = 0
+    graph = OrderedDict()
+    
+    for i in range(N):
+        graph[i] = []
+        d[i,i] = np.inf
+        for j in range(i-r, i+r+1):
+            if 0 <= j < N and i != j:
+                graph[i].append(j)
+                d[i,j] = np.inf
+                count += 1
+
+    while count < num:
+        ix = np.argmin(d)
+        i, j = ix // N, ix % N
+
+        if d[i,j] < thresh:
+            graph[i].append(j)
+            d[i,j] = np.inf
+            count += 1
+        else:
+            break
+    
+    return graph
+
+
+
+def build_frame_graph_v2(poses, disps, intrinsics, num=16, thresh=24.0, r=2):
+    """ construct a frame graph between co-visible frames """
+    N = poses.shape[1]
+    # poses = poses[0].cpu().numpy()
+    # disps = disps[0].cpu().numpy()
+    # intrinsics = intrinsics[0].cpu().numpy()
+    d = compute_distance_matrix_flow2(poses, disps, intrinsics)
+
+    # import matplotlib.pyplot as plt
+    # plt.imshow(d)
+    # plt.show()
+
+    count = 0
+    graph = OrderedDict()
+    
+    for i in range(N):
+        graph[i] = []
+        d[i,i] = np.inf
+        for j in range(i-r, i+r+1):
+            if 0 <= j < N and i != j:
+                graph[i].append(j)
+                d[i,j] = np.inf
+                count += 1
+
+    while 1:
+        ix = np.argmin(d)
+        i, j = ix // N, ix % N
+
+        if d[i,j] < thresh:
+            graph[i].append(j)
+
+            for i1 in range(i-1, i+2):
+                for j1 in range(j-1, j+2):
+                    if 0 <= i1 < N and 0 <= j1 < N:
+                        d[i1, j1] = np.inf
+
+            count += 1
+        else:
+            break
+    
+    return graph
+

third_party/Dyn-HaMR/third-party/DROID-SLAM/droid_slam/geom/losses.py

@@ -0,0 +1,118 @@
+from collections import OrderedDict
+import numpy as np
+import torch
+from lietorch import SO3, SE3, Sim3
+from .graph_utils import graph_to_edge_list
+from .projective_ops import projective_transform
+
+
+def pose_metrics(dE):
+    """ Translation/Rotation/Scaling metrics from Sim3 """
+    t, q, s = dE.data.split([3, 4, 1], -1)
+    ang = SO3(q).log().norm(dim=-1)
+
+    # convert radians to degrees
+    r_err = (180 / np.pi) * ang
+    t_err = t.norm(dim=-1)
+    s_err = (s - 1.0).abs()
+    return r_err, t_err, s_err
+
+
+def fit_scale(Ps, Gs):
+    b = Ps.shape[0]
+    t1 = Ps.data[...,:3].detach().reshape(b, -1)
+    t2 = Gs.data[...,:3].detach().reshape(b, -1)
+
+    s = (t1*t2).sum(-1) / ((t2*t2).sum(-1) + 1e-8)
+    return s
+
+
+def geodesic_loss(Ps, Gs, graph, gamma=0.9, do_scale=True):
+    """ Loss function for training network """
+
+    # relative pose
+    ii, jj, kk = graph_to_edge_list(graph)
+    dP = Ps[:,jj] * Ps[:,ii].inv()
+
+    n = len(Gs)
+    geodesic_loss = 0.0
+
+    for i in range(n):
+        w = gamma ** (n - i - 1)
+        dG = Gs[i][:,jj] * Gs[i][:,ii].inv()
+
+        if do_scale:
+            s = fit_scale(dP, dG)
+            dG = dG.scale(s[:,None])
+        
+        # pose error
+        d = (dG * dP.inv()).log()
+
+        if isinstance(dG, SE3):
+            tau, phi = d.split([3,3], dim=-1)
+            geodesic_loss += w * (
+                tau.norm(dim=-1).mean() + 
+                phi.norm(dim=-1).mean())
+
+        elif isinstance(dG, Sim3):
+            tau, phi, sig = d.split([3,3,1], dim=-1)
+            geodesic_loss += w * (
+                tau.norm(dim=-1).mean() + 
+                phi.norm(dim=-1).mean() + 
+                0.05 * sig.norm(dim=-1).mean())
+            
+        dE = Sim3(dG * dP.inv()).detach()
+        r_err, t_err, s_err = pose_metrics(dE)
+
+    metrics = {
+        'rot_error': r_err.mean().item(),
+        'tr_error': t_err.mean().item(),
+        'bad_rot': (r_err < .1).float().mean().item(),
+        'bad_tr': (t_err < .01).float().mean().item(),
+    }
+
+    return geodesic_loss, metrics
+
+
+def residual_loss(residuals, gamma=0.9):
+    """ loss on system residuals """
+    residual_loss = 0.0
+    n = len(residuals)
+
+    for i in range(n):
+        w = gamma ** (n - i - 1)
+        residual_loss += w * residuals[i].abs().mean()
+
+    return residual_loss, {'residual': residual_loss.item()}
+
+
+def flow_loss(Ps, disps, poses_est, disps_est, intrinsics, graph, gamma=0.9):
+    """ optical flow loss """
+
+    N = Ps.shape[1]
+    graph = OrderedDict()
+    for i in range(N):
+        graph[i] = [j for j in range(N) if abs(i-j)==1]
+
+    ii, jj, kk = graph_to_edge_list(graph)
+    coords0, val0 = projective_transform(Ps, disps, intrinsics, ii, jj)
+    val0 = val0 * (disps[:,ii] > 0).float().unsqueeze(dim=-1)
+
+    n = len(poses_est)
+    flow_loss = 0.0
+
+    for i in range(n):
+        w = gamma ** (n - i - 1)
+        coords1, val1 = projective_transform(poses_est[i], disps_est[i], intrinsics, ii, jj)
+
+        v = (val0 * val1).squeeze(dim=-1)
+        epe = v * (coords1 - coords0).norm(dim=-1)
+        flow_loss += w * epe.mean()
+
+    epe = epe.reshape(-1)[v.reshape(-1) > 0.5]
+    metrics = {
+        'f_error': epe.mean().item(),
+        '1px': (epe<1.0).float().mean().item(),
+    }
+
+    return flow_loss, metrics

third_party/Dyn-HaMR/third-party/DROID-SLAM/droid_slam/geom/projective_ops.py

@@ -0,0 +1,139 @@
+import torch
+import torch.nn.functional as F
+
+from lietorch import SE3, Sim3
+
+MIN_DEPTH = 0.2
+
+def extract_intrinsics(intrinsics):
+    return intrinsics[...,None,None,:].unbind(dim=-1)
+
+def coords_grid(ht, wd, **kwargs):
+    y, x = torch.meshgrid(
+        torch.arange(ht).to(**kwargs).float(),
+        torch.arange(wd).to(**kwargs).float())
+
+    return torch.stack([x, y], dim=-1)
+
+def iproj(disps, intrinsics, jacobian=False):
+    """ pinhole camera inverse projection """
+    ht, wd = disps.shape[2:]
+    fx, fy, cx, cy = extract_intrinsics(intrinsics)
+    
+    y, x = torch.meshgrid(
+        torch.arange(ht).to(disps.device).float(),
+        torch.arange(wd).to(disps.device).float())
+
+    i = torch.ones_like(disps)
+    X = (x - cx) / fx
+    Y = (y - cy) / fy
+    pts = torch.stack([X, Y, i, disps], dim=-1)
+
+    if jacobian:
+        J = torch.zeros_like(pts)
+        J[...,-1] = 1.0
+        return pts, J
+
+    return pts, None
+
+def proj(Xs, intrinsics, jacobian=False, return_depth=False):
+    """ pinhole camera projection """
+    fx, fy, cx, cy = extract_intrinsics(intrinsics)
+    X, Y, Z, D = Xs.unbind(dim=-1)
+
+    Z = torch.where(Z < 0.5*MIN_DEPTH, torch.ones_like(Z), Z)
+    d = 1.0 / Z
+
+    x = fx * (X * d) + cx
+    y = fy * (Y * d) + cy
+    if return_depth:
+        coords = torch.stack([x, y, D*d], dim=-1)
+    else:
+        coords = torch.stack([x, y], dim=-1)
+
+    if jacobian:
+        B, N, H, W = d.shape
+        o = torch.zeros_like(d)
+        proj_jac = torch.stack([
+             fx*d,     o, -fx*X*d*d,  o,
+                o,  fy*d, -fy*Y*d*d,  o,
+                # o,     o,    -D*d*d,  d,
+        ], dim=-1).view(B, N, H, W, 2, 4)
+
+        return coords, proj_jac
+
+    return coords, None
+
+def actp(Gij, X0, jacobian=False):
+    """ action on point cloud """
+    X1 = Gij[:,:,None,None] * X0
+    
+    if jacobian:
+        X, Y, Z, d = X1.unbind(dim=-1)
+        o = torch.zeros_like(d)
+        B, N, H, W = d.shape
+
+        if isinstance(Gij, SE3):
+            Ja = torch.stack([
+                d,  o,  o,  o,  Z, -Y,
+                o,  d,  o, -Z,  o,  X, 
+                o,  o,  d,  Y, -X,  o,
+                o,  o,  o,  o,  o,  o,
+            ], dim=-1).view(B, N, H, W, 4, 6)
+
+        elif isinstance(Gij, Sim3):
+            Ja = torch.stack([
+                d,  o,  o,  o,  Z, -Y,  X,
+                o,  d,  o, -Z,  o,  X,  Y,
+                o,  o,  d,  Y, -X,  o,  Z,
+                o,  o,  o,  o,  o,  o,  o
+            ], dim=-1).view(B, N, H, W, 4, 7)
+
+        return X1, Ja
+
+    return X1, None
+
+def projective_transform(poses, depths, intrinsics, ii, jj, jacobian=False, return_depth=False):
+    """ map points from ii->jj """
+
+    # inverse project (pinhole)
+    X0, Jz = iproj(depths[:,ii], intrinsics[:,ii], jacobian=jacobian)
+    
+    # transform
+    Gij = poses[:,jj] * poses[:,ii].inv()
+
+    Gij.data[:,ii==jj] = torch.as_tensor([-0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], device="cuda")
+    X1, Ja = actp(Gij, X0, jacobian=jacobian)
+    
+    # project (pinhole)
+    x1, Jp = proj(X1, intrinsics[:,jj], jacobian=jacobian, return_depth=return_depth)
+
+    # exclude points too close to camera
+    valid = ((X1[...,2] > MIN_DEPTH) & (X0[...,2] > MIN_DEPTH)).float()
+    valid = valid.unsqueeze(-1)
+
+    if jacobian:
+        # Ji transforms according to dual adjoint
+        Jj = torch.matmul(Jp, Ja)
+        Ji = -Gij[:,:,None,None,None].adjT(Jj)
+
+        Jz = Gij[:,:,None,None] * Jz
+        Jz = torch.matmul(Jp, Jz.unsqueeze(-1))
+
+        return x1, valid, (Ji, Jj, Jz)
+
+    return x1, valid
+
+def induced_flow(poses, disps, intrinsics, ii, jj):
+    """ optical flow induced by camera motion """
+
+    ht, wd = disps.shape[2:]
+    y, x = torch.meshgrid(
+        torch.arange(ht).to(disps.device).float(),
+        torch.arange(wd).to(disps.device).float())
+
+    coords0 = torch.stack([x, y], dim=-1)
+    coords1, valid = projective_transform(poses, disps, intrinsics, ii, jj, False)
+
+    return coords1[...,:2] - coords0, valid
+