Global registration¶

大局的位置合わせ

ICP位置合わせと色付きポイントクラウド位置合わせの両方とも、初期設定として大まかなアラインメントに依存しているため、ローカル位置合わせメソッドとして知られている。このチュートリアルでは、グローバル位置合わせと呼ばれる別のクラスの位置合わせメソッドを示す。これらのアルゴリズムは、初期化のためのアライメントを必要としない。通常は、タイトな位置合わせ結果が生成されず、局所メソッドの初期化として使用される。

ヘルパー関数
Input
幾何的特徴量の抽出
RANSAC
局所的精緻化

注意: コードを走らせる環境に注意すること。TestDataディレクトリがカレント(作業)ディレクトリからみて、祖先ディレクトリの下にあることを確認しよう

# examples/Python/Advanced/global_registration.py

import open3d as o3d
import numpy as np
import copy


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 preprocess_point_cloud(pcd, voxel_size):
    print(":: Downsample with a voxel size %.3f." % voxel_size)
    pcd_down = pcd.voxel_down_sample(voxel_size)

    radius_normal = voxel_size * 2
    print(":: Estimate normal with search radius %.3f." % radius_normal)
    pcd_down.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

    radius_feature = voxel_size * 5
    print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
    pcd_fpfh = o3d.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh


def prepare_dataset(voxel_size):
    print(":: Load two point clouds and disturb initial pose.")
    source = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd")
    target = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd")
    trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0],
                             [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
    source.transform(trans_init)
    draw_registration_result(source, target, np.identity(4))

    source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
    target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)
    return source, target, source_down, target_down, source_fpfh, target_fpfh


def execute_global_registration(source_down, target_down, source_fpfh,
                                target_fpfh, voxel_size):
    distance_threshold = voxel_size * 1.5
    print(":: RANSAC registration on downsampled point clouds.")
    print("   Since the downsampling voxel size is %.3f," % voxel_size)
    print("   we use a liberal distance threshold %.3f." % distance_threshold)
    result = o3d.registration.registration_ransac_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh, distance_threshold,
        o3d.registration.TransformationEstimationPointToPoint(False), 4, [
            o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
            o3d.registration.CorrespondenceCheckerBasedOnDistance(
                distance_threshold)
        ], o3d.registration.RANSACConvergenceCriteria(4000000, 500))
    return result


def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size):
    distance_threshold = voxel_size * 0.4
    print(":: Point-to-plane ICP registration is applied on original point")
    print("   clouds to refine the alignment. This time we use a strict")
    print("   distance threshold %.3f." % distance_threshold)
    result = o3d.registration.registration_icp(
        source, target, distance_threshold, result_ransac.transformation,
        o3d.registration.TransformationEstimationPointToPlane())
    return result


if __name__ == "__main__":
    voxel_size = 0.05  # means 5cm for the dataset
    source, target, source_down, target_down, source_fpfh, target_fpfh = \
            prepare_dataset(voxel_size)

    result_ransac = execute_global_registration(source_down, target_down,
                                                source_fpfh, target_fpfh,
                                                voxel_size)
    print(result_ransac)
    draw_registration_result(source_down, target_down,
                             result_ransac.transformation)

    result_icp = refine_registration(source, target, source_fpfh, target_fpfh,
                                     voxel_size)
    print(result_icp)
    draw_registration_result(source, target, result_icp.transformation)

:: Load two point clouds and disturb initial pose.
:: Downsample with a voxel size 0.050.
:: Estimate normal with search radius 0.100.
:: Compute FPFH feature with search radius 0.250.
:: Downsample with a voxel size 0.050.
:: Estimate normal with search radius 0.100.
:: Compute FPFH feature with search radius 0.250.
:: RANSAC registration on downsampled point clouds.
   Since the downsampling voxel size is 0.050,
   we use a liberal distance threshold 0.075.
registration::RegistrationResult with fitness=6.777311e-01, inlier_rmse=3.606992e-02, and correspondence_set size of 3226
Access transformation to get result.
:: Point-to-plane ICP registration is applied on original point
   clouds to refine the alignment. This time we use a strict
   distance threshold 0.020.
registration::RegistrationResult with fitness=6.210275e-01, inlier_rmse=6.565177e-03, and correspondence_set size of 123482
Access transformation to get result.

Input¶

examples/Python/Advanced/global_registration.pyの3番目の関数prepare_dataset：

    print(":: Load two point clouds and disturb initial pose.")
    source = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd")
    target = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd")
    trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0],
                             [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
    source.transform(trans_init)
    draw_registration_result(source, target, np.identity(4))

    source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
    target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)
    return source, target, source_down, target_down, source_fpfh, target_fpfh

このスクリプトは、2つのファイルからソースのポイントクラウドとターゲットのポイントクラウドを読み込む。これらは変換として単位行列を用い、整列してはいない。

Extract geometric feature¶

幾何的特徴量の抽出

examples/Python/Advanced/global_registration.pyの2番目の関数:preprocess_point_cloud

	

def preprocess_point_cloud(pcd, voxel_size):
    print(":: Downsample with a voxel size %.3f." % voxel_size)
    pcd_down = pcd.voxel_down_sample(voxel_size)

    radius_normal = voxel_size * 2
    print(":: Estimate normal with search radius %.3f." % radius_normal)
    pcd_down.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

    radius_feature = voxel_size * 5
    print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
    pcd_fpfh = o3d.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh

ポイントクラウドをサンプリングして法線を推定し、各ポイントのFPFH特徴量を計算する。 FPFH特徴量は、点の局所的幾何学特性を記述する33次元ベクトルである。 33次元空間における最近傍検索により、似たような局所的幾何学構造を有する点を返すことができる。詳細はRasu et al.(2009)を参照のこと。

Rusu, R., Blodow, N. & M. Beetz. (2009). Fast Point Feature Histograms (FPFH) for 3D registration, ICRA.

注: FPFH特徴量については http://pointclouds.org/documentation/tutorials/fpfh_estimation.php

RANSAC¶

examples/Python/Advanced/global_registration.pyの4番目の関数:execute_global_registration

def execute_global_registration(source_down, target_down, source_fpfh,
                                target_fpfh, voxel_size):
    distance_threshold = voxel_size * 1.5
    print(":: RANSAC registration on downsampled point clouds.")
    print("   Since the downsampling voxel size is %.3f," % voxel_size)
    print("   we use a liberal distance threshold %.3f." % distance_threshold)
    result = o3d.registration.registration_ransac_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh, distance_threshold,
        o3d.registration.TransformationEstimationPointToPoint(False), 4, [
            o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
            o3d.registration.CorrespondenceCheckerBasedOnDistance(
                distance_threshold)
        ], o3d.registration.RANSACConvergenceCriteria(4000000, 500))
    return result

グローバル位置合わせにはRANSACを使用する。各RANSAC反復では、ソースのポイントクラウドからrandac_n個のランダムな点が選択される。ターゲットのポイントクラウド内における対応点は、33次元FPFH特徴量空間内の最も近い近傍を検索することによって検出される。プルーニング(枝きり)のステップでは、初期段階での誤った一致点を削除するための高速プルーニングアルゴリズムが必要である。

注: RANSACについては　https://qiita.com/smurakami/items/14202a83bd13e55d4c09　(OpenCVでも使用されている)

Open3Dは以下のプルーニングアルゴリズムを提供している：

CorrespondenceCheckerBasedOnDistanceは、整列したポイントクラウドがクローズしている（指定された閾値未満）かどうかをチェックする。
CorrespondenceCheckerBasedOnEdgeLengthは、ソースとターゲットの対応関係から個別に描画された任意の2つのエッジ（2つの頂点で形成された線）の長さが似ているかどうかをチェックする。このチュートリアルでは、 $\| edge_{source} \| > 0.9 \times \| edge_{target} \|$かつ$\| edge_{target} \| > 0.9 \times \| edge_{source} \| $が成り立っているかをチェック
CorrespondenceCheckerBasedOnNormal は、対応する頂点の法線同士が整合しているかを調べる。つまり、2つの法線ベクトルの内積を計算し、ラディアン値で指定された閾値と比較する。

プルーニング・ステップを通過したマッチのみが変換を計算するために使用され、変換はポイントクラウド全体で検証される。中核となる機能は、registration_ransac_based_on_feature_matchingである。この関数の最も重要なハイパーパラメータはRANSACConvergenceCriteriaである。これは、RANSAC反復の最大数と検証ステップの最大数を定義する。これらの2つの数値が大きいほど結果はより正確になるが、アルゴリズムに要する時間も長くなる。

我々は Choi(2015) で与えられた経験値に基づいてRANSACパラメータを設定している。結果は次のとおり：

注意¶

Open3Dはグローバル位置合わせのために速い実装を提供している。Fast global registrationを参照のこと。

ローカルな精緻化

パフォーマンス上の理由から、グローバル位置合わせは、大幅にダウンサンプリングされたポイントクラウドに対してのみ実行される。結果もタイトなものではない。そこで、アライメントをさらに洗練するために、ポイントツープレーンICPを使用する。

examples/Python/Advanced/global_registration.pyの5番目の関数:execute_global_registration

def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size):
    distance_threshold = voxel_size * 0.4
    print(":: Point-to-plane ICP registration is applied on original point")
    print("   clouds to refine the alignment. This time we use a strict")
    print("   distance threshold %.3f." % distance_threshold)
    result = o3d.registration.registration_icp(
        source, target, distance_threshold, result_ransac.transformation,
        o3d.registration.TransformationEstimationPointToPlane())
    return result

タイトなアライメントを出力する。これは完全なペアごとの位置合わせワークフローをまとめたものである。

Previous:色付きポイントクラウド位置合わせ　　　　　　　　 Next:高速グローバル位置合わせ

Global registration¶

Input¶

Extract geometric feature¶

RANSAC¶

注意¶

Local refinement¶