Inputting internal and external parameters images, the output does not correspond to the GT lidar data

[sunshinejuanjuan123]

Inputting internal and external parameters images, the output does not correspond to the GT lidar data

[sunshinejuanjuan123]

mycode

# Optional config for better memory efficiency
import os
os.environ["PYTORCH_CUDA_ALLOC_CONF"] = "expandable_segments:True"

# Required imports
from sympy import Point3D
import torch
from mapanything.models import MapAnything
from mapanything.utils.image import load_images
import numpy as np
import open3d as o3d
from mapanything.utils.image import preprocess_inputs
import imageio
def homogenize_points(
    points,
):
    """Convert batched points (xyz) to (xyz1)."""
    return torch.cat([points, torch.ones_like(points[..., :1])], dim=-1)

# Get inference device
device = "cuda" if torch.cuda.is_available() else "cpu"

# Init model - This requries internet access or the huggingface hub cache to be pre-downloaded
# For Apache 2.0 license model, use "facebook/map-anything-apache"
model = MapAnything.from_pretrained("facebook/map-anything").to(device)

# Load and preprocess images from a folder or list of paths
images_path = "/iag_ad_01/ad/yuanweizhong/datasets/senseauto/2024_09_08_07_53_23_pathway_pilotGtParser/camera/example_dataset/example_scene3/images"  # or ["path/to/img1.jpg", "path/to/img2.jpg", ...]
# load poses from txt files
poses = []
pose_files = sorted([f for f in os.listdir("/iag_ad_01/ad/yuanweizhong/datasets/senseauto/2024_09_08_07_53_23_pathway_pilotGtParser/camera/example_dataset/example_scene3/poses") if f.endswith('.txt')])
for i in range(len(pose_files)):
    pose = np.loadtxt(os.path.join("/iag_ad_01/ad/yuanweizhong/datasets/senseauto/2024_09_08_07_53_23_pathway_pilotGtParser/camera/example_dataset/example_scene3/poses", pose_files[i]))
    poses.append(pose)
poses = torch.tensor(poses).to(device)

intrinsic = torch.tensor([
    [7.34439013675e+03, 0.00000000000e+00, 1.79701000977e+03],
    [0.00000000000e+00, 7.34358007812e+03, 1.01757000732e+03],
    [0.00000000000e+00, 0.00000000000e+00, 1.00000000000e+00]
]).to(device)

len=20
images = sorted([f for f in os.listdir(images_path) if f.endswith('.jpg')])
images_data = [imageio.imread(images_path+'/'+img) for img in images]  #

image_tensor = torch.from_numpy(np.array(images_data)).float()[:len]
intrinsics_tensor = intrinsic.repeat(len, 1, 1)
pose_matrices = poses[:len]
views_example=[]
for i in range(len):
    views_example.append(
        {
            "img": image_tensor[i],  # (H, W, 3) - [0, 255]
            "intrinsics": intrinsics_tensor[i],  # (3, 3)
            "camera_poses": pose_matrices[i],  # (4, 4) or tuple of (quats, trans) in OpenCV cam2world convention
            "is_metric_scale": torch.tensor([True]),  # (1,)
        }
    )

# Preprocess inputs to the expected format
processed_views = preprocess_inputs(views_example)
# Run inference
predictions = model.infer(
    processed_views,                            # Input views
    memory_efficient_inference=False, # Trades off speed for more views (up to 2000 views on 140 GB)
    use_amp=True,                     # Use mixed precision inference (recommended)
    amp_dtype="bf16",                 # bf16 inference (recommended; falls back to fp16 if bf16 not supported)
    apply_mask=True,                  # Apply masking to dense geometry outputs
    mask_edges=True,                  # Remove edge artifacts by using normals and depth
    apply_confidence_mask=False,      # Filter low-confidence regions
    confidence_percentile=10,         # Remove bottom 10 percentile confidence pixels
)

# Access results for each view - Complete list of metric outputs
for i, pred in enumerate(predictions):
    # Geometry outputs
    pts3d = pred["pts3d"]                     # 3D points in world coordinates (B, H, W, 3)
    pts3d_cam = pred["pts3d_cam"]             # 3D points in camera coordinates (B, H, W, 3)
    depth_z = pred["depth_z"]                 # Z-depth in camera frame (B, H, W, 1)
    depth_along_ray = pred["depth_along_ray"] # Depth along ray in camera frame (B, H, W, 1)

    # Camera outputs
    ray_directions = pred["ray_directions"]   # Ray directions in camera frame (B, H, W, 3)
    intrinsics = pred["intrinsics"]           # Recovered pinhole camera intrinsics (B, 3, 3)
    camera_poses = pred["camera_poses"]       # OpenCV (+X - Right, +Y - Down, +Z - Forward) cam2world poses in world frame (B, 4, 4)
    cam_trans = pred["cam_trans"]             # OpenCV (+X - Right, +Y - Down, +Z - Forward) cam2world translation in world frame (B, 3)
    cam_quats = pred["cam_quats"]             # OpenCV (+X - Right, +Y - Down, +Z - Forward) cam2world quaternion in world frame (B, 4)

    # Quality and masking
    confidence = pred["conf"]                 # Per-pixel confidence scores (B, H, W)
    mask = pred["mask"]                       # Combined validity mask (B, H, W, 1)
    non_ambiguous_mask = pred["non_ambiguous_mask"]                # Non-ambiguous regions (B, H, W)
    non_ambiguous_mask_logits = pred["non_ambiguous_mask_logits"]  # Mask logits (B, H, W)

    # Scaling
    metric_scaling_factor = pred["metric_scaling_factor"]  # Applied metric scaling (B,)

    # Original input
    img_no_norm = pred["img_no_norm"]         # Denormalized input images for visualization (B, H, W, 3)

    # 使用 Open3D 将 pts3d 保存为 PLY 文件
    # 假设批处理大小 (B) 为 1
    if pts3d.shape[0] == 1:
        # 移动到 CPU 并转换为 numpy
        points_np = pts3d[0].cpu().numpy()
        mask_np = mask[0].cpu().numpy().astype(bool)

        # 应用掩码并重塑以获取有效点
        valid_points = points_np[mask_np.squeeze()]

        # 创建 Open3D 点云对象
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(valid_points)

        # 定义输出文件名并保存
        output_filename = f"view_{i}_points.ply"
        o3d.io.write_point_cloud(output_filename, pcd)
        print(f"Saved point cloud for view {i} to `{output_filename}")

[sunshinejuanjuan123]

output.zip

mydatasets

[sunshinejuanjuan123]

some prints

pred["intrinsics"]   
tensor([[[968.7162,   0.0000, 253.8624],
         [  0.0000, 961.9808, 146.1175],
         [  0.0000,   0.0000,   1.0000]]], device='cuda:0')
processed_views[0]['intrinsics']
tensor([[[999.6532,   0.0000, 241.8277],
         [  0.0000, 999.5429, 138.0706],
         [  0.0000,   0.0000,   1.0000]]], device='cuda:0')
processed_views[1]['intrinsics']
tensor([[[999.6532,   0.0000, 241.8277],
         [  0.0000, 999.5429, 138.0706],
         [  0.0000,   0.0000,   1.0000]]], device='cuda:0')

camera_poses[0]
tensor([[ 1.0000e+00, -3.9164e-05,  6.8954e-05, -1.2350e-03],
        [ 3.9158e-05,  1.0000e+00,  9.3204e-05, -8.9822e-04],
        [-6.8958e-05, -9.3201e-05,  1.0000e+00,  9.2373e-04],
        [ 0.0000e+00,  0.0000e+00,  0.0000e+00,  1.0000e+00]], device='cuda:0')
processed_views[0]['camera_poses']
tensor([[[ 0.7378, -0.0361,  0.6740,  2.7785],
         [-0.6742,  0.0084,  0.7385,  3.4219],
         [-0.0323, -0.9993, -0.0181,  0.5071],
         [ 0.0000,  0.0000,  0.0000,  1.0000]]], device='cuda:0',
       dtype=torch.float64)
poses[0]
tensor([[ 0.7378, -0.0361,  0.6740,  2.7785],
        [-0.6742,  0.0084,  0.7385,  3.4219],
        [-0.0323, -0.9993, -0.0181,  0.5071],
        [ 0.0000,  0.0000,  0.0000,  1.0000]], device='cuda:0',
       dtype=torch.float64)

[Nik-V9]

Hi, Thanks for trying it out and sharing the debug data!

I would like to understand the issue a bit better. Are you saying that the output of MapAnything doesn't match the GT LiDAR pointcloud? Could you share reference screenshots for what output visualization you are getting?

I'll try out the data. In addition to this, you can also input the sparse LiDAR points projected as depth maps to MapAnything.

[sunshinejuanjuan123]

Input the depth projected by the internal and external parameters of the images lidar, but the output does not correspond to the ground truth lidar either.

[sunshinejuanjuan123]

My code

# Optional config for better memory efficiency
import os
os.environ["PYTORCH_CUDA_ALLOC_CONF"] = "expandable_segments:True"

# Required imports
from sympy import Point3D
import torch
from mapanything.models import MapAnything
from mapanything.utils.image import load_images,preprocess_inputs
import numpy as np
import open3d as o3d
import cv2
import imageio
def homogenize_points(points):
    """Convert batched points (xyz) to (xyz1)."""
    return torch.cat([points, torch.ones_like(points[..., :1])], dim=-1)

def project_pointcloud_to_depth(ply_path, intrinsic, camera_pose, image_height, image_width, device):
    """
    读取PLY点云文件并投影到图像上生成深度图
    
    Args:
        ply_path: PLY文件路径
        intrinsic: 相机内参矩阵 (3, 3)
        camera_pose: 相机位姿矩阵 (4, 4) - cam2world
        image_height: 图像高度
        image_width: 图像宽度
        device: 计算设备
        
    Returns:
        depth_map: 深度图 (H, W)，无效点为0
    """
    # 读取PLY点云
    pcd = o3d.io.read_point_cloud(ply_path)
    if len(pcd.points) == 0:
        print(f"Warning: No points found in {ply_path}")
        return torch.zeros((image_height, image_width), dtype=torch.float32, device=device)
    
    # 转换为numpy数组
    points_3d = np.asarray(pcd.points)  # (N, 3)
    
    # 转换为tensor
    points_3d_tensor = torch.tensor(points_3d, dtype=torch.float32, device=device)  # (N, 3)
    
    # 将世界坐标系下的点转换到相机坐标系
    # camera_pose是cam2world，我们需要world2cam
    world2cam = torch.inverse(camera_pose)  # (4, 4)
    # world2cam = camera_pose
    
    # 齐次化点云
    points_3d_homo = homogenize_points(points_3d_tensor.unsqueeze(0)).squeeze(0)  # (N, 4)
    
    # 变换到相机坐标系
    points_cam_homo = (world2cam @ points_3d_homo.T).T  # (N, 4)
    points_cam = points_cam_homo[:, :3]  # (N, 3)
    
    # 过滤掉相机后方的点 (z <= 0)
    valid_mask = points_cam[:, 2] > 0
    if not valid_mask.any():
        print("Warning: No points in front of camera")
        return torch.zeros((image_height, image_width), dtype=torch.float32, device=device)
    
    points_cam_valid = points_cam[valid_mask]  # (M, 3)
    
    # 投影到图像平面
    # [u, v, 1] = K * [X/Z, Y/Z, 1]
    points_2d_homo = (intrinsic @ points_cam_valid.T).T  # (M, 3)
    points_2d = points_2d_homo[:, :2] / points_2d_homo[:, 2:3]  # (M, 2)
    
    # 获取深度值
    depths = points_cam_valid[:, 2]  # (M,)
    
    # 转换为像素坐标
    u = points_2d[:, 0].round().long()  # (M,)
    v = points_2d[:, 1].round().long()  # (M,)
    
    # 过滤图像边界内的点
    valid_pixel_mask = (u >= 0) & (u < image_width) & (v >= 0) & (v < image_height)
    
    if not valid_pixel_mask.any():
        print("Warning: No points project within image boundaries")
        return torch.zeros((image_height, image_width), dtype=torch.float32, device=device)
    
    u_valid = u[valid_pixel_mask]
    v_valid = v[valid_pixel_mask]
    depths_valid = depths[valid_pixel_mask]
    
    # 创建深度图
    depth_map = torch.zeros((image_height, image_width), dtype=torch.float32, device=device)
    
    # 处理多个点投影到同一像素的情况，保留最近的深度值
    for i in range(len(u_valid)):
        pixel_u, pixel_v, depth = u_valid[i], v_valid[i], depths_valid[i]
        if depth_map[pixel_v, pixel_u] == 0 or depth < depth_map[pixel_v, pixel_u]:
            depth_map[pixel_v, pixel_u] = depth
    
    return depth_map

def resize_intrinsics_and_depth(intrinsics, depth_map, original_size, target_size):
    """
    根据图像resize调整内参矩阵和深度图
    
    Args:
        intrinsics: 原始内参矩阵 (3, 3)
        depth_map: 原始深度图 (H, W) 或 None
        original_size: 原始图像尺寸 (width, height)
        target_size: 目标图像尺寸 (width, height)
        
    Returns:
        resized_intrinsics: 调整后的内参矩阵 (3, 3)
        resized_depth_map: 调整后的深度图 (H, W) 或 None
    """
    orig_w, orig_h = original_size
    target_w, target_h = target_size
    
    # 计算缩放因子
    scale_x = target_w / orig_w
    scale_y = target_h / orig_h
    
    # 调整内参矩阵
    resized_intrinsics = intrinsics.clone()
    resized_intrinsics[0, 0] *= scale_x  # fx
    resized_intrinsics[1, 1] *= scale_y  # fy
    resized_intrinsics[0, 2] *= scale_x  # cx
    resized_intrinsics[1, 2] *= scale_y  # cy
    
    # 调整深度图
    resized_depth_map = None
    if depth_map is not None:
        # 使用双线性插值resize深度图
        depth_np = depth_map.cpu().numpy()
        resized_depth_np = cv2.resize(depth_np, (target_w, target_h), interpolation=cv2.INTER_LINEAR)
        resized_depth_map = torch.tensor(resized_depth_np, dtype=torch.float32, device=depth_map.device)
    
    return resized_intrinsics, resized_depth_map
def convert_camera_pose2_first(poses: np.ndarray):
    out_put_pose = np.eye(4)[None, ...].repeat(poses.shape[0], axis=0)
    poses = poses[:,:3,:]
    R0 = poses[0][:, : 3].copy()
    R0_inv = R0.T
    t0_inv = - (R0_inv @ (poses[0][:, 3])).copy()
    N = poses.shape[0]
    for i in range(N):
        R_w_cari =  (poses[i][:,:3]).copy()
        t_w_cari =  (poses[i][:,3]).copy()
        out_put_pose[i][:3,:3] = R0_inv @ R_w_cari
        out_put_pose[i][:3, 3] = (R0_inv @ t_w_cari) + t0_inv
        # print(poses[i][:, 3])
# Get inference device
    return out_put_pose
device = "cuda" if torch.cuda.is_available() else "cpu"
# Init model - This requries internet access or the huggingface hub cache to be pre-downloaded
# For Apache 2.0 license model, use "facebook/map-anything-apache"
model = MapAnything.from_pretrained("facebook/map-anything").to(device)

# Load and preprocess images from a folder or list of paths
images = "/iag_ad_01/ad/yuanweizhong/datasets/senseauto/2024_09_08_07_53_23_pathway_pilotGtParser/camera/example_dataset/example_scene3/images"
lenth=20

# load poses from txt files
poses = []
pose_files = sorted([f for f in os.listdir("/iag_ad_01/ad/yuanweizhong/datasets/senseauto/2024_09_08_07_53_23_pathway_pilotGtParser/camera/example_dataset/example_scene3/poses") if f.endswith('.txt')])
for i in range(lenth):
    pose = np.loadtxt(os.path.join("/iag_ad_01/ad/yuanweizhong/datasets/senseauto/2024_09_08_07_53_23_pathway_pilotGtParser/camera/example_dataset/example_scene3/poses", pose_files[i]))
    poses.append(pose)
poses = torch.tensor(convert_camera_pose2_first(np.array(poses)), dtype=torch.float32).to(device)

# Load and preprocess images from a folder or list of paths
images_path = "/iag_ad_01/ad/yuanweizhong/datasets/senseauto/2024_09_08_07_53_23_pathway_pilotGtParser/camera/example_dataset/example_scene3/images"  # or ["path/to/img1.jpg", "path/to/img2.jpg", ...]
images = sorted([f for f in os.listdir(images_path) if f.endswith('.jpg')])
images_data = [imageio.imread(images_path+'/'+img) for img in images]  #
image_tensor = torch.from_numpy(np.array(images_data)).float()[:lenth].cuda()

# 原始内参矩阵和图像尺寸
original_intrinsic = torch.tensor([
    [7.34439013675e+03, 0.00000000000e+00, 1.79701000977e+03],
    [0.00000000000e+00, 7.34358007812e+03, 1.01757000732e+03],
    [0.00000000000e+00, 0.00000000000e+00, 1.00000000000e+00]
], dtype=torch.float32).to(device)
intrinsics_tensor = original_intrinsic.repeat(lenth, 1, 1)

original_image_size = (3840, 2160)  # (width, height)

# 读取PLY点云并生成深度图（使用原始图像尺寸）
ply_path = "ply_1.ply"  # 替换为您的PLY文件路径
depth_map = None

views_example=[]    
for i in range(lenth):
    if os.path.exists(ply_path):
        depth_map = project_pointcloud_to_depth(
            ply_path=ply_path,
            intrinsic=original_intrinsic,
            camera_pose=poses[i],  # 使用第i个相机的位姿
            image_height=original_image_size[1],  # 2160
            image_width=original_image_size[0],   # 3840
            device=device
        )
        views_example.append(
            {
                "img": image_tensor[i],  # (H, W, 3) - [0, 255]
                "intrinsics": intrinsics_tensor[i],  # (3, 3)
                "camera_poses": poses[i],  # (4, 4) or tuple of (quats, trans) in OpenCV cam2world convention
                "depth_z": depth_map,
                "is_metric_scale": torch.tensor([True], device=device),  # (1,)
                
            }
        )
    else:
        print(f"PLY file not found: {ply_path}")

# Preprocess inputs to the expected format
processed_views = preprocess_inputs(views_example)
# Run inference
predictions = model.infer(
    processed_views,
    memory_efficient_inference=False,
    use_amp=True,
    amp_dtype="bf16",
    apply_mask=True,
    mask_edges=True,
    apply_confidence_mask=False,
    confidence_percentile=10,
)

# Access results for each view - Complete list of metric outputs
for i, pred in enumerate(predictions):
    # Geometry outputs
    pts3d = pred["pts3d"]                     # 3D points in world coordinates (B, H, W, 3)
    pts3d_cam = pred["pts3d_cam"]             # 3D points in camera coordinates (B, H, W, 3)
    depth_z = pred["depth_z"]                 # Z-depth in camera frame (B, H, W, 1)
    depth_along_ray = pred["depth_along_ray"] # Depth along ray in camera frame (B, H, W, 1)

    # Camera outputs
    ray_directions = pred["ray_directions"]   # Ray directions in camera frame (B, H, W, 3)
    intrinsics = pred["intrinsics"]           # Recovered pinhole camera intrinsics (B, 3, 3)
    camera_poses = pred["camera_poses"]       # OpenCV (+X - Right, +Y - Down, +Z - Forward) cam2world poses in world frame (B, 4, 4)
    cam_trans = pred["cam_trans"]             # OpenCV (+X - Right, +Y - Down, +Z - Forward) cam2world translation in world frame (B, 3)
    cam_quats = pred["cam_quats"]             # OpenCV (+X - Right, +Y - Down, +Z - Forward) cam2world quaternion in world frame (B, 4)

    # Quality and masking
    confidence = pred["conf"]                 # Per-pixel confidence scores (B, H, W)
    mask = pred["mask"]                       # Combined validity mask (B, H, W, 1)
    non_ambiguous_mask = pred["non_ambiguous_mask"]                # Non-ambiguous regions (B, H, W)
    non_ambiguous_mask_logits = pred["non_ambiguous_mask_logits"]  # Mask logits (B, H, W)

    # Scaling
    metric_scaling_factor = pred["metric_scaling_factor"]  # Applied metric scaling (B,)

    # Original input
    img_no_norm = pred["img_no_norm"]         # Denormalized input images for visualization (B, H, W, 3)

    # 使用 Open3D 将 pts3d 保存为 PLY 文件
    # 假设批处理大小 (B) 为 1
    if pts3d.shape[0] == 1:
        # 移动到 CPU 并转换为 numpy
        points_np = pts3d[0].cpu().numpy()
        mask_np = mask[0].cpu().numpy().astype(bool)

        # 应用掩码并重塑以获取有效点
        valid_points = points_np[mask_np.squeeze()]

        # 创建 Open3D 点云对象
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(valid_points)

        # 定义输出文件名并保存
        output_filename = f"./output_with_intrinsics_and poses/view_{i}_points.ply"
        o3d.io.write_point_cloud(output_filename, pcd)
        print(f"Saved point cloud for view {i} to {output_filename}")``

[sunshinejuanjuan123]

intrins_poses.zip

[sunshinejuanjuan123]

[sunshinejuanjuan123]

first 9 frames

[sunshinejuanjuan123]

Does the point cloud input not align with the GT lidar in principle?
Specifically, when provided, the ray depths are first decoupled into average perview depth zˆdi ∈ R+ and normalized ray depths Dˆi/zˆdi. Furthermore, when translations Tˆ are provided,
However, the scale derived from the average depth does not represent the final scale.