Open3d manual registration (손으로 point cloud 정합하기)

Point cloud 여러개를 ICP를 이용해 registration(정합)하는 일은 생각보다 빈번하게 발생하는데, 이 때 초기 transformation을 제대로 정해주지 않으면 정합이 이상하게 된다. 특히 정합 대상인 두 point cloud가 noise point를 많이 갖고 있을수록 초기화가 어렵고 정합 성능도 떨어진다.
 
이럴 때 정합 성능을 높일 방법으로 RANSAC 같이 trial-and-error 접근을 할수도 있지만 경험적으로 보았을 때는 수가 많지 않다면 그냥 손으로 기준점을 잡아주는 것이 최고였다. 다른 말로, 사람이 직접 손으로 correspondence 몇 개를 잡아주고 이를 바탕으로 초기 transformation을 계산하여 사용하는 것이다. 
 
http://www.open3d.org/docs/0.9.0/tutorial/Advanced/interactive_visualization.html

Interactive visualization — Open3D 0.9.0 documentation

 
그 방식은 직접 구현하기는 까다롭지만 open3d에 잘 구현되어 있다. 아래 적을 코드는 위 링크에서 제공하는 코드를 재배열/재조합한 것일 뿐 특별한 것은 없고, 약간의 사용성을 높이기 위해 한 가지 기능만 더했다. (아래 3.이 내가 추가한 것이다.)
 
아래 코드를 돌려보면 GUI가 뜰 것인데 사용법은 다음과 같다.

1. source point cloud가 먼저 뜬다. shift + left click 을 통해 correspondence로 사용할 포인트를 M개 선택한다. q를 눌러 종료한다. (잘못 클릭했다면 shift + right click 하면 지워진다.)
2. destination point cloud가 이어서 뜬다. 역시 shift + left click을 통해 직전에 correspondence로 찍은 점 M개를 찾아 선택한다. q를 눌러 종료한다. 
3. 커맨드 라인에 지속할 것인지 y/n가 뜬다. 한 번 더 1~2를 반복하면서 기존 M개 이상의 correspondence를 사용하고 싶을 경우, y를 커맨드 라인에 입력한다. 아니라면 n을 입력하여 종료한다.
4. 결과로 return되는 4 x 4 transformation matrix가 source pcd를 destimation pcd에 갖다붙여주는 matrix다. T_dst_src 다.

 

import copy
import open3d as o3d
import numpy as np

def pick_points(pcd):
    print("")
    print("1) Please pick at least three correspondences using [shift + left click]")
    print("   Press [shift + right click] to undo point picking")
    print("2) Afther picking points, press q for close the window")
    vis = o3d.visualization.VisualizerWithEditing()
    vis.create_window()
    vis.add_geometry(pcd)
    vis.run()  # user picks points
    vis.destroy_window()
    print("")
    return vis.get_picked_points()

def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp])

def manual_registration(src_pcd, dst_pcd):
    print("Demo for manual ICP")
    print("Visualization of two point clouds before manual alignment")
    draw_registration_result(dst_pcd, src_pcd, np.identity(4))

    ### pick points from two point clouds and builds correspondences
    picked_id_source = []
    picked_id_target = []

    while True:
        picked_id_source += pick_points(src_pcd)
        picked_id_target += pick_points(dst_pcd)

        answer = input("continue (y/n) : ")
        if answer.lower() != "y":
            break


    print(picked_id_source)
    print(picked_id_target)
    assert (len(picked_id_source) >= 3 and len(picked_id_target) >= 3)
    assert (len(picked_id_source) == len(picked_id_target))

    corr = np.zeros((len(picked_id_source), 2))
    corr[:, 0] = picked_id_source
    corr[:, 1] = picked_id_target

    # estimate rough transformation using correspondences
    print("Compute a rough transform using the correspondences given by user")
    p2p = o3d.pipelines.registration.TransformationEstimationPointToPoint()
    trans_init = p2p.compute_transformation(src_pcd, dst_pcd, o3d.utility.Vector2iVector(corr))

    # point-to-point ICP for refinement
    print("Perform point-to-point ICP refinement")
    threshold = 0.001  # 3cm distance threshold
    reg_p2p = o3d.pipelines.registration.registration_icp(src_pcd, dst_pcd, threshold, trans_init, o3d.pipelines.registration.TransformationEstimationPointToPoint())


    draw_registration_result(src_pcd, dst_pcd, reg_p2p.transformation)
    return reg_p2p.transformation

 

*주의*

1~3 과정을 반복하면서 source/destimation 양쪽에서 찍은 correspondence 수는 당연하게도 같아야 한다. 아니라면 오류가 난다.