Open3d를 이용한 디버깅용 camera, bbox, origin visualization

데이터를 다루다 보면 주어진 3D point와 camera pose가 좌표계가 잘 맞춰져 있는지, coordinate origin을 어디인지, 방향은 맞는지 확인하는 것이 생각보다 귀찮고 까다롭다. 

 

수십, 수백개나 되는 xyz 값이나 se3 값을 봐서는 알 방법이 없기 때문에 보통 시각화해서 보는데, colmap 외 사용 가능한 툴들도 제한적이고 매번 포맷을 맞추는 것도 귀찮다.

 

나 같은 경우, 간단히 open3d를 이용해 시각화하는 식으로 해결해왔는데 매번 반복적으로 코드를 구현하는 것이 귀찮아서 여기에  내가 주로 사용하는 디버깅용 코드를 기록해두려고 한다. 

 

Helper function들

def Rt2T(R, t):
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, -1] = t.reshape(-1)
    return T

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

매번 사용하는 helper function들. 그냥 SE3 transformation 만들고 적용하는 코드다.

 

camera 생성하기 

def make_cam(T_gk, color=(0, 1, 0), scale=1):
    camera_line = [[0, 1], [0, 2], [0, 3], [0, 4], [1, 2], [2, 3], [3, 4], [4, 1]]

    camera_colors = [color for _ in range(8)]
    T = T_gk
    k = scale/17
    camera_points = np.array([[0, 0, 0],
                              [-17 * k, -10 * k, 40 * k],
                              [17 * k, -10 * k, 40 * k],
                              [17 * k, 10 * k, 40 * k],
                              [-17 * k, 10 * k, 40 * k],
                              ])
    camera_points = apply_T(T, camera_points)
    camera = o3d.geometry.LineSet(
        points=o3d.utility.Vector3dVector(camera_points),
        lines=o3d.utility.Vector2iVector(camera_line),
    )

    camera.colors = o3d.utility.Vector3dVector(camera_colors)
    return camera

 

T_gk가 camea pose를 말하는 것인데 gk라는 notation은 내가 쓰는 표기법이다. g는 global coordinate, k는 k번째 camera coordinate다. 즉 T_gk는 cam. coord to global. coord transformation이다.

 

조금 더 자세한 내용은 3D transformation(R, t) matrix notation 여기에 기록해두었다. 

 

위 함수에 T_gk를 던져주면 카메라 모양으로 조립된 open3d lineset을 얻을 수 있다. scale는 카메라의 크기를 말하며 m scale이다.

 

origin 생성하기

def make_origin(T_gk=np.eye(4), scale=1):
    points = np.array([[0,0,0],
                       [1,0,0],
                       [0,1,0],
                       [0,0,1]]) * scale

    points = apply_T(T_gk, points)
    origin_line = [[0,1], [0,2], [0,3]]
    origin_color = [(1,0,0), (0,1,0), (0,0,1)]

    origin = o3d.geometry.LineSet(
        points= o3d.utility.Vector3dVector(points),
        lines = o3d.utility.Vector2iVector(origin_line),
    )
    origin.colors = o3d.utility.Vector3dVector(origin_color)
    return origin

 

좌표계 원점을 그리고 싶을 때도 많다. 카메라 모양은 보통 대칭적이라서 가끔 뒤집히는 경우에 구분 못할 때가 있는데 이 때는 좌표계 방향을 같이 찍어주면 구분이 쉬워서 많이 활용한다. 

 

위 함수 역시 카메라와 동일하게 T_gk에 camera pose나 새로운 좌표계 pose를 던져 주면 x축, y축, z축이 각각 R,G,B로 표기된 좌표계 open3d lineset을 얻을 수 있다. global coordinate를 얻고 싶을 땐 np.eye(4) 넣으면 된다.

 

bbox 생성하기

def make_bbox(pcd=None, points=None):
    if pcd is not None and points is not None:
        raise Exception("duplicated input.")
    elif pcd is not None:
        points = np.array(pcd.points)
    else:
        points = points

    xmin, ymin, zmin = np.min(points, axis=0)

    xmax, ymax, zmax = np.max(points, axis=0)

    bbox_points = np.array([[xmin, ymin, zmin],
                            [xmax, ymin, zmin],
                            [xmax, ymax, zmin],
                            [xmin, ymax, zmin],
                            [xmin, ymax, zmax],
                            [xmin, ymin, zmax],
                            [xmax, ymin, zmax],
                            [xmax, ymax, zmax]])
    bbox_lines = [[0, 1], [1, 2], [2, 3], [3, 0],
                  [0 ,5], [1, 6], [2, 7], [3, 4],
                  [4, 5], [5, 6], [6, 7], [7, 4]]
    bbox_colors = [(0, 1, 0) for _ in range(len(bbox_lines))]
    bbox = o3d.geometry.LineSet(
        points = o3d.utility.Vector3dVector(bbox_points),
        lines = o3d.utility.Vector2iVector(bbox_lines)
    )
    bbox.colors = o3d.utility.Vector3dVector(bbox_colors)
    return bbox

 

갖고 있는 open3d pcd를 넣거나 3d point array (N, 3)을 넣으면 그걸 둘러싸는 bounding box, open3d lineset을 얻을 수 있다. 

이게 제일 귀찮다. 매번 line 12개 순서대로 적기..

 

Visualization

def vis_cams(poses, pcd=None, curr_idx=0, scale=1.0):
    cams = []
    origins = []
    for i, pose in enumerate(poses):
        if i == curr_idx:
            cams.append(make_cam(pose, color=(1,0,0), scale=scale)) # red
        else:
            cams.append(make_cam(pose, color=(0,1,0), scale=scale)) # green
        origins.append(make_origin(pose, scale=scale*0.5))

    if pcd is not None:
        if isinstance(pcd, list):
            o3d.visualization.draw_geometries(cams + origins + pcd)
        else:
            o3d.visualization.draw_geometries(cams + origins + [pcd])
    else:
        o3d.visualization.draw_geometries(cams + origins)

 

위 make_cam과 make_origin으로 open3d geometry들을 잔뜩 만들어 두었다면 이걸 띄우는 코드다. 그냥 어찌보면 o3d.visualization.draw_geometries 사용한 것이지만 조금 더 예쁘게 띄우기 위해서 이런 저런 설정을 추가한 것이다. 

 

1) 첫번째 카메라는 빨간색으로 표기, 나머지는 초록색으로 표기되어 시작이 어딘지 구분하기 쉬움

2) 카메라마다 카메라 좌표계가 같이 뜨도록 되어 있어 카메라 포즈를 시각적으로 보기 좋음

3) 카메라와 동시에 띄우고 싶은 3D point cloud가 따로 있다면 argument: pcd로 받아 같이 띄울 수 있음

vis_cams(poses, pcd = [house_pcd, bbox]

 

시각화 예시