    for notAssignedId in notAssignedIds:
        falsePositives.append(notAssignedId)
    gtIds = np.arange(len(gtList))
    # returns values of gtIds that are not in gtAssigned
    notAssignedIdsGt = np.setdiff1d(gtIds, gtAssigned)

    # handle false negatives/misses
    for notAssignedIdGt in notAssignedIdsGt:
        if not(valid is None):  # if using the new matching
            if valid[notAssignedIdGt]:
                #print(imgPath + ': miss')
                misses.append(notAssignedIdGt)
            else:
                excludedGtBecauseInvalid.append(notAssignedIdGt)
        else:
            #print(imgPath + ': miss')
            misses.append(notAssignedIdGt)

    return bestMatch, falsePositives, misses  # tuples are (idx_pred_kps, idx_gt_kps)

def get_bbx_overlap(p1, p2):
    """
    Code modifed from https://github.com/Arthur151/ROMP/blob/4eebd3647f57d291d26423e51f0d514ff7197cb3/simple_romp/trace2/evaluation/eval_3DPW.py#L185
    """
    min_p1 = np.min(p1, axis=0)
    min_p2 = np.min(p2, axis=0)
    max_p1 = np.max(p1, axis=0)
    max_p2 = np.max(p2, axis=0)

    bb1 = {}
    bb2 = {}

    bb1['x1'] = min_p1[0]
    bb1['x2'] = max_p1[0]
    bb1['y1'] = min_p1[1]
    bb1['y2'] = max_p1[1]
    bb2['x1'] = min_p2[0]
    bb2['x2'] = max_p2[0]
    bb2['y1'] = min_p2[1]
    bb2['y2'] = max_p2[1]

    assert bb1['x1'] < bb1['x2']
    assert bb1['y1'] < bb1['y2']
    assert bb2['x1'] < bb2['x2']
    assert bb2['y1'] < bb2['y2']
    # determine the coordinates of the intersection rectangle
    x_left = max(bb1['x1'], bb2['x1'])
    y_top = max(bb1['y1'], bb2['y1'])
    x_right = min(bb1['x2'], bb2['x2'])
    y_bottom = min(bb1['y2'], bb2['y2'])

    # The intersection of two axis-aligned bounding boxes is always an
    # axis-aligned bounding box
    intersection_area = max(0, x_right - x_left + 1) * \
        max(0, y_bottom - y_top + 1)

    # compute the area of both AABBs
    bb1_area = (bb1['x2'] - bb1['x1'] + 1) * (bb1['y2'] - bb1['y1'] + 1)
    bb2_area = (bb2['x2'] - bb2['x1'] + 1) * (bb2['y2'] - bb2['y1'] + 1)

    # compute the intersection over union by taking the intersection
    # area and dividing it by the sum of prediction + ground-truth
    # areas - the interesection area
    iou = intersection_area / float(bb1_area + bb2_area - intersection_area)

    return iou

def avg_per_human(lst, default=0.0):
    values = [x for arr in lst for x in np.array(arr).flatten()]
    return np.mean(values) if values else default

def extract_metrics(file_path):
    with open(file_path, "r") as file:
        content = file.read()

    matches = re.findall(
        r'n_human: (\d+).*?PVE: ([\d.]+), PA-PVE: ([\d.]+), Metric-PVE: ([\d.]+), MPJPE: ([\d.]+), PA-MPJPE: ([\d.]+), Metric-MPJPE: ([\d.]+), RootError: ([\d.]+), W-MPJPE: ([\d.]+), WA-MPJPE: ([\d.]+), RTE: ([\d.]+), Scaled-RTE: ([\d.]+), Jitter: ([\d.]+), Foot-Sliding: ([\d.]+), Precision: ([\d.]+), Recall: ([\d.]+), F1-Score: ([\d.]+)',
        content
    )

    if not matches:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    total_humans = 0
    weighted_sums = [0.0] * 16


    x_right = min(bb1['x2'], bb2['x2'])
    y_bottom = min(bb1['y2'], bb2['y2'])

    # The intersection of two axis-aligned bounding boxes is always an
    # axis-aligned bounding box
    intersection_area = max(0, x_right - x_left + 1) * \
        max(0, y_bottom - y_top + 1)

    # compute the area of both AABBs
    bb1_area = (bb1['x2'] - bb1['x1'] + 1) * (bb1['y2'] - bb1['y1'] + 1)
    bb2_area = (bb2['x2'] - bb2['x1'] + 1) * (bb2['y2'] - bb2['y1'] + 1)

    # compute the intersection over union by taking the intersection
    # area and dividing it by the sum of prediction + ground-truth
    # areas - the interesection area
    iou = intersection_area / float(bb1_area + bb2_area - intersection_area)

    return iou

def avg_per_human(lst, default=0.0):
    values = [x for arr in lst for x in np.array(arr).flatten()]
    return np.mean(values) if values else default

def extract_metrics(file_path):
    with open(file_path, "r") as file:
        content = file.read()

    matches = re.findall(
        r'n_human: (\d+).*?PVE: ([\d.]+), PA-PVE: ([\d.]+), Metric-PVE: ([\d.]+), MPJPE: ([\d.]+), PA-MPJPE: ([\d.]+), Metric-MPJPE: ([\d.]+), RootError: ([\d.]+), W-MPJPE: ([\d.]+), WA-MPJPE: ([\d.]+), RTE: ([\d.]+), Scaled-RTE: ([\d.]+), Jitter: ([\d.]+), Foot-Sliding: ([\d.]+), Precision: ([\d.]+), Recall: ([\d.]+), F1-Score: ([\d.]+)',
        content
    )

    if not matches:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    total_humans = 0
    weighted_sums = [0.0] * 16

    for match in matches:
        n_human = int(match[0])
        metrics = [float(x) for x in match[1:]]


    # axis-aligned bounding box
    intersection_area = max(0, x_right - x_left + 1) * \
        max(0, y_bottom - y_top + 1)

    # compute the area of both AABBs
    bb1_area = (bb1['x2'] - bb1['x1'] + 1) * (bb1['y2'] - bb1['y1'] + 1)
    bb2_area = (bb2['x2'] - bb2['x1'] + 1) * (bb2['y2'] - bb2['y1'] + 1)

    # compute the intersection over union by taking the intersection
    # area and dividing it by the sum of prediction + ground-truth
    # areas - the interesection area
    iou = intersection_area / float(bb1_area + bb2_area - intersection_area)

    return iou

def avg_per_human(lst, default=0.0):
    values = [x for arr in lst for x in np.array(arr).flatten()]
    return np.mean(values) if values else default

def extract_metrics(file_path):
    with open(file_path, "r") as file:
        content = file.read()

    matches = re.findall(
        r'n_human: (\d+).*?PVE: ([\d.]+), PA-PVE: ([\d.]+), Metric-PVE: ([\d.]+), MPJPE: ([\d.]+), PA-MPJPE: ([\d.]+), Metric-MPJPE: ([\d.]+), RootError: ([\d.]+), W-MPJPE: ([\d.]+), WA-MPJPE: ([\d.]+), RTE: ([\d.]+), Scaled-RTE: ([\d.]+), Jitter: ([\d.]+), Foot-Sliding: ([\d.]+), Precision: ([\d.]+), Recall: ([\d.]+), F1-Score: ([\d.]+)',
        content
    )

    if not matches:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    total_humans = 0
    weighted_sums = [0.0] * 16

    for match in matches:
        n_human = int(match[0])
        metrics = [float(x) for x in match[1:]]

        total_humans += n_human
        for i, metric in enumerate(metrics):
            weighted_sums[i] += metric * n_human

    if total_humans == 0:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    averages = [ws / total_humans for ws in weighted_sums]

    return total_humans, *averages

def process_directory(directory):
    results = []
    for root, _, files in os.walk(directory):
        for file in sorted(files):
            if file.endswith(".txt"):
                file_path = os.path.join(root, file)
                result = extract_metrics(file_path)
                if result[0] > 0:
                    results.append(result)
    return results

def calculate_averages(results):
    if not results:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    total_humans = sum(r[0] for r in results)
    if total_humans == 0:


    total_humans = 0
    weighted_sums = [0.0] * 16

    for match in matches:
        n_human = int(match[0])
        metrics = [float(x) for x in match[1:]]

        total_humans += n_human
        for i, metric in enumerate(metrics):
            weighted_sums[i] += metric * n_human

    if total_humans == 0:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    averages = [ws / total_humans for ws in weighted_sums]

    return total_humans, *averages

def process_directory(directory):
    results = []
    for root, _, files in os.walk(directory):
        for file in sorted(files):
            if file.endswith(".txt"):
                file_path = os.path.join(root, file)
                result = extract_metrics(file_path)
                if result[0] > 0:
                    results.append(result)
    return results

def calculate_averages(results):
    if not results:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    total_humans = sum(r[0] for r in results)
    if total_humans == 0:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    weighted_sums = [sum(r[i] * r[0] for r in results) for i in range(1, 17)]
    averages = [ws / total_humans for ws in weighted_sums]

    return total_humans, *averages

def visualize(
    save_dir, img_path, view, gt_v3d_c, pred_v3d_c,
    K_to_proj, gt_K, bestMatch, smpl_face
):
    from dust3r.utils import render_meshes, denormalize_rgb


    if total_humans == 0:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    averages = [ws / total_humans for ws in weighted_sums]

    return total_humans, *averages

def process_directory(directory):
    results = []
    for root, _, files in os.walk(directory):
        for file in sorted(files):
            if file.endswith(".txt"):
                file_path = os.path.join(root, file)
                result = extract_metrics(file_path)
                if result[0] > 0:
                    results.append(result)
    return results

def calculate_averages(results):
    if not results:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    total_humans = sum(r[0] for r in results)
    if total_humans == 0:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    weighted_sums = [sum(r[i] * r[0] for r in results) for i in range(1, 17)]
    averages = [ws / total_humans for ws in weighted_sums]

    return total_humans, *averages

def visualize(
    save_dir, img_path, view, gt_v3d_c, pred_v3d_c,
    K_to_proj, gt_K, bestMatch, smpl_face
):
    from dust3r.utils import render_meshes, denormalize_rgb
    from viser_utils import get_color
    from PIL import Image

    os.makedirs(save_dir, exist_ok=True)
    n_humans_i = pred_v3d_c.shape[0]

    # image
    img_array = denormalize_rgb(view.cpu().numpy())

    focal = K_to_proj[[0,1],[0,1]].cpu().numpy()
    princpt = K_to_proj[[0,1],[-1,-1]].cpu().numpy()
    gt_focal = gt_K[[0,1],[0,1]].cpu().numpy()
    gt_princpt = gt_K[[0,1],[-1,-1]].cpu().numpy()


visualize

                file_path = os.path.join(root, file)
                result = extract_metrics(file_path)
                if result[0] > 0:
                    results.append(result)
    return results

def calculate_averages(results):
    if not results:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    total_humans = sum(r[0] for r in results)
    if total_humans == 0:
        return 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

    weighted_sums = [sum(r[i] * r[0] for r in results) for i in range(1, 17)]
    averages = [ws / total_humans for ws in weighted_sums]

    return total_humans, *averages

def visualize(
    save_dir, img_path, view, gt_v3d_c, pred_v3d_c,
    K_to_proj, gt_K, bestMatch, smpl_face
):
    from dust3r.utils import render_meshes, denormalize_rgb
    from viser_utils import get_color
    from PIL import Image

    os.makedirs(save_dir, exist_ok=True)
    n_humans_i = pred_v3d_c.shape[0]

    # image
    img_array = denormalize_rgb(view.cpu().numpy())

    focal = K_to_proj[[0,1],[0,1]].cpu().numpy()
    princpt = K_to_proj[[0,1],[-1,-1]].cpu().numpy()
    gt_focal = gt_K[[0,1],[0,1]].cpu().numpy()
    gt_princpt = gt_K[[0,1],[-1,-1]].cpu().numpy()

    gt_color_indices = list(range(gt_v3d_c.shape[0]))
    pred_color_indices = [-1] * n_humans_i
    for (pid, gid) in bestMatch:
        pred_color_indices[pid] = gt_color_indices[gid]

    next_color_idx = max(gt_color_indices) + 1 if gt_color_indices else 0
    for i in range(len(pred_color_indices)):
        if pred_color_indices[i] == -1:
            pred_color_indices[i] = next_color_idx
            next_color_idx += 1

    # gt
    gt_verts, gt_faces = [], []
    for j in range(gt_v3d_c.shape[0]):
        gt_verts.append(gt_v3d_c[j].cpu().numpy().reshape(-1,3))
        gt_faces.append(smpl_face)

    gt_colors = [get_color(gt_color_indices[j])/255 for j in range(len(gt_verts))]
    gt_rend_array = render_meshes(img_array.copy(),
                                    gt_verts,
                                    gt_faces,
                                    {'focal': gt_focal, 'princpt': gt_princpt},
                                    color=gt_colors)

    # pred
    pred_verts, pred_faces = [], []
    for j in range(n_humans_i):
        pred_verts.append(pred_v3d_c[j].cpu().numpy().reshape(-1,3))
        pred_faces.append(smpl_face)

    pred_colors = [get_color(pred_color_indices[j])/255 for j in range(len(pred_verts))]
    pred_rend_array = render_meshes(img_array.copy(),
                                    pred_verts,
                                    pred_faces,
                                    {'focal': focal, 'princpt': princpt},
                                    color=pred_colors)

    img = np.concatenate([img_array, pred_rend_array, gt_rend_array], 1)
    Image.fromarray(img).save(
        os.path.join(
            f"{save_dir}/{os.path.splitext(os.path.basename(img_path))[0]}.jpg"
            ))

def write_log(log_path, dataset, seq, counter, metrics):
    with open(log_path, "a") as f:
        f.write(
            f"{dataset}-{seq: <16} |  "
            f"n_human: {counter['n_human']:06d} | "
            f"PVE: {avg_per_human(metrics['ca_pve']):.1f}, "
            f"PA-PVE: {avg_per_human(metrics['pa_pve']):.1f}, "
            f"Metric-PVE: {avg_per_human(metrics['me_pve']):.1f}, "
            f"MPJPE: {avg_per_human(metrics['ca_mpjpe']):.1f}, "
            f"PA-MPJPE: {avg_per_human(metrics['pa_mpjpe']):.1f}, "
            f"Metric-MPJPE: {avg_per_human(metrics['me_mpjpe']):.1f}, "
            f"RootError: {avg_per_human(metrics['rt_error']):.1f}, "
            f"W-MPJPE: {avg_per_human(metrics['wa2_mpjpe']):.1f}, "
            f"WA-MPJPE: {avg_per_human(metrics['waa_mpjpe']):.1f}, "
            f"RTE: {avg_per_human(metrics['rte']):.1f}, "
            f"Scaled-RTE: {avg_per_human(metrics['rte_scaled']):.1f}, "
            f"Jitter: {avg_per_human(metrics['jitter']):.1f}, "
            f"Foot-Sliding: {avg_per_human(metrics['fs']):.1f}, "
            f"Precision: {metrics['precision']:.1f}, "


    # pred
    pred_verts, pred_faces = [], []
    for j in range(n_humans_i):
        pred_verts.append(pred_v3d_c[j].cpu().numpy().reshape(-1,3))
        pred_faces.append(smpl_face)

    pred_colors = [get_color(pred_color_indices[j])/255 for j in range(len(pred_verts))]
    pred_rend_array = render_meshes(img_array.copy(),
                                    pred_verts,
                                    pred_faces,
                                    {'focal': focal, 'princpt': princpt},
                                    color=pred_colors)

    img = np.concatenate([img_array, pred_rend_array, gt_rend_array], 1)
    Image.fromarray(img).save(
        os.path.join(
            f"{save_dir}/{os.path.splitext(os.path.basename(img_path))[0]}.jpg"
            ))

def write_log(log_path, dataset, seq, counter, metrics):
    with open(log_path, "a") as f:
        f.write(
            f"{dataset}-{seq: <16} |  "
            f"n_human: {counter['n_human']:06d} | "
            f"PVE: {avg_per_human(metrics['ca_pve']):.1f}, "
            f"PA-PVE: {avg_per_human(metrics['pa_pve']):.1f}, "
            f"Metric-PVE: {avg_per_human(metrics['me_pve']):.1f}, "
            f"MPJPE: {avg_per_human(metrics['ca_mpjpe']):.1f}, "
            f"PA-MPJPE: {avg_per_human(metrics['pa_mpjpe']):.1f}, "
            f"Metric-MPJPE: {avg_per_human(metrics['me_mpjpe']):.1f}, "
            f"RootError: {avg_per_human(metrics['rt_error']):.1f}, "
            f"W-MPJPE: {avg_per_human(metrics['wa2_mpjpe']):.1f}, "
            f"WA-MPJPE: {avg_per_human(metrics['waa_mpjpe']):.1f}, "
            f"RTE: {avg_per_human(metrics['rte']):.1f}, "
            f"Scaled-RTE: {avg_per_human(metrics['rte_scaled']):.1f}, "
            f"Jitter: {avg_per_human(metrics['jitter']):.1f}, "
            f"Foot-Sliding: {avg_per_human(metrics['fs']):.1f}, "
            f"Precision: {metrics['precision']:.1f}, "
            f"Recall: {metrics['recall']:.1f}, "
            f"F1-Score: {metrics['f1_score']:.1f}\n"
        )

def get_summary_log(summary):
    """Generate summary log for evaluation results"""
    return (
        f"EVALUATION SUMMARY\n"
        f"{'='*7}EVALUATION SUMMARY{'='*7}\n"
        f"Total Humans: {summary[0]}\n"
        f"\n"
        f"Camera Coordinate Metrics (mm):\n"
        f"  PVE:         {summary[1]:6.1f}\n"
        f"  PA-PVE:      {summary[2]:6.1f}\n"
        f"  Metric-PVE:  {summary[3]:6.1f}\n"
        f"\n"
        f"  MPJPE:       {summary[4]:6.1f}\n"
        f"  PA-MPJPE:    {summary[5]:6.1f}\n"
        f"  Metric-MPJPE: {summary[6]:6.1f}\n"
        f"  Root-Error:  {summary[7]:6.1f}\n"
        f"\n"
        f"Global Coordinate Metrics (cm):\n"
        f"  W-MPJPE:     {summary[8]:6.1f}\n"


            f"{dataset}-{seq: <16} |  "
            f"n_human: {counter['n_human']:06d} | "
            f"PVE: {avg_per_human(metrics['ca_pve']):.1f}, "
            f"PA-PVE: {avg_per_human(metrics['pa_pve']):.1f}, "
            f"Metric-PVE: {avg_per_human(metrics['me_pve']):.1f}, "
            f"MPJPE: {avg_per_human(metrics['ca_mpjpe']):.1f}, "
            f"PA-MPJPE: {avg_per_human(metrics['pa_mpjpe']):.1f}, "
            f"Metric-MPJPE: {avg_per_human(metrics['me_mpjpe']):.1f}, "
            f"RootError: {avg_per_human(metrics['rt_error']):.1f}, "
            f"W-MPJPE: {avg_per_human(metrics['wa2_mpjpe']):.1f}, "
            f"WA-MPJPE: {avg_per_human(metrics['waa_mpjpe']):.1f}, "
            f"RTE: {avg_per_human(metrics['rte']):.1f}, "
            f"Scaled-RTE: {avg_per_human(metrics['rte_scaled']):.1f}, "
            f"Jitter: {avg_per_human(metrics['jitter']):.1f}, "
            f"Foot-Sliding: {avg_per_human(metrics['fs']):.1f}, "
            f"Precision: {metrics['precision']:.1f}, "
            f"Recall: {metrics['recall']:.1f}, "
            f"F1-Score: {metrics['f1_score']:.1f}\n"
        )

def get_summary_log(summary):
    """Generate summary log for evaluation results"""
    return (
        f"EVALUATION SUMMARY\n"
        f"{'='*7}EVALUATION SUMMARY{'='*7}\n"
        f"Total Humans: {summary[0]}\n"
        f"\n"
        f"Camera Coordinate Metrics (mm):\n"
        f"  PVE:         {summary[1]:6.1f}\n"
        f"  PA-PVE:      {summary[2]:6.1f}\n"
        f"  Metric-PVE:  {summary[3]:6.1f}\n"
        f"\n"
        f"  MPJPE:       {summary[4]:6.1f}\n"
        f"  PA-MPJPE:    {summary[5]:6.1f}\n"
        f"  Metric-MPJPE: {summary[6]:6.1f}\n"
        f"  Root-Error:  {summary[7]:6.1f}\n"
        f"\n"
        f"Global Coordinate Metrics (cm):\n"
        f"  W-MPJPE:     {summary[8]:6.1f}\n"
        f"  WA-MPJPE:    {summary[9]:6.1f}\n"
        f"  RTE:         {summary[10]:6.1f}\n"
        f"  Scaled-RTE:  {summary[11]:6.1f}\n"
        f"  Jitter:      {summary[12]:6.1f}\n"
        f"  Foot-Sliding: {summary[13]:6.1f}\n"
        f"\n"
        f"Detection Metrics (%):\n"
        f"  Precision:   {summary[14]:6.1f}\n"
        f"  Recall:      {summary[15]:6.1f}\n"
        f"  F1-Score:    {summary[16]:6.1f}\n"
        f"{'='*32}\n"
    )

# Evaluation metrics
# Code modifed from https://github.com/zju3dv/GVHMR/blob/088caff492aa38c2d82cea363b78a3c65a83118f/hmr4d/utils/eval/eval_utils.py

def compute_jpe(S1, S2):
    return torch.sqrt(((S1 - S2) ** 2).sum(dim=-1)).mean(dim=-1).numpy()

def compute_perjoint_jpe(S1, S2):
    return torch.sqrt(((S1 - S2) ** 2).sum(dim=-1)).numpy()

def batch_compute_similarity_transform_torch(S1, S2):
    '''
    Computes a similarity transform (sR, t) that takes
    a set of 3D points S1 (3 x N) closest to a set of 3D points S2,
    where R is an 3x3 rotation matrix, t 3x1 translation, s scale.
    i.e. solves the orthogonal Procrutes problem.
    '''
    S1 = S1.permute(0,2,1)
    S2 = S2.permute(0,2,1)


        f"\n"
        f"Global Coordinate Metrics (cm):\n"
        f"  W-MPJPE:     {summary[8]:6.1f}\n"
        f"  WA-MPJPE:    {summary[9]:6.1f}\n"
        f"  RTE:         {summary[10]:6.1f}\n"
        f"  Scaled-RTE:  {summary[11]:6.1f}\n"
        f"  Jitter:      {summary[12]:6.1f}\n"
        f"  Foot-Sliding: {summary[13]:6.1f}\n"
        f"\n"
        f"Detection Metrics (%):\n"
        f"  Precision:   {summary[14]:6.1f}\n"
        f"  Recall:      {summary[15]:6.1f}\n"
        f"  F1-Score:    {summary[16]:6.1f}\n"
        f"{'='*32}\n"
    )

# Evaluation metrics
# Code modifed from https://github.com/zju3dv/GVHMR/blob/088caff492aa38c2d82cea363b78a3c65a83118f/hmr4d/utils/eval/eval_utils.py

def compute_jpe(S1, S2):
    return torch.sqrt(((S1 - S2) ** 2).sum(dim=-1)).mean(dim=-1).numpy()

def compute_perjoint_jpe(S1, S2):
    return torch.sqrt(((S1 - S2) ** 2).sum(dim=-1)).numpy()

def batch_compute_similarity_transform_torch(S1, S2):
    '''
    Computes a similarity transform (sR, t) that takes
    a set of 3D points S1 (3 x N) closest to a set of 3D points S2,
    where R is an 3x3 rotation matrix, t 3x1 translation, s scale.
    i.e. solves the orthogonal Procrutes problem.
    '''
    S1 = S1.permute(0,2,1)
    S2 = S2.permute(0,2,1)
    transposed = True

    # 1. Remove mean.
    mu1 = S1.mean(axis=-1, keepdims=True)
    mu2 = S2.mean(axis=-1, keepdims=True)

    X1 = S1 - mu1


        f"  WA-MPJPE:    {summary[9]:6.1f}\n"
        f"  RTE:         {summary[10]:6.1f}\n"
        f"  Scaled-RTE:  {summary[11]:6.1f}\n"
        f"  Jitter:      {summary[12]:6.1f}\n"
        f"  Foot-Sliding: {summary[13]:6.1f}\n"
        f"\n"
        f"Detection Metrics (%):\n"
        f"  Precision:   {summary[14]:6.1f}\n"
        f"  Recall:      {summary[15]:6.1f}\n"
        f"  F1-Score:    {summary[16]:6.1f}\n"
        f"{'='*32}\n"
    )

# Evaluation metrics
# Code modifed from https://github.com/zju3dv/GVHMR/blob/088caff492aa38c2d82cea363b78a3c65a83118f/hmr4d/utils/eval/eval_utils.py

def compute_jpe(S1, S2):
    return torch.sqrt(((S1 - S2) ** 2).sum(dim=-1)).mean(dim=-1).numpy()

def compute_perjoint_jpe(S1, S2):
    return torch.sqrt(((S1 - S2) ** 2).sum(dim=-1)).numpy()

def batch_compute_similarity_transform_torch(S1, S2):
    '''
    Computes a similarity transform (sR, t) that takes
    a set of 3D points S1 (3 x N) closest to a set of 3D points S2,
    where R is an 3x3 rotation matrix, t 3x1 translation, s scale.
    i.e. solves the orthogonal Procrutes problem.
    '''
    S1 = S1.permute(0,2,1)
    S2 = S2.permute(0,2,1)
    transposed = True

    # 1. Remove mean.
    mu1 = S1.mean(axis=-1, keepdims=True)
    mu2 = S2.mean(axis=-1, keepdims=True)

    X1 = S1 - mu1
    X2 = S2 - mu2

    # 2. Compute variance of X1 used for scale.


        f"  Jitter:      {summary[12]:6.1f}\n"
        f"  Foot-Sliding: {summary[13]:6.1f}\n"
        f"\n"
        f"Detection Metrics (%):\n"
        f"  Precision:   {summary[14]:6.1f}\n"
        f"  Recall:      {summary[15]:6.1f}\n"
        f"  F1-Score:    {summary[16]:6.1f}\n"
        f"{'='*32}\n"
    )

# Evaluation metrics
# Code modifed from https://github.com/zju3dv/GVHMR/blob/088caff492aa38c2d82cea363b78a3c65a83118f/hmr4d/utils/eval/eval_utils.py

def compute_jpe(S1, S2):
    return torch.sqrt(((S1 - S2) ** 2).sum(dim=-1)).mean(dim=-1).numpy()

def compute_perjoint_jpe(S1, S2):
    return torch.sqrt(((S1 - S2) ** 2).sum(dim=-1)).numpy()

def batch_compute_similarity_transform_torch(S1, S2):
    '''
    Computes a similarity transform (sR, t) that takes
    a set of 3D points S1 (3 x N) closest to a set of 3D points S2,
    where R is an 3x3 rotation matrix, t 3x1 translation, s scale.
    i.e. solves the orthogonal Procrutes problem.
    '''
    S1 = S1.permute(0,2,1)
    S2 = S2.permute(0,2,1)
    transposed = True

    # 1. Remove mean.
    mu1 = S1.mean(axis=-1, keepdims=True)
    mu2 = S2.mean(axis=-1, keepdims=True)

    X1 = S1 - mu1
    X2 = S2 - mu2

    # 2. Compute variance of X1 used for scale.
    var1 = torch.sum(X1**2, dim=1).sum(dim=1)

    # 3. The outer product of X1 and X2.
    K = X1.bmm(X2.permute(0,2,1))

    # 4. Solution that Maximizes trace(R'K) is R=U*V', where U, V are
    # singular vectors of K.
    U, s, V = torch.svd(K)

    # Construct Z that fixes the orientation of R to get det(R)=1.
    Z = torch.eye(U.shape[1], device=S1.device).unsqueeze(0)
    Z = Z.repeat(U.shape[0],1,1)
    Z[:,-1, -1] *= torch.sign(torch.det(U.bmm(V.permute(0,2,1))))

    # Construct R.
    R = V.bmm(Z.bmm(U.permute(0,2,1)))

    # 5. Recover scale.
    scale = torch.cat([torch.trace(x).unsqueeze(0) for x in R.bmm(K)]) / var1

    # 6. Recover translation.
    t = mu2 - (scale.unsqueeze(-1).unsqueeze(-1) * (R.bmm(mu1)))

    # 7. Error:
    S1_hat = scale.unsqueeze(-1).unsqueeze(-1) * R.bmm(S1) + t

    if transposed:
        S1_hat = S1_hat.permute(0,2,1)

    return S1_hat

def batch_align_by_pelvis(data_list, pelvis_idxs):
    """
    Assumes data is given as [pred_j3d, target_j3d, pred_verts, target_verts].
    Each data is in shape of (batch, num_points, 3)
    Pelvis is notated as one / two joints indices.
    Align all data to the corresponding pelvis location.
    """

    pred_j3d, target_j3d, pred_verts, target_verts = data_list

    pred_pelvis = pred_j3d[:, pelvis_idxs].mean(dim=1, keepdims=True).clone()
    target_pelvis = target_j3d[:, pelvis_idxs].mean(dim=1, keepdims=True).clone()

    # Align to the pelvis
    pred_j3d = pred_j3d - pred_pelvis
    target_j3d = target_j3d - target_pelvis
    pred_verts = pred_verts - pred_pelvis
    target_verts = target_verts - target_pelvis


    Z = Z.repeat(U.shape[0],1,1)
    Z[:,-1, -1] *= torch.sign(torch.det(U.bmm(V.permute(0,2,1))))

    # Construct R.
    R = V.bmm(Z.bmm(U.permute(0,2,1)))

    # 5. Recover scale.
    scale = torch.cat([torch.trace(x).unsqueeze(0) for x in R.bmm(K)]) / var1

    # 6. Recover translation.
    t = mu2 - (scale.unsqueeze(-1).unsqueeze(-1) * (R.bmm(mu1)))

    # 7. Error:
    S1_hat = scale.unsqueeze(-1).unsqueeze(-1) * R.bmm(S1) + t

    if transposed:
        S1_hat = S1_hat.permute(0,2,1)

    return S1_hat

def batch_align_by_pelvis(data_list, pelvis_idxs):
    """
    Assumes data is given as [pred_j3d, target_j3d, pred_verts, target_verts].
    Each data is in shape of (batch, num_points, 3)
    Pelvis is notated as one / two joints indices.
    Align all data to the corresponding pelvis location.
    """

    pred_j3d, target_j3d, pred_verts, target_verts = data_list

    pred_pelvis = pred_j3d[:, pelvis_idxs].mean(dim=1, keepdims=True).clone()
    target_pelvis = target_j3d[:, pelvis_idxs].mean(dim=1, keepdims=True).clone()

    # Align to the pelvis
    pred_j3d = pred_j3d - pred_pelvis
    target_j3d = target_j3d - target_pelvis
    pred_verts = pred_verts - pred_pelvis
    target_verts = target_verts - target_pelvis

    return (pred_j3d, target_j3d, pred_verts, target_verts, pred_pelvis, target_pelvis)

def align_pcl(Y, X, weight=None, fixed_scale=False):
    """
    align similarity transform to align X with Y using umeyama method
    X' = s * R * X + t is aligned with Y
    :param Y (*, N, 3) first trajectory
    :param X (*, N, 3) second trajectory
    :param weight (*, N, 1) optional weight of valid correspondences
    :returns s (*, 1), R (*, 3, 3), t (*, 3)
    """
    *dims, N, _ = Y.shape
    N = torch.ones(*dims, 1, 1) * N

    if weight is not None:
        Y = Y * weight
        X = X * weight
        N = weight.sum(dim=-2, keepdim=True)  # (*, 1, 1)

    # subtract mean
    my = Y.sum(dim=-2) / N[..., 0]  # (*, 3)


    """
    Assumes data is given as [pred_j3d, target_j3d, pred_verts, target_verts].
    Each data is in shape of (batch, num_points, 3)
    Pelvis is notated as one / two joints indices.
    Align all data to the corresponding pelvis location.
    """

    pred_j3d, target_j3d, pred_verts, target_verts = data_list

    pred_pelvis = pred_j3d[:, pelvis_idxs].mean(dim=1, keepdims=True).clone()
    target_pelvis = target_j3d[:, pelvis_idxs].mean(dim=1, keepdims=True).clone()

    # Align to the pelvis
    pred_j3d = pred_j3d - pred_pelvis
    target_j3d = target_j3d - target_pelvis
    pred_verts = pred_verts - pred_pelvis
    target_verts = target_verts - target_pelvis

    return (pred_j3d, target_j3d, pred_verts, target_verts, pred_pelvis, target_pelvis)

def align_pcl(Y, X, weight=None, fixed_scale=False):
    """
    align similarity transform to align X with Y using umeyama method
    X' = s * R * X + t is aligned with Y
    :param Y (*, N, 3) first trajectory
    :param X (*, N, 3) second trajectory
    :param weight (*, N, 1) optional weight of valid correspondences
    :returns s (*, 1), R (*, 3, 3), t (*, 3)
    """
    *dims, N, _ = Y.shape
    N = torch.ones(*dims, 1, 1) * N

    if weight is not None:
        Y = Y * weight
        X = X * weight
        N = weight.sum(dim=-2, keepdim=True)  # (*, 1, 1)

    # subtract mean
    my = Y.sum(dim=-2) / N[..., 0]  # (*, 3)
    mx = X.sum(dim=-2) / N[..., 0]
    y0 = Y - my[..., None, :]  # (*, N, 3)
    x0 = X - mx[..., None, :]

    if weight is not None:
        y0 = y0 * weight
        x0 = x0 * weight

    # correlation
    C = torch.matmul(y0.transpose(-1, -2), x0) / N  # (*, 3, 3)
    U, D, Vh = torch.linalg.svd(C)  # (*, 3, 3), (*, 3), (*, 3, 3)

    S = torch.eye(3).reshape(*(1,) * (len(dims)), 3, 3).repeat(*dims, 1, 1)
    neg = torch.det(U) * torch.det(Vh.transpose(-1, -2)) < 0
    S[neg, 2, 2] = -1

    R = torch.matmul(U, torch.matmul(S, Vh))  # (*, 3, 3)

    D = torch.diag_embed(D)  # (*, 3, 3)
    if fixed_scale:
        s = torch.ones(*dims, 1, device=Y.device, dtype=torch.float32)
    else:
        var = torch.sum(torch.square(x0), dim=(-1, -2), keepdim=True) / N  # (*, 1, 1)
        s = torch.diagonal(torch.matmul(D, S), dim1=-2, dim2=-1).sum(dim=-1, keepdim=True) / var[..., 0]  # (*, 1)

    t = my - s * torch.matmul(R, mx[..., None])[..., 0]  # (*, 3)

    return s, R, t

def global_align_joints(gt_joints, pred_joints):
    """
    :param gt_joints (T, J, 3)
    :param pred_joints (T, J, 3)
    """
    s_glob, R_glob, t_glob = align_pcl(gt_joints.reshape(-1, 3), pred_joints.reshape(-1, 3))
    pred_glob = s_glob * torch.einsum("ij,tnj->tni", R_glob, pred_joints) + t_glob[None, None]
    return pred_glob

def first_align_joints(gt_joints, pred_joints):
    """
    align the first two frames
    :param gt_joints (T, J, 3)
    :param pred_joints (T, J, 3)
    """
    # (1, 1), (1, 3, 3), (1, 3)
    s_first, R_first, t_first = align_pcl(gt_joints[:2].reshape(1, -1, 3), pred_joints[:2].reshape(1, -1, 3))
    pred_first = s_first * torch.einsum("tij,tnj->tni", R_first, pred_joints) + t_first[:, None]
    return pred_first


    C = torch.matmul(y0.transpose(-1, -2), x0) / N  # (*, 3, 3)
    U, D, Vh = torch.linalg.svd(C)  # (*, 3, 3), (*, 3), (*, 3, 3)

    S = torch.eye(3).reshape(*(1,) * (len(dims)), 3, 3).repeat(*dims, 1, 1)
    neg = torch.det(U) * torch.det(Vh.transpose(-1, -2)) < 0
    S[neg, 2, 2] = -1

    R = torch.matmul(U, torch.matmul(S, Vh))  # (*, 3, 3)

    D = torch.diag_embed(D)  # (*, 3, 3)
    if fixed_scale:
        s = torch.ones(*dims, 1, device=Y.device, dtype=torch.float32)
    else:
        var = torch.sum(torch.square(x0), dim=(-1, -2), keepdim=True) / N  # (*, 1, 1)
        s = torch.diagonal(torch.matmul(D, S), dim1=-2, dim2=-1).sum(dim=-1, keepdim=True) / var[..., 0]  # (*, 1)

    t = my - s * torch.matmul(R, mx[..., None])[..., 0]  # (*, 3)

    return s, R, t

def global_align_joints(gt_joints, pred_joints):
    """
    :param gt_joints (T, J, 3)
    :param pred_joints (T, J, 3)
    """
    s_glob, R_glob, t_glob = align_pcl(gt_joints.reshape(-1, 3), pred_joints.reshape(-1, 3))
    pred_glob = s_glob * torch.einsum("ij,tnj->tni", R_glob, pred_joints) + t_glob[None, None]
    return pred_glob

def first_align_joints(gt_joints, pred_joints):
    """
    align the first two frames
    :param gt_joints (T, J, 3)
    :param pred_joints (T, J, 3)
    """
    # (1, 1), (1, 3, 3), (1, 3)
    s_first, R_first, t_first = align_pcl(gt_joints[:2].reshape(1, -1, 3), pred_joints[:2].reshape(1, -1, 3))
    pred_first = s_first * torch.einsum("tij,tnj->tni", R_first, pred_joints) + t_first[:, None]
    return pred_first

def compute_rte(target_trans, pred_trans, fixed_scale=True):
    # Compute the global alignment
    scale, rot, trans = align_pcl(target_trans[None, :], pred_trans[None, :], fixed_scale=fixed_scale)
    pred_trans_hat = (scale * torch.einsum("tij,tnj->tni", rot, pred_trans[None, :]) + trans[None, :])[0]

    # Compute the entire displacement of ground truth trajectory
    disps, disp = [], 0
    for p1, p2 in zip(target_trans, target_trans[1:]):


    D = torch.diag_embed(D)  # (*, 3, 3)
    if fixed_scale:
        s = torch.ones(*dims, 1, device=Y.device, dtype=torch.float32)
    else:
        var = torch.sum(torch.square(x0), dim=(-1, -2), keepdim=True) / N  # (*, 1, 1)
        s = torch.diagonal(torch.matmul(D, S), dim1=-2, dim2=-1).sum(dim=-1, keepdim=True) / var[..., 0]  # (*, 1)

    t = my - s * torch.matmul(R, mx[..., None])[..., 0]  # (*, 3)

    return s, R, t

def global_align_joints(gt_joints, pred_joints):
    """
    :param gt_joints (T, J, 3)
    :param pred_joints (T, J, 3)
    """
    s_glob, R_glob, t_glob = align_pcl(gt_joints.reshape(-1, 3), pred_joints.reshape(-1, 3))
    pred_glob = s_glob * torch.einsum("ij,tnj->tni", R_glob, pred_joints) + t_glob[None, None]
    return pred_glob

def first_align_joints(gt_joints, pred_joints):
    """
    align the first two frames
    :param gt_joints (T, J, 3)
    :param pred_joints (T, J, 3)
    """
    # (1, 1), (1, 3, 3), (1, 3)
    s_first, R_first, t_first = align_pcl(gt_joints[:2].reshape(1, -1, 3), pred_joints[:2].reshape(1, -1, 3))
    pred_first = s_first * torch.einsum("tij,tnj->tni", R_first, pred_joints) + t_first[:, None]
    return pred_first

def compute_rte(target_trans, pred_trans, fixed_scale=True):
    # Compute the global alignment
    scale, rot, trans = align_pcl(target_trans[None, :], pred_trans[None, :], fixed_scale=fixed_scale)
    pred_trans_hat = (scale * torch.einsum("tij,tnj->tni", rot, pred_trans[None, :]) + trans[None, :])[0]

    # Compute the entire displacement of ground truth trajectory
    disps, disp = [], 0
    for p1, p2 in zip(target_trans, target_trans[1:]):
        delta = (p2 - p1).norm(2, dim=-1)
        disp += delta
        disps.append(disp)

    # Compute absolute root-translation-error (RTE)
    rte = torch.norm(target_trans - pred_trans_hat, 2, dim=-1)

    # Normalize it to the displacement
    return (rte / disp).numpy()


def global_align_joints(gt_joints, pred_joints):
    """
    :param gt_joints (T, J, 3)
    :param pred_joints (T, J, 3)
    """
    s_glob, R_glob, t_glob = align_pcl(gt_joints.reshape(-1, 3), pred_joints.reshape(-1, 3))
    pred_glob = s_glob * torch.einsum("ij,tnj->tni", R_glob, pred_joints) + t_glob[None, None]
    return pred_glob

def first_align_joints(gt_joints, pred_joints):
    """
    align the first two frames
    :param gt_joints (T, J, 3)
    :param pred_joints (T, J, 3)
    """
    # (1, 1), (1, 3, 3), (1, 3)
    s_first, R_first, t_first = align_pcl(gt_joints[:2].reshape(1, -1, 3), pred_joints[:2].reshape(1, -1, 3))
    pred_first = s_first * torch.einsum("tij,tnj->tni", R_first, pred_joints) + t_first[:, None]
    return pred_first

def compute_rte(target_trans, pred_trans, fixed_scale=True):
    # Compute the global alignment
    scale, rot, trans = align_pcl(target_trans[None, :], pred_trans[None, :], fixed_scale=fixed_scale)
    pred_trans_hat = (scale * torch.einsum("tij,tnj->tni", rot, pred_trans[None, :]) + trans[None, :])[0]

    # Compute the entire displacement of ground truth trajectory
    disps, disp = [], 0
    for p1, p2 in zip(target_trans, target_trans[1:]):
        delta = (p2 - p1).norm(2, dim=-1)
        disp += delta
        disps.append(disp)

    # Compute absolute root-translation-error (RTE)
    rte = torch.norm(target_trans - pred_trans_hat, 2, dim=-1)

    # Normalize it to the displacement
    return (rte / disp).numpy()

def compute_jitter(joints, fps=30):
    """compute jitter of the motion
    Args:
        joints (N, J, 3).
        fps (float).
    Returns:
        jitter (N-3).
    """
    pred_jitter = torch.norm(
        (joints[3:] - 3 * joints[2:-1] + 3 * joints[1:-2] - joints[:-3]) * (fps**3),
        dim=2,
    ).mean(dim=-1)

    return pred_jitter.cpu().numpy() / 10.0

def compute_foot_sliding(target_verts, pred_verts, thr=1e-2):
    """compute foot sliding error


def compute_rte(target_trans, pred_trans, fixed_scale=True):
    # Compute the global alignment
    scale, rot, trans = align_pcl(target_trans[None, :], pred_trans[None, :], fixed_scale=fixed_scale)
    pred_trans_hat = (scale * torch.einsum("tij,tnj->tni", rot, pred_trans[None, :]) + trans[None, :])[0]

    # Compute the entire displacement of ground truth trajectory
    disps, disp = [], 0
    for p1, p2 in zip(target_trans, target_trans[1:]):
        delta = (p2 - p1).norm(2, dim=-1)
        disp += delta
        disps.append(disp)

    # Compute absolute root-translation-error (RTE)
    rte = torch.norm(target_trans - pred_trans_hat, 2, dim=-1)

    # Normalize it to the displacement
    return (rte / disp).numpy()

def compute_jitter(joints, fps=30):
    """compute jitter of the motion
    Args:
        joints (N, J, 3).
        fps (float).
    Returns:
        jitter (N-3).
    """
    pred_jitter = torch.norm(
        (joints[3:] - 3 * joints[2:-1] + 3 * joints[1:-2] - joints[:-3]) * (fps**3),
        dim=2,
    ).mean(dim=-1)

    return pred_jitter.cpu().numpy() / 10.0

def compute_foot_sliding(target_verts, pred_verts, thr=1e-2):
    """compute foot sliding error
    The foot ground contact label is computed by the threshold of 1 cm/frame
    Args:
        target_verts (N, 6890, 3).
        pred_verts (N, 6890, 3).
    Returns:
        error (N frames in contact).
    """
    assert target_verts.shape == pred_verts.shape
    assert target_verts.shape[-2] == 6890

    # Foot vertices idxs
    foot_idxs = [3216, 3387, 6617, 6787]

    # Compute contact label
    foot_loc = target_verts[:, foot_idxs]
    foot_disp = (foot_loc[1:] - foot_loc[:-1]).norm(2, dim=-1)


    # Normalize it to the displacement
    return (rte / disp).numpy()

def compute_jitter(joints, fps=30):
    """compute jitter of the motion
    Args:
        joints (N, J, 3).
        fps (float).
    Returns:
        jitter (N-3).
    """
    pred_jitter = torch.norm(
        (joints[3:] - 3 * joints[2:-1] + 3 * joints[1:-2] - joints[:-3]) * (fps**3),
        dim=2,
    ).mean(dim=-1)

    return pred_jitter.cpu().numpy() / 10.0

def compute_foot_sliding(target_verts, pred_verts, thr=1e-2):
    """compute foot sliding error
    The foot ground contact label is computed by the threshold of 1 cm/frame
    Args:
        target_verts (N, 6890, 3).
        pred_verts (N, 6890, 3).
    Returns:
        error (N frames in contact).
    """
    assert target_verts.shape == pred_verts.shape
    assert target_verts.shape[-2] == 6890

    # Foot vertices idxs
    foot_idxs = [3216, 3387, 6617, 6787]

    # Compute contact label
    foot_loc = target_verts[:, foot_idxs]
    foot_disp = (foot_loc[1:] - foot_loc[:-1]).norm(2, dim=-1)
    contact = foot_disp[:] < thr

    pred_feet_loc = pred_verts[:, foot_idxs]
    pred_disp = (pred_feet_loc[1:] - pred_feet_loc[:-1]).norm(2, dim=-1)

    error = pred_disp[contact]

    return error.cpu().numpy()

def eval_camcoord(batch, pelvis_idxs=[1, 2], fps=30):
    """
    Args:
        batch (dict): {
            "pred_j3d": (..., J, 3) tensor
            "target_j3d":
            "pred_v3d":
            "target_v3d":
        }
    Returns:
        cam_coord_metrics (dict): {
            "pa_mpjpe": (..., ) numpy array
            "mpjpe":
            "pve":
            "accel":
        }
    """
    # All data is in camera coordinates
    pred_j3d = batch["pred_j3d"]  # (..., J, 3)


        error (N frames in contact).
    """
    assert target_verts.shape == pred_verts.shape
    assert target_verts.shape[-2] == 6890

    # Foot vertices idxs
    foot_idxs = [3216, 3387, 6617, 6787]

    # Compute contact label
    foot_loc = target_verts[:, foot_idxs]
    foot_disp = (foot_loc[1:] - foot_loc[:-1]).norm(2, dim=-1)
    contact = foot_disp[:] < thr

    pred_feet_loc = pred_verts[:, foot_idxs]
    pred_disp = (pred_feet_loc[1:] - pred_feet_loc[:-1]).norm(2, dim=-1)

    error = pred_disp[contact]

    return error.cpu().numpy()

def eval_camcoord(batch, pelvis_idxs=[1, 2], fps=30):
    """
    Args:
        batch (dict): {
            "pred_j3d": (..., J, 3) tensor
            "target_j3d":
            "pred_v3d":
            "target_v3d":
        }
    Returns:
        cam_coord_metrics (dict): {
            "pa_mpjpe": (..., ) numpy array
            "mpjpe":
            "pve":
            "accel":
        }
    """
    # All data is in camera coordinates
    pred_j3d = batch["pred_j3d"]  # (..., J, 3)
    target_j3d = batch["target_j3d"]
    pred_verts = batch["pred_v3d"]
    target_verts = batch["target_v3d"]

    # Center Align by pelvis
    (   ca_pred_j3d, ca_target_j3d, ca_pred_verts, ca_target_verts, pred_pelvis, target_pelvis
     ) = batch_align_by_pelvis(
        [pred_j3d, target_j3d, pred_verts, target_verts], pelvis_idxs
    )

    pa_pred_j3d = batch_compute_similarity_transform_torch(ca_pred_j3d, ca_target_j3d)
    pa_pred_verts = batch_compute_similarity_transform_torch(ca_pred_verts, ca_target_verts)

    # Metrics
    m2mm = 1000

    # metric scale
    rt_error = compute_jpe(pred_pelvis, target_pelvis) * m2mm
    me_mpjpe = compute_jpe(pred_j3d, target_j3d) * m2mm
    me_pve = compute_jpe(pred_verts, target_verts) * m2mm

    # center aligned
    ca_mpjpe = compute_jpe(ca_pred_j3d, ca_target_j3d) * m2mm
    ca_pve = compute_jpe(ca_pred_verts, ca_target_verts) * m2mm

    # procrustes aligned
    pa_mpjpe = compute_jpe(pa_pred_j3d, ca_target_j3d) * m2mm
    pa_pve = compute_jpe(pa_pred_verts, ca_target_verts) * m2mm

    camcoord_metrics = {
        "me_mpjpe": me_mpjpe,
        "ca_mpjpe": ca_mpjpe,
        "pa_mpjpe": pa_mpjpe,
        "me_pve": me_pve,
        "ca_pve": ca_pve,
        "pa_pve": pa_pve,
        "rt_error": rt_error,
    }
    return camcoord_metrics

def eval_global(batch, subsample=1):
    """Follow WHAM, the input has skipped invalid frames
    Args:
        batch (dict): {
            "pred_j3d": (F, J, 3) tensor
            "target_j3d":
            "pred_v3d":
            "target_v3d":
        }
    Returns:
        global_metrics (dict): {
            "wa2_mpjpe": (F, ) numpy array
            "waa_mpjpe":
            "rte":
            "jitter":
            "fs":
        }
    """
    # All data is in global coordinates


    # center aligned
    ca_mpjpe = compute_jpe(ca_pred_j3d, ca_target_j3d) * m2mm
    ca_pve = compute_jpe(ca_pred_verts, ca_target_verts) * m2mm

    # procrustes aligned
    pa_mpjpe = compute_jpe(pa_pred_j3d, ca_target_j3d) * m2mm
    pa_pve = compute_jpe(pa_pred_verts, ca_target_verts) * m2mm

    camcoord_metrics = {
        "me_mpjpe": me_mpjpe,
        "ca_mpjpe": ca_mpjpe,
        "pa_mpjpe": pa_mpjpe,
        "me_pve": me_pve,
        "ca_pve": ca_pve,
        "pa_pve": pa_pve,
        "rt_error": rt_error,
    }
    return camcoord_metrics

def eval_global(batch, subsample=1):
    """Follow WHAM, the input has skipped invalid frames
    Args:
        batch (dict): {
            "pred_j3d": (F, J, 3) tensor
            "target_j3d":
            "pred_v3d":
            "target_v3d":
        }
    Returns:
        global_metrics (dict): {
            "wa2_mpjpe": (F, ) numpy array
            "waa_mpjpe":
            "rte":
            "jitter":
            "fs":
        }
    """
    # All data is in global coordinates
    pred_j3d_glob = batch["pred_j3d"]  # (..., J, 3)
    target_j3d_glob = batch["target_j3d"]
    pred_verts_glob = batch["pred_v3d"]
    target_verts_glob = batch["target_v3d"]

    seq_length = pred_j3d_glob.shape[0]

    # Use chunk to compare
    chunk_length = int(100 / subsample)
    wa2_mpjpe, waa_mpjpe = [], []
    for start in range(0, seq_length, chunk_length):
        end = min(seq_length, start + chunk_length)

        target_j3d = target_j3d_glob[start:end].clone().cpu()
        pred_j3d = pred_j3d_glob[start:end].clone().cpu()

        w_j3d = first_align_joints(target_j3d, pred_j3d)
        wa_j3d = global_align_joints(target_j3d, pred_j3d)

        wa2_mpjpe.append(compute_jpe(target_j3d, w_j3d))
        waa_mpjpe.append(compute_jpe(target_j3d, wa_j3d))

    # Metrics
    m2mm = 1000
    wa2_mpjpe = np.concatenate(wa2_mpjpe) * m2mm
    waa_mpjpe = np.concatenate(waa_mpjpe) * m2mm

    # Additional Metrics
    rte = compute_rte(target_j3d_glob[:, 0].cpu(), pred_j3d_glob[:, 0].cpu()) * 1e2
    rte_scaled = compute_rte(
        target_j3d_glob[:, 0].cpu(), pred_j3d_glob[:, 0].cpu(), fixed_scale=False) * 1e2
    jitter = compute_jitter(pred_j3d_glob, fps=30)
    foot_sliding = compute_foot_sliding(target_verts_glob, pred_verts_glob) * m2mm

    global_metrics = {
        "wa2_mpjpe": wa2_mpjpe,
        "waa_mpjpe": waa_mpjpe,
        "rte": rte,
        "rte_scaled": rte_scaled,
        "jitter": jitter,
        "fs": foot_sliding,
    }
    return global_metrics


import os
import sys
import gc

sys.path.append(os.path.dirname(os.path.dirname(os.path.dirname(__file__))))
import numpy as np
import torch
import argparse

from copy import deepcopy
from eval.global_human.metadata import dataset_metadata
from eval.global_human.utils import *

from accelerate import PartialState
from add_ckpt_path import add_path_to_dust3r

from collections import defaultdict, Counter
from tqdm import tqdm
import warnings
warnings.filterwarnings("ignore")

def get_args_parser():
    parser = argparse.ArgumentParser()

    parser.add_argument(
        "--weights",
        type=str,
        help="path to the model weights",
        default="",
    )
    parser.add_argument("--device", type=str, default="cuda", help="pytorch device")
    parser.add_argument(
        "--output_dir",
        type=str,
        default="",
        help="value for outdir",
    )
    parser.add_argument(
        "--no_crop", type=bool, default=True, help="whether to crop input data"
    )
    parser.add_argument(
        "--eval_dataset",
        type=str,
        default="bedlam",
        choices=list(dataset_metadata.keys()),
    )

    parser.add_argument("--crop_res", type=int, nargs=2, metavar=("W", "H"), default=None)
    parser.add_argument("--size", type=int, default="224")
    parser.add_argument("--shuffle", action="store_true", default=False)
    parser.add_argument("--revisit", type=int, default=1)
    parser.add_argument("--freeze_state", action="store_true", default=False)
    parser.add_argument("--solve_pose", action="store_true", default=False)
    parser.add_argument("--vis", action="store_true", default=False)
    parser.add_argument("--save", action="store_true", default=False)
    parser.add_argument("--is_naive", action="store_true", default=False)
    parser.add_argument("--use_ttt3r", action="store_true", default=False)
    parser.add_argument("--reset_interval", type=int, default=100000000)
    parser.add_argument("--use_fake_K", action="store_true", default=False)
    return parser

def get_seq_list(metadata, img_path):
    get_seq_func = metadata.get("get_seq_func", None)
    split = metadata.get("split", "")

    if get_seq_func:
        annots = metadata["get_annot_func"](img_path, split)
        seq_list, seq_to_images = get_seq_func(img_path, split, annots)
        return seq_list, seq_to_images, annots

    if metadata.get("full_seq", False):
        seq_dir = f"{img_path}/{split}"
        seq_list = [seq for seq in os.listdir(seq_dir)
                   if os.path.isdir(os.path.join(seq_dir, seq))]
        return sorted(seq_list), None, None
    else:
        return  sorted(metadata.get("seq_list", [])), None, None

def get_file_list(metadata, img_path, seq, seq_to_images=None):
    dir_path = metadata["dir_path_func"](img_path, seq)
    subsample = metadata.get("subsample", 1)
    max_frames = metadata.get("max_frames", None)

    if seq_to_images is not None:
        filelist = [os.path.join(dir_path, name) for name in seq_to_images[seq]]
    else:
        filelist = [os.path.join(dir_path, name) for name in os.listdir(dir_path)]

    filelist.sort()

    if max_frames is not None:
        filelist = filelist[:max_frames]

    sampled_indices = list(range(0, len(filelist), subsample))
    filelist = filelist[::subsample]
    return filelist, sampled_indices

def run_inference(
        device, args, metadata, filelist, sampled_indices, annots, model, smpl_model):
    from dust3r.inference import inference_recurrent_lighter

    get_view_func=metadata.get("get_view_func", None)
    mask_path_func = metadata.get("mask_path_func", None)
    mask_path_list = mask_path_func(filelist) if mask_path_func is not None else []

    views = prepare_input(
        filelist,
        [True for _ in filelist],
        msk_paths=mask_path_list,
        size=args.size,
        crop=not args.no_crop,
        revisit=args.revisit,
        update=not args.freeze_state,
        load_func=get_view_func,
        annots=annots,
        sampled_indices=sampled_indices,
        reset_interval=args.reset_interval,
        crop_res=args.crop_res
    )
    with torch.no_grad():
        smpl_model.update_smpl_gt_eval(views, args.eval_dataset)
    gt = prepare_gt(views)

    keep_keys = set(
        ["img", "img_mask", "true_shape", "img_mhmr", "reset", "update", "K_mhmr"])
    for i, view in enumerate(views):
        views[i] = {key: view[key] for key in keep_keys if key in view}

    outputs, _ = inference_recurrent_lighter(
        views, model, device, verbose=False, is_naive=args.is_naive, use_ttt3r=args.use_ttt3r)

    pred = prepare_output(
        outputs, revisit=args.revisit, solve_pose=args.solve_pose, is_save=args.save)

    del outputs, views
    gc.collect()
    torch.cuda.empty_cache()

    return gt, pred

def get_pred_smpl(pred, gt, f_id, is_naive, smpl_layer, mhmr_img_res, K_to_proj):
    n_humans_i = pred['shape'][f_id].shape[0]
    expand = lambda x: x.expand(n_humans_i, -1, -1)

    with torch.no_grad():
        if is_naive:
            dist = pred['transl'][f_id][:, 0].unsqueeze(-1)
            dist = to_euclidean_dist(
                mhmr_img_res, dist, expand(gt['K_mhmr'][f_id]))
            smpl_out = smpl_layer(
                pred['rotvec'][f_id],
                pred['shape'][f_id],
                None,
                pred['loc'][f_id],
                dist,
                K=expand(gt['K_mhmr'][f_id]),
                expression=pred['expression'][f_id],
                K_to_proj=expand(gt['K'][f_id]),
                )
            pred['transl'][f_id] = smpl_out['smpl_transl']
        else:
            smpl_out = smpl_layer(
                pred['rotvec'][f_id],
                pred['shape'][f_id],
                pred['transl'][f_id],
                None, None,
                K=expand(K_to_proj[f_id]),
                expression=pred['expression'][f_id])

    return smpl_out['smpl_v3d']

def match_2d(pr_j2d, gt_j2d):
    # match pred to gt - based on 2d bbox
    gt_j2d = gt_j2d.numpy()
    bestMatch, falsePositives, misses = match_2d_greedy(
        pr_j2d.numpy()[:,:gt_j2d.shape[1]],
        gt_j2d,
        np.ones_like(gt_j2d[...,0]).astype(np.bool_))

    update = {
        'count': len(gt_j2d),
        'miss': len(misses),
        'fp': len(falsePositives)
    }

    return bestMatch, update

def eval_smpl_error(args, model, smpl_model, smpl_layer, save_dir=None):
    from dust3r.utils.geometry import perspective_projection, geotrf

    metadata = dataset_metadata.get(args.eval_dataset)
    img_path = metadata["img_path"]
    mhmr_img_res = getattr(model, "mhmr_img_res", None)
    subsample = metadata.get("subsample", 1)
    is_global = metadata["is_global"](metadata.get("split", ""))
    pelvis_idx = smpl_model.pelvis_idx

    seq_list, seq_to_images, annots = get_seq_list(metadata, img_path)

    if save_dir is None:
        save_dir = args.output_dir

    distributed_state = PartialState()
    model.to(distributed_state.device)
    device = distributed_state.device

    with distributed_state.split_between_processes(seq_list) as seqs:
        if len(seq_list) < distributed_state.num_processes:
            if distributed_state.process_index >= len(seq_list):
                seqs = []
        error_log_path = f"{save_dir}/_error_log_{distributed_state.process_index}.txt"

        for seq_idx, seq in enumerate(tqdm(seqs)):
            try:
                print(f"Evaluating sequence: {seq}")
                filelist, sampled_indices = get_file_list(metadata, img_path, seq, seq_to_images)
                gt, pred = run_inference(
                    device, args, metadata, filelist, sampled_indices, annots, model, smpl_model)

                K_to_proj = gt['K'] if args.is_naive else pred['K'] # CHOOSE ONE in: pred['K'] or gt['K']
                T_c2w = pred['T_c2w'] # CHOOSE ONE in: pred['T_c2w'] or gt['T_c2w']

                global_batch = []
                metrics = defaultdict(list)
                counter = Counter()
                for f_id in range(len(filelist)):
                    n_humans_i = pred['shape'][f_id].shape[0]
                    if n_humans_i > 0:
                        pred_v3d_c = get_pred_smpl(
                            pred, gt, f_id, args.is_naive, smpl_layer, mhmr_img_res, K_to_proj)

                        pred_v3d_c = smpl_model.smplx2smpl @ pred_v3d_c
                        pred_j3d_c = smpl_model.j_regressor @ pred_v3d_c
                        pr_j2d = perspective_projection(
                            pred_j3d_c, K_to_proj[f_id].expand(n_humans_i, -1 , -1))
                    else:
                        pred_v3d_c = torch.empty(0, 6890, 3, dtype=torch.float32)
                        pr_j2d = torch.empty(0, 24, 2, dtype=torch.float32)

                    bestMatch, update = match_2d(pr_j2d, gt['j2d'][f_id])
                    counter.update(update)

                    # 3d metrics
                    if len(bestMatch) > 0:
                        counter.update({'n_human': len(bestMatch)})
                        pid, gid = bestMatch[:, 0], bestMatch[:, 1]

                        # camera coordinate metrics
                        camcoord_batch = {
                            "pred_j3d": pred_j3d_c[pid],
                            "target_j3d": gt['j3d_c'][f_id][gid],
                            "pred_v3d": pred_v3d_c[pid],
                            "target_v3d": gt['v3d_c'][f_id][gid],
                        }
                        camcoord_metrics = eval_camcoord(camcoord_batch, pelvis_idx)

                        for k, v in camcoord_metrics.items():
                            metrics[k].append(v)

                        if is_global:
                            expand = lambda x: x.expand(len(bestMatch), -1, -1)
                            global_batch.append({
                                "pred_j3d": geotrf(expand(T_c2w[f_id]), pred_j3d_c[pid]),
                                "target_j3d": gt['j3d_w'][f_id][gid],
                                "pred_v3d": geotrf(expand(T_c2w[f_id]), pred_v3d_c[pid]),
                                "target_v3d": gt['v3d_w'][f_id][gid],
                            })

                    if args.save:
                        color = 0.5 * (gt['img'][f_id].permute(1, 2, 0) + 1.0)
                        out_dir = f"{save_dir}/{seq}/{f_id:06d}"
                        for k in ["pts3d", "conf", "color", "camera", "smpl", "mask"]:
                            os.makedirs(os.path.join(out_dir, k), exist_ok=True)
                        np.save(os.path.join(out_dir, "pts3d", f"{f_id:06d}.npy"), pred['pts3d_self'][f_id])
                        np.save(os.path.join(out_dir, "conf", f"pred_{f_id:06d}.npy"), pred['conf_self'][f_id])
                        np.save(os.path.join(out_dir, "color", f"{f_id:06d}.npy"), color)
                        np.save(os.path.join(out_dir, "mask", f"pred_{f_id:06d}.npy"), pred['msk'][f_id])
                        np.savez(os.path.join(out_dir, "camera", f"pred_{f_id:06d}.npz"),
                                pose=pred['T_c2w'][f_id], K=pred['K'][f_id])
                        np.savez(os.path.join(out_dir, "camera", f"gt_{f_id:06d}.npz"),
                                pose=gt['T_c2w'][f_id], K=gt['K'][f_id])
                        if len(bestMatch) > 0:
                            np.save(os.path.join(out_dir, "smpl", f"pred_{f_id:06d}.npy"), pred_v3d_c[pid])
                            np.save(os.path.join(out_dir, "smpl", f"gt_{f_id:06d}.npy"), gt['v3d_c'][f_id][gid])
                        else:
                            np.save(os.path.join(out_dir, "smpl", f"pred_{f_id:06d}.npy"), pred_v3d_c)
                            np.save(os.path.join(out_dir, "smpl", f"gt_{f_id:06d}.npy"), gt['v3d_c'][f_id])

                    if args.vis:
                        visualize(
                            save_dir=f"{save_dir}/{seq}",
                            img_path=filelist[f_id],
                            view=gt['img'][f_id],
                            gt_v3d_c=gt['v3d_c'][f_id],
                            pred_v3d_c=pred_v3d_c,
                            K_to_proj=K_to_proj[f_id],
                            gt_K=gt['K'][f_id],
                            bestMatch=bestMatch,
                            smpl_face=smpl_model.smpl_faces['smpl'],
                            )

                metrics['precision'], metrics['recall'], metrics['f1_score']= compute_prf1(
                    counter['count'], counter['miss'], counter['fp'])

                # global coordinate metrics
                if is_global:
                    global_batch = {k: torch.cat([b[k] for b in global_batch]) for k in global_batch[0]}
                    global_metrics = eval_global(global_batch, subsample)
                    for k, v in global_metrics.items():
                        metrics[k].append(v)

                torch.cuda.empty_cache()

                # Write to error log after each sequence
                os.makedirs(save_dir, exist_ok=True)
                write_log(error_log_path, args.eval_dataset, seq, counter, metrics)

                del gt, pred, filelist, sampled_indices
                del global_batch, metrics, counter
                if 'global_metrics' in locals():
                    del global_metrics
                gc.collect()

            except Exception as e:
                print(f"Exception in sequence {seq}: {str(e)}")
                if "out of memory" in str(e):
                    torch.cuda.empty_cache()
                    with open(error_log_path, "a") as f:
                        f.write(
                            f"OOM error in sequence {seq}, skipping this sequence.\n"
                        )
                    print(f"OOM error in sequence {seq}, skipping...")
                elif "Degenerate covariance rank" in str(
                    e
                ) or "Eigenvalues did not converge" in str(e):
                    with open(error_log_path, "a") as f:
                        f.write(f"Exception in sequence {seq}: {str(e)}\n")
                    print(f"Traj evaluation error in sequence {seq}, skipping.")
                else:
                    raise e

    distributed_state.wait_for_everyone()
    torch.cuda.empty_cache()

    results = process_directory(save_dir)
    summary = calculate_averages(results)

    if distributed_state.is_main_process:
        with open(f"{save_dir}/_error_log.txt", "a") as f:
            for i in range(distributed_state.num_processes):
                if not os.path.exists(f"{save_dir}/_error_log_{i}.txt"):
                    break
                with open(f"{save_dir}/_error_log_{i}.txt", "r") as f_sub:
                    f.write(f_sub.read())

            log = get_summary_log(summary)
            f.write(log)

        print(log.strip())

if __name__ == "__main__":
    args = get_args_parser()
    args = args.parse_args()
    add_path_to_dust3r(args.weights)
    from dust3r.utils.image import load_images_for_eval as load_images
    from dust3r.utils.image import load_masks_for_eval as load_masks
    from dust3r.post_process import estimate_focal_knowing_depth
    from dust3r.model import ARCroco3DStereo
    from dust3r.utils.camera import pose_encoding_to_camera
    from dust3r.utils.geometry import weighted_procrustes, to_euclidean_dist, matrix_cumprod
    from dust3r.smpl_model import SMPLModel
    from dust3r.utils import SMPL_Layer
    from dust3r.utils.image import unpad_image

    args.no_crop = False

    def recover_cam_params(pts3ds_self, pts3ds_other, conf_self, conf_other):
        B, H, W, _ = pts3ds_self.shape
        pp = (
            torch.tensor([W // 2, H // 2], device=pts3ds_self.device)
            .float()
            .repeat(B, 1)
            .reshape(B, 1, 2)
        )
        focal = estimate_focal_knowing_depth(pts3ds_self, pp, focal_mode="weiszfeld")

        pts3ds_self = pts3ds_self.reshape(B, -1, 3)
        pts3ds_other = pts3ds_other.reshape(B, -1, 3)
        conf_self = conf_self.reshape(B, -1)
        conf_other = conf_other.reshape(B, -1)
        # weighted procrustes
        c2w = weighted_procrustes(
            pts3ds_self,
            pts3ds_other,
            torch.log(conf_self) * torch.log(conf_other),
            use_weights=True,
            return_T=True,
        )
        return c2w, focal, pp.reshape(B, 2)

    def _crop_resize(image, intrinsics, crop_res):
        import dust3r.datasets.utils.cropping as cropping
        from dust3r.utils.image import ImgNorm

        # image is a tensor in CHW with values in [-1, 1]; convert to HWC uint8 for PIL
        img_device = image.device if isinstance(image, torch.Tensor) else None
        had_batch_dim = False
        if isinstance(image, torch.Tensor):
            # accept [3,H,W] or [1,3,H,W]; squeeze batch if present
            if image.dim() == 4:
                assert image.shape[0] == 1, "_crop_resize expects a single image; got a batch"
                image = image.squeeze(0)
                had_batch_dim = True
            elif image.dim() != 3:
                raise RuntimeError(f"Unexpected image tensor shape {tuple(image.shape)}; expected CHW or 1xCHW")

            image_np = (
                (image.detach().cpu().permute(1, 2, 0).numpy() * 0.5 +0.5) * 255
            ).clip(0, 255).astype(np.uint8)
        else:
            image_np = image

        target_resolution = np.array(crop_res)
        image_pil, _, intrinsics = cropping.rescale_image_depthmap(
            image_np, None, intrinsics, target_resolution
        )

        # actual cropping (if necessary) with bilinear interpolation
        intrinsics2 = cropping.camera_matrix_of_crop(
            intrinsics, image_pil.size, crop_res, offset_factor=0.5
        )
        crop_bbox = cropping.bbox_from_intrinsics_in_out(
            intrinsics, intrinsics2, crop_res
        )
        image_pil, _, intrinsics2 = cropping.crop_image_depthmap(
            image_pil, None, intrinsics, crop_bbox
        )

        # convert back to normalized CHW tensor on original device
        image_arr = np.array(image_pil)
        if image_arr.ndim == 2:
            image_arr = np.repeat(image_arr[..., None], 3, axis=2)
        image_tensor = ImgNorm(image_pil)  # CHW, [-1, 1]
        if had_batch_dim:
            image_tensor = image_tensor.unsqueeze(0)
        if img_device is not None:
            image_tensor = image_tensor.to(img_device)

        # return intrinsics as torch tensor with batch dim like upstream expects
        intrinsics_tensor = torch.from_numpy(intrinsics2).unsqueeze(0)

        return image_tensor, intrinsics_tensor

    def prepare_input(
        img_paths,
        img_mask,
        msk_paths,
        size,
        raymaps=None,
        raymap_mask=None,
        revisit=1,
        update=True,
        crop=True,
        load_func=None,
        annots=None,
        sampled_indices=None,
        reset_interval=100,
        crop_res=None
    ):
        images = load_images(img_paths, size=size, verbose=False, crop=crop)
        images = load_func((img_paths, images, annots, sampled_indices))

        has_msk = len(msk_paths) > 0
        if has_msk:
            msks = load_masks(msk_paths, size=size, verbose=False, crop=crop)

        views = []
        if raymaps is None and raymap_mask is None:
            num_views = len(images)

            for i in range(num_views):
                view = {
                    "img": images[i]["img"],
                    "ray_map": torch.full(
                        (
                            images[i]["img"].shape[0],
                            6,
                            images[i]["img"].shape[-2],
                            images[i]["img"].shape[-1],
                        ),
                        torch.nan,
# ... truncated ...


import sys
import gc

sys.path.append(os.path.dirname(os.path.dirname(os.path.dirname(__file__))))
import numpy as np
import torch
import argparse

from copy import deepcopy
from eval.global_human.metadata import dataset_metadata
from eval.global_human.utils import *

from accelerate import PartialState
from add_ckpt_path import add_path_to_dust3r

from collections import defaultdict, Counter
from tqdm import tqdm
import warnings
warnings.filterwarnings("ignore")

def get_args_parser():
    parser = argparse.ArgumentParser()

    parser.add_argument(
        "--weights",
        type=str,
        help="path to the model weights",
        default="",
    )
    parser.add_argument("--device", type=str, default="cuda", help="pytorch device")
    parser.add_argument(
        "--output_dir",
        type=str,
        default="",
        help="value for outdir",
    )
    parser.add_argument(
        "--no_crop", type=bool, default=True, help="whether to crop input data"
    )
    parser.add_argument(
        "--eval_dataset",
        type=str,
        default="bedlam",
        choices=list(dataset_metadata.keys()),
    )

    parser.add_argument("--crop_res", type=int, nargs=2, metavar=("W", "H"), default=None)
    parser.add_argument("--size", type=int, default="224")
    parser.add_argument("--shuffle", action="store_true", default=False)
    parser.add_argument("--revisit", type=int, default=1)
    parser.add_argument("--freeze_state", action="store_true", default=False)
    parser.add_argument("--solve_pose", action="store_true", default=False)
    parser.add_argument("--vis", action="store_true", default=False)
    parser.add_argument("--save", action="store_true", default=False)
    parser.add_argument("--is_naive", action="store_true", default=False)
    parser.add_argument("--use_ttt3r", action="store_true", default=False)
    parser.add_argument("--reset_interval", type=int, default=100000000)
    parser.add_argument("--use_fake_K", action="store_true", default=False)
    return parser

def get_seq_list(metadata, img_path):
    get_seq_func = metadata.get("get_seq_func", None)
    split = metadata.get("split", "")

    if get_seq_func:
        annots = metadata["get_annot_func"](img_path, split)
        seq_list, seq_to_images = get_seq_func(img_path, split, annots)
        return seq_list, seq_to_images, annots

    if metadata.get("full_seq", False):
        seq_dir = f"{img_path}/{split}"
        seq_list = [seq for seq in os.listdir(seq_dir)
                   if os.path.isdir(os.path.join(seq_dir, seq))]
        return sorted(seq_list), None, None
    else:
        return  sorted(metadata.get("seq_list", [])), None, None

def get_file_list(metadata, img_path, seq, seq_to_images=None):
    dir_path = metadata["dir_path_func"](img_path, seq)


        "--eval_dataset",
        type=str,
        default="bedlam",
        choices=list(dataset_metadata.keys()),
    )

    parser.add_argument("--crop_res", type=int, nargs=2, metavar=("W", "H"), default=None)
    parser.add_argument("--size", type=int, default="224")
    parser.add_argument("--shuffle", action="store_true", default=False)
    parser.add_argument("--revisit", type=int, default=1)
    parser.add_argument("--freeze_state", action="store_true", default=False)
    parser.add_argument("--solve_pose", action="store_true", default=False)
    parser.add_argument("--vis", action="store_true", default=False)
    parser.add_argument("--save", action="store_true", default=False)
    parser.add_argument("--is_naive", action="store_true", default=False)
    parser.add_argument("--use_ttt3r", action="store_true", default=False)
    parser.add_argument("--reset_interval", type=int, default=100000000)
    parser.add_argument("--use_fake_K", action="store_true", default=False)
    return parser

def get_seq_list(metadata, img_path):
    get_seq_func = metadata.get("get_seq_func", None)
    split = metadata.get("split", "")

    if get_seq_func:
        annots = metadata["get_annot_func"](img_path, split)
        seq_list, seq_to_images = get_seq_func(img_path, split, annots)
        return seq_list, seq_to_images, annots

    if metadata.get("full_seq", False):
        seq_dir = f"{img_path}/{split}"
        seq_list = [seq for seq in os.listdir(seq_dir)
                   if os.path.isdir(os.path.join(seq_dir, seq))]
        return sorted(seq_list), None, None
    else:
        return  sorted(metadata.get("seq_list", [])), None, None

def get_file_list(metadata, img_path, seq, seq_to_images=None):
    dir_path = metadata["dir_path_func"](img_path, seq)
    subsample = metadata.get("subsample", 1)
    max_frames = metadata.get("max_frames", None)

    if seq_to_images is not None:
        filelist = [os.path.join(dir_path, name) for name in seq_to_images[seq]]
    else:
        filelist = [os.path.join(dir_path, name) for name in os.listdir(dir_path)]

    filelist.sort()

    if max_frames is not None:
        filelist = filelist[:max_frames]

    sampled_indices = list(range(0, len(filelist), subsample))
    filelist = filelist[::subsample]
    return filelist, sampled_indices


    parser.add_argument("--use_fake_K", action="store_true", default=False)
    return parser

def get_seq_list(metadata, img_path):
    get_seq_func = metadata.get("get_seq_func", None)
    split = metadata.get("split", "")

    if get_seq_func:
        annots = metadata["get_annot_func"](img_path, split)
        seq_list, seq_to_images = get_seq_func(img_path, split, annots)
        return seq_list, seq_to_images, annots

    if metadata.get("full_seq", False):
        seq_dir = f"{img_path}/{split}"
        seq_list = [seq for seq in os.listdir(seq_dir)
                   if os.path.isdir(os.path.join(seq_dir, seq))]
        return sorted(seq_list), None, None
    else:
        return  sorted(metadata.get("seq_list", [])), None, None

def get_file_list(metadata, img_path, seq, seq_to_images=None):
    dir_path = metadata["dir_path_func"](img_path, seq)
    subsample = metadata.get("subsample", 1)
    max_frames = metadata.get("max_frames", None)

    if seq_to_images is not None:
        filelist = [os.path.join(dir_path, name) for name in seq_to_images[seq]]
    else:
        filelist = [os.path.join(dir_path, name) for name in os.listdir(dir_path)]

    filelist.sort()

    if max_frames is not None:
        filelist = filelist[:max_frames]

    sampled_indices = list(range(0, len(filelist), subsample))
    filelist = filelist[::subsample]
    return filelist, sampled_indices

def run_inference(
        device, args, metadata, filelist, sampled_indices, annots, model, smpl_model):
    from dust3r.inference import inference_recurrent_lighter

    get_view_func=metadata.get("get_view_func", None)
    mask_path_func = metadata.get("mask_path_func", None)
    mask_path_list = mask_path_func(filelist) if mask_path_func is not None else []

    views = prepare_input(
        filelist,
        [True for _ in filelist],
        msk_paths=mask_path_list,
        size=args.size,
        crop=not args.no_crop,
        revisit=args.revisit,
        update=not args.freeze_state,
        load_func=get_view_func,
        annots=annots,
        sampled_indices=sampled_indices,


def get_file_list(metadata, img_path, seq, seq_to_images=None):
    dir_path = metadata["dir_path_func"](img_path, seq)
    subsample = metadata.get("subsample", 1)
    max_frames = metadata.get("max_frames", None)

    if seq_to_images is not None:
        filelist = [os.path.join(dir_path, name) for name in seq_to_images[seq]]
    else:
        filelist = [os.path.join(dir_path, name) for name in os.listdir(dir_path)]

    filelist.sort()

    if max_frames is not None:
        filelist = filelist[:max_frames]

    sampled_indices = list(range(0, len(filelist), subsample))
    filelist = filelist[::subsample]
    return filelist, sampled_indices

def run_inference(
        device, args, metadata, filelist, sampled_indices, annots, model, smpl_model):
    from dust3r.inference import inference_recurrent_lighter

    get_view_func=metadata.get("get_view_func", None)
    mask_path_func = metadata.get("mask_path_func", None)
    mask_path_list = mask_path_func(filelist) if mask_path_func is not None else []

    views = prepare_input(
        filelist,
        [True for _ in filelist],
        msk_paths=mask_path_list,
        size=args.size,
        crop=not args.no_crop,
        revisit=args.revisit,
        update=not args.freeze_state,
        load_func=get_view_func,
        annots=annots,
        sampled_indices=sampled_indices,
        reset_interval=args.reset_interval,
        crop_res=args.crop_res
    )
    with torch.no_grad():
        smpl_model.update_smpl_gt_eval(views, args.eval_dataset)
    gt = prepare_gt(views)

    keep_keys = set(
        ["img", "img_mask", "true_shape", "img_mhmr", "reset", "update", "K_mhmr"])
    for i, view in enumerate(views):
        views[i] = {key: view[key] for key in keep_keys if key in view}

    outputs, _ = inference_recurrent_lighter(
        views, model, device, verbose=False, is_naive=args.is_naive, use_ttt3r=args.use_ttt3r)

    pred = prepare_output(
        outputs, revisit=args.revisit, solve_pose=args.solve_pose, is_save=args.save)

    del outputs, views
    gc.collect()
    torch.cuda.empty_cache()

    return gt, pred

def get_pred_smpl(pred, gt, f_id, is_naive, smpl_layer, mhmr_img_res, K_to_proj):
    n_humans_i = pred['shape'][f_id].shape[0]
    expand = lambda x: x.expand(n_humans_i, -1, -1)

    with torch.no_grad():
        if is_naive:
            dist = pred['transl'][f_id][:, 0].unsqueeze(-1)
            dist = to_euclidean_dist(
                mhmr_img_res, dist, expand(gt['K_mhmr'][f_id]))
            smpl_out = smpl_layer(
                pred['rotvec'][f_id],
                pred['shape'][f_id],
                None,
                pred['loc'][f_id],
                dist,
                K=expand(gt['K_mhmr'][f_id]),
                expression=pred['expression'][f_id],
                K_to_proj=expand(gt['K'][f_id]),
                )


        smpl_model.update_smpl_gt_eval(views, args.eval_dataset)
    gt = prepare_gt(views)

    keep_keys = set(
        ["img", "img_mask", "true_shape", "img_mhmr", "reset", "update", "K_mhmr"])
    for i, view in enumerate(views):
        views[i] = {key: view[key] for key in keep_keys if key in view}

    outputs, _ = inference_recurrent_lighter(
        views, model, device, verbose=False, is_naive=args.is_naive, use_ttt3r=args.use_ttt3r)

    pred = prepare_output(
        outputs, revisit=args.revisit, solve_pose=args.solve_pose, is_save=args.save)

    del outputs, views
    gc.collect()
    torch.cuda.empty_cache()

    return gt, pred

def get_pred_smpl(pred, gt, f_id, is_naive, smpl_layer, mhmr_img_res, K_to_proj):
    n_humans_i = pred['shape'][f_id].shape[0]
    expand = lambda x: x.expand(n_humans_i, -1, -1)

    with torch.no_grad():
        if is_naive:
            dist = pred['transl'][f_id][:, 0].unsqueeze(-1)
            dist = to_euclidean_dist(
                mhmr_img_res, dist, expand(gt['K_mhmr'][f_id]))
            smpl_out = smpl_layer(
                pred['rotvec'][f_id],
                pred['shape'][f_id],
                None,
                pred['loc'][f_id],
                dist,
                K=expand(gt['K_mhmr'][f_id]),
                expression=pred['expression'][f_id],
                K_to_proj=expand(gt['K'][f_id]),
                )
            pred['transl'][f_id] = smpl_out['smpl_transl']
        else:
            smpl_out = smpl_layer(
                pred['rotvec'][f_id],
                pred['shape'][f_id],
                pred['transl'][f_id],
                None, None,
                K=expand(K_to_proj[f_id]),
                expression=pred['expression'][f_id])

    return smpl_out['smpl_v3d']

def match_2d(pr_j2d, gt_j2d):
    # match pred to gt - based on 2d bbox
    gt_j2d = gt_j2d.numpy()
    bestMatch, falsePositives, misses = match_2d_greedy(
        pr_j2d.numpy()[:,:gt_j2d.shape[1]],
        gt_j2d,
        np.ones_like(gt_j2d[...,0]).astype(np.bool_))

    update = {
        'count': len(gt_j2d),
        'miss': len(misses),
        'fp': len(falsePositives)
    }

    return bestMatch, update

def eval_smpl_error(args, model, smpl_model, smpl_layer, save_dir=None):
    from dust3r.utils.geometry import perspective_projection, geotrf


                pred['shape'][f_id],
                None,
                pred['loc'][f_id],
                dist,
                K=expand(gt['K_mhmr'][f_id]),
                expression=pred['expression'][f_id],
                K_to_proj=expand(gt['K'][f_id]),
                )
            pred['transl'][f_id] = smpl_out['smpl_transl']
        else:
            smpl_out = smpl_layer(
                pred['rotvec'][f_id],
                pred['shape'][f_id],
                pred['transl'][f_id],
                None, None,
                K=expand(K_to_proj[f_id]),
                expression=pred['expression'][f_id])

    return smpl_out['smpl_v3d']

def match_2d(pr_j2d, gt_j2d):
    # match pred to gt - based on 2d bbox
    gt_j2d = gt_j2d.numpy()
    bestMatch, falsePositives, misses = match_2d_greedy(
        pr_j2d.numpy()[:,:gt_j2d.shape[1]],
        gt_j2d,
        np.ones_like(gt_j2d[...,0]).astype(np.bool_))

    update = {
        'count': len(gt_j2d),
        'miss': len(misses),
        'fp': len(falsePositives)
    }

    return bestMatch, update

def eval_smpl_error(args, model, smpl_model, smpl_layer, save_dir=None):
    from dust3r.utils.geometry import perspective_projection, geotrf

    metadata = dataset_metadata.get(args.eval_dataset)
    img_path = metadata["img_path"]
    mhmr_img_res = getattr(model, "mhmr_img_res", None)
    subsample = metadata.get("subsample", 1)
    is_global = metadata["is_global"](metadata.get("split", ""))
    pelvis_idx = smpl_model.pelvis_idx

    seq_list, seq_to_images, annots = get_seq_list(metadata, img_path)

    if save_dir is None:
        save_dir = args.output_dir

    distributed_state = PartialState()
    model.to(distributed_state.device)
    device = distributed_state.device


    return smpl_out['smpl_v3d']

def match_2d(pr_j2d, gt_j2d):
    # match pred to gt - based on 2d bbox
    gt_j2d = gt_j2d.numpy()
    bestMatch, falsePositives, misses = match_2d_greedy(
        pr_j2d.numpy()[:,:gt_j2d.shape[1]],
        gt_j2d,
        np.ones_like(gt_j2d[...,0]).astype(np.bool_))

    update = {
        'count': len(gt_j2d),
        'miss': len(misses),
        'fp': len(falsePositives)
    }

    return bestMatch, update

def eval_smpl_error(args, model, smpl_model, smpl_layer, save_dir=None):
    from dust3r.utils.geometry import perspective_projection, geotrf

    metadata = dataset_metadata.get(args.eval_dataset)
    img_path = metadata["img_path"]
    mhmr_img_res = getattr(model, "mhmr_img_res", None)
    subsample = metadata.get("subsample", 1)
    is_global = metadata["is_global"](metadata.get("split", ""))
    pelvis_idx = smpl_model.pelvis_idx

    seq_list, seq_to_images, annots = get_seq_list(metadata, img_path)

    if save_dir is None:
        save_dir = args.output_dir

    distributed_state = PartialState()
    model.to(distributed_state.device)
    device = distributed_state.device

    with distributed_state.split_between_processes(seq_list) as seqs:
        if len(seq_list) < distributed_state.num_processes:
            if distributed_state.process_index >= len(seq_list):
                seqs = []
        error_log_path = f"{save_dir}/_error_log_{distributed_state.process_index}.txt"

        for seq_idx, seq in enumerate(tqdm(seqs)):
            try:
                print(f"Evaluating sequence: {seq}")
                filelist, sampled_indices = get_file_list(metadata, img_path, seq, seq_to_images)
                gt, pred = run_inference(
                    device, args, metadata, filelist, sampled_indices, annots, model, smpl_model)

                K_to_proj = gt['K'] if args.is_naive else pred['K'] # CHOOSE ONE in: pred['K'] or gt['K']
                T_c2w = pred['T_c2w'] # CHOOSE ONE in: pred['T_c2w'] or gt['T_c2w']

                global_batch = []
                metrics = defaultdict(list)
                counter = Counter()
                for f_id in range(len(filelist)):
                    n_humans_i = pred['shape'][f_id].shape[0]
                    if n_humans_i > 0:
                        pred_v3d_c = get_pred_smpl(
                            pred, gt, f_id, args.is_naive, smpl_layer, mhmr_img_res, K_to_proj)

                        pred_v3d_c = smpl_model.smplx2smpl @ pred_v3d_c
                        pred_j3d_c = smpl_model.j_regressor @ pred_v3d_c
                        pr_j2d = perspective_projection(
                            pred_j3d_c, K_to_proj[f_id].expand(n_humans_i, -1 , -1))
                    else:
                        pred_v3d_c = torch.empty(0, 6890, 3, dtype=torch.float32)
                        pr_j2d = torch.empty(0, 24, 2, dtype=torch.float32)

                    bestMatch, update = match_2d(pr_j2d, gt['j2d'][f_id])
                    counter.update(update)

                    # 3d metrics
                    if len(bestMatch) > 0:
                        counter.update({'n_human': len(bestMatch)})
                        pid, gid = bestMatch[:, 0], bestMatch[:, 1]

                        # camera coordinate metrics
                        camcoord_batch = {
                            "pred_j3d": pred_j3d_c[pid],
                            "target_j3d": gt['j3d_c'][f_id][gid],
                            "pred_v3d": pred_v3d_c[pid],
                            "target_v3d": gt['v3d_c'][f_id][gid],
                        }
                        camcoord_metrics = eval_camcoord(camcoord_batch, pelvis_idx)

                        for k, v in camcoord_metrics.items():
                            metrics[k].append(v)

                        if is_global:
                            expand = lambda x: x.expand(len(bestMatch), -1, -1)
                            global_batch.append({
                                "pred_j3d": geotrf(expand(T_c2w[f_id]), pred_j3d_c[pid]),
                                "target_j3d": gt['j3d_w'][f_id][gid],
                                "pred_v3d": geotrf(expand(T_c2w[f_id]), pred_v3d_c[pid]),
                                "target_v3d": gt['v3d_w'][f_id][gid],
                            })

                    if args.save:
                        color = 0.5 * (gt['img'][f_id].permute(1, 2, 0) + 1.0)
                        out_dir = f"{save_dir}/{seq}/{f_id:06d}"
                        for k in ["pts3d", "conf", "color", "camera", "smpl", "mask"]:
                            os.makedirs(os.path.join(out_dir, k), exist_ok=True)
                        np.save(os.path.join(out_dir, "pts3d", f"{f_id:06d}.npy"), pred['pts3d_self'][f_id])
                        np.save(os.path.join(out_dir, "conf", f"pred_{f_id:06d}.npy"), pred['conf_self'][f_id])
                        np.save(os.path.join(out_dir, "color", f"{f_id:06d}.npy"), color)
                        np.save(os.path.join(out_dir, "mask", f"pred_{f_id:06d}.npy"), pred['msk'][f_id])
                        np.savez(os.path.join(out_dir, "camera", f"pred_{f_id:06d}.npz"),
                                pose=pred['T_c2w'][f_id], K=pred['K'][f_id])
                        np.savez(os.path.join(out_dir, "camera", f"gt_{f_id:06d}.npz"),
                                pose=gt['T_c2w'][f_id], K=gt['K'][f_id])
                        if len(bestMatch) > 0:
                            np.save(os.path.join(out_dir, "smpl", f"pred_{f_id:06d}.npy"), pred_v3d_c[pid])
                            np.save(os.path.join(out_dir, "smpl", f"gt_{f_id:06d}.npy"), gt['v3d_c'][f_id][gid])
                        else:
                            np.save(os.path.join(out_dir, "smpl", f"pred_{f_id:06d}.npy"), pred_v3d_c)
                            np.save(os.path.join(out_dir, "smpl", f"gt_{f_id:06d}.npy"), gt['v3d_c'][f_id])

                    if args.vis:
                        visualize(
                            save_dir=f"{save_dir}/{seq}",
                            img_path=filelist[f_id],
                            view=gt['img'][f_id],
                            gt_v3d_c=gt['v3d_c'][f_id],
                            pred_v3d_c=pred_v3d_c,
                            K_to_proj=K_to_proj[f_id],
                            gt_K=gt['K'][f_id],
                            bestMatch=bestMatch,
                            smpl_face=smpl_model.smpl_faces['smpl'],
                            )

                metrics['precision'], metrics['recall'], metrics['f1_score']= compute_prf1(
                    counter['count'], counter['miss'], counter['fp'])

                # global coordinate metrics
                if is_global:
                    global_batch = {k: torch.cat([b[k] for b in global_batch]) for k in global_batch[0]}
                    global_metrics = eval_global(global_batch, subsample)
                    for k, v in global_metrics.items():
                        metrics[k].append(v)

                torch.cuda.empty_cache()

                # Write to error log after each sequence
                os.makedirs(save_dir, exist_ok=True)
                write_log(error_log_path, args.eval_dataset, seq, counter, metrics)

                del gt, pred, filelist, sampled_indices
                del global_batch, metrics, counter
                if 'global_metrics' in locals():
                    del global_metrics
                gc.collect()

            except Exception as e:
                print(f"Exception in sequence {seq}: {str(e)}")
                if "out of memory" in str(e):
                    torch.cuda.empty_cache()
                    with open(error_log_path, "a") as f:
                        f.write(
                            f"OOM error in sequence {seq}, skipping this sequence.\n"
                        )
                    print(f"OOM error in sequence {seq}, skipping...")
                elif "Degenerate covariance rank" in str(
                    e
                ) or "Eigenvalues did not converge" in str(e):
                    with open(error_log_path, "a") as f:
                        f.write(f"Exception in sequence {seq}: {str(e)}\n")
                    print(f"Traj evaluation error in sequence {seq}, skipping.")
                else:
                    raise e

    distributed_state.wait_for_everyone()
    torch.cuda.empty_cache()

    results = process_directory(save_dir)
    summary = calculate_averages(results)

    if distributed_state.is_main_process:
        with open(f"{save_dir}/_error_log.txt", "a") as f:
            for i in range(distributed_state.num_processes):
                if not os.path.exists(f"{save_dir}/_error_log_{i}.txt"):
                    break
                with open(f"{save_dir}/_error_log_{i}.txt", "r") as f_sub:
                    f.write(f_sub.read())

            log = get_summary_log(summary)
            f.write(log)

        print(log.strip())

if __name__ == "__main__":
    args = get_args_parser()
    args = args.parse_args()
    add_path_to_dust3r(args.weights)
    from dust3r.utils.image import load_images_for_eval as load_images
    from dust3r.utils.image import load_masks_for_eval as load_masks
    from dust3r.post_process import estimate_focal_knowing_depth
    from dust3r.model import ARCroco3DStereo
    from dust3r.utils.camera import pose_encoding_to_camera
    from dust3r.utils.geometry import weighted_procrustes, to_euclidean_dist, matrix_cumprod
    from dust3r.smpl_model import SMPLModel
    from dust3r.utils import SMPL_Layer
    from dust3r.utils.image import unpad_image

    args.no_crop = False

    def recover_cam_params(pts3ds_self, pts3ds_other, conf_self, conf_other):


        print(log.strip())

if __name__ == "__main__":
    args = get_args_parser()
    args = args.parse_args()
    add_path_to_dust3r(args.weights)
    from dust3r.utils.image import load_images_for_eval as load_images
    from dust3r.utils.image import load_masks_for_eval as load_masks
    from dust3r.post_process import estimate_focal_knowing_depth
    from dust3r.model import ARCroco3DStereo
    from dust3r.utils.camera import pose_encoding_to_camera
    from dust3r.utils.geometry import weighted_procrustes, to_euclidean_dist, matrix_cumprod
    from dust3r.smpl_model import SMPLModel
    from dust3r.utils import SMPL_Layer
    from dust3r.utils.image import unpad_image

    args.no_crop = False

    def recover_cam_params(pts3ds_self, pts3ds_other, conf_self, conf_other):
        B, H, W, _ = pts3ds_self.shape
        pp = (
            torch.tensor([W // 2, H // 2], device=pts3ds_self.device)
            .float()
            .repeat(B, 1)
            .reshape(B, 1, 2)
        )
        focal = estimate_focal_knowing_depth(pts3ds_self, pp, focal_mode="weiszfeld")

        pts3ds_self = pts3ds_self.reshape(B, -1, 3)
        pts3ds_other = pts3ds_other.reshape(B, -1, 3)
        conf_self = conf_self.reshape(B, -1)
        conf_other = conf_other.reshape(B, -1)
        # weighted procrustes
        c2w = weighted_procrustes(
            pts3ds_self,
            pts3ds_other,
            torch.log(conf_self) * torch.log(conf_other),
            use_weights=True,
            return_T=True,
        )
        return c2w, focal, pp.reshape(B, 2)

    def _crop_resize(image, intrinsics, crop_res):
        import dust3r.datasets.utils.cropping as cropping
        from dust3r.utils.image import ImgNorm

        # image is a tensor in CHW with values in [-1, 1]; convert to HWC uint8 for PIL
        img_device = image.device if isinstance(image, torch.Tensor) else None
        had_batch_dim = False
        if isinstance(image, torch.Tensor):
            # accept [3,H,W] or [1,3,H,W]; squeeze batch if present
            if image.dim() == 4:
                assert image.shape[0] == 1, "_crop_resize expects a single image; got a batch"
                image = image.squeeze(0)
                had_batch_dim = True
            elif image.dim() != 3:
                raise RuntimeError(f"Unexpected image tensor shape {tuple(image.shape)}; expected CHW or 1xCHW")

            image_np = (
                (image.detach().cpu().permute(1, 2, 0).numpy() * 0.5 +0.5) * 255
            ).clip(0, 255).astype(np.uint8)


            .float()
            .repeat(B, 1)
            .reshape(B, 1, 2)
        )
        focal = estimate_focal_knowing_depth(pts3ds_self, pp, focal_mode="weiszfeld")

        pts3ds_self = pts3ds_self.reshape(B, -1, 3)
        pts3ds_other = pts3ds_other.reshape(B, -1, 3)
        conf_self = conf_self.reshape(B, -1)
        conf_other = conf_other.reshape(B, -1)
        # weighted procrustes
        c2w = weighted_procrustes(
            pts3ds_self,
            pts3ds_other,
            torch.log(conf_self) * torch.log(conf_other),
            use_weights=True,
            return_T=True,
        )
        return c2w, focal, pp.reshape(B, 2)

    def _crop_resize(image, intrinsics, crop_res):
        import dust3r.datasets.utils.cropping as cropping
        from dust3r.utils.image import ImgNorm

        # image is a tensor in CHW with values in [-1, 1]; convert to HWC uint8 for PIL
        img_device = image.device if isinstance(image, torch.Tensor) else None
        had_batch_dim = False
        if isinstance(image, torch.Tensor):
            # accept [3,H,W] or [1,3,H,W]; squeeze batch if present
            if image.dim() == 4:
                assert image.shape[0] == 1, "_crop_resize expects a single image; got a batch"
                image = image.squeeze(0)
                had_batch_dim = True
            elif image.dim() != 3:
                raise RuntimeError(f"Unexpected image tensor shape {tuple(image.shape)}; expected CHW or 1xCHW")

            image_np = (
                (image.detach().cpu().permute(1, 2, 0).numpy() * 0.5 +0.5) * 255
            ).clip(0, 255).astype(np.uint8)
        else:
            image_np = image

        target_resolution = np.array(crop_res)
        image_pil, _, intrinsics = cropping.rescale_image_depthmap(
            image_np, None, intrinsics, target_resolution
        )

        # actual cropping (if necessary) with bilinear interpolation
        intrinsics2 = cropping.camera_matrix_of_crop(
            intrinsics, image_pil.size, crop_res, offset_factor=0.5
        )
        crop_bbox = cropping.bbox_from_intrinsics_in_out(
            intrinsics, intrinsics2, crop_res
        )
        image_pil, _, intrinsics2 = cropping.crop_image_depthmap(
            image_pil, None, intrinsics, crop_bbox
        )

        # convert back to normalized CHW tensor on original device
        image_arr = np.array(image_pil)
        if image_arr.ndim == 2:
            image_arr = np.repeat(image_arr[..., None], 3, axis=2)
        image_tensor = ImgNorm(image_pil)  # CHW, [-1, 1]
        if had_batch_dim:
            image_tensor = image_tensor.unsqueeze(0)
        if img_device is not None:
            image_tensor = image_tensor.to(img_device)

        # return intrinsics as torch tensor with batch dim like upstream expects
        intrinsics_tensor = torch.from_numpy(intrinsics2).unsqueeze(0)

        return image_tensor, intrinsics_tensor

    def prepare_input(
        img_paths,
        img_mask,
        msk_paths,
        size,
        raymaps=None,
        raymap_mask=None,
        revisit=1,
        update=True,
        crop=True,
        load_func=None,
        annots=None,
        sampled_indices=None,
        reset_interval=100,
        crop_res=None
    ):
        images = load_images(img_paths, size=size, verbose=False, crop=crop)
        images = load_func((img_paths, images, annots, sampled_indices))