Open3d ray casting 쉽게 하기, 2d point to mesh intersection 찾기

camera pose, intrinsic parameter, 2d keypoint를 알고 있을 때 이를 3D mesh으로 back projection하려면 ray casting을 해야 한다. 직접 구현하려면 연산량 문제로 속도가 어마어마하게 느리기 때문에 최적화가 잘 된 기능을 가져와서 사용하는게 무조건 낫다. 

 

우회법으로 3d mesh vertex를 projection한 뒤, 2d keypoint와 가장 가까운 vertex를 찾아내는 식으로 할 수 있겠지만, 이 방법은 vertex가 충분히 많아야 하고, mesh가 1겹일 때만 가능하다. 

 

얼굴을 예로 들면, face keypoint와 head mesh 간의 비교 시 우회법으로 구현하면, 눈 keypoint가 뒤통수에서 나오는 경우가 있다. 당연하게도, uv 좌표계 차이로 계산하다보니, 거리 z를 무시했기 때문이다.

 

따라서 결국 2d point back projection은 ray casting을 써야 한다. 

 

내가 쓰는 가장 쉬운 방법. Open3d 를 쓰는 방법이다. trimesh도 있고 다른 library도 있는데 왜 open3d를 쓰느냐 할 수 있는데, 가장 쉽다고 한 이유는 그냥 내가 가장 익숙해서다. 

 

def apply_T(T, points):
    if len(T.shape) != 2 or len(points.shape) != 2:
        raise Exception("ERROR : the dimensions of transformation matrix and points are wrong.")

    points_ = np.matmul(T[:3, :3], points.transpose(1,0)).transpose(1,0) + T[:3, -1].reshape(-1,3)
    return points_

def get_intersects(self, mesh, lmk2d, lmk2d_c, K, pose):
    # pt3d_cam = np.matmul(pose[:3, :3], verts.transpose(1, 0)).transpose(1, 0) + pose[:3, -1].reshape(-1, 3)
    # pt2ds, _ = cv2.projectPoints(pt3d_cam, np.zeros([3]), np.zeros([3]), K, np.zeros([5]))
    # pt2ds = pt2ds.squeeze() # N 2
    T_kg = pose # global g to cam k
    T_gk = np.linalg.inv(pose) # cam k to global g

	rx = (lmk2d[:,0] - K[0,2]) / K[0,0]
    ry = (lmk2d[:,1] - K[1,2]) / K[1,1]
    rz = np.ones_like(rx) # normalized ray
    
    rays_d = np.concatenate([rx[:,None], ry[:,None], rz[:,None]], axis=1)
    rays_d = rays_d / np.linalg.norm(rays_d, axis=1).reshape(-1,1) # length to 1
    rays_h = rays_d.copy() # ray direction in camera coordinate
    rays_o = np.zeros_like(rays_d) # zero origin. rays start from camera center
    rays_o = apply_T(T_gk, rays_o) # ray origin in global coordinate
    rays_d = apply_T(T_gk, rays_d)
    rays_d = rays_d - rays_o # ray direction in global coordinate

    rays_np = np.concatenate([rays_o, rays_d], axis=1)

    scene = o3d.t.geometry.RaycastingScene()
    mesh_t = o3d.t.geometry.TriangleMesh.from_legacy(mesh)
    scene.add_triangles(mesh_t)
    rays = o3d.core.Tensor(rays_np, dtype=o3d.core.Dtype.Float32)

    ans = scene.cast_rays(rays)
    zs = ans["t_hit"].numpy()

    success = ~np.isinf(zs) # if ray casting fails, returns are inf.

    success = success * (lmk2d_c != 0) # if lmk2d confidence exists, filter out additionally


    intersections = rays_h * zs.reshape(-1, 1) # back proejct in camera coordinate
    intersections = apply_T(T_gk, intersections) # cam k to global g

    return intersections, success

 

이렇게 하면 불과 ms 수준에서 ray casting을 간단히 할 수 있다.