Learning Open3D→Tutorial→基礎編→RGBDオドメトリ

RGBD odometry

RGBDオドメトリは、2つの連続するRGBD画像対の間でカメラの動きを見つける。 入力はRGBDImageの2つのインスタンスで、 出力は剛体変換の形の動きである。 Open3Dは、Steinbrucker et al.(2011)とPark et al.(2017)という2つのRGBDオドメトリを実装している。

Steinbrucker, F., Sturm, J., & D. Cremers (2011). Real-time visual odometry from dense RGB-D images, In ICCV Workshops.

Park, J., Zhou, Q.-Y., & V. Koltun. (2017) Colored Point Cloud Registration Revisited, ICCV.

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

In [6]:
# examples/Python/Basic/rgbd_odometry.py

import open3d as o3d
import numpy as np

if __name__ == "__main__":
    # (1)Read camera intrinsic 
    pinhole_camera_intrinsic = o3d.io.read_pinhole_camera_intrinsic(
        "../../TestData/camera_primesense.json")
    print(pinhole_camera_intrinsic.intrinsic_matrix)
    # (2)Read RGBD image
    source_color = o3d.io.read_image("../../TestData/RGBD/color/00000.jpg")
    source_depth = o3d.io.read_image("../../TestData/RGBD/depth/00000.png")
    target_color = o3d.io.read_image("../../TestData/RGBD/color/00001.jpg")
    target_depth = o3d.io.read_image("../../TestData/RGBD/depth/00001.png")
    source_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
        source_color, source_depth)
    target_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
        target_color, target_depth)
    target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        target_rgbd_image, pinhole_camera_intrinsic)

    option = o3d.odometry.OdometryOption()
    odo_init = np.identity(4)
    print(option)
    # (3)Compute odometry from two RGBD image pairs
    [success_color_term, trans_color_term,
     info] = o3d.odometry.compute_rgbd_odometry(
         source_rgbd_image, target_rgbd_image, pinhole_camera_intrinsic,
         odo_init, o3d.odometry.RGBDOdometryJacobianFromColorTerm(), option)
    [success_hybrid_term, trans_hybrid_term,
     info] = o3d.odometry.compute_rgbd_odometry(
         source_rgbd_image, target_rgbd_image, pinhole_camera_intrinsic,
         odo_init, o3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
    #  (4)Visualize RGBD image pairs
    if success_color_term:
        print("Using RGB-D Odometry")
        print(trans_color_term)
        source_pcd_color_term = o3d.geometry.PointCloud.create_from_rgbd_image(
            source_rgbd_image, pinhole_camera_intrinsic)
        source_pcd_color_term.transform(trans_color_term)
        o3d.visualization.draw_geometries([target_pcd, source_pcd_color_term])
    if success_hybrid_term:
        print("Using Hybrid RGB-D Odometry")
        print(trans_hybrid_term)
        source_pcd_hybrid_term = o3d.geometry.PointCloud.create_from_rgbd_image(
            source_rgbd_image, pinhole_camera_intrinsic)
        source_pcd_hybrid_term.transform(trans_hybrid_term)
        o3d.visualization.draw_geometries([target_pcd, source_pcd_hybrid_term])

[[525.    0.  319.5]
 [  0.  525.  239.5]
 [  0.    0.    1. ]]
odometry::OdometryOption class.
iteration_number_per_pyramid_level = [ 20, 10, 5, ] 
max_depth_diff = 0.030000
min_depth = 0.000000
max_depth = 4.000000
Using RGB-D Odometry
[[ 9.99988286e-01 -7.53983409e-05 -4.83963172e-03  2.74054550e-04]
 [ 1.83909052e-05  9.99930634e-01 -1.17782559e-02  2.29634918e-02]
 [ 4.84018408e-03  1.17780289e-02  9.99918922e-01  6.02121265e-04]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
Using Hybrid RGB-D Odometry
[[ 9.99992973e-01 -2.51084541e-04 -3.74035273e-03 -1.07049775e-03]
 [ 2.07046059e-04  9.99930714e-01 -1.17696227e-02  2.32280983e-02]
 [ 3.74304875e-03  1.17687656e-02  9.99923740e-01  1.40592054e-03]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

Read camera intrinsic

(カメラ行列の読み込み）

まず、jsonファイルからカメラの組み込み行列を読み込みむ: (examples/Python/Basic/rgbd_odometry.pyの最初の部分コード)

In [ ]:
    # (1)Read camera intrinsic 
    pinhole_camera_intrinsic = o3d.io.read_pinhole_camera_intrinsic(
        "../../TestData/camera_primesense.json")
    print(pinhole_camera_intrinsic.intrinsic_matrix)

これにより、以下の表示がなされる:

[[ 525.     0.   319.5]
 [   0.   525.   239.5]
 [   0.     0.     1. ]]

注：

Open3Dの小さなデータ構造の多くは、jsonファイルから読み書きできる。 これには、カメラの組み込み関数、カメラの軌跡、ポーズグラフなどが含まれる。

Read RGBD image

(RGBD画像の読み込み)

(examples/Python/Basic/rgbd_odometry.pyの２番目の部分コード)

In [ ]:
    # (2)Read RGBD image
    source_color = o3d.io.read_image("../../TestData/RGBD/color/00000.jpg")
    source_depth = o3d.io.read_image("../../TestData/RGBD/depth/00000.png")
    target_color = o3d.io.read_image("../../TestData/RGBD/color/00001.jpg")
    target_depth = o3d.io.read_image("../../TestData/RGBD/depth/00001.png")
    source_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
        source_color, source_depth)
    target_rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
        target_color, target_depth)
    target_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        target_rgbd_image, pinhole_camera_intrinsic)

このコードブロックは、Redwood形式の2組のRGBD画像を読み取る。 この説明についてはRedwoodのデータセットを参照せよ。

注意：

Open3Dでは、カラー画像と奥行き画像が同期して同じ座標系に位置合わせされているものと仮定している。 これは、通常、RGBDカメラ設定の同期機能と位置合わせ機能の両方をオンにすることで実行できる

Compute odometry from two RGBD image pairs

(RGBD画像ペアからの距離計算）

(examples/Python/Basic/rgbd_odometry.pyの3番目の部分コード)

In [ ]:
    # (3)Compute odometry from two RGBD image pairs
    [success_color_term, trans_color_term,
     info] = o3d.odometry.compute_rgbd_odometry(
         source_rgbd_image, target_rgbd_image, pinhole_camera_intrinsic,
         odo_init, o3d.odometry.RGBDOdometryJacobianFromColorTerm(), option)
    [success_hybrid_term, trans_hybrid_term,
     info] = o3d.odometry.compute_rgbd_odometry(
         source_rgbd_image, target_rgbd_image, pinhole_camera_intrinsic,
         odo_init, o3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)

このコードブロックは、2つの異なるRGBDオドメトリメソッドを呼び出す。 最初のものはSteinbrucker et al.(2011)である。 整列した画像の写真の一貫性を最小限に抑える。 もう一つはPark et al.(2017)で、写真の一貫性に加えて、ジオメトリの制約も実装されている。 どちらの機能も同様の速度で動作する。 しかし、Park et al.(2017)のほうがベンチマークデータセットのテストにおいて精度が高いので、それをお勧めする。

Steinbrucker, F., Sturm, J., & D. Cremers (2011). Real-time visual odometry from dense RGB-D images, In ICCV Workshops.

Park, J., Zhou, Q.-Y., & V. Koltun. (2017) Colored Point Cloud Registration Revisited, ICCV.

OdometryOption()のパラメーター：

  • inimum_correspondence_ratio：位置合わせの後、2つのRGBD画像のオーバーラップ率を測定する。 2つのRGBD画像の重複領域が指定された比率よりも小さい場合、オドメトリモジュールは失敗と見なす。

  • max_depth_diff：奥行き画像では、2つの位置合わせされたピクセルの奥行きの差が指定された値よりも小さい場合、それらは対応しているとみなす。 値を大きくすると、より積極的な検索が行われるが、結果が不安定になる傾向がある。

  • min_depthおよびmax_depth：指定された奥行き値よりも小さい、または大きいピクセルは無視される。

Visualize RGBD image pairs

(RGBD画像ペアの可視化)

(examples/Python/Basic/rgbd_odometry.pyの4番目の部分コード)

In [ ]:
    #  (4)Visualize RGBD image pairs
    if success_color_term:
        print("Using RGB-D Odometry")
        print(trans_color_term)
        source_pcd_color_term = o3d.geometry.PointCloud.create_from_rgbd_image(
            source_rgbd_image, pinhole_camera_intrinsic)
        source_pcd_color_term.transform(trans_color_term)
        o3d.visualization.draw_geometries([target_pcd, source_pcd_color_term])
    if success_hybrid_term:
        print("Using Hybrid RGB-D Odometry")
        print(trans_hybrid_term)
        source_pcd_hybrid_term = o3d.geometry.PointCloud.create_from_rgbd_image(
            source_rgbd_image, pinhole_camera_intrinsic)
        source_pcd_hybrid_term.transform(trans_hybrid_term)
        o3d.visualization.draw_geometries([target_pcd, source_pcd_hybrid_term])

RGBD画像ペアは点群に変換され、一緒にレンダリングされる。 第1の（ソース）RGBD画像を表す点群は、オドメトリによって推定された変換で変換されることに留意されたい。 この変換の後、両方の点群が整列する。

RGB-Dオドメトリ出力:

Using RGB-D Odometry
[[  9.99985131e-01  -2.26255547e-04  -5.44848980e-03  -4.68289761e-04]
 [  1.48026964e-04   9.99896965e-01  -1.43539723e-02   2.88993731e-02]
 [  5.45117608e-03   1.43529524e-02   9.99882132e-01   7.82593526e-04]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   1.00000000e+00]]

http://www.open3d.org/docs/release/_images/color_term.png

ハイブリッドRGB-Dオドメトリ出力:

Using Hybrid RGB-D Odometry
[[  9.99994666e-01  -1.00290715e-03  -3.10826763e-03  -3.75410348e-03]
 [  9.64492959e-04   9.99923448e-01  -1.23356675e-02   2.54977516e-02]
 [  3.12040122e-03   1.23326038e-02   9.99919082e-01   1.88139799e-03]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   1.00000000e+00]]

http://www.open3d.org/docs/release/_images/hybrid_term.png

Previous: RGBD画像　　　　　 Next: 視覚化