fix mesh ray cast depth calculation

[yxlao]

Issue

Fixed: if the lengths of the camera rays are not normalized, the ray casting mesh to depth function returns wrong depths. This is because Open3D RaycastingScene's cast_rays returns t_hit where the t_hit is not the absolute depth. The correct depth is t_hit multiplied by the length of the camera ray.

Before

After

Demo code

from pathlib import Path
import open3d as o3d
import camtools as ct
import numpy as np

# Mesh.
mesh = o3d.geometry.TriangleMesh.create_box(width=2, height=2, depth=0.1)
mesh.compute_vertex_normals()
mesh_transform = np.array(
    [
        [1, 0, 0, -1],
        [0, 1, 0, -1],
        [0, 0, 1, 1],
        [0, 0, 0, 1],
    ]
)
mesh.transform(mesh_transform)

# Camera.
im_w = 320
im_h = 240
fx, fy = im_w, im_h
cx, cy = im_w / 2, im_h / 2
K = np.array(
    [
        [fx, 0, cx],
        [0, fy, cy],
        [0, 0, 1],
    ]
)
T = np.eye(4)
camera_frame = ct.camera.create_camera_ray_frames([K], [T], size=1.25)

# Ray cast.
im_depth = ct.raycast.mesh_to_depth(mesh, K, T, width=im_w, height=im_h)
rays = o3d.t.geometry.RaycastingScene.create_rays_pinhole(
    intrinsic_matrix=K,
    extrinsic_matrix=T,
    width_px=im_w,
    height_px=im_h,
)
rays = rays.numpy()  # (H, W, 6)
rays = rays.reshape(-1, 6)
centers = rays[:, :3]
dirs = rays[:, 3:]
dirs = dirs / np.linalg.norm(dirs, axis=1, keepdims=True)
points = centers + dirs * im_depth.flatten()[:, None]
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)

# Plot.
axes = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0)
o3d.visualization.draw_geometries([pcd, camera_frame, axes, mesh])