import os
os.environ['PYOPENGL_PLATFORM'] = 'egl'

import torch
from gsplat import rasterization
from dust3r.utils.geometry import inv, geotrf
from dust3r.utils.image import unpad_image
import numpy as np
try:
    import pyrender
except:
    import pyrender

import trimesh
from PIL import Image

def render(
    intrinsics: torch.Tensor,
    pts3d: torch.Tensor,
    rgbs: torch.Tensor | None = None,
    scale: float = 0.002,
    opacity: float = 0.95,
):

    device = pts3d.device
    batch_size = len(intrinsics)
    img_size = pts3d.shape[1:3]
    pts3d = pts3d.reshape(batch_size, -1, 3)
    num_pts = pts3d.shape[1]
    quats = torch.randn((num_pts, 4), device=device)
    quats = quats / quats.norm(dim=-1, keepdim=True)
    scales = scale * torch.ones((num_pts, 3), device=device)
    opacities = opacity * torch.ones((num_pts), device=device)
    if rgbs is not None:
        assert rgbs.shape[1] == 3
        rgbs = rgbs.reshape(batch_size, 3, -1).transpose(1, 2)
    else:
        rgbs = torch.ones_like(pts3d[:, :, :3])

    rendered_rgbs = []
    rendered_depths = []
    accs = []
    for i in range(batch_size):
        rgbd, acc, _ = rasterization(
            pts3d[i],
            quats,
            scales,
            opacities,
            rgbs[i],
            torch.eye(4, device=device)[None],
            intrinsics[[i]],
            width=img_size[1],
            height=img_size[0],
            packed=False,
            render_mode="RGB+D",
        )

        rendered_depths.append(rgbd[..., 3])

    rendered_depths = torch.cat(rendered_depths, dim=0)

    return rendered_rgbs, rendered_depths, accs

def get_render_results(gts, preds, self_view=False):
    device = preds[0]["pts3d_in_self_view"].device
    with torch.no_grad():
        depths = []
        gt_depths = []
        for i, (gt, pred) in enumerate(zip(gts, preds)):
            if self_view:
                camera = inv(gt["camera_pose"]).to(device)
                intrinsics = gt["camera_intrinsics"].to(device)
                pred = pred["pts3d_in_self_view"]
            else:
                camera = inv(gts[0]["camera_pose"]).to(device)
                intrinsics = gts[0]["camera_intrinsics"].to(device)
                pred = pred["pts3d_in_other_view"]
            gt_img = gt["img"].to(device)
            gt_pts3d = gt["pts3d"].to(device)

            _, depth, _ = render(intrinsics, pred, gt_img)
            _, gt_depth, _ = render(intrinsics, geotrf(camera, gt_pts3d), gt_img)
            depths.append(depth)
            gt_depths.append(gt_depth)
    return depths, gt_depths

def vis_heatmap(img, scores):
    hm = scores.clone()
    hm = torch.clamp(hm + 0.1, 0, 1)  # for visu purpose only
    hm = hm.unsqueeze(0).unsqueeze(0)
    hm = torch.nn.functional.interpolate(
        hm,
        size=(img.shape[0], img.shape[1]),
        mode='nearest'
    ).squeeze(0).squeeze(0)

    hm = hm.unsqueeze(-1)

    return img * hm

def get_render_smpl(gts, preds, smpl_model, loss_details, has_msk=False):
    with torch.no_grad():
        smpl_faces = {
            'neutral': {
                10: smpl_model.smplx_neutral_10.faces,
                11: smpl_model.smplx_neutral_11.faces,
            }
        }

        gt_hms_list, pr_hms_list, gt_smpls_list, pr_smpls_list= [], [], [], []
        gt_msks_list, pr_msks_list = [], []
        for i, (gt, pred) in enumerate(zip(gts, preds)):
            gt_img = gt["img"]
            smpl_mask = gt["smpl_mask"]
            K = gt["camera_intrinsics"]

            idx_h = torch.where(smpl_mask)
            if has_msk:
                gt_msk, pr_msk = gt["msk_mhmr"], pred["msk"][...,0]
                gt_msk = unpad_image(gt_msk[:,None], gt_img.shape[2:])[:, 0]
                pr_msk = unpad_image(pr_msk[:,None], gt_img.shape[2:])[:, 0]
            gt_scores, pr_scores = gt["smpl_scores"], pred["smpl_scores"][...,0]
            gt_scores = unpad_image(gt_scores[:,None], gt_img.shape[2:])[:, 0] # if use K of CUT3R, unpad the scores
            pr_scores = unpad_image(pr_scores[:,None], gt_img.shape[2:])[:, 0]

            gt_shape, pr_shape = gt["smpl_shape"].shape[-1], pred["smpl_shape"].shape[-1]
            gt_v3d = gt["smpl_v3d"][smpl_mask]
            if int(smpl_mask.sum()) == 0:
                pr_v3d = torch.zeros_like(gt_v3d)
            else:
                pr_v3d = loss_details[f"pred_smpl_v3d_{i+1}"][smpl_mask]

            gt_hms, pr_hms, gt_msks, pr_msks, gt_smpls, pr_smpls= [], [], [], [], [], []
            for k in range(len(gt_img)):
                img_array = (0.5 * (gt_img[k] + 1.0)).permute(1, 2, 0)

                if has_msk:
                    gt_msk_array = vis_heatmap(img_array, gt_msk[k])
                    pr_msk_array = vis_heatmap(img_array, pr_msk[k])

                gt_hm_array = vis_heatmap(img_array, gt_scores[k])
                pr_hm_array = vis_heatmap(img_array, pr_scores[k])

                img_array_np = (img_array * 255).cpu().numpy().astype(np.uint8)
                focal = K[k,[0,1],[0,1]].cpu().numpy()
                princpt = K[k,[0,1],[-1,-1]].cpu().numpy()
                gt_verts, gt_faces, pr_verts, pr_faces = [], [], [], []
                for j in range(len(idx_h[0])):
                    if idx_h[0][j] == k:
                        gt_verts.append(gt_v3d[j].detach().cpu().numpy().reshape(-1,3))
                        gt_faces.append(smpl_faces["neutral"][gt_shape])
                        pr_verts.append(pr_v3d[j].detach().cpu().numpy().reshape(-1,3))
                        pr_faces.append(smpl_faces["neutral"][pr_shape])
                gt_rend_array = torch.as_tensor(
                    render_meshes(img_array_np.copy(),
                    gt_verts, gt_faces,
                    {'focal': focal, 'princpt': princpt}),
                    ) / 255.0
                pr_rend_array = torch.as_tensor(
                    render_meshes(img_array_np.copy(),
                    pr_verts, pr_faces,
                    {'focal': focal, 'princpt': princpt}),
                    ) / 255.0

                gt_hms.append(gt_hm_array)
                pr_hms.append(pr_hm_array)
                gt_smpls.append(gt_rend_array)
                pr_smpls.append(pr_rend_array)
                if has_msk:
                    gt_msks.append(gt_msk_array)
                    pr_msks.append(pr_msk_array)

            gt_hms_list.append(torch.stack(gt_hms, 0))
            pr_hms_list.append(torch.stack(pr_hms, 0))
            gt_smpls_list.append(torch.stack(gt_smpls, 0))
            pr_smpls_list.append(torch.stack(pr_smpls, 0))
            if has_msk:
                gt_msks_list.append(torch.stack(gt_msks, 0))
                pr_msks_list.append(torch.stack(pr_msks, 0))
    return (
        gt_msks_list, pr_msks_list,
        gt_hms_list, pr_hms_list,
        gt_smpls_list, pr_smpls_list
        )

OPENCV_TO_OPENGL_CAMERA_CONVENTION = np.array([[1, 0, 0, 0],
                                               [0, -1, 0, 0],
                                               [0, 0, -1, 0],
                                               [0, 0, 0, 1]])

def render_meshes(img, l_mesh, l_face, cam_param, color=None, alpha=1.0,
                  show_camera=False,
                  intensity=3.0,
                  metallicFactor=0., roughnessFactor=0.5, smooth=True,
                  ):
    """
    Rendering multiple mesh and project then in the initial image.
    Args:
        - img: np.array [w,h,3]
        - l_mesh: np.array list of [v,3]
        - l_face: np.array list of [f,3]
        - cam_param: info about the camera intrinsics (focal, princpt) and (R,t) is possible
    Return:
        - img: np.array [w,h,3]
    """
    # scene
    scene = pyrender.Scene(ambient_light=(0.3, 0.3, 0.3))

    # mesh
    for i, mesh in enumerate(l_mesh):
        if color is None:
            _color = (np.random.choice(range(1,225))/255, np.random.choice(range(1,225))/255, np.random.choice(range(1,225))/255)
        else:
            if isinstance(color,list):
                _color = color[i]
            elif isinstance(color,tuple):
                _color = color
            else:
                raise NotImplementedError
        mesh = trimesh.Trimesh(mesh, l_face[i])

        material = pyrender.MetallicRoughnessMaterial(
            metallicFactor=metallicFactor,
            roughnessFactor=roughnessFactor,
            alphaMode='OPAQUE',
            baseColorFactor=(_color[0], _color[1], _color[2], 1.0))
        mesh = pyrender.Mesh.from_trimesh(mesh, material=material, smooth=smooth)
        scene.add(mesh, f"mesh_{i}")

    # Adding coordinate system at (0,0,2) for the moment
    # Using lines defined in pyramid https://docs.pyvista.org/version/stable/api/utilities/_autosummary/pyvista.Pyramid.html
    if show_camera:
        import pyvista

        def get_faces(x):
            return x.faces.astype(np.uint32).reshape((x.n_faces, 4))[:, 1:]

        # Camera = Box + Cone (or Cylinder?)
        material_cam = pyrender.MetallicRoughnessMaterial(metallicFactor=metallicFactor, roughnessFactor=roughnessFactor, alphaMode='OPAQUE', baseColorFactor=(0.5,0.5,0.5))
        height = 0.2
        radius = 0.1
        cone = pyvista.Cone(center=(0.0, 0.0, -height/2), direction=(0.0, 0.0, -1.0), height=height, radius=radius).extract_surface().triangulate()
        verts = cone.points
        mesh = pyrender.Mesh.from_trimesh(trimesh.Trimesh(verts, get_faces(cone)), material=material_cam, smooth=smooth)
        scene.add(mesh, f"cone")

        size = 0.1
        box = pyvista.Box(bounds=(-size, size,
                                  -size, size,
                                  verts[:,-1].min() - 3*size, verts[:,-1].min())).extract_surface().triangulate()
        verts = box.points
        mesh = pyrender.Mesh.from_trimesh(trimesh.Trimesh(verts, get_faces(box)), material=material_cam, smooth=smooth)
        scene.add(mesh, f"box")

        # Coordinate system
        # https://docs.pyvista.org/version/stable/api/utilities/_autosummary/pyvista.Arrow.html
        l_color = [(1,0,0,1.0), (0,1,0,1.0), (0,0,1,1.0)]
        l_direction = [(1,0,0), (0,1,0), (0,0,1)]
        scale = 0.2
        pose3d = [2*scale, 0.0, -scale]
        for i in range(len(l_color)):
            arrow = pyvista.Arrow(direction=l_direction[i], scale=scale)
            arrow = arrow.extract_surface().triangulate()
            verts = arrow.points + np.asarray([pose3d])
            faces = arrow.faces.astype(np.uint32).reshape((arrow.n_faces, 4))[:, 1:]
            mesh = trimesh.Trimesh(verts, faces)
            material = pyrender.MetallicRoughnessMaterial(metallicFactor=metallicFactor, roughnessFactor=roughnessFactor, alphaMode='OPAQUE', baseColorFactor=l_color[i])
            mesh = pyrender.Mesh.from_trimesh(mesh, material=material, smooth=smooth)
            scene.add(mesh, f"arrow_{i}")

    focal, princpt = cam_param['focal'], cam_param['princpt']
    camera_pose = np.eye(4)
    if 'R' in cam_param.keys():
        camera_pose[:3, :3] = cam_param['R']
    if 't' in cam_param.keys():
        camera_pose[:3, 3] = cam_param['t']
    camera = pyrender.IntrinsicsCamera(fx=focal[0], fy=focal[1], cx=princpt[0], cy=princpt[1])

    # camera
    camera_pose = OPENCV_TO_OPENGL_CAMERA_CONVENTION @ camera_pose
    camera_pose = np.linalg.inv(camera_pose)
    scene.add(camera, pose=camera_pose)

    # renderer
    renderer = pyrender.OffscreenRenderer(viewport_width=img.shape[1], viewport_height=img.shape[0], point_size=1.0)

    # light
    light = pyrender.DirectionalLight(intensity=intensity)
    scene.add(light, pose=camera_pose)

    # render
    rgb, depth = renderer.render(scene, flags=pyrender.RenderFlags.RGBA)
    rgb = rgb[:,:,:3].astype(np.float32)
    fg = (depth > 0)[:,:,None].astype(np.float32)

    # Simple smoothing of the mask
    bg_blending_radius = 1
    bg_blending_kernel = 2.0 * torch.ones((1, 1, 2 * bg_blending_radius + 1, 2 * bg_blending_radius + 1)) / (2 * bg_blending_radius + 1) ** 2
    bg_blending_bias =  -torch.ones(1)
    fg = fg.reshape((fg.shape[0],fg.shape[1]))
    fg = torch.from_numpy(fg).unsqueeze(0)
    fg = torch.clamp_min(torch.nn.functional.conv2d(fg, weight=bg_blending_kernel, bias=bg_blending_bias, padding=bg_blending_radius) * fg, 0.0)
    fg = fg.permute(1,2,0).numpy()

    # Alpha-blending
    img = (fg * (alpha * rgb + (1.0-alpha) * img) + (1-fg) * img).astype(np.uint8)

    renderer.delete()

    return img.astype(np.uint8)


render

import os
os.environ['PYOPENGL_PLATFORM'] = 'egl'

import torch
from gsplat import rasterization
from dust3r.utils.geometry import inv, geotrf
from dust3r.utils.image import unpad_image
import numpy as np
try:
    import pyrender
except:
    import pyrender

import trimesh
from PIL import Image

def render(
    intrinsics: torch.Tensor,
    pts3d: torch.Tensor,
    rgbs: torch.Tensor | None = None,
    scale: float = 0.002,
    opacity: float = 0.95,
):

    device = pts3d.device
    batch_size = len(intrinsics)
    img_size = pts3d.shape[1:3]
    pts3d = pts3d.reshape(batch_size, -1, 3)
    num_pts = pts3d.shape[1]
    quats = torch.randn((num_pts, 4), device=device)
    quats = quats / quats.norm(dim=-1, keepdim=True)
    scales = scale * torch.ones((num_pts, 3), device=device)
    opacities = opacity * torch.ones((num_pts), device=device)
    if rgbs is not None:
        assert rgbs.shape[1] == 3
        rgbs = rgbs.reshape(batch_size, 3, -1).transpose(1, 2)
    else:
        rgbs = torch.ones_like(pts3d[:, :, :3])

    rendered_rgbs = []
    rendered_depths = []
    accs = []
    for i in range(batch_size):
        rgbd, acc, _ = rasterization(
            pts3d[i],
            quats,
            scales,
            opacities,
            rgbs[i],
            torch.eye(4, device=device)[None],
            intrinsics[[i]],
            width=img_size[1],
            height=img_size[0],
            packed=False,
            render_mode="RGB+D",
        )

        rendered_depths.append(rgbd[..., 3])

    rendered_depths = torch.cat(rendered_depths, dim=0)

    return rendered_rgbs, rendered_depths, accs

def get_render_results(gts, preds, self_view=False):
    device = preds[0]["pts3d_in_self_view"].device
    with torch.no_grad():
        depths = []
        gt_depths = []
        for i, (gt, pred) in enumerate(zip(gts, preds)):
            if self_view:
                camera = inv(gt["camera_pose"]).to(device)
                intrinsics = gt["camera_intrinsics"].to(device)
                pred = pred["pts3d_in_self_view"]
            else:
                camera = inv(gts[0]["camera_pose"]).to(device)
                intrinsics = gts[0]["camera_intrinsics"].to(device)
                pred = pred["pts3d_in_other_view"]
            gt_img = gt["img"].to(device)
            gt_pts3d = gt["pts3d"].to(device)

            _, depth, _ = render(intrinsics, pred, gt_img)


            pts3d[i],
            quats,
            scales,
            opacities,
            rgbs[i],
            torch.eye(4, device=device)[None],
            intrinsics[[i]],
            width=img_size[1],
            height=img_size[0],
            packed=False,
            render_mode="RGB+D",
        )

        rendered_depths.append(rgbd[..., 3])

    rendered_depths = torch.cat(rendered_depths, dim=0)

    return rendered_rgbs, rendered_depths, accs

def get_render_results(gts, preds, self_view=False):
    device = preds[0]["pts3d_in_self_view"].device
    with torch.no_grad():
        depths = []
        gt_depths = []
        for i, (gt, pred) in enumerate(zip(gts, preds)):
            if self_view:
                camera = inv(gt["camera_pose"]).to(device)
                intrinsics = gt["camera_intrinsics"].to(device)
                pred = pred["pts3d_in_self_view"]
            else:
                camera = inv(gts[0]["camera_pose"]).to(device)
                intrinsics = gts[0]["camera_intrinsics"].to(device)
                pred = pred["pts3d_in_other_view"]
            gt_img = gt["img"].to(device)
            gt_pts3d = gt["pts3d"].to(device)

            _, depth, _ = render(intrinsics, pred, gt_img)
            _, gt_depth, _ = render(intrinsics, geotrf(camera, gt_pts3d), gt_img)
            depths.append(depth)
            gt_depths.append(gt_depth)
    return depths, gt_depths

def vis_heatmap(img, scores):
    hm = scores.clone()
    hm = torch.clamp(hm + 0.1, 0, 1)  # for visu purpose only
    hm = hm.unsqueeze(0).unsqueeze(0)
    hm = torch.nn.functional.interpolate(
        hm,
        size=(img.shape[0], img.shape[1]),
        mode='nearest'
    ).squeeze(0).squeeze(0)

    hm = hm.unsqueeze(-1)

    return img * hm

def get_render_smpl(gts, preds, smpl_model, loss_details, has_msk=False):
    with torch.no_grad():
        smpl_faces = {
            'neutral': {


        gt_depths = []
        for i, (gt, pred) in enumerate(zip(gts, preds)):
            if self_view:
                camera = inv(gt["camera_pose"]).to(device)
                intrinsics = gt["camera_intrinsics"].to(device)
                pred = pred["pts3d_in_self_view"]
            else:
                camera = inv(gts[0]["camera_pose"]).to(device)
                intrinsics = gts[0]["camera_intrinsics"].to(device)
                pred = pred["pts3d_in_other_view"]
            gt_img = gt["img"].to(device)
            gt_pts3d = gt["pts3d"].to(device)

            _, depth, _ = render(intrinsics, pred, gt_img)
            _, gt_depth, _ = render(intrinsics, geotrf(camera, gt_pts3d), gt_img)
            depths.append(depth)
            gt_depths.append(gt_depth)
    return depths, gt_depths

def vis_heatmap(img, scores):
    hm = scores.clone()
    hm = torch.clamp(hm + 0.1, 0, 1)  # for visu purpose only
    hm = hm.unsqueeze(0).unsqueeze(0)
    hm = torch.nn.functional.interpolate(
        hm,
        size=(img.shape[0], img.shape[1]),
        mode='nearest'
    ).squeeze(0).squeeze(0)

    hm = hm.unsqueeze(-1)

    return img * hm

def get_render_smpl(gts, preds, smpl_model, loss_details, has_msk=False):
    with torch.no_grad():
        smpl_faces = {
            'neutral': {
                10: smpl_model.smplx_neutral_10.faces,
                11: smpl_model.smplx_neutral_11.faces,
            }
        }

        gt_hms_list, pr_hms_list, gt_smpls_list, pr_smpls_list= [], [], [], []
        gt_msks_list, pr_msks_list = [], []
        for i, (gt, pred) in enumerate(zip(gts, preds)):
            gt_img = gt["img"]
            smpl_mask = gt["smpl_mask"]
            K = gt["camera_intrinsics"]

            idx_h = torch.where(smpl_mask)
            if has_msk:
                gt_msk, pr_msk = gt["msk_mhmr"], pred["msk"][...,0]


            _, gt_depth, _ = render(intrinsics, geotrf(camera, gt_pts3d), gt_img)
            depths.append(depth)
            gt_depths.append(gt_depth)
    return depths, gt_depths

def vis_heatmap(img, scores):
    hm = scores.clone()
    hm = torch.clamp(hm + 0.1, 0, 1)  # for visu purpose only
    hm = hm.unsqueeze(0).unsqueeze(0)
    hm = torch.nn.functional.interpolate(
        hm,
        size=(img.shape[0], img.shape[1]),
        mode='nearest'
    ).squeeze(0).squeeze(0)

    hm = hm.unsqueeze(-1)

    return img * hm

def get_render_smpl(gts, preds, smpl_model, loss_details, has_msk=False):
    with torch.no_grad():
        smpl_faces = {
            'neutral': {
                10: smpl_model.smplx_neutral_10.faces,
                11: smpl_model.smplx_neutral_11.faces,
            }
        }

        gt_hms_list, pr_hms_list, gt_smpls_list, pr_smpls_list= [], [], [], []
        gt_msks_list, pr_msks_list = [], []
        for i, (gt, pred) in enumerate(zip(gts, preds)):
            gt_img = gt["img"]
            smpl_mask = gt["smpl_mask"]
            K = gt["camera_intrinsics"]

            idx_h = torch.where(smpl_mask)
            if has_msk:
                gt_msk, pr_msk = gt["msk_mhmr"], pred["msk"][...,0]
                gt_msk = unpad_image(gt_msk[:,None], gt_img.shape[2:])[:, 0]
                pr_msk = unpad_image(pr_msk[:,None], gt_img.shape[2:])[:, 0]
            gt_scores, pr_scores = gt["smpl_scores"], pred["smpl_scores"][...,0]
            gt_scores = unpad_image(gt_scores[:,None], gt_img.shape[2:])[:, 0] # if use K of CUT3R, unpad the scores
            pr_scores = unpad_image(pr_scores[:,None], gt_img.shape[2:])[:, 0]

            gt_shape, pr_shape = gt["smpl_shape"].shape[-1], pred["smpl_shape"].shape[-1]
            gt_v3d = gt["smpl_v3d"][smpl_mask]
            if int(smpl_mask.sum()) == 0:
                pr_v3d = torch.zeros_like(gt_v3d)
            else:
                pr_v3d = loss_details[f"pred_smpl_v3d_{i+1}"][smpl_mask]

            gt_hms, pr_hms, gt_msks, pr_msks, gt_smpls, pr_smpls= [], [], [], [], [], []
            for k in range(len(gt_img)):
                img_array = (0.5 * (gt_img[k] + 1.0)).permute(1, 2, 0)

                if has_msk:
                    gt_msk_array = vis_heatmap(img_array, gt_msk[k])
                    pr_msk_array = vis_heatmap(img_array, pr_msk[k])

                gt_hm_array = vis_heatmap(img_array, gt_scores[k])
                pr_hm_array = vis_heatmap(img_array, pr_scores[k])

                img_array_np = (img_array * 255).cpu().numpy().astype(np.uint8)
                focal = K[k,[0,1],[0,1]].cpu().numpy()
                princpt = K[k,[0,1],[-1,-1]].cpu().numpy()
                gt_verts, gt_faces, pr_verts, pr_faces = [], [], [], []
                for j in range(len(idx_h[0])):
                    if idx_h[0][j] == k:
                        gt_verts.append(gt_v3d[j].detach().cpu().numpy().reshape(-1,3))
                        gt_faces.append(smpl_faces["neutral"][gt_shape])
                        pr_verts.append(pr_v3d[j].detach().cpu().numpy().reshape(-1,3))
                        pr_faces.append(smpl_faces["neutral"][pr_shape])
                gt_rend_array = torch.as_tensor(
                    render_meshes(img_array_np.copy(),
                    gt_verts, gt_faces,
                    {'focal': focal, 'princpt': princpt}),
                    ) / 255.0
                pr_rend_array = torch.as_tensor(
                    render_meshes(img_array_np.copy(),
                    pr_verts, pr_faces,
                    {'focal': focal, 'princpt': princpt}),
                    ) / 255.0

                gt_hms.append(gt_hm_array)
                pr_hms.append(pr_hm_array)
                gt_smpls.append(gt_rend_array)
                pr_smpls.append(pr_rend_array)
                if has_msk:
                    gt_msks.append(gt_msk_array)
                    pr_msks.append(pr_msk_array)

            gt_hms_list.append(torch.stack(gt_hms, 0))
            pr_hms_list.append(torch.stack(pr_hms, 0))
            gt_smpls_list.append(torch.stack(gt_smpls, 0))
            pr_smpls_list.append(torch.stack(pr_smpls, 0))
            if has_msk:
                gt_msks_list.append(torch.stack(gt_msks, 0))
                pr_msks_list.append(torch.stack(pr_msks, 0))
    return (
        gt_msks_list, pr_msks_list,
        gt_hms_list, pr_hms_list,
        gt_smpls_list, pr_smpls_list
        )

OPENCV_TO_OPENGL_CAMERA_CONVENTION = np.array([[1, 0, 0, 0],
                                               [0, -1, 0, 0],
                                               [0, 0, -1, 0],
                                               [0, 0, 0, 1]])

def render_meshes(img, l_mesh, l_face, cam_param, color=None, alpha=1.0,
                  show_camera=False,
                  intensity=3.0,
                  metallicFactor=0., roughnessFactor=0.5, smooth=True,
                  ):
    """
    Rendering multiple mesh and project then in the initial image.
    Args:
        - img: np.array [w,h,3]
        - l_mesh: np.array list of [v,3]
        - l_face: np.array list of [f,3]
        - cam_param: info about the camera intrinsics (focal, princpt) and (R,t) is possible
    Return:


            gt_hms_list.append(torch.stack(gt_hms, 0))
            pr_hms_list.append(torch.stack(pr_hms, 0))
            gt_smpls_list.append(torch.stack(gt_smpls, 0))
            pr_smpls_list.append(torch.stack(pr_smpls, 0))
            if has_msk:
                gt_msks_list.append(torch.stack(gt_msks, 0))
                pr_msks_list.append(torch.stack(pr_msks, 0))
    return (
        gt_msks_list, pr_msks_list,
        gt_hms_list, pr_hms_list,
        gt_smpls_list, pr_smpls_list
        )

OPENCV_TO_OPENGL_CAMERA_CONVENTION = np.array([[1, 0, 0, 0],
                                               [0, -1, 0, 0],
                                               [0, 0, -1, 0],
                                               [0, 0, 0, 1]])

def render_meshes(img, l_mesh, l_face, cam_param, color=None, alpha=1.0,
                  show_camera=False,
                  intensity=3.0,
                  metallicFactor=0., roughnessFactor=0.5, smooth=True,
                  ):
    """
    Rendering multiple mesh and project then in the initial image.
    Args:
        - img: np.array [w,h,3]
        - l_mesh: np.array list of [v,3]
        - l_face: np.array list of [f,3]
        - cam_param: info about the camera intrinsics (focal, princpt) and (R,t) is possible
    Return:
        - img: np.array [w,h,3]
    """
    # scene
    scene = pyrender.Scene(ambient_light=(0.3, 0.3, 0.3))

    # mesh
    for i, mesh in enumerate(l_mesh):
        if color is None:
            _color = (np.random.choice(range(1,225))/255, np.random.choice(range(1,225))/255, np.random.choice(range(1,225))/255)
        else:
            if isinstance(color,list):
                _color = color[i]
            elif isinstance(color,tuple):
                _color = color
            else:
                raise NotImplementedError
        mesh = trimesh.Trimesh(mesh, l_face[i])

        material = pyrender.MetallicRoughnessMaterial(
            metallicFactor=metallicFactor,
            roughnessFactor=roughnessFactor,
            alphaMode='OPAQUE',
            baseColorFactor=(_color[0], _color[1], _color[2], 1.0))
        mesh = pyrender.Mesh.from_trimesh(mesh, material=material, smooth=smooth)
        scene.add(mesh, f"mesh_{i}")

    # Adding coordinate system at (0,0,2) for the moment
    # Using lines defined in pyramid https://docs.pyvista.org/version/stable/api/utilities/_autosummary/pyvista.Pyramid.html
    if show_camera:
        import pyvista

        def get_faces(x):
            return x.faces.astype(np.uint32).reshape((x.n_faces, 4))[:, 1:]

        # Camera = Box + Cone (or Cylinder?)
        material_cam = pyrender.MetallicRoughnessMaterial(metallicFactor=metallicFactor, roughnessFactor=roughnessFactor, alphaMode='OPAQUE', baseColorFactor=(0.5,0.5,0.5))
        height = 0.2
        radius = 0.1
        cone = pyvista.Cone(center=(0.0, 0.0, -height/2), direction=(0.0, 0.0, -1.0), height=height, radius=radius).extract_surface().triangulate()
        verts = cone.points
        mesh = pyrender.Mesh.from_trimesh(trimesh.Trimesh(verts, get_faces(cone)), material=material_cam, smooth=smooth)
        scene.add(mesh, f"cone")

        size = 0.1
        box = pyvista.Box(bounds=(-size, size,
                                  -size, size,
                                  verts[:,-1].min() - 3*size, verts[:,-1].min())).extract_surface().triangulate()
        verts = box.points
        mesh = pyrender.Mesh.from_trimesh(trimesh.Trimesh(verts, get_faces(box)), material=material_cam, smooth=smooth)
        scene.add(mesh, f"box")

        # Coordinate system
        # https://docs.pyvista.org/version/stable/api/utilities/_autosummary/pyvista.Arrow.html
        l_color = [(1,0,0,1.0), (0,1,0,1.0), (0,0,1,1.0)]
        l_direction = [(1,0,0), (0,1,0), (0,0,1)]
        scale = 0.2
        pose3d = [2*scale, 0.0, -scale]
        for i in range(len(l_color)):
            arrow = pyvista.Arrow(direction=l_direction[i], scale=scale)
            arrow = arrow.extract_surface().triangulate()
            verts = arrow.points + np.asarray([pose3d])
            faces = arrow.faces.astype(np.uint32).reshape((arrow.n_faces, 4))[:, 1:]
            mesh = trimesh.Trimesh(verts, faces)
            material = pyrender.MetallicRoughnessMaterial(metallicFactor=metallicFactor, roughnessFactor=roughnessFactor, alphaMode='OPAQUE', baseColorFactor=l_color[i])
            mesh = pyrender.Mesh.from_trimesh(mesh, material=material, smooth=smooth)
            scene.add(mesh, f"arrow_{i}")

    focal, princpt = cam_param['focal'], cam_param['princpt']
    camera_pose = np.eye(4)
    if 'R' in cam_param.keys():
        camera_pose[:3, :3] = cam_param['R']
    if 't' in cam_param.keys():
        camera_pose[:3, 3] = cam_param['t']
    camera = pyrender.IntrinsicsCamera(fx=focal[0], fy=focal[1], cx=princpt[0], cy=princpt[1])

    # camera
    camera_pose = OPENCV_TO_OPENGL_CAMERA_CONVENTION @ camera_pose
    camera_pose = np.linalg.inv(camera_pose)
    scene.add(camera, pose=camera_pose)

    # renderer
    renderer = pyrender.OffscreenRenderer(viewport_width=img.shape[1], viewport_height=img.shape[0], point_size=1.0)

    # light
    light = pyrender.DirectionalLight(intensity=intensity)
    scene.add(light, pose=camera_pose)

    # render
    rgb, depth = renderer.render(scene, flags=pyrender.RenderFlags.RGBA)
    rgb = rgb[:,:,:3].astype(np.float32)
    fg = (depth > 0)[:,:,None].astype(np.float32)

    # Simple smoothing of the mask
    bg_blending_radius = 1
    bg_blending_kernel = 2.0 * torch.ones((1, 1, 2 * bg_blending_radius + 1, 2 * bg_blending_radius + 1)) / (2 * bg_blending_radius + 1) ** 2
    bg_blending_bias =  -torch.ones(1)
    fg = fg.reshape((fg.shape[0],fg.shape[1]))
    fg = torch.from_numpy(fg).unsqueeze(0)
    fg = torch.clamp_min(torch.nn.functional.conv2d(fg, weight=bg_blending_kernel, bias=bg_blending_bias, padding=bg_blending_radius) * fg, 0.0)
    fg = fg.permute(1,2,0).numpy()

    # Alpha-blending
    img = (fg * (alpha * rgb + (1.0-alpha) * img) + (1-fg) * img).astype(np.uint8)

    renderer.delete()

    return img.astype(np.uint8)


                _color = color[i]
            elif isinstance(color,tuple):
                _color = color
            else:
                raise NotImplementedError
        mesh = trimesh.Trimesh(mesh, l_face[i])

        material = pyrender.MetallicRoughnessMaterial(
            metallicFactor=metallicFactor,
            roughnessFactor=roughnessFactor,
            alphaMode='OPAQUE',
            baseColorFactor=(_color[0], _color[1], _color[2], 1.0))
        mesh = pyrender.Mesh.from_trimesh(mesh, material=material, smooth=smooth)
        scene.add(mesh, f"mesh_{i}")

    # Adding coordinate system at (0,0,2) for the moment
    # Using lines defined in pyramid https://docs.pyvista.org/version/stable/api/utilities/_autosummary/pyvista.Pyramid.html
    if show_camera:
        import pyvista

        def get_faces(x):
            return x.faces.astype(np.uint32).reshape((x.n_faces, 4))[:, 1:]

        # Camera = Box + Cone (or Cylinder?)
        material_cam = pyrender.MetallicRoughnessMaterial(metallicFactor=metallicFactor, roughnessFactor=roughnessFactor, alphaMode='OPAQUE', baseColorFactor=(0.5,0.5,0.5))
        height = 0.2
        radius = 0.1
        cone = pyvista.Cone(center=(0.0, 0.0, -height/2), direction=(0.0, 0.0, -1.0), height=height, radius=radius).extract_surface().triangulate()
        verts = cone.points
        mesh = pyrender.Mesh.from_trimesh(trimesh.Trimesh(verts, get_faces(cone)), material=material_cam, smooth=smooth)
        scene.add(mesh, f"cone")

        size = 0.1
        box = pyvista.Box(bounds=(-size, size,
                                  -size, size,
                                  verts[:,-1].min() - 3*size, verts[:,-1].min())).extract_surface().triangulate()
        verts = box.points
        mesh = pyrender.Mesh.from_trimesh(trimesh.Trimesh(verts, get_faces(box)), material=material_cam, smooth=smooth)
        scene.add(mesh, f"box")

        # Coordinate system


# Multi-HMR
# Copyright (c) 2024-present NAVER Corp.
# CC BY-NC-SA 4.0 license

import torch
from torch import nn
import smplx
import torch
from dust3r.utils.geometry import inverse_perspective_projection, perspective_projection
import roma
from dust3r.smpl_model import SMPLX_DIR
from smplx.joint_names import JOINT_NAMES

class SMPL_Layer(nn.Module):
    """
    Extension of the SMPL Layer with information about the camera for (inverse) projection the camera plane.
    """
    def __init__(self,
                 type='smplx',
                 gender='neutral',
                 num_betas=10,
                 kid=False,
                 person_center=None,
                 *args,
                 **kwargs,
                 ):
        super().__init__()

        # Args
        assert type == 'smplx'
        self.type = type
        self.kid = kid
        self.num_betas = num_betas
        self.bm_x = smplx.create(SMPLX_DIR, 'smplx', gender=gender, use_pca=False, flat_hand_mean=True, num_betas=num_betas)

        # Primary keypoint - root
        self.joint_names = JOINT_NAMES[:127]
        self.person_center = person_center
        self.person_center_idx = None
        if self.person_center is not None:
            self.person_center_idx = self.joint_names.index(self.person_center)

    def forward(self,
                pose, shape, transl,
                loc, dist,
                K,
                expression=None, # facial expression
                K_to_proj=None,
                ):
        """
        Args:
            - pose: pose of the person in axis-angle - torch.Tensor [bs,24,3]
            - shape: torch.Tensor [bs,10]
            - loc: 2D location of the pelvis in pixel space - torch.Tensor [bs,2]
            - dist: distance of the pelvis from the camera in m - torch.Tensor [bs,1]
        Return:
            - dict containing a bunch of useful information about each person
        """

        if loc is not None and dist is not None:
            assert pose.shape[0] == shape.shape[0] == loc.shape[0] == dist.shape[0]
            assert len(loc.shape) == 2 and list(loc.shape[1:]) == [2]
            assert len(dist.shape) == 2 and list(dist.shape[1:]) == [1]
        if self.type == 'smpl':
            assert len(pose.shape) == 3 and list(pose.shape[1:]) == [24,3]
        elif self.type == 'smplx':
            assert len(pose.shape) == 3 and list(pose.shape[1:]) == [53,3] # taking root_orient, body_pose, lhand, rhan and jaw for the moment
        else:
            raise NameError
        assert len(shape.shape) == 2 and (list(shape.shape[1:]) == [self.num_betas] or list(shape.shape[1:]) == [self.num_betas+1])
        assert (transl is not None) or (loc is not None and dist is not None)

        bs = pose.shape[0]

        out = {}

        # No humans
        if bs == 0:
            return {}

        # Low dimensional parameters
        kwargs_pose = {
            'betas': shape,
        }
        kwargs_pose['global_orient'] = self.bm_x.global_orient.repeat(bs,1) # 0，0，0
        kwargs_pose['body_pose'] = pose[:,1:22].flatten(1)
        kwargs_pose['left_hand_pose'] = pose[:,22:37].flatten(1)
        kwargs_pose['right_hand_pose'] = pose[:,37:52].flatten(1)
        kwargs_pose['jaw_pose'] = pose[:,52:53].flatten(1)

        if expression is not None:
            kwargs_pose['expression'] = expression.flatten(1) # [bs,10]
        else:
            kwargs_pose['expression'] = self.bm_x.expression.repeat(bs,1)

        # default - to be generalized
        kwargs_pose['leye_pose'] = self.bm_x.leye_pose.repeat(bs,1)
        kwargs_pose['reye_pose'] = self.bm_x.reye_pose.repeat(bs,1)

        # Forward using the parametric 3d model SMPL-X layer
        output = self.bm_x(**kwargs_pose)
        verts = output.vertices
        j3d = output.joints # 45 joints
        R = roma.rotvec_to_rotmat(pose[:,0])

        # Apply global orientation on 3D points
        pelvis = j3d[:,[0]]
        j3d = (R.unsqueeze(1) @ (j3d - pelvis).unsqueeze(-1)).squeeze(-1)

        # Apply global orientation on 3D points - bis
        verts = (R.unsqueeze(1) @ (verts - pelvis).unsqueeze(-1)).squeeze(-1) # to pelvis-center coordinates: R(v-p)

        # Location of the person in 3D
        if transl is None:
            if K.dtype == torch.float16:
                # because of torch.inverse - not working with float16 at the moment
                transl = inverse_perspective_projection(loc.unsqueeze(1).float(), K.float(), dist.unsqueeze(1).float())[:,0]
                transl = transl.half()
            else:
                transl = inverse_perspective_projection(loc.unsqueeze(1), K, dist.unsqueeze(1))[:,0] # head center in camera coordinates

        # Updating transl if we choose a certain person center
        transl_up = transl.clone()

        # Definition of the translation depend on the args: 1) vanilla SMPL - 2) computed from a given joint
        if self.person_center_idx is None:
            # Add pelvis to transl - standard way for SMPLX layer
            transl_up = transl_up + pelvis[:,0] # back to smpl original coordinates
        else:
            # Center around the joint because teh translation is computed from this joint
            person_center = j3d[:, [self.person_center_idx]] # head center in pelvis-center coordinates
            verts = verts - person_center # nomalize to head-center coordinates
            j3d = j3d - person_center

        # Moving into the camera coordinate system
        j3d_cam = j3d + transl_up.unsqueeze(1) # move to camera coordinates
        verts_cam = verts + transl_up.unsqueeze(1)

        if K_to_proj is None:
            K_to_proj = K
        # Projection in camera plane
        j2d = perspective_projection(j3d_cam, K_to_proj)
        v2d = perspective_projection(verts_cam, K_to_proj)

        out.update({
            'smpl_v3d': verts_cam, # in 3d camera space
            'smpl_j3d': j3d_cam, # in 3d camera space
            'smpl_j2d': j2d,
            'smpl_v2d': v2d,
            'smpl_transl': transl, # translation of the primary keypoint in camera coordinates
            'smpl_transl_pelvis': j3d_cam[:,[0]], # root=pelvis in camera coordinates
        })

        return out


# Multi-HMR
# Copyright (c) 2024-present NAVER Corp.
# CC BY-NC-SA 4.0 license

import torch
from torch import nn
import smplx
import torch
from dust3r.utils.geometry import inverse_perspective_projection, perspective_projection
import roma
from dust3r.smpl_model import SMPLX_DIR
from smplx.joint_names import JOINT_NAMES

class SMPL_Layer(nn.Module):
    """
    Extension of the SMPL Layer with information about the camera for (inverse) projection the camera plane.
    """
    def __init__(self,
                 type='smplx',
                 gender='neutral',
                 num_betas=10,
                 kid=False,
                 person_center=None,
                 *args,
                 **kwargs,
                 ):
        super().__init__()

        # Args
        assert type == 'smplx'
        self.type = type
        self.kid = kid
        self.num_betas = num_betas
        self.bm_x = smplx.create(SMPLX_DIR, 'smplx', gender=gender, use_pca=False, flat_hand_mean=True, num_betas=num_betas)

        # Primary keypoint - root
        self.joint_names = JOINT_NAMES[:127]
        self.person_center = person_center
        self.person_center_idx = None
        if self.person_center is not None:
            self.person_center_idx = self.joint_names.index(self.person_center)

    def forward(self,
                pose, shape, transl,
                loc, dist,
                K,
                expression=None, # facial expression
                K_to_proj=None,
                ):
        """
        Args:
            - pose: pose of the person in axis-angle - torch.Tensor [bs,24,3]
            - shape: torch.Tensor [bs,10]
            - loc: 2D location of the pelvis in pixel space - torch.Tensor [bs,2]
            - dist: distance of the pelvis from the camera in m - torch.Tensor [bs,1]
        Return:
            - dict containing a bunch of useful information about each person
        """

        if loc is not None and dist is not None:
            assert pose.shape[0] == shape.shape[0] == loc.shape[0] == dist.shape[0]


forward

                 person_center=None,
                 *args,
                 **kwargs,
                 ):
        super().__init__()

        # Args
        assert type == 'smplx'
        self.type = type
        self.kid = kid
        self.num_betas = num_betas
        self.bm_x = smplx.create(SMPLX_DIR, 'smplx', gender=gender, use_pca=False, flat_hand_mean=True, num_betas=num_betas)

        # Primary keypoint - root
        self.joint_names = JOINT_NAMES[:127]
        self.person_center = person_center
        self.person_center_idx = None
        if self.person_center is not None:
            self.person_center_idx = self.joint_names.index(self.person_center)

    def forward(self,
                pose, shape, transl,
                loc, dist,
                K,
                expression=None, # facial expression
                K_to_proj=None,
                ):
        """
        Args:
            - pose: pose of the person in axis-angle - torch.Tensor [bs,24,3]
            - shape: torch.Tensor [bs,10]
            - loc: 2D location of the pelvis in pixel space - torch.Tensor [bs,2]
            - dist: distance of the pelvis from the camera in m - torch.Tensor [bs,1]
        Return:
            - dict containing a bunch of useful information about each person
        """

        if loc is not None and dist is not None:
            assert pose.shape[0] == shape.shape[0] == loc.shape[0] == dist.shape[0]
            assert len(loc.shape) == 2 and list(loc.shape[1:]) == [2]
            assert len(dist.shape) == 2 and list(dist.shape[1:]) == [1]
        if self.type == 'smpl':
            assert len(pose.shape) == 3 and list(pose.shape[1:]) == [24,3]
        elif self.type == 'smplx':
            assert len(pose.shape) == 3 and list(pose.shape[1:]) == [53,3] # taking root_orient, body_pose, lhand, rhan and jaw for the moment
        else:
            raise NameError
        assert len(shape.shape) == 2 and (list(shape.shape[1:]) == [self.num_betas] or list(shape.shape[1:]) == [self.num_betas+1])
        assert (transl is not None) or (loc is not None and dist is not None)

        bs = pose.shape[0]

        out = {}

        # No humans
        if bs == 0:
            return {}

        # Low dimensional parameters
        kwargs_pose = {
            'betas': shape,
        }
        kwargs_pose['global_orient'] = self.bm_x.global_orient.repeat(bs,1) # 0，0，0
        kwargs_pose['body_pose'] = pose[:,1:22].flatten(1)
        kwargs_pose['left_hand_pose'] = pose[:,22:37].flatten(1)
        kwargs_pose['right_hand_pose'] = pose[:,37:52].flatten(1)
        kwargs_pose['jaw_pose'] = pose[:,52:53].flatten(1)

        if expression is not None:
            kwargs_pose['expression'] = expression.flatten(1) # [bs,10]
        else:
            kwargs_pose['expression'] = self.bm_x.expression.repeat(bs,1)

        # default - to be generalized
        kwargs_pose['leye_pose'] = self.bm_x.leye_pose.repeat(bs,1)
        kwargs_pose['reye_pose'] = self.bm_x.reye_pose.repeat(bs,1)

        # Forward using the parametric 3d model SMPL-X layer
        output = self.bm_x(**kwargs_pose)
        verts = output.vertices
        j3d = output.joints # 45 joints
        R = roma.rotvec_to_rotmat(pose[:,0])

        # Apply global orientation on 3D points
        pelvis = j3d[:,[0]]
        j3d = (R.unsqueeze(1) @ (j3d - pelvis).unsqueeze(-1)).squeeze(-1)

        # Apply global orientation on 3D points - bis
        verts = (R.unsqueeze(1) @ (verts - pelvis).unsqueeze(-1)).squeeze(-1) # to pelvis-center coordinates: R(v-p)

        # Location of the person in 3D
        if transl is None:
            if K.dtype == torch.float16:
                # because of torch.inverse - not working with float16 at the moment
                transl = inverse_perspective_projection(loc.unsqueeze(1).float(), K.float(), dist.unsqueeze(1).float())[:,0]
                transl = transl.half()
            else:
                transl = inverse_perspective_projection(loc.unsqueeze(1), K, dist.unsqueeze(1))[:,0] # head center in camera coordinates

        # Updating transl if we choose a certain person center
        transl_up = transl.clone()

        # Definition of the translation depend on the args: 1) vanilla SMPL - 2) computed from a given joint
        if self.person_center_idx is None:
            # Add pelvis to transl - standard way for SMPLX layer
            transl_up = transl_up + pelvis[:,0] # back to smpl original coordinates
        else:
            # Center around the joint because teh translation is computed from this joint
            person_center = j3d[:, [self.person_center_idx]] # head center in pelvis-center coordinates
            verts = verts - person_center # nomalize to head-center coordinates
            j3d = j3d - person_center

        # Moving into the camera coordinate system
        j3d_cam = j3d + transl_up.unsqueeze(1) # move to camera coordinates
        verts_cam = verts + transl_up.unsqueeze(1)

        if K_to_proj is None:
            K_to_proj = K
        # Projection in camera plane
        j2d = perspective_projection(j3d_cam, K_to_proj)
        v2d = perspective_projection(verts_cam, K_to_proj)

        out.update({
            'smpl_v3d': verts_cam, # in 3d camera space
            'smpl_j3d': j3d_cam, # in 3d camera space
            'smpl_j2d': j2d,
            'smpl_v2d': v2d,
            'smpl_transl': transl, # translation of the primary keypoint in camera coordinates
            'smpl_transl_pelvis': j3d_cam[:,[0]], # root=pelvis in camera coordinates
        })

        return out


# Copyright (C) 2024-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).
#
# --------------------------------------------------------
# modified from DUSt3R

import sys
import os.path as path

HERE_PATH = path.normpath(path.dirname(__file__))
CROCO_REPO_PATH = path.normpath(path.join(HERE_PATH, "../../croco"))
CROCO_MODELS_PATH = path.join(CROCO_REPO_PATH, "models")

if path.isdir(CROCO_MODELS_PATH):

    sys.path.insert(0, CROCO_REPO_PATH)
else:
    raise ImportError(
        f"croco is not initialized, could not find: {CROCO_MODELS_PATH}.\n "
        "Did you forget to run 'git submodule update --init --recursive' ?"
    )


# Copyright (C) 2024-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).
#
# --------------------------------------------------------
# modified from DUSt3R

import torch
import numpy as np
from scipy.spatial import cKDTree as KDTree

from dust3r.utils.misc import invalid_to_zeros, invalid_to_nans
from dust3r.utils.device import to_numpy

def xy_grid(
    W,
    H,
    device=None,
    origin=(0, 0),
    unsqueeze=None,
    cat_dim=-1,
    homogeneous=False,
    **arange_kw,
):
    """Output a (H,W,2) array of int32
    with output[j,i,0] = i + origin[0]
         output[j,i,1] = j + origin[1]
    """
    if device is None:

        arange, meshgrid, stack, ones = np.arange, np.meshgrid, np.stack, np.ones
    else:

        arange = lambda *a, **kw: torch.arange(*a, device=device, **kw)
        meshgrid, stack = torch.meshgrid, torch.stack
        ones = lambda *a: torch.ones(*a, device=device)

    tw, th = [arange(o, o + s, **arange_kw) for s, o in zip((W, H), origin)]
    grid = meshgrid(tw, th, indexing="xy")
    if homogeneous:
        grid = grid + (ones((H, W)),)
    if unsqueeze is not None:
        grid = (grid[0].unsqueeze(unsqueeze), grid[1].unsqueeze(unsqueeze))
    if cat_dim is not None:
        grid = stack(grid, cat_dim)
    return grid

def geotrf(Trf, pts, ncol=None, norm=False):
    """Apply a geometric transformation to a list of 3-D points.

    H: 3x3 or 4x4 projection matrix (typically a Homography)
    p: numpy/torch/tuple of coordinates. Shape must be (...,2) or (...,3)

    ncol: int. number of columns of the result (2 or 3)
    norm: float. if != 0, the resut is projected on the z=norm plane.

    Returns an array of projected 2d points.
    """
    assert Trf.ndim >= 2
    if isinstance(Trf, np.ndarray):
        pts = np.asarray(pts)
    elif isinstance(Trf, torch.Tensor):
        pts = torch.as_tensor(pts, dtype=Trf.dtype)

    output_reshape = pts.shape[:-1]
    ncol = ncol or pts.shape[-1]

    if (
        isinstance(Trf, torch.Tensor)
        and isinstance(pts, torch.Tensor)
        and Trf.ndim == 3
        and pts.ndim == 4
    ):
        d = pts.shape[3]
        if Trf.shape[-1] == d:
            pts = torch.einsum("bij, bhwj -> bhwi", Trf, pts)
        elif Trf.shape[-1] == d + 1:
            pts = (
                torch.einsum("bij, bhwj -> bhwi", Trf[:, :d, :d], pts)
                + Trf[:, None, None, :d, d]
            )
        else:
            raise ValueError(f"bad shape, not ending with 3 or 4, for {pts.shape=}")
    else:
        if Trf.ndim >= 3:
            n = Trf.ndim - 2
            assert Trf.shape[:n] == pts.shape[:n], "batch size does not match"
            Trf = Trf.reshape(-1, Trf.shape[-2], Trf.shape[-1])

            if pts.ndim > Trf.ndim:

                pts = pts.reshape(Trf.shape[0], -1, pts.shape[-1])
            elif pts.ndim == 2:

                pts = pts[:, None, :]

        if pts.shape[-1] + 1 == Trf.shape[-1]:
            Trf = Trf.swapaxes(-1, -2)  # transpose Trf
            pts = pts @ Trf[..., :-1, :] + Trf[..., -1:, :]
        elif pts.shape[-1] == Trf.shape[-1]:
            Trf = Trf.swapaxes(-1, -2)  # transpose Trf
            pts = pts @ Trf
        else:
            pts = Trf @ pts.T
            if pts.ndim >= 2:
                pts = pts.swapaxes(-1, -2)

    if norm:
        pts = pts / pts[..., -1:]  # DONT DO /= BECAUSE OF WEIRD PYTORCH BUG
        if norm != 1:
            pts *= norm

    res = pts[..., :ncol].reshape(*output_reshape, ncol)
    return res

def inv(mat):
    """Invert a torch or numpy matrix"""
    if isinstance(mat, torch.Tensor):
        return torch.linalg.inv(mat)
    if isinstance(mat, np.ndarray):
        return np.linalg.inv(mat)
    raise ValueError(f"bad matrix type = {type(mat)}")

def depthmap_to_pts3d(depth, pseudo_focal, pp=None, **_):
    """
    Args:
        - depthmap (BxHxW array):
        - pseudo_focal: [B,H,W] ; [B,2,H,W] or [B,1,H,W]
    Returns:
        pointmap of absolute coordinates (BxHxWx3 array)
    """

    if len(depth.shape) == 4:
        B, H, W, n = depth.shape
    else:
        B, H, W = depth.shape
        n = None

    if len(pseudo_focal.shape) == 3:  # [B,H,W]
        pseudo_focalx = pseudo_focaly = pseudo_focal
    elif len(pseudo_focal.shape) == 4:  # [B,2,H,W] or [B,1,H,W]
        pseudo_focalx = pseudo_focal[:, 0]
        if pseudo_focal.shape[1] == 2:
            pseudo_focaly = pseudo_focal[:, 1]
        else:
            pseudo_focaly = pseudo_focalx
    else:
        raise NotImplementedError("Error, unknown input focal shape format.")

    assert pseudo_focalx.shape == depth.shape[:3]
    assert pseudo_focaly.shape == depth.shape[:3]
    grid_x, grid_y = xy_grid(W, H, cat_dim=0, device=depth.device)[:, None]

    if pp is None:
        grid_x = grid_x - (W - 1) / 2
        grid_y = grid_y - (H - 1) / 2
    else:
        grid_x = grid_x.expand(B, -1, -1) - pp[:, 0, None, None]
        grid_y = grid_y.expand(B, -1, -1) - pp[:, 1, None, None]

    if n is None:
        pts3d = torch.empty((B, H, W, 3), device=depth.device)
        pts3d[..., 0] = depth * grid_x / pseudo_focalx
        pts3d[..., 1] = depth * grid_y / pseudo_focaly
        pts3d[..., 2] = depth
    else:
        pts3d = torch.empty((B, H, W, 3, n), device=depth.device)
        pts3d[..., 0, :] = depth * (grid_x / pseudo_focalx)[..., None]
        pts3d[..., 1, :] = depth * (grid_y / pseudo_focaly)[..., None]
        pts3d[..., 2, :] = depth
    return pts3d

def depthmap_to_camera_coordinates(depthmap, camera_intrinsics, pseudo_focal=None):
    """
    Args:
        - depthmap (HxW array):
        - camera_intrinsics: a 3x3 matrix
    Returns:
        pointmap of absolute coordinates (HxWx3 array), and a mask specifying valid pixels.
    """
    camera_intrinsics = np.float32(camera_intrinsics)
    H, W = depthmap.shape

    assert camera_intrinsics[0, 1] == 0.0
    assert camera_intrinsics[1, 0] == 0.0
    if pseudo_focal is None:
        fu = camera_intrinsics[0, 0]
        fv = camera_intrinsics[1, 1]
    else:
        assert pseudo_focal.shape == (H, W)
        fu = fv = pseudo_focal
    cu = camera_intrinsics[0, 2]
    cv = camera_intrinsics[1, 2]

    u, v = np.meshgrid(np.arange(W), np.arange(H))
    z_cam = depthmap
    x_cam = (u - cu) * z_cam / fu
    y_cam = (v - cv) * z_cam / fv
    X_cam = np.stack((x_cam, y_cam, z_cam), axis=-1).astype(np.float32)

    valid_mask = depthmap > 0.0
    return X_cam, valid_mask

def depthmap_to_absolute_camera_coordinates(
    depthmap, camera_intrinsics, camera_pose, **kw
):
    """
    Args:
        - depthmap (HxW array):
        - camera_intrinsics: a 3x3 matrix
        - camera_pose: a 4x3 or 4x4 cam2world matrix
    Returns:
        pointmap of absolute coordinates (HxWx3 array), and a mask specifying valid pixels.
    """
    X_cam, valid_mask = depthmap_to_camera_coordinates(depthmap, camera_intrinsics)

    X_world = X_cam  # default
    if camera_pose is not None:

        R_cam2world = camera_pose[:3, :3]
        t_cam2world = camera_pose[:3, 3]

        X_world = (
            np.einsum("ik, vuk -> vui", R_cam2world, X_cam) + t_cam2world[None, None, :]
        )

    return X_world, valid_mask

def colmap_to_opencv_intrinsics(K):
    """
    Modify camera intrinsics to follow a different convention.
    Coordinates of the center of the top-left pixels are by default:
    - (0.5, 0.5) in Colmap
    - (0,0) in OpenCV
    """
    K = K.copy()
    K[0, 2] -= 0.5
    K[1, 2] -= 0.5
    return K

def opencv_to_colmap_intrinsics(K):
    """
    Modify camera intrinsics to follow a different convention.
    Coordinates of the center of the top-left pixels are by default:
    - (0.5, 0.5) in Colmap
    - (0,0) in OpenCV
    """
    K = K.copy()
    K[0, 2] += 0.5
    K[1, 2] += 0.5
    return K

def normalize_pointcloud(
    pts1, pts2, norm_mode="avg_dis", valid1=None, valid2=None, ret_factor=False
):
    """renorm pointmaps pts1, pts2 with norm_mode"""
    assert pts1.ndim >= 3 and pts1.shape[-1] == 3
    assert pts2 is None or (pts2.ndim >= 3 and pts2.shape[-1] == 3)
    norm_mode, dis_mode = norm_mode.split("_")

    if norm_mode == "avg":

        nan_pts1, nnz1 = invalid_to_zeros(pts1, valid1, ndim=3)
        nan_pts2, nnz2 = (
            invalid_to_zeros(pts2, valid2, ndim=3) if pts2 is not None else (None, 0)
        )
        all_pts = (
            torch.cat((nan_pts1, nan_pts2), dim=1) if pts2 is not None else nan_pts1
        )

        all_dis = all_pts.norm(dim=-1)
        if dis_mode == "dis":
            pass  # do nothing
        elif dis_mode == "log1p":
            all_dis = torch.log1p(all_dis)
        elif dis_mode == "warp-log1p":

            log_dis = torch.log1p(all_dis)
            warp_factor = log_dis / all_dis.clip(min=1e-8)
            H1, W1 = pts1.shape[1:-1]
            pts1 = pts1 * warp_factor[:, : W1 * H1].view(-1, H1, W1, 1)
            if pts2 is not None:
                H2, W2 = pts2.shape[1:-1]
                pts2 = pts2 * warp_factor[:, W1 * H1 :].view(-1, H2, W2, 1)
            all_dis = log_dis  # this is their true distance afterwards
        else:
            raise ValueError(f"bad {dis_mode=}")

        norm_factor = all_dis.sum(dim=1) / (nnz1 + nnz2 + 1e-8)
    else:

        nan_pts1 = invalid_to_nans(pts1, valid1, ndim=3)
        nan_pts2 = invalid_to_nans(pts2, valid2, ndim=3) if pts2 is not None else None
        all_pts = (
            torch.cat((nan_pts1, nan_pts2), dim=1) if pts2 is not None else nan_pts1
        )

        all_dis = all_pts.norm(dim=-1)

        if norm_mode == "avg":
            norm_factor = all_dis.nanmean(dim=1)
        elif norm_mode == "median":
            norm_factor = all_dis.nanmedian(dim=1).values.detach()
        elif norm_mode == "sqrt":
            norm_factor = all_dis.sqrt().nanmean(dim=1) ** 2
        else:
            raise ValueError(f"bad {norm_mode=}")

    norm_factor = norm_factor.clip(min=1e-8)
    while norm_factor.ndim < pts1.ndim:
        norm_factor.unsqueeze_(-1)

    res = pts1 / norm_factor
    if pts2 is not None:
        res = (res, pts2 / norm_factor)
    if ret_factor:
        res = res + (norm_factor,)
    return res

def normalize_pointcloud_group(
    pts_list,
    norm_mode="avg_dis",
    valid_list=None,
    conf_list=None,
    ret_factor=False,
    ret_factor_only=False,
):
    """renorm pointmaps pts1, pts2 with norm_mode"""
    for pts in pts_list:
        assert pts.ndim >= 3 and pts.shape[-1] == 3

    norm_mode, dis_mode = norm_mode.split("_")

    if norm_mode == "avg":

        nan_pts_list, nnz_list = zip(
            *[
                invalid_to_zeros(pts1, valid1, ndim=3)
                for pts1, valid1 in zip(pts_list, valid_list)
            ]
        )
        all_pts = torch.cat(nan_pts_list, dim=1)
        if conf_list is not None:
            nan_conf_list = [
                invalid_to_zeros(conf1[..., None], valid1, ndim=3)[0]
                for conf1, valid1 in zip(conf_list, valid_list)
            ]
            all_conf = torch.cat(nan_conf_list, dim=1)[..., 0]
        else:
            all_conf = torch.ones_like(all_pts[..., 0])

        all_dis = all_pts.norm(dim=-1)
        if dis_mode == "dis":
            pass  # do nothing
        elif dis_mode == "log1p":
            all_dis = torch.log1p(all_dis)
        elif dis_mode == "warp-log1p":

            log_dis = torch.log1p(all_dis)
            warp_factor = log_dis / all_dis.clip(min=1e-8)
            H_W_list = [pts.shape[1:-1] for pts in pts_list]
            pts_list = [
                pts
                * warp_factor[:, sum(H_W_list[:i]) : sum(H_W_list[: i + 1])].view(
                    -1, H, W, 1
                )
                for i, (pts, (H, W)) in enumerate(zip(pts_list, H_W_list))
            ]
            all_dis = log_dis  # this is their true distance afterwards
        else:
            raise ValueError(f"bad {dis_mode=}")

        norm_factor = (all_conf * all_dis).sum(dim=1) / (all_conf.sum(dim=1) + 1e-8)
    else:

        nan_pts_list = [
            invalid_to_nans(pts1, valid1, ndim=3)
            for pts1, valid1 in zip(pts_list, valid_list)
        ]

        all_pts = torch.cat(nan_pts_list, dim=1)

        all_dis = all_pts.norm(dim=-1)

        if norm_mode == "avg":
            norm_factor = all_dis.nanmean(dim=1)
        elif norm_mode == "median":
            norm_factor = all_dis.nanmedian(dim=1).values.detach()
        elif norm_mode == "sqrt":
            norm_factor = all_dis.sqrt().nanmean(dim=1) ** 2
        else:
            raise ValueError(f"bad {norm_mode=}")

    norm_factor = norm_factor.clip(min=1e-8)
    while norm_factor.ndim < pts_list[0].ndim:
        norm_factor.unsqueeze_(-1)

    if ret_factor_only:

        return norm_factor

    res = [pts / norm_factor for pts in pts_list]
    if ret_factor:
        return res, norm_factor
    return res

@torch.no_grad()
def get_joint_pointcloud_depth(z1, z2, valid_mask1, valid_mask2=None, quantile=0.5):

    _z1 = invalid_to_nans(z1, valid_mask1).reshape(len(z1), -1)
    _z2 = (
        invalid_to_nans(z2, valid_mask2).reshape(len(z2), -1)
        if z2 is not None
        else None
    )
    _z = torch.cat((_z1, _z2), dim=-1) if z2 is not None else _z1

    if quantile == 0.5:
        shift_z = torch.nanmedian(_z, dim=-1).values
    else:
        shift_z = torch.nanquantile(_z, quantile, dim=-1)
    return shift_z  # (B,)

@torch.no_grad()
def get_group_pointcloud_depth(zs, valid_masks, quantile=0.5):

    _zs = [
        invalid_to_nans(z1, valid_mask1).reshape(len(z1), -1)
        for z1, valid_mask1 in zip(zs, valid_masks)
    ]
    _z = torch.cat(_zs, dim=-1)

    if quantile == 0.5:
        shift_z = torch.nanmedian(_z, dim=-1).values
    else:
        shift_z = torch.nanquantile(_z, quantile, dim=-1)
    return shift_z  # (B,)

@torch.no_grad()
def get_joint_pointcloud_center_scale(
    pts1, pts2, valid_mask1=None, valid_mask2=None, z_only=False, center=True
):

    _pts1 = invalid_to_nans(pts1, valid_mask1).reshape(len(pts1), -1, 3)
    _pts2 = (
        invalid_to_nans(pts2, valid_mask2).reshape(len(pts2), -1, 3)
        if pts2 is not None
        else None
    )
    _pts = torch.cat((_pts1, _pts2), dim=1) if pts2 is not None else _pts1

    _center = torch.nanmedian(_pts, dim=1, keepdim=True).values  # (B,1,3)
    if z_only:
        _center[..., :2] = 0  # do not center X and Y

    _norm = ((_pts - _center) if center else _pts).norm(dim=-1)
    scale = torch.nanmedian(_norm, dim=1).values
    return _center[:, None, :, :], scale[:, None, None, None]

@torch.no_grad()
def get_group_pointcloud_center_scale(pts, valid_masks=None, z_only=False, center=True):

    _pts = [
        invalid_to_nans(pts1, valid_mask1).reshape(len(pts1), -1, 3)
        for pts1, valid_mask1 in zip(pts, valid_masks)
    ]
    _pts = torch.cat(_pts, dim=1)

    _center = torch.nanmedian(_pts, dim=1, keepdim=True).values  # (B,1,3)
    if z_only:
        _center[..., :2] = 0  # do not center X and Y

    _norm = ((_pts - _center) if center else _pts).norm(dim=-1)
    scale = torch.nanmedian(_norm, dim=1).values
    return _center[:, None, :, :], scale[:, None, None, None]

def find_reciprocal_matches(P1, P2):
    """
    returns 3 values:
    1 - reciprocal_in_P2: a boolean array of size P2.shape[0], a "True" value indicates a match
    2 - nn2_in_P1: a int array of size P2.shape[0], it contains the indexes of the closest points in P1
    3 - reciprocal_in_P2.sum(): the number of matches
    """
    tree1 = KDTree(P1)
    tree2 = KDTree(P2)

    _, nn1_in_P2 = tree2.query(P1, workers=8)
    _, nn2_in_P1 = tree1.query(P2, workers=8)

    reciprocal_in_P1 = nn2_in_P1[nn1_in_P2] == np.arange(len(nn1_in_P2))
    reciprocal_in_P2 = nn1_in_P2[nn2_in_P1] == np.arange(len(nn2_in_P1))
    assert reciprocal_in_P1.sum() == reciprocal_in_P2.sum()
    return reciprocal_in_P2, nn2_in_P1, reciprocal_in_P2.sum()

def get_med_dist_between_poses(poses):
    from scipy.spatial.distance import pdist

    return np.median(pdist([to_numpy(p[:3, 3]) for p in poses]))

def weighted_procrustes(A, B, w, use_weights=True, eps=1e-16, return_T=False):
    """
    X: torch tensor B x N x 3
    Y: torch tensor B x N x 3
    w: torch tensor B x N
    """
    assert len(A) == len(B)
    if use_weights:
        W1 = torch.abs(w).sum(1, keepdim=True)
        w_norm = (w / (W1 + eps)).unsqueeze(-1)
        a_mean = (w_norm * A).sum(dim=1, keepdim=True)
        b_mean = (w_norm * B).sum(dim=1, keepdim=True)

        A_c = A - a_mean
        B_c = B - b_mean

        H = torch.einsum("bni,bnj->bij", A_c, w_norm * B_c)

    else:
        a_mean = A.mean(axis=1, keepdim=True)
        b_mean = B.mean(axis=1, keepdim=True)

        A_c = A - a_mean
        B_c = B - b_mean

        H = torch.einsum("bij,bik->bjk", A_c, B_c)

    U, S, V = torch.svd(H)  # U: B x 3 x 3, S: B x 3, V: B x 3 x 3
    Z = torch.eye(3).unsqueeze(0).repeat(A.shape[0], 1, 1).to(A.device)
    Z[:, -1, -1] = torch.sign(torch.linalg.det(U @ V.transpose(1, 2)))  # B x 3 x 3
    R = V @ Z @ U.transpose(1, 2)  # B x 3 x 3
    t = b_mean - torch.einsum("bij,bjk->bik", R, a_mean.transpose(-2, -1)).transpose(
        -2, -1
    )
    if return_T:
        T = torch.eye(4).unsqueeze(0).repeat(A.shape[0], 1, 1).to(A.device)
        T[:, :3, :3] = R
        T[:, :3, 3] = t.squeeze()
        return T
    return R, t.squeeze()

def perspective_projection(x, K):
    """
    This function computes the perspective projection of a set of points assuming the extrinsinc params have already been applied
    Args:
        - x [bs,N,3]: 3D points
        - K [bs,3,3]: Camera instrincs params
    """
    # Apply perspective distortion
    y = x / x[:, :, -1].unsqueeze(-1)  # (bs, N, 3)

    # Apply camera intrinsics
    y = torch.einsum('bij,bkj->bki', K, y)  # (bs, N, 3)

    return y[:, :, :2]

def inverse_perspective_projection(points, K, distance):
    """
    This function computes the inverse perspective projection of a set of points given an estimated distance.
    Input:
        points (bs, N, 2): 2D points
        K (bs,3,3): camera intrinsics params
        distance (bs, N, 1): distance in the 3D world
    Similar to:
        - pts_l_norm = cv2.undistortPoints(np.expand_dims(pts_l, axis=1), cameraMatrix=K_l, distCoeffs=None)
    """
    # Apply camera intrinsics
    points = torch.cat([points, torch.ones_like(points[..., :1])], -1)
    points = torch.einsum('bij,bkj->bki', torch.inverse(K), points)

    # Apply perspective distortion
    if distance == None:
        return points
    points = points * distance
    return points

def get_focalLength_from_fieldOfView(fov=60, img_size=512):
    """
    Compute the focal length of the camera lens by assuming a certain FOV for the entire image
    Args:
        - fov: float, expressed in degree
        - img_size: int
    Return:
        focal: float
    """
    focal = img_size / (2 * np.tan(np.radians(fov) /2))
    return focal

def focal_length_normalization(x, f, fovn=60, img_size=448):
    """
    Section 3.1 of https://arxiv.org/pdf/1904.02028.pdf
    E = (fn/f) * E' where E is 1/d
    """
    fn = get_focalLength_from_fieldOfView(fov=fovn, img_size=img_size)
    y = x * (fn/f)
    return y

def undo_focal_length_normalization(y, f, fovn=60, img_size=448):
    """
    Undo focal_length_normalization()
    """
    fn = get_focalLength_from_fieldOfView(fov=fovn, img_size=img_size)
    x = y * (f/fn)
    return x

EPS_LOG = 1e-10
def log_depth(x, eps=EPS_LOG):
    """
    Move depth to log space
    """
    return torch.log(x + eps)

def undo_log_depth(y, eps=EPS_LOG):
    """
    Undo log_depth()
    """
    return torch.exp(y) - eps

def get_camera_parameters(img_size, fov=60, p_x=None, p_y=None, device=torch.device('cuda')):
    """ Given image size, fov and principal point coordinates, return K the camera parameter matrix"""
    K = torch.
# ... truncated ...


# Copyright (C) 2024-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).
#
# --------------------------------------------------------
# modified from DUSt3R

import torch
import numpy as np
from scipy.spatial import cKDTree as KDTree

from dust3r.utils.misc import invalid_to_zeros, invalid_to_nans
from dust3r.utils.device import to_numpy

def xy_grid(
    W,
    H,
    device=None,
    origin=(0, 0),
    unsqueeze=None,
    cat_dim=-1,
    homogeneous=False,
    **arange_kw,
):
    """Output a (H,W,2) array of int32
    with output[j,i,0] = i + origin[0]
         output[j,i,1] = j + origin[1]
    """
    if device is None:

        arange, meshgrid, stack, ones = np.arange, np.meshgrid, np.stack, np.ones
    else:

        arange = lambda *a, **kw: torch.arange(*a, device=device, **kw)
        meshgrid, stack = torch.meshgrid, torch.stack
        ones = lambda *a: torch.ones(*a, device=device)

    tw, th = [arange(o, o + s, **arange_kw) for s, o in zip((W, H), origin)]
    grid = meshgrid(tw, th, indexing="xy")
    if homogeneous:
        grid = grid + (ones((H, W)),)
    if unsqueeze is not None:
        grid = (grid[0].unsqueeze(unsqueeze), grid[1].unsqueeze(unsqueeze))
    if cat_dim is not None:
        grid = stack(grid, cat_dim)
    return grid

def geotrf(Trf, pts, ncol=None, norm=False):
    """Apply a geometric transformation to a list of 3-D points.

    H: 3x3 or 4x4 projection matrix (typically a Homography)
    p: numpy/torch/tuple of coordinates. Shape must be (...,2) or (...,3)

    ncol: int. number of columns of the result (2 or 3)
    norm: float. if != 0, the resut is projected on the z=norm plane.

    Returns an array of projected 2d points.
    """
    assert Trf.ndim >= 2
    if isinstance(Trf, np.ndarray):
        pts = np.asarray(pts)
    elif isinstance(Trf, torch.Tensor):
        pts = torch.as_tensor(pts, dtype=Trf.dtype)

    output_reshape = pts.shape[:-1]


geotrf

    if device is None:

        arange, meshgrid, stack, ones = np.arange, np.meshgrid, np.stack, np.ones
    else:

        arange = lambda *a, **kw: torch.arange(*a, device=device, **kw)
        meshgrid, stack = torch.meshgrid, torch.stack
        ones = lambda *a: torch.ones(*a, device=device)

    tw, th = [arange(o, o + s, **arange_kw) for s, o in zip((W, H), origin)]
    grid = meshgrid(tw, th, indexing="xy")
    if homogeneous:
        grid = grid + (ones((H, W)),)
    if unsqueeze is not None:
        grid = (grid[0].unsqueeze(unsqueeze), grid[1].unsqueeze(unsqueeze))
    if cat_dim is not None:
        grid = stack(grid, cat_dim)
    return grid

def geotrf(Trf, pts, ncol=None, norm=False):
    """Apply a geometric transformation to a list of 3-D points.

    H: 3x3 or 4x4 projection matrix (typically a Homography)
    p: numpy/torch/tuple of coordinates. Shape must be (...,2) or (...,3)

    ncol: int. number of columns of the result (2 or 3)
    norm: float. if != 0, the resut is projected on the z=norm plane.

    Returns an array of projected 2d points.
    """
    assert Trf.ndim >= 2
    if isinstance(Trf, np.ndarray):
        pts = np.asarray(pts)
    elif isinstance(Trf, torch.Tensor):
        pts = torch.as_tensor(pts, dtype=Trf.dtype)

    output_reshape = pts.shape[:-1]
    ncol = ncol or pts.shape[-1]

    if (
        isinstance(Trf, torch.Tensor)
        and isinstance(pts, torch.Tensor)
        and Trf.ndim == 3
        and pts.ndim == 4
    ):
        d = pts.shape[3]
        if Trf.shape[-1] == d:
            pts = torch.einsum("bij, bhwj -> bhwi", Trf, pts)
        elif Trf.shape[-1] == d + 1:
            pts = (
                torch.einsum("bij, bhwj -> bhwi", Trf[:, :d, :d], pts)
                + Trf[:, None, None, :d, d]
            )
        else:
            raise ValueError(f"bad shape, not ending with 3 or 4, for {pts.shape=}")
    else:
        if Trf.ndim >= 3:
            n = Trf.ndim - 2
            assert Trf.shape[:n] == pts.shape[:n], "batch size does not match"
            Trf = Trf.reshape(-1, Trf.shape[-2], Trf.shape[-1])

            if pts.ndim > Trf.ndim:

                pts = pts.reshape(Trf.shape[0], -1, pts.shape[-1])
            elif pts.ndim == 2:

                pts = pts[:, None, :]

        if pts.shape[-1] + 1 == Trf.shape[-1]:
            Trf = Trf.swapaxes(-1, -2)  # transpose Trf
            pts = pts @ Trf[..., :-1, :] + Trf[..., -1:, :]
        elif pts.shape[-1] == Trf.shape[-1]:
            Trf = Trf.swapaxes(-1, -2)  # transpose Trf
            pts = pts @ Trf
        else:
            pts = Trf @ pts.T
            if pts.ndim >= 2:
                pts = pts.swapaxes(-1, -2)

    if norm:
        pts = pts / pts[..., -1:]  # DONT DO /= BECAUSE OF WEIRD PYTORCH BUG
        if norm != 1:
            pts *= norm

    res = pts[..., :ncol].reshape(*output_reshape, ncol)
    return res

def inv(mat):
    """Invert a torch or numpy matrix"""
    if isinstance(mat, torch.Tensor):
        return torch.linalg.inv(mat)
    if isinstance(mat, np.ndarray):
        return np.linalg.inv(mat)
    raise ValueError(f"bad matrix type = {type(mat)}")

def depthmap_to_pts3d(depth, pseudo_focal, pp=None, **_):
    """
    Args:
        - depthmap (BxHxW array):
        - pseudo_focal: [B,H,W] ; [B,2,H,W] or [B,1,H,W]
    Returns:
        pointmap of absolute coordinates (BxHxWx3 array)
    """


inv

        if pts.shape[-1] + 1 == Trf.shape[-1]:
            Trf = Trf.swapaxes(-1, -2)  # transpose Trf
            pts = pts @ Trf[..., :-1, :] + Trf[..., -1:, :]
        elif pts.shape[-1] == Trf.shape[-1]:
            Trf = Trf.swapaxes(-1, -2)  # transpose Trf
            pts = pts @ Trf
        else:
            pts = Trf @ pts.T
            if pts.ndim >= 2:
                pts = pts.swapaxes(-1, -2)

    if norm:
        pts = pts / pts[..., -1:]  # DONT DO /= BECAUSE OF WEIRD PYTORCH BUG
        if norm != 1:
            pts *= norm

    res = pts[..., :ncol].reshape(*output_reshape, ncol)
    return res

def inv(mat):
    """Invert a torch or numpy matrix"""
    if isinstance(mat, torch.Tensor):
        return torch.linalg.inv(mat)
    if isinstance(mat, np.ndarray):
        return np.linalg.inv(mat)
    raise ValueError(f"bad matrix type = {type(mat)}")

def depthmap_to_pts3d(depth, pseudo_focal, pp=None, **_):
    """
    Args:
        - depthmap (BxHxW array):
        - pseudo_focal: [B,H,W] ; [B,2,H,W] or [B,1,H,W]
    Returns:
        pointmap of absolute coordinates (BxHxWx3 array)
    """

    if len(depth.shape) == 4:
        B, H, W, n = depth.shape
    else:
        B, H, W = depth.shape
        n = None

    if len(pseudo_focal.shape) == 3:  # [B,H,W]
        pseudo_focalx = pseudo_focaly = pseudo_focal
    elif len(pseudo_focal.shape) == 4:  # [B,2,H,W] or [B,1,H,W]


                pts = pts.swapaxes(-1, -2)

    if norm:
        pts = pts / pts[..., -1:]  # DONT DO /= BECAUSE OF WEIRD PYTORCH BUG
        if norm != 1:
            pts *= norm

    res = pts[..., :ncol].reshape(*output_reshape, ncol)
    return res

def inv(mat):
    """Invert a torch or numpy matrix"""
    if isinstance(mat, torch.Tensor):
        return torch.linalg.inv(mat)
    if isinstance(mat, np.ndarray):
        return np.linalg.inv(mat)
    raise ValueError(f"bad matrix type = {type(mat)}")

def depthmap_to_pts3d(depth, pseudo_focal, pp=None, **_):
    """
    Args:
        - depthmap (BxHxW array):
        - pseudo_focal: [B,H,W] ; [B,2,H,W] or [B,1,H,W]
    Returns:
        pointmap of absolute coordinates (BxHxWx3 array)
    """

    if len(depth.shape) == 4:
        B, H, W, n = depth.shape
    else:
        B, H, W = depth.shape
        n = None

    if len(pseudo_focal.shape) == 3:  # [B,H,W]
        pseudo_focalx = pseudo_focaly = pseudo_focal
    elif len(pseudo_focal.shape) == 4:  # [B,2,H,W] or [B,1,H,W]
        pseudo_focalx = pseudo_focal[:, 0]
        if pseudo_focal.shape[1] == 2:
            pseudo_focaly = pseudo_focal[:, 1]
        else:
            pseudo_focaly = pseudo_focalx
    else:
        raise NotImplementedError("Error, unknown input focal shape format.")

    assert pseudo_focalx.shape == depth.shape[:3]
    assert pseudo_focaly.shape == depth.shape[:3]
    grid_x, grid_y = xy_grid(W, H, cat_dim=0, device=depth.device)[:, None]

    if pp is None:
        grid_x = grid_x - (W - 1) / 2
        grid_y = grid_y - (H - 1) / 2
    else:
        grid_x = grid_x.expand(B, -1, -1) - pp[:, 0, None, None]
        grid_y = grid_y.expand(B, -1, -1) - pp[:, 1, None, None]

    if n is None:
        pts3d = torch.empty((B, H, W, 3), device=depth.device)
        pts3d[..., 0] = depth * grid_x / pseudo_focalx
        pts3d[..., 1] = depth * grid_y / pseudo_focaly
        pts3d[..., 2] = depth
    else:
        pts3d = torch.empty((B, H, W, 3, n), device=depth.device)
        pts3d[..., 0, :] = depth * (grid_x / pseudo_focalx)[..., None]
        pts3d[..., 1, :] = depth * (grid_y / pseudo_focaly)[..., None]
        pts3d[..., 2, :] = depth
    return pts3d

def depthmap_to_camera_coordinates(depthmap, camera_intrinsics, pseudo_focal=None):
    """
    Args:
        - depthmap (HxW array):
        - camera_intrinsics: a 3x3 matrix
    Returns:
        pointmap of absolute coordinates (HxWx3 array), and a mask specifying valid pixels.
    """
    camera_intrinsics = np.float32(camera_intrinsics)
    H, W = depthmap.shape

    assert camera_intrinsics[0, 1] == 0.0
    assert camera_intrinsics[1, 0] == 0.0
    if pseudo_focal is None:
        fu = camera_intrinsics[0, 0]
        fv = camera_intrinsics[1, 1]
    else:
        assert pseudo_focal.shape == (H, W)


    if pp is None:
        grid_x = grid_x - (W - 1) / 2
        grid_y = grid_y - (H - 1) / 2
    else:
        grid_x = grid_x.expand(B, -1, -1) - pp[:, 0, None, None]
        grid_y = grid_y.expand(B, -1, -1) - pp[:, 1, None, None]

    if n is None:
        pts3d = torch.empty((B, H, W, 3), device=depth.device)
        pts3d[..., 0] = depth * grid_x / pseudo_focalx
        pts3d[..., 1] = depth * grid_y / pseudo_focaly
        pts3d[..., 2] = depth
    else:
        pts3d = torch.empty((B, H, W, 3, n), device=depth.device)
        pts3d[..., 0, :] = depth * (grid_x / pseudo_focalx)[..., None]
        pts3d[..., 1, :] = depth * (grid_y / pseudo_focaly)[..., None]
        pts3d[..., 2, :] = depth
    return pts3d

def depthmap_to_camera_coordinates(depthmap, camera_intrinsics, pseudo_focal=None):
    """
    Args:
        - depthmap (HxW array):
        - camera_intrinsics: a 3x3 matrix
    Returns:
        pointmap of absolute coordinates (HxWx3 array), and a mask specifying valid pixels.
    """
    camera_intrinsics = np.float32(camera_intrinsics)
    H, W = depthmap.shape

    assert camera_intrinsics[0, 1] == 0.0
    assert camera_intrinsics[1, 0] == 0.0
    if pseudo_focal is None:
        fu = camera_intrinsics[0, 0]
        fv = camera_intrinsics[1, 1]
    else:
        assert pseudo_focal.shape == (H, W)
        fu = fv = pseudo_focal
    cu = camera_intrinsics[0, 2]
    cv = camera_intrinsics[1, 2]

    u, v = np.meshgrid(np.arange(W), np.arange(H))
    z_cam = depthmap
    x_cam = (u - cu) * z_cam / fu
    y_cam = (v - cv) * z_cam / fv
    X_cam = np.stack((x_cam, y_cam, z_cam), axis=-1).astype(np.float32)

    valid_mask = depthmap > 0.0
    return X_cam, valid_mask

def depthmap_to_absolute_camera_coordinates(
    depthmap, camera_intrinsics, camera_pose, **kw
):
    """
    Args:
        - depthmap (HxW array):
        - camera_intrinsics: a 3x3 matrix
        - camera_pose: a 4x3 or 4x4 cam2world matrix
    Returns:
        pointmap of absolute coordinates (HxWx3 array), and a mask specifying valid pixels.
    """
    X_cam, valid_mask = depthmap_to_camera_coordinates(depthmap, camera_intrinsics)

    X_world = X_cam  # default
    if camera_pose is not None:

        R_cam2world = camera_pose[:3, :3]
        t_cam2world = camera_pose[:3, 3]


    assert camera_intrinsics[1, 0] == 0.0
    if pseudo_focal is None:
        fu = camera_intrinsics[0, 0]
        fv = camera_intrinsics[1, 1]
    else:
        assert pseudo_focal.shape == (H, W)
        fu = fv = pseudo_focal
    cu = camera_intrinsics[0, 2]
    cv = camera_intrinsics[1, 2]

    u, v = np.meshgrid(np.arange(W), np.arange(H))
    z_cam = depthmap
    x_cam = (u - cu) * z_cam / fu
    y_cam = (v - cv) * z_cam / fv
    X_cam = np.stack((x_cam, y_cam, z_cam), axis=-1).astype(np.float32)

    valid_mask = depthmap > 0.0
    return X_cam, valid_mask

def depthmap_to_absolute_camera_coordinates(
    depthmap, camera_intrinsics, camera_pose, **kw
):
    """
    Args:
        - depthmap (HxW array):
        - camera_intrinsics: a 3x3 matrix
        - camera_pose: a 4x3 or 4x4 cam2world matrix
    Returns:
        pointmap of absolute coordinates (HxWx3 array), and a mask specifying valid pixels.
    """
    X_cam, valid_mask = depthmap_to_camera_coordinates(depthmap, camera_intrinsics)

    X_world = X_cam  # default
    if camera_pose is not None:

        R_cam2world = camera_pose[:3, :3]
        t_cam2world = camera_pose[:3, 3]

        X_world = (
            np.einsum("ik, vuk -> vui", R_cam2world, X_cam) + t_cam2world[None, None, :]
        )

    return X_world, valid_mask

def colmap_to_opencv_intrinsics(K):
    """
    Modify camera intrinsics to follow a different convention.
    Coordinates of the center of the top-left pixels are by default:
    - (0.5, 0.5) in Colmap
    - (0,0) in OpenCV
    """
    K = K.copy()
    K[0, 2] -= 0.5
    K[1, 2] -= 0.5
    return K

def opencv_to_colmap_intrinsics(K):
    """
    Modify camera intrinsics to follow a different convention.
    Coordinates of the center of the top-left pixels are by default:
    - (0.5, 0.5) in Colmap


        - camera_intrinsics: a 3x3 matrix
        - camera_pose: a 4x3 or 4x4 cam2world matrix
    Returns:
        pointmap of absolute coordinates (HxWx3 array), and a mask specifying valid pixels.
    """
    X_cam, valid_mask = depthmap_to_camera_coordinates(depthmap, camera_intrinsics)

    X_world = X_cam  # default
    if camera_pose is not None:

        R_cam2world = camera_pose[:3, :3]
        t_cam2world = camera_pose[:3, 3]

        X_world = (
            np.einsum("ik, vuk -> vui", R_cam2world, X_cam) + t_cam2world[None, None, :]
        )

    return X_world, valid_mask

def colmap_to_opencv_intrinsics(K):
    """
    Modify camera intrinsics to follow a different convention.
    Coordinates of the center of the top-left pixels are by default:
    - (0.5, 0.5) in Colmap
    - (0,0) in OpenCV
    """
    K = K.copy()
    K[0, 2] -= 0.5
    K[1, 2] -= 0.5
    return K

def opencv_to_colmap_intrinsics(K):
    """
    Modify camera intrinsics to follow a different convention.
    Coordinates of the center of the top-left pixels are by default:
    - (0.5, 0.5) in Colmap
    - (0,0) in OpenCV
    """
    K = K.copy()
    K[0, 2] += 0.5
    K[1, 2] += 0.5
    return K

def normalize_pointcloud(
    pts1, pts2, norm_mode="avg_dis", valid1=None, valid2=None, ret_factor=False
):
    """renorm pointmaps pts1, pts2 with norm_mode"""
    assert pts1.ndim >= 3 and pts1.shape[-1] == 3


        X_world = (
            np.einsum("ik, vuk -> vui", R_cam2world, X_cam) + t_cam2world[None, None, :]
        )

    return X_world, valid_mask

def colmap_to_opencv_intrinsics(K):
    """
    Modify camera intrinsics to follow a different convention.
    Coordinates of the center of the top-left pixels are by default:
    - (0.5, 0.5) in Colmap
    - (0,0) in OpenCV
    """
    K = K.copy()
    K[0, 2] -= 0.5
    K[1, 2] -= 0.5
    return K

def opencv_to_colmap_intrinsics(K):
    """
    Modify camera intrinsics to follow a different convention.
    Coordinates of the center of the top-left pixels are by default:
    - (0.5, 0.5) in Colmap
    - (0,0) in OpenCV
    """
    K = K.copy()
    K[0, 2] += 0.5
    K[1, 2] += 0.5
    return K

def normalize_pointcloud(
    pts1, pts2, norm_mode="avg_dis", valid1=None, valid2=None, ret_factor=False
):
    """renorm pointmaps pts1, pts2 with norm_mode"""
    assert pts1.ndim >= 3 and pts1.shape[-1] == 3
    assert pts2 is None or (pts2.ndim >= 3 and pts2.shape[-1] == 3)
    norm_mode, dis_mode = norm_mode.split("_")

    if norm_mode == "avg":

        nan_pts1, nnz1 = invalid_to_zeros(pts1, valid1, ndim=3)
        nan_pts2, nnz2 = (
            invalid_to_zeros(pts2, valid2, ndim=3) if pts2 is not None else (None, 0)
        )
        all_pts = (
            torch.cat((nan_pts1, nan_pts2), dim=1) if pts2 is not None else nan_pts1
        )


    """
    K = K.copy()
    K[0, 2] -= 0.5
    K[1, 2] -= 0.5
    return K

def opencv_to_colmap_intrinsics(K):
    """
    Modify camera intrinsics to follow a different convention.
    Coordinates of the center of the top-left pixels are by default:
    - (0.5, 0.5) in Colmap
    - (0,0) in OpenCV
    """
    K = K.copy()
    K[0, 2] += 0.5
    K[1, 2] += 0.5
    return K

def normalize_pointcloud(
    pts1, pts2, norm_mode="avg_dis", valid1=None, valid2=None, ret_factor=False
):
    """renorm pointmaps pts1, pts2 with norm_mode"""
    assert pts1.ndim >= 3 and pts1.shape[-1] == 3
    assert pts2 is None or (pts2.ndim >= 3 and pts2.shape[-1] == 3)
    norm_mode, dis_mode = norm_mode.split("_")

    if norm_mode == "avg":

        nan_pts1, nnz1 = invalid_to_zeros(pts1, valid1, ndim=3)
        nan_pts2, nnz2 = (
            invalid_to_zeros(pts2, valid2, ndim=3) if pts2 is not None else (None, 0)
        )
        all_pts = (
            torch.cat((nan_pts1, nan_pts2), dim=1) if pts2 is not None else nan_pts1
        )

        all_dis = all_pts.norm(dim=-1)
        if dis_mode == "dis":
            pass  # do nothing
        elif dis_mode == "log1p":
            all_dis = torch.log1p(all_dis)
        elif dis_mode == "warp-log1p":

            log_dis = torch.log1p(all_dis)
            warp_factor = log_dis / all_dis.clip(min=1e-8)
            H1, W1 = pts1.shape[1:-1]
            pts1 = pts1 * warp_factor[:, : W1 * H1].view(-1, H1, W1, 1)
            if pts2 is not None:
                H2, W2 = pts2.shape[1:-1]
                pts2 = pts2 * warp_factor[:, W1 * H1 :].view(-1, H2, W2, 1)
            all_dis = log_dis  # this is their true distance afterwards
        else:
            raise ValueError(f"bad {dis_mode=}")

        norm_factor = all_dis.sum(dim=1) / (nnz1 + nnz2 + 1e-8)
    else:

        nan_pts1 = invalid_to_nans(pts1, valid1, ndim=3)
        nan_pts2 = invalid_to_nans(pts2, valid2, ndim=3) if pts2 is not None else None
        all_pts = (
            torch.cat((nan_pts1, nan_pts2), dim=1) if pts2 is not None else nan_pts1
        )

        all_dis = all_pts.norm(dim=-1)

        if norm_mode == "avg":
            norm_factor = all_dis.nanmean(dim=1)
        elif norm_mode == "median":
            norm_factor = all_dis.nanmedian(dim=1).values.detach()
        elif norm_mode == "sqrt":
            norm_factor = all_dis.sqrt().nanmean(dim=1) ** 2
        else:
            raise ValueError(f"bad {norm_mode=}")

    norm_factor = norm_factor.clip(min=1e-8)
    while norm_factor.ndim < pts1.ndim:
        norm_factor.unsqueeze_(-1)

    res = pts1 / norm_factor
    if pts2 is not None:
        res = (res, pts2 / norm_factor)
    if ret_factor:
        res = res + (norm_factor,)
    return res

def normalize_pointcloud_group(
    pts_list,
    norm_mode="avg_dis",
    valid_list=None,
    conf_list=None,
    ret_factor=False,
    ret_factor_only=False,
):
    """renorm pointmaps pts1, pts2 with norm_mode"""
    for pts in pts_list:
        assert pts.ndim >= 3 and pts.shape[-1] == 3

    norm_mode, dis_mode = norm_mode.split("_")

    if norm_mode == "avg":

        nan_pts_list, nnz_list = zip(
            *[


            norm_factor = all_dis.nanmean(dim=1)
        elif norm_mode == "median":
            norm_factor = all_dis.nanmedian(dim=1).values.detach()
        elif norm_mode == "sqrt":
            norm_factor = all_dis.sqrt().nanmean(dim=1) ** 2
        else:
            raise ValueError(f"bad {norm_mode=}")

    norm_factor = norm_factor.clip(min=1e-8)
    while norm_factor.ndim < pts1.ndim:
        norm_factor.unsqueeze_(-1)

    res = pts1 / norm_factor
    if pts2 is not None:
        res = (res, pts2 / norm_factor)
    if ret_factor:
        res = res + (norm_factor,)
    return res

def normalize_pointcloud_group(
    pts_list,
    norm_mode="avg_dis",
    valid_list=None,
    conf_list=None,
    ret_factor=False,
    ret_factor_only=False,
):
    """renorm pointmaps pts1, pts2 with norm_mode"""
    for pts in pts_list:
        assert pts.ndim >= 3 and pts.shape[-1] == 3

    norm_mode, dis_mode = norm_mode.split("_")

    if norm_mode == "avg":

        nan_pts_list, nnz_list = zip(
            *[
                invalid_to_zeros(pts1, valid1, ndim=3)
                for pts1, valid1 in zip(pts_list, valid_list)
            ]
        )
        all_pts = torch.cat(nan_pts_list, dim=1)
        if conf_list is not None:
            nan_conf_list = [
                invalid_to_zeros(conf1[..., None], valid1, ndim=3)[0]
                for conf1, valid1 in zip(conf_list, valid_list)
            ]
            all_conf = torch.cat(nan_conf_list, dim=1)[..., 0]
        else:
            all_conf = torch.ones_like(all_pts[..., 0])

        all_dis = all_pts.norm(dim=-1)
        if dis_mode == "dis":
            pass  # do nothing
        elif dis_mode == "log1p":
            all_dis = torch.log1p(all_dis)
        elif dis_mode == "warp-log1p":

            log_dis = torch.log1p(all_dis)
            warp_factor = log_dis / all_dis.clip(min=1e-8)
            H_W_list = [pts.shape[1:-1] for pts in pts_list]
            pts_list = [
                pts
                * warp_factor[:, sum(H_W_list[:i]) : sum(H_W_list[: i + 1])].view(
                    -1, H, W, 1
                )
                for i, (pts, (H, W)) in enumerate(zip(pts_list, H_W_list))
            ]
            all_dis = log_dis  # this is their true distance afterwards
        else:
            raise ValueError(f"bad {dis_mode=}")

        norm_factor = (all_conf * all_dis).sum(dim=1) / (all_conf.sum(dim=1) + 1e-8)
    else:

        nan_pts_list = [
            invalid_to_nans(pts1, valid1, ndim=3)
            for pts1, valid1 in zip(pts_list, valid_list)
        ]

        all_pts = torch.cat(nan_pts_list, dim=1)

        all_dis = all_pts.norm(dim=-1)

        if norm_mode == "avg":
            norm_factor = all_dis.nanmean(dim=1)
        elif norm_mode == "median":
            norm_factor = all_dis.nanmedian(dim=1).values.detach()
        elif norm_mode == "sqrt":
            norm_factor = all_dis.sqrt().nanmean(dim=1) ** 2
        else:
            raise ValueError(f"bad {norm_mode=}")

    norm_factor = norm_factor.clip(min=1e-8)
    while norm_factor.ndim < pts_list[0].ndim:
        norm_factor.unsqueeze_(-1)

    if ret_factor_only:

        return norm_factor

    res = [pts / norm_factor for pts in pts_list]
    if ret_factor:
        return res, norm_factor
    return res

@torch.no_grad()
def get_joint_pointcloud_depth(z1, z2, valid_mask1, valid_mask2=None, quantile=0.5):

    _z1 = invalid_to_nans(z1, valid_mask1).reshape(len(z1), -1)
    _z2 = (
        invalid_to_nans(z2, valid_mask2).reshape(len(z2), -1)
        if z2 is not None
        else None
    )
    _z = torch.cat((_z1, _z2), dim=-1) if z2 is not None else _z1

    if quantile == 0.5:
        shift_z = torch.nanmedian(_z, dim=-1).values
    else:
        shift_z = torch.nanquantile(_z, quantile, dim=-1)
    return shift_z  # (B,)


        elif norm_mode == "sqrt":
            norm_factor = all_dis.sqrt().nanmean(dim=1) ** 2
        else:
            raise ValueError(f"bad {norm_mode=}")

    norm_factor = norm_factor.clip(min=1e-8)
    while norm_factor.ndim < pts_list[0].ndim:
        norm_factor.unsqueeze_(-1)

    if ret_factor_only:

        return norm_factor

    res = [pts / norm_factor for pts in pts_list]
    if ret_factor:
        return res, norm_factor
    return res

@torch.no_grad()
def get_joint_pointcloud_depth(z1, z2, valid_mask1, valid_mask2=None, quantile=0.5):

    _z1 = invalid_to_nans(z1, valid_mask1).reshape(len(z1), -1)
    _z2 = (
        invalid_to_nans(z2, valid_mask2).reshape(len(z2), -1)
        if z2 is not None
        else None
    )
    _z = torch.cat((_z1, _z2), dim=-1) if z2 is not None else _z1

    if quantile == 0.5:
        shift_z = torch.nanmedian(_z, dim=-1).values
    else:
        shift_z = torch.nanquantile(_z, quantile, dim=-1)
    return shift_z  # (B,)

@torch.no_grad()
def get_group_pointcloud_depth(zs, valid_masks, quantile=0.5):

    _zs = [
        invalid_to_nans(z1, valid_mask1).reshape(len(z1), -1)
        for z1, valid_mask1 in zip(zs, valid_masks)
    ]
    _z = torch.cat(_zs, dim=-1)

    if quantile == 0.5:
        shift_z = torch.nanmedian(_z, dim=-1).values
    else:
        shift_z = torch.nanquantile(_z, quantile, dim=-1)
    return shift_z  # (B,)

@torch.no_grad()
def get_joint_pointcloud_center_scale(


@torch.no_grad()
def get_joint_pointcloud_depth(z1, z2, valid_mask1, valid_mask2=None, quantile=0.5):

    _z1 = invalid_to_nans(z1, valid_mask1).reshape(len(z1), -1)
    _z2 = (
        invalid_to_nans(z2, valid_mask2).reshape(len(z2), -1)
        if z2 is not None
        else None
    )
    _z = torch.cat((_z1, _z2), dim=-1) if z2 is not None else _z1

    if quantile == 0.5:
        shift_z = torch.nanmedian(_z, dim=-1).values
    else:
        shift_z = torch.nanquantile(_z, quantile, dim=-1)
    return shift_z  # (B,)

@torch.no_grad()
def get_group_pointcloud_depth(zs, valid_masks, quantile=0.5):

    _zs = [
        invalid_to_nans(z1, valid_mask1).reshape(len(z1), -1)
        for z1, valid_mask1 in zip(zs, valid_masks)
    ]
    _z = torch.cat(_zs, dim=-1)

    if quantile == 0.5:
        shift_z = torch.nanmedian(_z, dim=-1).values
    else:
        shift_z = torch.nanquantile(_z, quantile, dim=-1)
    return shift_z  # (B,)

@torch.no_grad()
def get_joint_pointcloud_center_scale(
    pts1, pts2, valid_mask1=None, valid_mask2=None, z_only=False, center=True
):

    _pts1 = invalid_to_nans(pts1, valid_mask1).reshape(len(pts1), -1, 3)
    _pts2 = (
        invalid_to_nans(pts2, valid_mask2).reshape(len(pts2), -1, 3)
        if pts2 is not None
        else None
    )
    _pts = torch.cat((_pts1, _pts2), dim=1) if pts2 is not None else _pts1

    _center = torch.nanmedian(_pts, dim=1, keepdim=True).values  # (B,1,3)
    if z_only:
        _center[..., :2] = 0  # do not center X and Y

    _norm = ((_pts - _center) if center else _pts).norm(dim=-1)


    return shift_z  # (B,)

@torch.no_grad()
def get_group_pointcloud_depth(zs, valid_masks, quantile=0.5):

    _zs = [
        invalid_to_nans(z1, valid_mask1).reshape(len(z1), -1)
        for z1, valid_mask1 in zip(zs, valid_masks)
    ]
    _z = torch.cat(_zs, dim=-1)

    if quantile == 0.5:
        shift_z = torch.nanmedian(_z, dim=-1).values
    else:
        shift_z = torch.nanquantile(_z, quantile, dim=-1)
    return shift_z  # (B,)

@torch.no_grad()
def get_joint_pointcloud_center_scale(
    pts1, pts2, valid_mask1=None, valid_mask2=None, z_only=False, center=True
):

    _pts1 = invalid_to_nans(pts1, valid_mask1).reshape(len(pts1), -1, 3)
    _pts2 = (
        invalid_to_nans(pts2, valid_mask2).reshape(len(pts2), -1, 3)
        if pts2 is not None
        else None
    )
    _pts = torch.cat((_pts1, _pts2), dim=1) if pts2 is not None else _pts1

    _center = torch.nanmedian(_pts, dim=1, keepdim=True).values  # (B,1,3)
    if z_only:
        _center[..., :2] = 0  # do not center X and Y

    _norm = ((_pts - _center) if center else _pts).norm(dim=-1)
    scale = torch.nanmedian(_norm, dim=1).values
    return _center[:, None, :, :], scale[:, None, None, None]

@torch.no_grad()
def get_group_pointcloud_center_scale(pts, valid_masks=None, z_only=False, center=True):

    _pts = [
        invalid_to_nans(pts1, valid_mask1).reshape(len(pts1), -1, 3)
        for pts1, valid_mask1 in zip(pts, valid_masks)
    ]
    _pts = torch.cat(_pts, dim=1)

    _center = torch.nanmedian(_pts, dim=1, keepdim=True).values  # (B,1,3)
    if z_only:
        _center[..., :2] = 0  # do not center X and Y

    _norm = ((_pts - _center) if center else _pts).norm(dim=-1)
    scale = torch.nanmedian(_norm, dim=1).values
    return _center[:, None, :, :], scale[:, None, None, None]


):

    _pts1 = invalid_to_nans(pts1, valid_mask1).reshape(len(pts1), -1, 3)
    _pts2 = (
        invalid_to_nans(pts2, valid_mask2).reshape(len(pts2), -1, 3)
        if pts2 is not None
        else None
    )
    _pts = torch.cat((_pts1, _pts2), dim=1) if pts2 is not None else _pts1

    _center = torch.nanmedian(_pts, dim=1, keepdim=True).values  # (B,1,3)
    if z_only:
        _center[..., :2] = 0  # do not center X and Y

    _norm = ((_pts - _center) if center else _pts).norm(dim=-1)
    scale = torch.nanmedian(_norm, dim=1).values
    return _center[:, None, :, :], scale[:, None, None, None]

@torch.no_grad()
def get_group_pointcloud_center_scale(pts, valid_masks=None, z_only=False, center=True):

    _pts = [
        invalid_to_nans(pts1, valid_mask1).reshape(len(pts1), -1, 3)
        for pts1, valid_mask1 in zip(pts, valid_masks)
    ]
    _pts = torch.cat(_pts, dim=1)

    _center = torch.nanmedian(_pts, dim=1, keepdim=True).values  # (B,1,3)
    if z_only:
        _center[..., :2] = 0  # do not center X and Y

    _norm = ((_pts - _center) if center else _pts).norm(dim=-1)
    scale = torch.nanmedian(_norm, dim=1).values
    return _center[:, None, :, :], scale[:, None, None, None]

def find_reciprocal_matches(P1, P2):
    """
    returns 3 values:
    1 - reciprocal_in_P2: a boolean array of size P2.shape[0], a "True" value indicates a match
    2 - nn2_in_P1: a int array of size P2.shape[0], it contains the indexes of the closest points in P1
    3 - reciprocal_in_P2.sum(): the number of matches
    """
    tree1 = KDTree(P1)
    tree2 = KDTree(P2)

    _, nn1_in_P2 = tree2.query(P1, workers=8)
    _, nn2_in_P1 = tree1.query(P2, workers=8)

    reciprocal_in_P1 = nn2_in_P1[nn1_in_P2] == np.arange(len(nn1_in_P2))
    reciprocal_in_P2 = nn1_in_P2[nn2_in_P1] == np.arange(len(nn2_in_P1))
    assert reciprocal_in_P1.sum() == reciprocal_in_P2.sum()
    return reciprocal_in_P2, nn2_in_P1, reciprocal_in_P2.sum()


@torch.no_grad()
def get_group_pointcloud_center_scale(pts, valid_masks=None, z_only=False, center=True):

    _pts = [
        invalid_to_nans(pts1, valid_mask1).reshape(len(pts1), -1, 3)
        for pts1, valid_mask1 in zip(pts, valid_masks)
    ]
    _pts = torch.cat(_pts, dim=1)

    _center = torch.nanmedian(_pts, dim=1, keepdim=True).values  # (B,1,3)
    if z_only:
        _center[..., :2] = 0  # do not center X and Y

    _norm = ((_pts - _center) if center else _pts).norm(dim=-1)
    scale = torch.nanmedian(_norm, dim=1).values
    return _center[:, None, :, :], scale[:, None, None, None]

def find_reciprocal_matches(P1, P2):
    """
    returns 3 values:
    1 - reciprocal_in_P2: a boolean array of size P2.shape[0], a "True" value indicates a match
    2 - nn2_in_P1: a int array of size P2.shape[0], it contains the indexes of the closest points in P1
    3 - reciprocal_in_P2.sum(): the number of matches
    """
    tree1 = KDTree(P1)
    tree2 = KDTree(P2)

    _, nn1_in_P2 = tree2.query(P1, workers=8)
    _, nn2_in_P1 = tree1.query(P2, workers=8)

    reciprocal_in_P1 = nn2_in_P1[nn1_in_P2] == np.arange(len(nn1_in_P2))
    reciprocal_in_P2 = nn1_in_P2[nn2_in_P1] == np.arange(len(nn2_in_P1))
    assert reciprocal_in_P1.sum() == reciprocal_in_P2.sum()
    return reciprocal_in_P2, nn2_in_P1, reciprocal_in_P2.sum()

def get_med_dist_between_poses(poses):
    from scipy.spatial.distance import pdist

    return np.median(pdist([to_numpy(p[:3, 3]) for p in poses]))

def weighted_procrustes(A, B, w, use_weights=True, eps=1e-16, return_T=False):
    """
    X: torch tensor B x N x 3
    Y: torch tensor B x N x 3
    w: torch tensor B x N
    """
    assert len(A) == len(B)
    if use_weights:
        W1 = torch.abs(w).sum(1, keepdim=True)
        w_norm = (w / (W1 + eps)).unsqueeze(-1)
        a_mean = (w_norm * A).sum(dim=1, keepdim=True)
        b_mean = (w_norm * B).sum(dim=1, keepdim=True)