【Windows11】Python + Open3D で点群データの合成1

Python
2022.12.29
スポンサーリンク

スポンサーリンク

はじめに

前回の記事では、Open3D と Blender で点群データの表示を行いました。

【Windows11】Python + RealSenseD435 + Open3D + Blender で点群データの表示
【Windows11】Python + RealSenseD435 + Open3D + Blender で点群データの表示 Open3D と Blender それぞれで .ply ファイルの表示する方法について説明します。
nutritionfoodtech.com
2022.12.27

今回の記事では、二つの点群データを合成する方法について説明します。

前提条件

前提条件は以下の通りです。

  • pyrealsense2
  • OpenCV ==4.6.0
  • Python == 3.9.13
  • Windows11
  • open3D = 0.16.0

事前準備

事前準備として、Realsense で二つの角度から撮影した点群データを用意します。

正面から撮影したファイルを “output-front.ply”
斜めから撮影したファイルを “output-side.ply”
として保存します。

二つの点群データを一つの window 上で合成せずに表示すると以下のようになります。

ずれていますね。今回はこれを合成していきます。

点群データ合成プログラム

早速、合成プログラムを以下に記載します。
こちらのプログラムは、Open3D の 公式example のコードを少し変更したものです。

import open3d as o3d
import numpy as np

# load pcd data
def load_point_clouds(voxel_size=0.0, pcd_data=""):
    pcds = []
    for i in range(len(pcd_data)):
        pcd = o3d.io.read_point_cloud(pcd_data[i])
        pcd.paint_uniform_color([1-i, 0.706, i])
        pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
        pcds.append(pcd_down)
    return pcds


def pairwise_registration(source, target, max_correspondence_distance_coarse,
                          max_correspondence_distance_fine):
    print("Apply point-to-plane ICP")
    icp_coarse = o3d.pipelines.registration.registration_icp(
        source, target, max_correspondence_distance_coarse, np.identity(4),
        o3d.pipelines.registration.TransformationEstimationPointToPlane())
    icp_fine = o3d.pipelines.registration.registration_icp(
        source, target, max_correspondence_distance_fine,
        icp_coarse.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane())
    transformation_icp = icp_fine.transformation
    information_icp = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
        source, target, max_correspondence_distance_fine,
        icp_fine.transformation)
    return transformation_icp, information_icp


def full_registration(pcds, max_correspondence_distance_coarse, max_correspondence_distance_fine):
    pose_graph = o3d.pipelines.registration.PoseGraph()
    odometry = np.identity(4)
    pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))
    n_pcds = len(pcds)
    for source_id in range(n_pcds):
        for target_id in range(source_id + 1, n_pcds):
            transformation_icp, information_icp = pairwise_registration(
                pcds[source_id], pcds[target_id],
                max_correspondence_distance_coarse,
                max_correspondence_distance_fine)
            print("Build o3d.pipelines.registration.PoseGraph")
            if target_id == source_id + 1:  # odometry case
                print("odometry case")
                odometry = np.dot(transformation_icp, odometry)
                pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(np.linalg.inv(odometry)))
                pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id,
                    target_id, transformation_icp, information_icp, uncertain=False))

            else:  # loop closure case
                print("loop closure")
                pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id,
                    target_id, transformation_icp, information_icp, uncertain=True))
    return pose_graph


if __name__ == "__main__":
    voxel_size = 0.005
    pcd_data = ["output-front.ply", "output-side.ply"]
    pcds_down = load_point_clouds(voxel_size, pcd_data)
    o3d.visualization.draw_geometries(pcds_down, "vovel sampling")

    print("Full registration ...")
    max_correspondence_distance_coarse = voxel_size * 15
    max_correspondence_distance_fine = voxel_size * 1.5
    # debug full registration
    with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
        pose_graph = full_registration(pcds_down,
                                        max_correspondence_distance_coarse,
                                        max_correspondence_distance_fine)

    print("Optimizing PoseGraph ...")
    option = o3d.pipelines.registration.GlobalOptimizationOption(
        max_correspondence_distance=max_correspondence_distance_fine,
        edge_prune_threshold=0.25,
        reference_node=0)
    # debug optimizing posegraph
    with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
        o3d.pipelines.registration.global_optimization(
            pose_graph,
            o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
            o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
            option)

    print("Transform points and display")
    for point_id in range(len(pcds_down)):
        print(pose_graph.nodes[point_id].pose)
        pcds_down[point_id].transform(pose_graph.nodes[point_id].pose)
    o3d.visualization.draw_geometries(pcds_down)

非常に難解でした。理解できていない部分がありますので、説明しながら理解していきます。

早速、実行します。

最初の window の表示はこのようになります。

黄色と水色はそれぞれ角度の異なる点群データです。
window を閉じて、1s 程度経過すると合成された結果が表示されます。

完全に合成されてはいないですが、角度変換は合っているので、続けていきます。

プログラム説明

元データの読込と表示

はじめに、メイン関数から説明していきます。サブ関数が現れた時点で、サブ関数の説明に移ります。

voxel_size = 0.005
pcd_data = ["output-front.ply", "output-side.ply"]
pcds_down = load_point_clouds(voxel_size, pcd_data)
o3d.visualization.draw_geometries(pcds_down, "vovel sampling")

voxel_size と draw_geometries による複数の点群データの表示方法については、前回説明したので割愛します。

load_point_clouds

load_point_clouds 関数について説明していきます。

# load pcd data
def load_point_clouds(voxel_size=0.0, pcd_data=""):
    pcds = []
    for i in range(len(pcd_data)):
        pcd = o3d.io.read_point_cloud(pcd_data[i])
        pcd.paint_uniform_color([1-i, 0.706, i])
        pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
        pcds.append(pcd_down)
    return pcds

o3d.io.read_point_cloud() で、.ply データを読み込みます。

paint_uniform_color([1-i, 0.706, i]) で、読み込んだ点群に色を付けます。
paint_uniform_color() の説明は公式ドキュメントの中腹にあります。
リスト内は RGB 表記で、全て 0 – 1 で記述します。

vocel_down_sample で、ダウンサンプリングを実行します。

複数の点群リストを pcds としてまとめます。

Pose Graph の作成

print("Full registration ...")
max_correspondence_distance_coarse = voxel_size * 15
max_correspondence_distance_fine = voxel_size * 1.5
# debug full registration
with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
    pose_graph = full_registration(pcds_down,
                                    max_correspondence_distance_coarse,
                                    max_correspondence_distance_fine)
  • max_correspondence_distance_coarse … 対応するボクセル間を切り離す最大距離
    　　　　　　　　　　　　　　　　　　　　　初期スクリーニングで相関有とみなす距離？
  • max_correspondence_distance_fine … 対応するボクセル間の相関が強いとする最大距離

o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm
は、with 内の Open3D の挙動に関して、Debugログ を表示する関数です。

pose_graph = full_registration(pcds_down,
                                max_correspondence_distance_coarse,
                                max_correspondence_distance_fine)

full_registration について説明していきます。

full_registration

full_registration では、各点群データをまとめて Pose Graph 形式に変換します。

def full_registration(pcds, max_correspondence_distance_coarse, max_correspondence_distance_fine):
    pose_graph = o3d.pipelines.registration.PoseGraph()
    odometry = np.identity(4)
    pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))
    n_pcds = len(pcds)
    for source_id in range(n_pcds):
        for target_id in range(source_id + 1, n_pcds):
            transformation_icp, information_icp = pairwise_registration(
                pcds[source_id], pcds[target_id],
                max_correspondence_distance_coarse,
                max_correspondence_distance_fine)
            print("Build o3d.pipelines.registration.PoseGraph")
            if target_id == source_id + 1:  # odometry case
                print("odometry case")
                odometry = np.dot(transformation_icp, odometry)
                pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(np.linalg.inv(odometry)))
                pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id,
                    target_id, transformation_icp, information_icp, uncertain=False))

            else:  # loop closure case
                print("loop closure")
                pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id,
                    target_id, transformation_icp, information_icp, uncertain=True))
    return pose_graph

こちらの関数も順に説明していきます。

pose_graph = o3d.pipelines.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))

空の PoseGraph を作成し、4×4 の単位行列を作成します。
この単位行列を 空のPoseGraph の先頭ノードに登録します。

次は、点群データの数だけ source と target をペアにしていきます。

transformation_icp, information_icp = pairwise_registration(
                pcds[source_id], pcds[target_id],
                max_correspondence_distance_coarse,
                max_correspondence_distance_fine)

pairwise_registration について説明していきます。

pairwise_registration

先に、コードを掲載します。

def pairwise_registration(source, target, max_correspondence_distance_coarse,
                          max_correspondence_distance_fine):
    print("Apply point-to-plane ICP")
    icp_coarse = o3d.pipelines.registration.registration_icp(
        source, target, max_correspondence_distance_coarse, np.identity(4),
        o3d.pipelines.registration.TransformationEstimationPointToPlane())
    icp_fine = o3d.pipelines.registration.registration_icp(
        source, target, max_correspondence_distance_fine,
        icp_coarse.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane())
    transformation_icp = icp_fine.transformation
    information_icp = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
        source, target, max_correspondence_distance_fine,
        icp_fine.transformation)
    return transformation_icp, information_icp

pairwise_registration 関数は、source と target を引数とします。

icp_coarse = o3d.pipelines.registration.registration_icp(
        source, target, max_correspondence_distance_coarse, np.identity(4),
        o3d.pipelines.registration.TransformationEstimationPointToPlane())

o3d.pipelines.registration.registration_icp() はこちらの公式ドキュメントに記載があります。
o3d.pipelines.registration.TransformationEstimationPointToPlane() は、点群間の変換推定の方法を指定します。

ICPについては、こちらの Wikipedia を参照ください。
簡単に言うと、点群sourceと点群targetの各点を比較し、距離が最も近いものを同じ点とみなします。なので、大きなずれや角度変化に弱く、Realsense の撮影場所は近い必要があります。

今回のデータとは全くマッチしていません。改善策は別の記事にて説明します。

icp_fine = o3d.pipelines.registration.registration_icp(
        source, target, max_correspondence_distance_fine,
        icp_coarse.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane())

こちらも同じ関数です、与える変数が少し異なります。
icp_coarse で初期スクリーニングを行い、icp_fine で、本命の点を洗いだします。

戻り値は、変換に関する情報 (open3d.pipelines.registration.RegistrationResult) です。こちらに記載があります。

transformation_icp = icp_fine.transformation

変換行列を transformation_icp 変数に代入します。

information_icp = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
        source, target, max_correspondence_distance_fine,
        icp_fine.transformation)

変換行列から情報行列を作成します。関数の詳細はこちらです。

full_registration 続き

pairwise_registration が完了したら、戻り値の変換行列と情報行列をしようして PoseGraph を作成していきます。

if target_id == source_id + 1:  # odometry case
    print("odometry case")
    odometry = np.dot(transformation_icp, odometry)
    pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(np.linalg.inv(odometry)))
    pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id,
                                 target_id, transformation_icp, information_icp, uncertain=False))

隣り合う点群データの場合、(ICPならでは？) 単位行列と変換行列の積で odometry を更新します。
PoseGraphノードに更新した odometry の逆行列
PoseGraphエッジに sourceのID, targetのID, 変換行列、情報行列を登録します。source と隣り合う点群データなので、本当に先端エッジかどうかわかりません。この場合、Uncertain=False とします。詳細はこちらです。

else:  # loop closure case
    print("loop closure")
    pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id,
                                     target_id, transformation_icp, information_icp, uncertain=True))

隣り合う点群データでない場合、PoseGraphエッジのみに登録を行います。エッジであることは明確なので、uncertain=True とします。

グローバル最適化

PoseGraph の作成が終わったら、グローバル最適化を実行します。
その前に、グローバル最適化用のオプションを作成します。

option = o3d.pipelines.registration.GlobalOptimizationOption(
        max_correspondence_distance=max_correspondence_distance_fine,
        edge_prune_threshold=0.25,
        reference_node=0)

o3d.pipelines.registration.GlobalOptimizationOption() の説明はこちらにあります。
使用する点群データが少ないので、とりあえずデフォルトにしておきます。

各引数は

  • max_correspondence_distance … 隣接点とみなす最大距離
  • edge_prune_threshold … 枝刈りのしきい値
  • reference_node … 固定するノード

となります。

o3d.pipelines.registration.global_optimization(
            pose_graph,
            o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
            o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
            option)

o3d.pipelines.registration.global_optimization() は、作成した PoseGraph を最適化します。
詳細はこちらにあります。

o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt() は、最適化メソッドを指定します。GlobalOptimizationGaussNewton() もあります。

o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria() は、収束基準を指定します。

最適化結果の表示

最後に、表示部分です。

print("Transform points and display")
for point_id in range(len(pcds_down)):
    print(pose_graph.nodes[point_id].pose)
    pcds_down[point_id].transform(pose_graph.nodes[point_id].pose)
o3d.visualization.draw_geometries(pcds_down)

pcds_down[point_id].transform(pose_graph.nodes[point_id].pose) で、元の点群データを変換します。

色付けしていない結果

改めて、合成前の結果を載せておきます。

合成結果は、以下のようになります。

別の角度から見てみます。

何となく合っています。

おわりに

今回は点群合成の方法について説明しました。
問題点は

  • PoseGraph作成メソッドが今回のデータに適していないこと
  • 点群データが少ないこと

が、挙げられます。

次回は、上記の問題点を改善しようと思います。

【Windows11】Python + Open3D で点群データの合成2
【Windows11】Python + Open3D で点群データの合成について説明します。 前回は、合成がうまくいかなかったので、合成の検証を行いました。
nutritionfoodtech.com
2022.12.31
スポンサーリンク

コメント

タイトルとURLをコピーしました